Fix Ros2ControlManager chained controller logic (#3301)

Signed-off-by: Paul Gesel <paul.gesel@picknik.ai>

Author: pac48

moveit_plugins/moveit_ros_control_interface/src/controller_manager_plugin.cpp

@@ -516,10 +516,12 @@ class Ros2ControlManager : public moveit_controller_manager::MoveItControllerMan
       {
         auto ind = controller_name_map[chained_controller.name];
         dependency_map_[controller.name].push_back(chained_controller.name);
-        controller.required_command_interfaces = result->controller[ind].required_command_interfaces;
-        controller.claimed_interfaces = result->controller[ind].claimed_interfaces;
-        result->controller[ind].claimed_interfaces.clear();
-        result->controller[ind].required_command_interfaces.clear();
+        std::copy(result->controller[ind].required_command_interfaces.begin(),
+                  result->controller[ind].required_command_interfaces.end(),
+                  std::back_inserter(controller.required_command_interfaces));
+        std::copy(result->controller[ind].reference_interfaces.begin(),
+                  result->controller[ind].reference_interfaces.end(),
+                  std::back_inserter(controller.required_command_interfaces));
       }
     }