Added parameters robot_param and robot_param_node (#275) (#280)

Signed-off-by: Alejandro Hernández Cordero <[email protected]>
(cherry picked from commit 53b6c74)

Co-authored-by: Alejandro Hernández Cordero <[email protected]>

gz_ros2_control/src/gz_ros2_control_plugin.cpp

@@ -102,11 +102,9 @@ class GazeboSimROS2ControlPluginPrivate
   controller_manager_{nullptr};
 
   /// \brief String with the robot description param_name
-  // TODO(ahcorde): Add param in plugin tag
   std::string robot_description_ = "robot_description";
 
   /// \brief String with the name of the node that contains the robot_description
-  // TODO(ahcorde): Add param in plugin tag
   std::string robot_description_node_ = "robot_state_publisher";
 
   /// \brief Last time the update method was called
@@ -285,6 +283,23 @@ void GazeboSimROS2ControlPlugin::Configure(
     return;
   }
 
+  // Get params from SDF
+  std::string robot_param_node = _sdf->Get<std::string>("robot_param_node");
+  if (!robot_param_node.empty()) {
+    this->dataPtr->robot_description_node_ = robot_param_node;
+  }
+  RCLCPP_INFO(
+    logger,
+    "robot_param_node is %s", this->dataPtr->robot_description_node_.c_str());
+
+  std::string robot_description = _sdf->Get<std::string>("robot_param");
+  if (!robot_description.empty()) {
+    this->dataPtr->robot_description_ = robot_description;
+  }
+  RCLCPP_INFO(
+    logger,
+    "robot_param_node is %s", this->dataPtr->robot_description_.c_str());
+
   std::vector<std::string> arguments = {"--ros-args"};
 
   auto sdfPtr = const_cast<sdf::Element *>(_sdf.get());