From 794da79c0b46a8adc4a7ff909429a69d518eea99 Mon Sep 17 00:00:00 2001
From: ahcorde <ahcorde@gmail.com>
Date: Tue, 2 Feb 2021 08:45:17 +0100
Subject: [PATCH] Support more than one ros2_control tag

Signed-off-by: ahcorde <ahcorde@gmail.com>
---
 .../gazebo_ros2_control/gazebo_system.hpp     |  2 +-
 .../gazebo_system_interface.hpp               |  2 +-
 .../src/gazebo_ros2_control_plugin.cpp        | 37 +++++++++----------
 gazebo_ros2_control/src/gazebo_system.cpp     |  3 +-
 4 files changed, 21 insertions(+), 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
index 8540fc0c..3c53af21 100644
--- 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
index 8f9f69c9..598f884c 100644
--- 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
index e95a2916..fc89defa 100644
--- 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
index 6692ea25..1602af1f 100644
--- a/gazebo_ros2_control/src/gazebo_system.cpp
+++ b/gazebo_ros2_control/src/gazebo_system.cpp
@@ -85,7 +85,7 @@ 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>();
@@ -93,7 +93,6 @@ bool GazeboSystem::initSim(
 
   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_);