Removed leadings slash from rosparam for robot padding

moveit · Oct 22, 2014 · 85d78d4 · 85d78d4
1 parent a305f1b
commit 85d78d4
Showing 1 changed file with 15 additions and 6 deletions.
diff --git a/planning/planning_scene_monitor/src/planning_scene_monitor.cpp b/planning/planning_scene_monitor/src/planning_scene_monitor.cpp
@@ -1254,10 +1254,19 @@ void planning_scene_monitor::PlanningSceneMonitor::configureDefaultPadding()
     default_attached_padd_ = 0.0;
     return;
   }
-  nh_.param(robot_description_ + "_planning/default_robot_padding", default_robot_padd_, 0.0);
-  nh_.param(robot_description_ + "_planning/default_robot_scale", default_robot_scale_, 1.0);
-  nh_.param(robot_description_ + "_planning/default_object_padding", default_object_padd_, 0.0);
-  nh_.param(robot_description_ + "_planning/default_attached_padding", default_attached_padd_, 0.0);
-  nh_.param(robot_description_ + "_planning/default_robot_link_padding", default_robot_link_padd_, std::map<std::string, double>());
-  nh_.param(robot_description_ + "_planning/default_robot_link_scale", default_robot_link_scale_, std::map<std::string, double>());
+
+  // Ensure no leading slash creates a bad param server address
+  static const std::string robot_description = (robot_description_[0] == '/') ? robot_description_.substr(1) : robot_description_;
+
+  nh_.param(robot_description + "_planning/default_robot_padding", default_robot_padd_, 0.0);
+  nh_.param(robot_description + "_planning/default_robot_scale", default_robot_scale_, 1.0);
+  nh_.param(robot_description + "_planning/default_object_padding", default_object_padd_, 0.0);
+  nh_.param(robot_description + "_planning/default_attached_padding", default_attached_padd_, 0.0);
+  nh_.param(robot_description + "_planning/default_robot_link_padding", default_robot_link_padd_, std::map<std::string, double>());
+  nh_.param(robot_description + "_planning/default_robot_link_scale", default_robot_link_scale_, std::map<std::string, double>());
+
+  ROS_DEBUG_STREAM_NAMED("planning_scene_monitor", "Loaded " << default_robot_link_padd_.size()
+                         << " default link paddings");
+  ROS_DEBUG_STREAM_NAMED("planning_scene_monitor", "Loaded " << default_robot_link_scale_.size()
+                         << " default link scales");
 }