add getters in vic_msg_utils

Author: Thibault Poignonec

cartesian_vic_controller/include/cartesian_vic_controller/vic_msgs_utils.hpp

@@ -64,6 +64,20 @@ bool get_robot_velocity(
   tf2::fromMsg(vic_state_msg.velocity, twist);
   return true;
 }
+
+bool get_robot_acceleration(
+  const cartesian_control_msgs::msg::VicControllerState & vic_state_msg,
+  Eigen::Matrix<double, 6, 1> & acceleration)
+{
+  acceleration[0] = vic_state_msg.acceleration.linear.x;
+  acceleration[1] = vic_state_msg.acceleration.linear.y;
+  acceleration[2] = vic_state_msg.acceleration.linear.z;
+  acceleration[3] = vic_state_msg.acceleration.angular.x;
+  acceleration[4] = vic_state_msg.acceleration.angular.y;
+  acceleration[5] = vic_state_msg.acceleration.angular.z;
+  return true;
+}
+
 bool get_robot_wrench(
   const cartesian_control_msgs::msg::VicControllerState & vic_state_msg,
   Eigen::Matrix<double, 6, 1> & wrench)
@@ -144,7 +158,8 @@ bool get_natural_robot_inertia(
   const cartesian_control_msgs::msg::VicControllerState & vic_state_msg,
   Eigen::Matrix<double, 6, 6> & M_natural)
 {
-  return fromMsg(vic_state_msg.natural_inertia, M_natural);
+   bool success = fromMsg(vic_state_msg.natural_inertia, M_natural);
+   return success;
 }