Refine the logic of the global state interfaces declare parameters

ros-controls · Oct 8, 2024 · 0d6f733 · 0d6f733
1 parent 1b3af76
commit 0d6f733
Showing 1 changed file with 9 additions and 5 deletions.
diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp
@@ -725,13 +725,17 @@ controller_interface::return_type ControllerManager::configure_controller(
   try
   {
     const auto avilable_state_interfaces = resource_manager_->available_state_interfaces();
-    std::map<std::string, std::string> default_state_interfaces_remap;
     std::for_each(
       avilable_state_interfaces.begin(), avilable_state_interfaces.end(),
-      [&default_state_interfaces_remap](const auto & state_interface)
-      { default_state_interfaces_remap[state_interface] = state_interface; });
-    controller->get_node()->declare_parameters(
-      "remap.state_interfaces", default_state_interfaces_remap);
+      [&controller](const auto & state_interface)
+      {
+        const auto ctrl_node = controller->get_node();
+        if (!ctrl_node->has_parameter("remap.state_interfaces." + state_interface))
+        {
+          ctrl_node->declare_parameter(
+            "remap.state_interfaces." + state_interface, state_interface);
+        }
+      });
     new_state = controller->configure();
     if (new_state.id() != lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE)
     {