Don't ignore robotNamespace in gazebo_ros_control nodes (lunar-devel) (…

…#706) * Don't ignore robotNamespace When creating the NodeHandle for reading the PID parameters, the model_nh was always ignored. Instead, all parameters were read from /gazebo_ros_control/pid_gains/<joint_name>/* instead of /<robot_name>/gazebo_ros_control/pid_gains/<joint_name>/*. This commit restores the intended behavior, i.e., the parameters will now read from <robot_name>/..., where <robot_name> is specified via the robotNamespace plugin parameter or the parent name.
ros-simulation · Apr 9, 2018 · be2c750 · be2c750
1 parent c4d320b
commit be2c750
Show file tree

Hide file tree

Showing 2 changed files with 20 additions and 3 deletions.
diff --git a/gazebo_ros_control/src/default_robot_hw_sim.cpp b/gazebo_ros_control/src/default_robot_hw_sim.cpp
@@ -217,9 +217,9 @@ bool DefaultRobotHWSim::initSim(
     {
       // Initialize the PID controller. If no PID gain values are found, use joint->SetAngle() or
       // joint->SetParam("vel") to control the joint.
-      const ros::NodeHandle nh(model_nh, "/gazebo_ros_control/pid_gains/" +
+      const ros::NodeHandle nh(robot_namespace + "/gazebo_ros_control/pid_gains/" +
                                joint_names_[j]);
-      if (pid_controllers_[j].init(nh, true))
+      if (pid_controllers_[j].init(nh))
       {
         switch (joint_control_methods_[j])
         {

diff --git a/gazebo_ros_control/src/gazebo_ros_control_plugin.cpp b/gazebo_ros_control/src/gazebo_ros_control_plugin.cpp
@@ -110,6 +110,23 @@ void GazeboRosControlPlugin::Load(gazebo::physics::ModelPtr parent, sdf::Element
     ROS_DEBUG_STREAM_NAMED("loadThread","Using default plugin for RobotHWSim (none specified in URDF/SDF)\""<<robot_hw_sim_type_str_<<"\"");
   }
 
+  // temporary fix to bug regarding the robotNamespace in default_robot_hw_sim.cpp (see #637)
+  std::string robot_ns = robot_namespace_;
+  if(robot_hw_sim_type_str_ == "gazebo_ros_control/DefaultRobotHWSim"){
+      if (sdf_->HasElement("legacyModeNS")) {
+          if( sdf_->GetElement("legacyModeNS")->Get<bool>() ){
+              robot_ns = "";
+          }
+      }else{
+          robot_ns = "";
+          ROS_ERROR("GazeboRosControlPlugin missing <legacyModeNS> while using DefaultRobotHWSim, defaults to true.\n"
+                    "This setting assumes you have an old package with an old implementation of DefaultRobotHWSim, "
+                    "where the robotNamespace is disregarded and absolute paths are used instead.\n"
+                    "If you do not want to fix this issue in an old package just set <legacyModeNS> to true.\n"
+                    );
+      }
+  }
+
   // Get the Gazebo simulation period
 #if GAZEBO_MAJOR_VERSION >= 8
   ros::Duration gazebo_period(parent_model_->GetWorld()->Physics()->GetMaxStepSize());
@@ -177,7 +194,7 @@ void GazeboRosControlPlugin::Load(gazebo::physics::ModelPtr parent, sdf::Element
     urdf::Model urdf_model;
     const urdf::Model *const urdf_model_ptr = urdf_model.initString(urdf_string) ? &urdf_model : NULL;
 
-    if(!robot_hw_sim_->initSim(robot_namespace_, model_nh_, parent_model_, urdf_model_ptr, transmissions_))
+    if(!robot_hw_sim_->initSim(robot_ns, model_nh_, parent_model_, urdf_model_ptr, transmissions_))
     {
       ROS_FATAL_NAMED("gazebo_ros_control","Could not initialize robot simulation interface");
       return;