Support more than one ros2_control tag

Signed-off-by: ahcorde <[email protected]>
ros-controls · Feb 2, 2021 · 794da79 · 794da79
1 parent da262d9
commit 794da79
Show file tree

Hide file tree

Showing 4 changed files with 21 additions and 23 deletions.
diff --git a/gazebo_ros2_control/include/gazebo_ros2_control/gazebo_system.hpp b/gazebo_ros2_control/include/gazebo_ros2_control/gazebo_system.hpp
@@ -63,7 +63,7 @@ class GazeboSystem : public GazeboSystemInterface
   bool initSim(
     rclcpp::Node::SharedPtr & model_nh,
     gazebo::physics::ModelPtr parent_model,
-    const std::vector<hardware_interface::HardwareInfo> & control_hardware,
+    const hardware_interface::HardwareInfo & hardware_info,
     sdf::ElementPtr sdf) override;
 
 private:

diff --git a/gazebo_ros2_control/include/gazebo_ros2_control/gazebo_system_interface.hpp b/gazebo_ros2_control/include/gazebo_ros2_control/gazebo_system_interface.hpp
@@ -71,7 +71,7 @@ class GazeboSystemInterface
   virtual bool initSim(
     rclcpp::Node::SharedPtr & model_nh,
     gazebo::physics::ModelPtr parent_model,
-    const std::vector<hardware_interface::HardwareInfo> & control_hardware,
+    const hardware_interface::HardwareInfo & hardware_info,
     sdf::ElementPtr sdf) = 0;
 
   // Methods used to control a joint.

diff --git a/gazebo_ros2_control/src/gazebo_ros2_control_plugin.cpp b/gazebo_ros2_control/src/gazebo_ros2_control_plugin.cpp
@@ -96,9 +96,6 @@ class GazeboRosControlPrivate
   // Name of the file with the controllers configuration
   std::string param_file_;
 
-  // Robot simulator interface
-  std::string robot_hw_sim_type_str_;
-
   // Executor to spin the controller
   rclcpp::executors::MultiThreadedExecutor::SharedPtr executor_;
 
@@ -228,8 +225,6 @@ void GazeboRosControlPlugin::Load(gazebo::physics::ModelPtr parent, sdf::Element
   try {
     urdf_string = impl_->getURDF(impl_->robot_description_);
     control_hardware = hardware_interface::parse_control_resources_from_urdf(urdf_string);
-    const auto hardware_info = control_hardware.front();
-    impl_->robot_hw_sim_type_str_ = hardware_info.hardware_class_type;
   } catch (const std::runtime_error & ex) {
     RCLCPP_ERROR_STREAM(
       impl_->model_nh_->get_logger(),
@@ -246,22 +241,26 @@ void GazeboRosControlPlugin::Load(gazebo::physics::ModelPtr parent, sdf::Element
         "gazebo_ros2_control",
         "gazebo_ros2_control::GazeboSystemInterface"));
 
-    auto gazeboSystem = std::unique_ptr<gazebo_ros2_control::GazeboSystemInterface>(
-      impl_->robot_hw_sim_loader_->createUnmanagedInstance(impl_->robot_hw_sim_type_str_));
-
-    rclcpp::Node::SharedPtr node_ros2 = std::dynamic_pointer_cast<rclcpp::Node>(impl_->model_nh_);
-    if (!gazeboSystem->initSim(
-        node_ros2,
-        impl_->parent_model_,
-        control_hardware,
-        sdf))
+    for (unsigned int i = 0; i < control_hardware.size(); i++)
     {
-      RCLCPP_FATAL(
-        impl_->model_nh_->get_logger(), "Could not initialize robot simulation interface");
-      return;
-    }
+      std::string robot_hw_sim_type_str_ = control_hardware[i].hardware_class_type;
+      auto gazeboSystem = std::unique_ptr<gazebo_ros2_control::GazeboSystemInterface>(
+        impl_->robot_hw_sim_loader_->createUnmanagedInstance(robot_hw_sim_type_str_));
+
+      rclcpp::Node::SharedPtr node_ros2 = std::dynamic_pointer_cast<rclcpp::Node>(impl_->model_nh_);
+      if (!gazeboSystem->initSim(
+          node_ros2,
+          impl_->parent_model_,
+          control_hardware[i],
+          sdf))
+      {
+        RCLCPP_FATAL(
+          impl_->model_nh_->get_logger(), "Could not initialize robot simulation interface");
+        return;
+      }
 
-    resource_manager_->import_component(std::move(gazeboSystem));
+      resource_manager_->import_component(std::move(gazeboSystem));
+    }
 
     impl_->executor_ = std::make_shared<rclcpp::executors::MultiThreadedExecutor>();
 

diff --git a/gazebo_ros2_control/src/gazebo_system.cpp b/gazebo_ros2_control/src/gazebo_system.cpp
@@ -85,15 +85,14 @@ namespace gazebo_ros2_control
 bool GazeboSystem::initSim(
   rclcpp::Node::SharedPtr & model_nh,
   gazebo::physics::ModelPtr parent_model,
-  const std::vector<hardware_interface::HardwareInfo> & control_hardware,
+  const hardware_interface::HardwareInfo & hardware_info,
   sdf::ElementPtr sdf)
 {
   this->dataPtr = std::make_unique<GazeboSystemPrivate>();
   this->dataPtr->last_update_sim_time_ros_ = rclcpp::Time();
 
   this->nh_ = model_nh;
   this->dataPtr->parent_model_ = parent_model;
-  const auto hardware_info = control_hardware.front();
   this->dataPtr->n_dof_ = hardware_info.joints.size();
 
   this->dataPtr->joint_names_.resize(this->dataPtr->n_dof_);