RVIZ RobotModel dynamic update

.github/workflows/build-others.yml

@@ -34,13 +34,15 @@ jobs:
       env:
        GITHUB_CONTEXT: ${{ toJson(github) }}
     - name: Set repository name based on event (pull_request/push)
+      shell: bash
       run: |
          if [[ "${{ github.event_name }}" == "pull_request" ]]; then
            echo "GITHUB_REPO_TRIGGER=${{ github.event.pull_request.head.repo.full_name }}" >> $GITHUB_ENV
          else
            echo "GITHUB_REPO_TRIGGER=${{ github.repository }}" >> $GITHUB_ENV
          fi
     - name: Use repository name
+      shell: bash
       run: echo "The triggering repository is $GITHUB_REPO_TRIGGER"
     - uses: actions/checkout@v4
       with:

.github/workflows/build.yml

@@ -20,8 +20,6 @@ on:
 env:
   # Branch that triggered this workflow
   GITHUB_BRANCH: ${{ github.head_ref || github.ref_name }}
-  # Repository that triggered this workflow
-  GITHUB_REPO_TRIGGER: ${{ github.actor }}/${{ github.event.repository.name }}
 
 jobs:
   build:
@@ -43,6 +41,22 @@ jobs:
             build-type: Debug
     runs-on: ${{ matrix.os }}
     steps:
+    - name: Dump github context
+      run:   echo "$GITHUB_CONTEXT"
+      shell: bash
+      env:
+       GITHUB_CONTEXT: ${{ toJson(github) }}
+    - name: Set repository name based on event (pull_request/push)
+      shell: bash
+      run: |
+         if [[ "${{ github.event_name }}" == "pull_request" ]]; then
+           echo "GITHUB_REPO_TRIGGER=${{ github.event.pull_request.head.repo.full_name }}" >> $GITHUB_ENV
+         else
+           echo "GITHUB_REPO_TRIGGER=${{ github.repository }}" >> $GITHUB_ENV
+         fi
+    - name: Use repository name
+      shell: bash
+      run: echo "The triggering repository is $GITHUB_REPO_TRIGGER"
     - name: Linux cleanup
       if: matrix.os == 'ubuntu-22.04'
       run: |

include/mc_rbdyn/RobotModule.h

@@ -510,7 +510,10 @@ struct MC_RBDYN_DLLAPI RobotModule
   /** Returns the list of parameters passed to mc_rbdyn::RobotLoader::get_robot_module to obtain this module */
   inline const std::vector<std::string> & parameters() const { return _parameters; }
 
-  /** Returns the list of parameters to get a RobotModule that is a canonical representation of this module */
+  /** Returns the list of parameters to get a RobotModule that is a canonical representation of this module
+   *
+   * An empty vector means that the canonical and control module are the same
+   **/
   inline const std::vector<std::string> & canonicalParameters() const { return _canonicalParameters; }
 
   /** Returns the configuration for the control to canonical conversion

plugins/ROS/include/mc_rtc_ros/ros.h

@@ -8,6 +8,8 @@
 
 #include <mc_rtc/config.h>
 
+#include <mc_rbdyn/Robots.h>
+
 #include <SpaceVecAlg/SpaceVecAlg>
 
 #include <Eigen/Geometry>
@@ -100,6 +102,12 @@ struct MC_RTC_ROS_DLLAPI ROSBridge
    */
   static void stop_robot_publisher(const std::string & publisher);
 
+  /** Remove the publisher of every removed robot
+   *
+   * \param robots Controller's robots
+   */
+  static void remove_extra_robot_publishers(const mc_rbdyn::Robots & robots);
+
   /*! \brief Stop ROS */
   static void shutdown();
 

plugins/ROS/src/mc_rtc_ros/ros.cpp

@@ -2,7 +2,6 @@
  * Copyright 2015-2019 CNRS-UM LIRMM, CNRS-AIST JRL
  */
 
-#include <mc_rbdyn/Robots.h>
 #include <mc_rtc/config.h>
 #include <mc_rtc/logging.h>
 #include <mc_rtc/utils.h>
@@ -670,6 +669,23 @@ void ROSBridge::stop_robot_publisher(const std::string & publisher)
   impl.rpubs.erase(it);
 }
 
+void ROSBridge::remove_extra_robot_publishers(const mc_rbdyn::Robots & robots)
+{
+  static auto & impl = impl_();
+
+  for(auto it = impl.rpubs.begin(); it != impl.rpubs.end();)
+  {
+    const std::string & topic = it->first;
+
+    size_t pos = topic.find('/');
+    if(pos != std::string::npos && pos + 1 < topic.size())
+    {
+      if(!robots.hasRobot(topic.substr(pos + 1))) { it = impl.rpubs.erase(it); }
+      else { ++it; }
+    }
+  }
+}
+
 void ROSBridge::shutdown()
 {
 #ifdef MC_RTC_ROS_IS_ROS2

plugins/ROS/src/plugin/ROS.cpp

@@ -33,64 +33,40 @@ void ROSPlugin::init(mc_control::MCGlobalController & controller, const mc_rtc::
 
 void ROSPlugin::reset(mc_control::MCGlobalController & controller)
 {
-  if(publish_control)
+  auto publish_robots = [&controller](const std::string & prefix, mc_rbdyn::Robots & robots, bool use_real)
   {
-    mc_rtc::ROSBridge::init_robot_publisher("control", controller.timestep(), controller.controller().outputRobot());
-  }
-  if(publish_env)
-  {
-    auto publish_env = [&controller](const std::string & prefix, mc_rbdyn::Robots & robots, bool use_real)
+    for(const auto & r : robots)
     {
-      for(size_t i = 1; i < robots.size(); ++i)
-      {
-        mc_rtc::ROSBridge::init_robot_publisher(prefix + "_" + std::to_string(i), controller.timestep(),
-                                                robots.robot(i), use_real);
-      }
-    };
-    publish_env("control/env", controller.controller().outputRobots(), false);
-    if(publish_real) { publish_env("real/env", controller.controller().outputRealRobots(), true); }
-  }
-  if(publish_real)
-  {
-    const auto & real_robot = controller.controller().outputRealRobot();
-    mc_rtc::ROSBridge::init_robot_publisher("real", controller.timestep(), real_robot, true);
-  }
+      mc_rtc::ROSBridge::init_robot_publisher(prefix + r.name(), controller.timestep(), r, use_real);
+    }
+  };
+
+  publish_robots("control/", controller.robots(), false);
+
+  if(publish_real) { publish_robots("real/", controller.robots(), true); }
 }
 
 void ROSPlugin::after(mc_control::MCGlobalController & controller)
 {
-  if(publish_control)
+  auto update_robots = [this, &controller](const std::string & prefix, mc_rbdyn::Robots & robots)
   {
-    mc_rtc::ROSBridge::update_robot_publisher("control", controller.timestep(), controller.controller().outputRobot());
-  }
-  // Publish environment state
-  if(publish_env)
-  {
-    auto update_env = [this, &controller](const std::string & prefix, mc_rbdyn::Robots & robots)
+    for(const auto & r : robots)
     {
-      for(size_t i = 1; i < robots.size(); ++i)
-      {
-        mc_rtc::ROSBridge::update_robot_publisher(prefix + "_" + std::to_string(i), controller.timestep(),
-                                                  robots.robot(i));
-      }
-      published_env = std::max<size_t>(publish_env, robots.size() - 1);
-    };
-    update_env("control/env", controller.controller().outputRobots());
-    if(publish_real) { update_env("real/env", controller.controller().outputRealRobots()); }
-  }
-  // Publish real robot
-  if(publish_real)
-  {
-    auto & real_robot = controller.controller().outputRealRobot();
-    mc_rtc::ROSBridge::update_robot_publisher("real", controller.timestep(), real_robot);
-  }
+      mc_rtc::ROSBridge::update_robot_publisher(prefix + r.name(), controller.timestep(), r);
+    }
+    published_topics = robots.size() - 1;
+  };
+  update_robots("control/", controller.robots());
+  if(publish_real) { update_robots("real/", controller.robots()); }
+
+  mc_rtc::ROSBridge::remove_extra_robot_publishers(controller.robots());
 }
 
 ROSPlugin::~ROSPlugin()
 {
   mc_rtc::ROSBridge::stop_robot_publisher("control");
   mc_rtc::ROSBridge::stop_robot_publisher("real");
-  for(size_t i = 0; i < published_env; ++i)
+  for(size_t i = 0; i < published_topics; ++i)
   {
     mc_rtc::ROSBridge::stop_robot_publisher("control/env_" + std::to_string(i + 1));
     mc_rtc::ROSBridge::stop_robot_publisher("real/env_" + std::to_string(i + 1));

plugins/ROS/src/plugin/ROS.h

@@ -37,7 +37,7 @@ struct ROSPlugin : public mc_control::GlobalPlugin
   bool publish_env = true;
   bool publish_real = true;
   double publish_timestep = 0.01;
-  size_t published_env = 0;
+  size_t published_topics = 0;
   std::unique_ptr<ROSServices> services_;
 
   void build(mc_control::MCGlobalController & controller);