Simplify access for robot description from CM by overriding RM.

[destogl]

This changes are neccessary to adjust compatibility with cleanup done in ros-controls/ros2_control#1354.

Besides that I have simplified how robot description is accessed and also passing of update_rate is not simplified since CM already reads update_rate inside itself.

[saikishor]

Just few minor changes as @ahcorde pointed out, rest look good to me :)

Thanks for this very nice refactor

[saikishor]

Looking good now!
Thank you :)

[ahcorde]

CI is not working, but locally I'm getting some error when building

/root/sim_ws/src/gz_ros_control/gz_ros2_control/src/gz_system.cpp: In member function ‘virtual hardware_interface::return_type gz_ros2_control::GazeboSimSystem::write(const rclcpp::Time&, const rclcpp::Duration&)’:
/root/sim_ws/src/gz_ros_control/gz_ros2_control/src/gz_system.cpp:692:53: error: invalid type argument of unary ‘*’ (have ‘unsigned int’)
  692 |     double velocity_sp = (-1.0) * position_error * (*this->dataPtr->update_rate);
      |                                                     ^~~~~~~~~~~~~~~~~~~~~~~~~~~
/root/sim_ws/src/gz_ros_control/gz_ros2_control/src/gz_ros2_control_plugin.cpp:69:8: error: ‘bool gz_ros2_control::GZResourceManager::load_and_initialize_components(const std::string&, unsigned int)’ marked ‘override’, but does not override
   69 |   bool load_and_initialize_components(
      |        ^~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
/root/sim_ws/src/gz_ros_control/gz_ros2_control/src/gz_ros2_control_plugin.cpp: In member function ‘bool gz_ros2_control::GZResourceManager::load_and_initialize_components(const std::string&, unsigned int)’:
/root/sim_ws/src/gz_ros_control/gz_ros2_control/src/gz_ros2_control_plugin.cpp:73:5: error: ‘components_are_loaded_and_initialized_’ was not declared in this scope
   73 |     components_are_loaded_and_initialized_ = true;
      |     ^~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
/root/sim_ws/src/gz_ros_control/gz_ros2_control/src/gz_ros2_control_plugin.cpp:85:30: error: ‘std::recursive_mutex hardware_interface::ResourceManager::resource_interfaces_lock_’ is private within this context
   85 |       std::scoped_lock guard(resource_interfaces_lock_, claimed_command_interfaces_lock_);
      |                              ^~~~~~~~~~~~~~~~~~~~~~~~~
In file included from /opt/ros/rolling/include/controller_manager/controller_manager/controller_manager.hpp:46,
                 from /root/sim_ws/src/gz_ros_control/gz_ros2_control/src/gz_ros2_control_plugin.cpp:35:
/opt/ros/rolling/include/hardware_interface/hardware_interface/resource_manager.hpp:423:32: note: declared private here
  423 |   mutable std::recursive_mutex resource_interfaces_lock_;
      |                                ^~~~~~~~~~~~~~~~~~~~~~~~~
/root/sim_ws/src/gz_ros_control/gz_ros2_control/src/gz_ros2_control_plugin.cpp:85:57: error: ‘std::recursive_mutex hardware_interface::ResourceManager::claimed_command_interfaces_lock_’ is private within this context
   85 |       std::scoped_lock guard(resource_interfaces_lock_, claimed_command_interfaces_lock_);
      |                                                         ^~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
/opt/ros/rolling/include/hardware_interface/hardware_interface/resource_manager.hpp:424:32: note: declared private here
  424 |   mutable std::recursive_mutex claimed_command_interfaces_lock_;
      |                                ^~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
/root/sim_ws/src/gz_ros_control/gz_ros2_control/src/gz_ros2_control_plugin.cpp: In member function ‘virtual void gz_ros2_control::GazeboSimROS2ControlPlugin::Configure(const gz::sim::v8::Entity&, const std::shared_ptr<const sdf::v14::Element>&, gz::sim::v8::EntityComponentManager&, gz::sim::v8::EventManager&)’:
/root/sim_ws/src/gz_ros_control/gz_ros2_control/src/gz_ros2_control_plugin.cpp:297:20: error: ‘class gz_ros2_control::GazeboSimROS2ControlPluginPrivate’ has no member named ‘robot_description_node_’
  297 |     this->dataPtr->robot_description_node_ = robot_param_node;
      |                    ^~~~~~~~~~~~~~~~~~~~~~~
In file included from /opt/ros/rolling/include/rclcpp/rclcpp/logging.hpp:24,
                 from /opt/ros/rolling/include/rclcpp/rclcpp/copy_all_parameter_values.hpp:27,
                 from /opt/ros/rolling/include/rclcpp/rclcpp/rclcpp.hpp:171,
                 from /opt/ros/rolling/include/controller_interface/controller_interface/controller_interface_base.hpp:28,
                 from /opt/ros/rolling/include/controller_interface/controller_interface/async_controller.hpp:22,
                 from /opt/ros/rolling/include/controller_manager/controller_manager/controller_manager.hpp:26:
/root/sim_ws/src/gz_ros_control/gz_ros2_control/src/gz_ros2_control_plugin.cpp:301:46: error: ‘class gz_ros2_control::GazeboSimROS2ControlPluginPrivate’ has no member named ‘robot_description_node_’
  301 |     "robot_param_node is %s", this->dataPtr->robot_description_node_.c_str());
      |                                              ^~~~~~~~~~~~~~~~~~~~~~~
/root/sim_ws/src/gz_ros_control/gz_ros2_control/src/gz_ros2_control_plugin.cpp:305:20: error: ‘class gz_ros2_control::GazeboSimROS2ControlPluginPrivate’ has no member named ‘robot_description_’
  305 |     this->dataPtr->robot_description_ = robot_description;
      |                    ^~~~~~~~~~~~~~~~~~
/root/sim_ws/src/gz_ros_control/gz_ros2_control/src/gz_ros2_control_plugin.cpp:309:46: error: ‘class gz_ros2_control::GazeboSimROS2ControlPluginPrivate’ has no member named ‘robot_description_’
  309 |     "robot_param_node is %s", this->dataPtr->robot_description_.c_str());
      |                                              ^~~~~~~~~~~~~~~~~~
/root/sim_ws/src/gz_ros_control/gz_ros2_control/src/gz_ros2_control_plugin.cpp:446:47: error: ‘using std::__shared_ptr_access<controller_manager::ControllerManager, __gnu_cxx::_S_atomic, false, false>::element_type = class controller_manager::ControllerManager’ {aka ‘class controller_manager::ControllerManager’} has no member named ‘is_resource_manager_initialized’
  446 |   while (!this->dataPtr->controller_manager_->is_resource_manager_initialized()) {
      |                                               ^~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
gmake[2]: *** [CMakeFiles/gz_hardware_plugins.dir/build.make:76: CMakeFiles/gz_hardware_plugins.dir/src/gz_system.cpp.o] Error 1
gmake[1]: *** [CMakeFiles/Makefile2:165: CMakeFiles/gz_hardware_plugins.dir/all] Error 2
gmake[1]: *** Waiting for unfinished jobs....
gmake[2]: *** [CMakeFiles/gz_ros2_control-system.dir/build.make:76: CMakeFiles/gz_ros2_control-system.dir/src/gz_ros2_control_plugin.cpp.o] Error 1
gmake[1]: *** [CMakeFiles/Makefile2:139: CMakeFiles/gz_ros2_control-system.dir/all] Error 2
gmake: *** [Makefile:146: all] Error 2
--- stderr: gz_ros2_control
/root/sim_ws/src/gz_ros_control/gz_ros2_control/src/gz_system.cpp: In member function ‘virtual hardware_interface::return_type gz_ros2_control::GazeboSimSystem::write(const rclcpp::Time&, const rclcpp::Duration&)’:
/root/sim_ws/src/gz_ros_control/gz_ros2_control/src/gz_system.cpp:692:53: error: invalid type argument of unary ‘*’ (have ‘unsigned int’)
  692 |     double velocity_sp = (-1.0) * position_error * (*this->dataPtr->update_rate);
      |                                                     ^~~~~~~~~~~~~~~~~~~~~~~~~~~
/root/sim_ws/src/gz_ros_control/gz_ros2_control/src/gz_ros2_control_plugin.cpp:69:8: error: ‘bool gz_ros2_control::GZResourceManager::load_and_initialize_components(const std::string&, unsigned int)’ marked ‘override’, but does not override
   69 |   bool load_and_initialize_components(
      |        ^~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
/root/sim_ws/src/gz_ros_control/gz_ros2_control/src/gz_ros2_control_plugin.cpp: In member function ‘bool gz_ros2_control::GZResourceManager::load_and_initialize_components(const std::string&, unsigned int)’:
/root/sim_ws/src/gz_ros_control/gz_ros2_control/src/gz_ros2_control_plugin.cpp:73:5: error: ‘components_are_loaded_and_initialized_’ was not declared in this scope
   73 |     components_are_loaded_and_initialized_ = true;
      |     ^~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
/root/sim_ws/src/gz_ros_control/gz_ros2_control/src/gz_ros2_control_plugin.cpp:85:30: error: ‘std::recursive_mutex hardware_interface::ResourceManager::resource_interfaces_lock_’ is private within this context
   85 |       std::scoped_lock guard(resource_interfaces_lock_, claimed_command_interfaces_lock_);
      |                              ^~~~~~~~~~~~~~~~~~~~~~~~~
In file included from /opt/ros/rolling/include/controller_manager/controller_manager/controller_manager.hpp:46,
                 from /root/sim_ws/src/gz_ros_control/gz_ros2_control/src/gz_ros2_control_plugin.cpp:35:
/opt/ros/rolling/include/hardware_interface/hardware_interface/resource_manager.hpp:423:32: note: declared private here
  423 |   mutable std::recursive_mutex resource_interfaces_lock_;
      |                                ^~~~~~~~~~~~~~~~~~~~~~~~~
/root/sim_ws/src/gz_ros_control/gz_ros2_control/src/gz_ros2_control_plugin.cpp:85:57: error: ‘std::recursive_mutex hardware_interface::ResourceManager::claimed_command_interfaces_lock_’ is private within this context
   85 |       std::scoped_lock guard(resource_interfaces_lock_, claimed_command_interfaces_lock_);
      |                                                         ^~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
/opt/ros/rolling/include/hardware_interface/hardware_interface/resource_manager.hpp:424:32: note: declared private here
  424 |   mutable std::recursive_mutex claimed_command_interfaces_lock_;
      |                                ^~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
/root/sim_ws/src/gz_ros_control/gz_ros2_control/src/gz_ros2_control_plugin.cpp: In member function ‘virtual void gz_ros2_control::GazeboSimROS2ControlPlugin::Configure(const gz::sim::v8::Entity&, const std::shared_ptr<const sdf::v14::Element>&, gz::sim::v8::EntityComponentManager&, gz::sim::v8::EventManager&)’:
/root/sim_ws/src/gz_ros_control/gz_ros2_control/src/gz_ros2_control_plugin.cpp:297:20: error: ‘class gz_ros2_control::GazeboSimROS2ControlPluginPrivate’ has no member named ‘robot_description_node_’
  297 |     this->dataPtr->robot_description_node_ = robot_param_node;
      |                    ^~~~~~~~~~~~~~~~~~~~~~~
In file included from /opt/ros/rolling/include/rclcpp/rclcpp/logging.hpp:24,
                 from /opt/ros/rolling/include/rclcpp/rclcpp/copy_all_parameter_values.hpp:27,
                 from /opt/ros/rolling/include/rclcpp/rclcpp/rclcpp.hpp:171,
                 from /opt/ros/rolling/include/controller_interface/controller_interface/controller_interface_base.hpp:28,
                 from /opt/ros/rolling/include/controller_interface/controller_interface/async_controller.hpp:22,
                 from /opt/ros/rolling/include/controller_manager/controller_manager/controller_manager.hpp:26:
/root/sim_ws/src/gz_ros_control/gz_ros2_control/src/gz_ros2_control_plugin.cpp:301:46: error: ‘class gz_ros2_control::GazeboSimROS2ControlPluginPrivate’ has no member named ‘robot_description_node_’
  301 |     "robot_param_node is %s", this->dataPtr->robot_description_node_.c_str());
      |                                              ^~~~~~~~~~~~~~~~~~~~~~~
/root/sim_ws/src/gz_ros_control/gz_ros2_control/src/gz_ros2_control_plugin.cpp:305:20: error: ‘class gz_ros2_control::GazeboSimROS2ControlPluginPrivate’ has no member named ‘robot_description_’
  305 |     this->dataPtr->robot_description_ = robot_description;
      |                    ^~~~~~~~~~~~~~~~~~
/root/sim_ws/src/gz_ros_control/gz_ros2_control/src/gz_ros2_control_plugin.cpp:309:46: error: ‘class gz_ros2_control::GazeboSimROS2ControlPluginPrivate’ has no member named ‘robot_description_’
  309 |     "robot_param_node is %s", this->dataPtr->robot_description_.c_str());
      |                                              ^~~~~~~~~~~~~~~~~~
/root/sim_ws/src/gz_ros_control/gz_ros2_control/src/gz_ros2_control_plugin.cpp:446:47: error: ‘using std::__shared_ptr_access<controller_manager::ControllerManager, __gnu_cxx::_S_atomic, false, false>::element_type = class controller_manager::ControllerManager’ {aka ‘class controller_manager::ControllerManager’} has no member named ‘is_resource_manager_initialized’
  446 |   while (!this->dataPtr->controller_manager_->is_resource_manager_initialized()) {

[saikishor]

@ahcorde I believe you will need the changes in this PR: ros-controls/ros2_control#1354
This PR should be merged after the above PR

[ahcorde]

@saikishor This one at least is wrong

  692 |     double velocity_sp = (-1.0) * position_error * (*this->dataPtr->update_rate);

[saikishor]

@saikishor This one at least is wrong

  692 |     double velocity_sp = (-1.0) * position_error * (*this->dataPtr->update_rate);

@ahcorde Yes, you are right! This one needs to be changed. The update_rate is no more a pointer now

[destogl]

@ahcorde I have fixed this and now we have to wait for ros2_control PR to be merged

[bmagyar]

Merged, re-running the CI

[ahcorde]

@bmagyar and @destogl we need a ros2_control release, isn't it ? we are using rolling testing packages.

[bmagyar]

Ah right. Somehow I expected it to also run a semi binary build. Ok no worries I'll push one today