Merge pull request #434 from antodld/topic/AddForceSensorRosFix

[mc_rtc_ros] Fix force sensor publishing
jrl-umi3218 · Feb 20, 2024 · d1464f6 · d1464f6
2 parents 89d0567 + ebb6fab
commit d1464f6
Show file tree

Hide file tree

Showing 3 changed files with 35 additions and 15 deletions.
diff --git a/.github/workflows/build.yml b/.github/workflows/build.yml
@@ -126,7 +126,12 @@ jobs:
           - path: jrl-umi3218/RBDyn
           - path: jrl-umi3218/Tasks
           - path: jrl-umi3218/mc_rbdyn_urdf
-          - path: jrl-umi3218/mc_rtc_data
+          - path: jrl-umi3218/jvrc_description
+            options: -DDISABLE_ROS:BOOL=ON
+          - path: jrl-umi3218/mc_env_description
+            options: -DDISABLE_ROS:BOOL=ON
+          - path: jrl-umi3218/mc_int_obj_description
+            options: -DDISABLE_ROS:BOOL=ON
           - path: jrl-umi3218/eigen-quadprog
           - path: jrl-umi3218/state-observation
           - path: jrl-umi3218/tvm

diff --git a/plugins/ROS/src/mc_rtc_ros/ros.cpp b/plugins/ROS/src/mc_rtc_ros/ros.cpp
@@ -195,6 +195,23 @@ struct RobotPublisherImpl
   std::thread th;
 
   void publishThread();
+
+  void add_force_sensor(const mc_rbdyn::Robot & robot, const mc_rbdyn::ForceSensor & fs)
+  {
+#ifdef MC_RTC_ROS_IS_ROS2
+    auto tm = nh.now();
+#else
+    auto tm = Time::now();
+#endif
+    const std::string & name = fs.name();
+    data.wrenches.emplace_back();
+    auto & msg = data.wrenches.back();
+    msg.header.frame_id = prefix + name;
+    if(!robot.hasBody(fs.name()))
+    {
+      data.tfs.push_back(PT2TF(fs.X_p_f(), tm, prefix + fs.parentBody(), prefix + name, 0));
+    }
+  }
 };
 
 RobotPublisherImpl::RobotPublisherImpl(NodeHandle & nh, const std::string & prefix, double rate, double dt)
@@ -277,17 +294,7 @@ void RobotPublisherImpl::init(const mc_rbdyn::Robot & robot, bool use_real)
     data.tfs.push_back(PT2TF(id, tm, prefix + predName, prefix + succName, 0));
   }
 
-  for(const auto & fs : robot.forceSensors())
-  {
-    const std::string & name = fs.name();
-    data.wrenches.emplace_back();
-    auto & msg = data.wrenches.back();
-    msg.header.frame_id = prefix + name;
-    if(!robot.hasBody(fs.name()))
-    {
-      data.tfs.push_back(PT2TF(fs.X_p_f(), tm, prefix + fs.parentBody(), prefix + name, 0));
-    }
-  }
+  for(const auto & fs : robot.forceSensors()) { add_force_sensor(robot, fs); }
 
   for(const auto & s : robot.surfaces())
   {
@@ -426,7 +433,8 @@ void RobotPublisherImpl::update(double, const mc_rbdyn::Robot & robot)
     size_t wrench_i = 0;
     for(const auto & fs : robot.forceSensors())
     {
-      auto & msg = data.wrenches[wrench_i++];
+      if(wrench_i >= data.wrenches.size()) { add_force_sensor(robot, fs); }
+      auto & msg = data.wrenches[wrench_i];
       const sva::ForceVecd & wrench_sva = fs.wrench();
       msg.header.stamp = data.js.header.stamp;
 #ifndef MC_RTC_ROS_IS_ROS2
@@ -438,6 +446,7 @@ void RobotPublisherImpl::update(double, const mc_rbdyn::Robot & robot)
       msg.wrench.torque.x = wrench_sva.couple().x();
       msg.wrench.torque.y = wrench_sva.couple().y();
       msg.wrench.torque.z = wrench_sva.couple().z();
+      wrench_i++;
     }
   }
 

diff --git a/utils/build_and_install.sh b/utils/build_and_install.sh
@@ -941,7 +941,9 @@ then
   check_and_clone_git_dependency jrl-umi3218/mc_rtc_msgs $CATKIN_DATA_WORKSPACE_SRC
   check_and_clone_git_dependency jrl-umi3218/mc_rtc_ros $CATKIN_WORKSPACE_SRC
 else
-  check_and_clone_git_dependency jrl-umi3218/mc_rtc_data $SOURCE_DIR
+  check_and_clone_git_dependency jrl-umi3218/jvrc_description $SOURCE_DIR
+  check_and_clone_git_dependency jrl-umi3218/mc_env_description $SOURCE_DIR
+  check_and_clone_git_dependency jrl-umi3218/mc_int_obj_description $SOURCE_DIR
 fi
 
 echo_log "-- [OK] All manadatory repositories successfuly cloned or updated"
@@ -1270,7 +1272,11 @@ if $WITH_ROS_SUPPORT
 then
   build_catkin_workspace $CATKIN_DATA_WORKSPACE
 else
-  build_git_dependency jrl-umi3218/mc_rtc_data
+  export CMAKE_ADDITIONAL_OPTIONS="${OLD_CMAKE_OPTIONS} -DDISABLE_ROS:BOOL=ON"
+  build_git_dependency jrl-umi3218/jvrc_description
+  build_git_dependency jrl-umi3218/mc_env_description
+  build_git_dependency jrl-umi3218/mc_int_obj_description
+  export CMAKE_ADDITIONAL_OPTIONS="${OLD_CMAKE_OPTIONS}"
 fi
 
 echo_log "-- [SUCCESS] All mandatory dependencies have been successfully built, tested and installed"