From: nlamprian <nlamprian@gmail.com>
Date: Thu, 21 Feb 2019 11:21:56 +0000 (+0100)
Subject: Add option to change the namespace of the pid
X-Git-Tag: 0.1.0~4^2
X-Git-Url: http://git.ece.ufrgs.br/?a=commitdiff_plain;h=95512be1c0e9025267fc9d4ec428b425361290c2;p=roboticsgroup_upatras_gazebo_plugins.git

Add option to change the namespace of the pid
---

diff --git a/README.md b/README.md
index 7d4ccea..0a6e567 100644
--- a/README.md
+++ b/README.md
@@ -41,7 +41,7 @@ A simple (Model) plugin for Gazebo in order to add to Gazebo the mimic joint fun
 
     - hasPID
 
-      Determines whether the joint has PID in order to be controlled via PID position/effort controller. Takes no value: *\<hasPID/\>* means that the mimic joint is controlled via PID. Omit it so that the mimic joint is controlled via setAngle.
+      Determines whether the joint has PID in order to be controlled via PID position/effort controller. *\<hasPID/\>* means that the mimic joint is controlled via PID. Omit it so that the mimic joint is controlled via setAngle. Optionally, specify a value to set the pid namespace.
 
 DisableLinkPlugin
 -----------------
diff --git a/src/mimic_joint_plugin.cpp b/src/mimic_joint_plugin.cpp
index 49f72d7..ca53e2f 100644
--- a/src/mimic_joint_plugin.cpp
+++ b/src/mimic_joint_plugin.cpp
@@ -43,13 +43,12 @@ namespace gazebo {
 
     void MimicJointPlugin::Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf)
     {
-        ros::NodeHandle model_nh;
         model_ = _parent;
         world_ = model_->GetWorld();
 
         // Error message if the model couldn't be found
         if (!model_) {
-            ROS_ERROR("Parent model is NULL! GazeboNaoqiControlPlugin could not be loaded.");
+            ROS_ERROR("Parent model is NULL! MimicJointPlugin could not be loaded.");
             return;
         }
 
@@ -64,6 +63,7 @@ namespace gazebo {
         if (_sdf->HasElement("robotNamespace")) {
             robot_namespace_ = _sdf->GetElement("robotNamespace")->Get<std::string>();
         }
+        ros::NodeHandle model_nh(robot_namespace_);
 
         // Check for joint element
         if (!_sdf->HasElement("joint")) {
@@ -81,12 +81,14 @@ namespace gazebo {
 
         mimic_joint_name_ = _sdf->GetElement("mimicJoint")->Get<std::string>();
 
-        has_pid_ = false;
         // Check if PID controller wanted
-        if (_sdf->HasElement("hasPID")) {
-            has_pid_ = true;
-
-            const ros::NodeHandle nh(model_nh, std::string(robot_namespace_ + "/gazebo_ros_control/pid_gains/") + mimic_joint_name_);
+        has_pid_ = _sdf->HasElement("hasPID");
+        if (has_pid_) {
+            std::string name = _sdf->GetElement("hasPID")->Get<std::string>();
+            if (name.empty()) {
+                name = "gazebo_ros_control/pid_gains/" + mimic_joint_name_;
+            }
+            const ros::NodeHandle nh(model_nh, name);
             pid_.init(nh);
         }