Add option to change the namespace of the pid

diff --git a/README.md b/README.md
index 7d4ccea..0a6e567 100644 (file)
--- 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 (file)
--- 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);
         }