Simplify access for robot description from CM by overriding RM.

gz_ros2_control/include/gz_ros2_control/gz_system.hpp

@@ -77,7 +77,7 @@ class GazeboSimSystem : public GazeboSimSystemInterface
     std::map<std::string, sim::Entity> & joints,
     const hardware_interface::HardwareInfo & hardware_info,
     sim::EntityComponentManager & _ecm,
-    int & update_rate) override;
+    unsigned int update_rate) override;
 
 private:
   // Register a sensor (for now just IMUs)

gz_ros2_control/include/gz_ros2_control/gz_system_interface.hpp

@@ -88,7 +88,7 @@ class GazeboSimSystemInterface
     std::map<std::string, sim::Entity> & joints,
     const hardware_interface::HardwareInfo & hardware_info,
     sim::EntityComponentManager & _ecm,
-    int & update_rate) = 0;
+    unsigned int update_rate) = 0;
 
   // Methods used to control a joint.
   enum ControlMethod_

gz_ros2_control/src/gz_ros2_control_plugin.cpp

@@ -17,6 +17,7 @@
 #include <chrono>
 #include <map>
 #include <memory>
+#include <mutex>
 #include <string>
 #include <thread>
 #include <utility>
@@ -46,13 +47,92 @@
 
 namespace gz_ros2_control
 {
+class GZResourceManager : public hardware_interface::ResourceManager
+{
+public:
+  GZResourceManager(
+    rclcpp::Node::SharedPtr & node,
+    sim::EntityComponentManager & ecm,
+    std::map<std::string, sim::Entity> enabledJoints)
+  : hardware_interface::ResourceManager(),
+    gz_system_loader_("gz_ros2_control", "gz_ros2_control::GazeboSimSystemInterface"),
+    logger_(node->get_logger().get_child("GZResourceManager"))
+  {
+    node_ = node;
+    ecm_ = &ecm;
+    enabledJoints_ = enabledJoints;
+  }
+
+  GZResourceManager(const GZResourceManager &) = delete;
+
+  // Called from Controller Manager when robot description is initialized from callback
+  bool load_and_initialize_components(
+    const std::string & urdf,
+    unsigned int update_rate) override
+  {
+    components_are_loaded_and_initialized_ = true;
+
+    const auto hardware_info = hardware_interface::parse_control_resources_from_urdf(urdf);
+
+    for (const auto & individual_hardware_info : hardware_info) {
+      std::string robot_hw_sim_type_str_ = individual_hardware_info.hardware_plugin_name;
+      RCLCPP_DEBUG(
+        logger_, "Load hardware interface %s ...",
+        robot_hw_sim_type_str_.c_str());
+
+      // Load hardware
+      std::unique_ptr<gz_ros2_control::GazeboSimSystemInterface> gzSimSystem;
+      std::scoped_lock guard(resource_interfaces_lock_, claimed_command_interfaces_lock_);
+      try {
+        gzSimSystem = std::unique_ptr<gz_ros2_control::GazeboSimSystemInterface>(
+          gz_system_loader_.createUnmanagedInstance(robot_hw_sim_type_str_));
+      } catch (pluginlib::PluginlibException & ex) {
+        RCLCPP_ERROR(
+          logger_,
+          "The plugin failed to load for some reason. Error: %s\n",
+          ex.what());
+        continue;
+      }
+
+      // initialize simulation requirements
+      if (!gzSimSystem->initSim(
+          node_,
+          enabledJoints_,
+          individual_hardware_info,
+          *ecm_,
+          update_rate))
+      {
+        RCLCPP_FATAL(
+          logger_, "Could not initialize robot simulation interface");
+        components_are_loaded_and_initialized_ = false;
+        break;
+      }
+      RCLCPP_DEBUG(
+        logger_, "Initialized robot simulation interface %s!",
+        robot_hw_sim_type_str_.c_str());
+
+      // initialize hardware
+      import_component(std::move(gzSimSystem), individual_hardware_info);
+    }
+
+    return components_are_loaded_and_initialized_;
+  }
+
+private:
+  std::shared_ptr<rclcpp::Node> node_;
+  sim::EntityComponentManager * ecm_;
+  std::map<std::string, sim::Entity> enabledJoints_;
+
+  /// \brief Interface loader
+  pluginlib::ClassLoader<gz_ros2_control::GazeboSimSystemInterface> gz_system_loader_;
+
+  rclcpp::Logger logger_;
+};
+
 //////////////////////////////////////////////////
 class GazeboSimROS2ControlPluginPrivate
 {
 public:
-  /// \brief Get the URDF XML from the parameter server
-  std::string getURDF() const;
-
   /// \brief Get a list of enabled, unique, 1-axis joints of the model. If no
   /// joint names are specified in the plugin configuration, all valid 1-axis
   /// joints are returned
@@ -88,12 +168,6 @@ class GazeboSimROS2ControlPluginPrivate
   std::shared_ptr<controller_manager::ControllerManager>
   controller_manager_{nullptr};
 
-  /// \brief String with the robot description param_name
-  std::string robot_description_ = "robot_description";
-
-  /// \brief String with the name of the node that contains the robot_description
-  std::string robot_description_node_ = "robot_state_publisher";
-
   /// \brief Last time the update method was called
   rclcpp::Time last_update_sim_time_ros_ =
     rclcpp::Time((int64_t)0, RCL_ROS_TIME);
@@ -170,61 +244,6 @@ GazeboSimROS2ControlPluginPrivate::GetEnabledJoints(
   return output;
 }
 
-//////////////////////////////////////////////////
-std::string GazeboSimROS2ControlPluginPrivate::getURDF() const
-{
-  std::string urdf_string;
-
-  using namespace std::chrono_literals;
-  auto parameters_client = std::make_shared<rclcpp::AsyncParametersClient>(
-    node_, robot_description_node_);
-  while (!parameters_client->wait_for_service(0.5s)) {
-    if (!rclcpp::ok()) {
-      RCLCPP_ERROR(
-        node_->get_logger(), "Interrupted while waiting for %s service. Exiting.",
-        robot_description_node_.c_str());
-      return 0;
-    }
-    RCLCPP_ERROR(
-      node_->get_logger(), "%s service not available, waiting again...",
-      robot_description_node_.c_str());
-  }
-
-  RCLCPP_INFO(
-    node_->get_logger(), "connected to service!! %s asking for %s",
-    robot_description_node_.c_str(),
-    this->robot_description_.c_str());
-
-  // search and wait for robot_description on param server
-  while (urdf_string.empty()) {
-    RCLCPP_DEBUG(
-      node_->get_logger(), "param_name %s",
-      this->robot_description_.c_str());
-
-    try {
-      auto f = parameters_client->get_parameters({this->robot_description_});
-      f.wait();
-      std::vector<rclcpp::Parameter> values = f.get();
-      urdf_string = values[0].as_string();
-    } catch (const std::exception & e) {
-      RCLCPP_ERROR(node_->get_logger(), "%s", e.what());
-    }
-
-    if (!urdf_string.empty()) {
-      break;
-    } else {
-      RCLCPP_ERROR(
-        node_->get_logger(), "gz_ros2_control plugin is waiting for model"
-        " URDF in parameter [%s] on the ROS param server.",
-        this->robot_description_.c_str());
-    }
-    std::this_thread::sleep_for(std::chrono::microseconds(100000));
-  }
-  RCLCPP_INFO(node_->get_logger(), "Received URDF from param server");
-
-  return urdf_string;
-}
-
 //////////////////////////////////////////////////
 GazeboSimROS2ControlPlugin::GazeboSimROS2ControlPlugin()
 : dataPtr(std::make_unique<GazeboSimROS2ControlPluginPrivate>())
@@ -327,10 +346,6 @@ void GazeboSimROS2ControlPlugin::Configure(
       if (ns.empty() || ns[0] != '/') {
         ns = '/' + ns;
       }
-
-      if (ns.length() > 1) {
-        this->dataPtr->robot_description_node_ = ns + "/" + this->dataPtr->robot_description_node_;
-      }
     }
 
     // Get list of remapping rules from SDF
@@ -385,102 +400,28 @@ void GazeboSimROS2ControlPlugin::Configure(
     return;
   }
 
-  // Read urdf from ros parameter server then
-  // setup actuators and mechanism control node.
-  // This call will block if ROS is not properly initialized.
-  std::string urdf_string;
-  std::vector<hardware_interface::HardwareInfo> control_hardware_info;
   try {
-    urdf_string = this->dataPtr->getURDF();
-    control_hardware_info = hardware_interface::parse_control_resources_from_urdf(urdf_string);
-  } catch (const std::runtime_error & ex) {
-    RCLCPP_ERROR_STREAM(
-      this->dataPtr->node_->get_logger(),
-      "Error parsing URDF in gz_ros2_control plugin, plugin not active : " << ex.what());
-    return;
-  }
-
-  std::unique_ptr<hardware_interface::ResourceManager> resource_manager_ =
-    std::make_unique<hardware_interface::ResourceManager>();
-
-  try {
-    resource_manager_->load_urdf(urdf_string, false, false);
-  } catch (...) {
+    this->dataPtr->node_->declare_parameter("hold_joints", rclcpp::ParameterValue(hold_joints));
+  } catch (const rclcpp::exceptions::ParameterAlreadyDeclaredException & e) {
     RCLCPP_ERROR(
-      this->dataPtr->node_->get_logger(), "Error initializing URDF to resource manager!");
-  }
-  try {
-    this->dataPtr->robot_hw_sim_loader_.reset(
-      new pluginlib::ClassLoader<gz_ros2_control::GazeboSimSystemInterface>(
-        "gz_ros2_control",
-        "gz_ros2_control::GazeboSimSystemInterface"));
-  } catch (pluginlib::LibraryLoadException & ex) {
+      this->dataPtr->node_->get_logger(), "Parameter 'hold_joints' has already been declared, %s",
+      e.what());
+  } catch (const rclcpp::exceptions::InvalidParametersException & e) {
     RCLCPP_ERROR(
-      this->dataPtr->node_->get_logger(), "Failed to create robot simulation interface loader: %s ",
-      ex.what());
-    return;
+      this->dataPtr->node_->get_logger(), "Parameter 'hold_joints' has invalid name, %s",
+      e.what());
+  } catch (const rclcpp::exceptions::InvalidParameterValueException & e) {
+    RCLCPP_ERROR(
+      this->dataPtr->node_->get_logger(), "Parameter 'hold_joints' value is invalid, %s",
+      e.what());
+  } catch (const rclcpp::exceptions::InvalidParameterTypeException & e) {
+    RCLCPP_ERROR(
+      this->dataPtr->node_->get_logger(), "Parameter 'hold_joints' value has wrong type, %s",
+      e.what());
   }
 
-  for (unsigned int i = 0; i < control_hardware_info.size(); ++i) {
-    std::string robot_hw_sim_type_str_ = control_hardware_info[i].hardware_plugin_name;
-    RCLCPP_DEBUG(
-      this->dataPtr->node_->get_logger(), "Load hardware interface %s ...",
-      robot_hw_sim_type_str_.c_str());
-
-    std::unique_ptr<gz_ros2_control::GazeboSimSystemInterface> gzSimSystem;
-    try {
-      gzSimSystem = std::unique_ptr<gz_ros2_control::GazeboSimSystemInterface>(
-        this->dataPtr->robot_hw_sim_loader_->createUnmanagedInstance(robot_hw_sim_type_str_));
-    } catch (pluginlib::PluginlibException & ex) {
-      RCLCPP_ERROR(
-        this->dataPtr->node_->get_logger(),
-        "The plugin failed to load for some reason. Error: %s\n",
-        ex.what());
-      continue;
-    }
-
-    try {
-      this->dataPtr->node_->declare_parameter("hold_joints", rclcpp::ParameterValue(hold_joints));
-    } catch (const rclcpp::exceptions::ParameterAlreadyDeclaredException & e) {
-      RCLCPP_ERROR(
-        this->dataPtr->node_->get_logger(), "Parameter 'hold_joints' has already been declared, %s",
-        e.what());
-    } catch (const rclcpp::exceptions::InvalidParametersException & e) {
-      RCLCPP_ERROR(
-        this->dataPtr->node_->get_logger(), "Parameter 'hold_joints' has invalid name, %s",
-        e.what());
-    } catch (const rclcpp::exceptions::InvalidParameterValueException & e) {
-      RCLCPP_ERROR(
-        this->dataPtr->node_->get_logger(), "Parameter 'hold_joints' value is invalid, %s",
-        e.what());
-    } catch (const rclcpp::exceptions::InvalidParameterTypeException & e) {
-      RCLCPP_ERROR(
-        this->dataPtr->node_->get_logger(), "Parameter 'hold_joints' value has wrong type, %s",
-        e.what());
-    }
-
-    if (!gzSimSystem->initSim(
-        this->dataPtr->node_,
-        enabledJoints,
-        control_hardware_info[i],
-        _ecm,
-        this->dataPtr->update_rate))
-    {
-      RCLCPP_FATAL(
-        this->dataPtr->node_->get_logger(), "Could not initialize robot simulation interface");
-      return;
-    }
-    RCLCPP_DEBUG(
-      this->dataPtr->node_->get_logger(), "Initialized robot simulation interface %s!",
-      robot_hw_sim_type_str_.c_str());
-
-    resource_manager_->import_component(std::move(gzSimSystem), control_hardware_info[i]);
-
-    rclcpp_lifecycle::State state(
-      lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE,
-      hardware_interface::lifecycle_state_names::ACTIVE);
-    resource_manager_->set_component_state(control_hardware_info[i].name, state);
-  }
+  std::unique_ptr<hardware_interface::ResourceManager> resource_manager_ =
+    std::make_unique<gz_ros2_control::GZResourceManager>(this->dataPtr->node_, _ecm, enabledJoints);
 
   // Create the controller manager
   RCLCPP_INFO(this->dataPtr->node_->get_logger(), "Loading controller_manager");
@@ -492,15 +433,7 @@ void GazeboSimROS2ControlPlugin::Configure(
       this->dataPtr->node_->get_namespace()));
   this->dataPtr->executor_->add_node(this->dataPtr->controller_manager_);
 
-  if (!this->dataPtr->controller_manager_->has_parameter("update_rate")) {
-    RCLCPP_ERROR_STREAM(
-      this->dataPtr->node_->get_logger(),
-      "controller manager doesn't have an update_rate parameter");
-    return;
-  }
-
-  this->dataPtr->update_rate =
-    this->dataPtr->controller_manager_->get_parameter("update_rate").as_int();
+  this->dataPtr->update_rate = this->dataPtr->controller_manager_->get_update_rate();
   this->dataPtr->control_period_ = rclcpp::Duration(
     std::chrono::duration_cast<std::chrono::nanoseconds>(
       std::chrono::duration<double>(1.0 / static_cast<double>(this->dataPtr->update_rate))));
@@ -509,6 +442,14 @@ void GazeboSimROS2ControlPlugin::Configure(
   this->dataPtr->controller_manager_->set_parameter(
     rclcpp::Parameter("use_sim_time", rclcpp::ParameterValue(true)));
 
+  // Wait for CM to receive robot description from the topic and then initialize Resource Manager
+  while (!this->dataPtr->controller_manager_->is_resource_manager_initialized()) {
+    RCLCPP_WARN(
+      this->dataPtr->node_->get_logger(),
+      "Waiting RM to load and initialize hardware...");
+    std::this_thread::sleep_for(std::chrono::microseconds(2000000));
+  }
+
   this->dataPtr->entity_ = _entity;
 }