Fix LERP planner example (#671)

* Propagate planning pipeline's namespace
* Provide fallback default for `num_steps` parameter
* Simplify code

Author: rhaschke

doc/creating_moveit_plugins/lerp_motion_planner/include/lerp_interface/lerp_planning_context.h

@@ -47,7 +47,8 @@ MOVEIT_CLASS_FORWARD(LERPPlanningContext);
 class LERPPlanningContext : public planning_interface::PlanningContext
 {
 public:
-  LERPPlanningContext(const std::string& name, const std::string& group, const moveit::core::RobotModelConstPtr& model);
+  LERPPlanningContext(const std::string& name, const std::string& ns, const std::string& group,
+                      const moveit::core::RobotModelConstPtr& model);
   ~LERPPlanningContext() override
   {
   }

doc/creating_moveit_plugins/lerp_motion_planner/launch/lerp_example.launch

@@ -1,28 +1,10 @@
 <launch>
 
-  <!-- GDB Debug Arguments -->
-  <arg name="debug" default="false" />
-  <arg unless="$(arg debug)" name="launch_prefix" value="" />
-  <arg     if="$(arg debug)" name="launch_prefix"
-           value="gdb -x $(find moveit_planners_lerp)/launch/gdb_settings.gdb --ex run --args" />
-
-  <!-- Callgrind Arguments -->
-  <arg name="callgrind" default="false" />
-  <arg unless="$(arg callgrind)" name="launch_prefix2" value="" />
-  <arg     if="$(arg callgrind)" name="launch_prefix2" value="valgrind --tool=callgrind --collect-atstart=no" />
-
-  <!-- Valgrind Arguments -->
-  <arg name="valgrind" default="false" />
-  <arg unless="$(arg valgrind)" name="launch_prefix3" value="" />
-  <arg     if="$(arg valgrind)" name="launch_prefix3" value="valgrind --tool=memcheck --leak-check=full --show-leak-kinds=all" />
-
   <!-- Launch main node -->
-  <node name="$(anon moveit_tutorials)" launch-prefix="$(arg launch_prefix) $(arg launch_prefix2) $(arg launch_prefix3)" pkg="moveit_tutorials" type="lerp_example" respawn="false" output="screen">
+  <node name="$(anon moveit_tutorials)" pkg="moveit_tutorials" type="lerp_example" respawn="false" output="screen">
 
     <!-- Robot-specific settings -->
     <rosparam file="$(find panda_moveit_config)/config/lerp_planning.yaml" command="load"/>
   </node>
 
-
-
 </launch>

doc/creating_moveit_plugins/lerp_motion_planner/src/lerp_interface.cpp

@@ -60,7 +60,7 @@ bool LERPInterface::solve(const planning_scene::PlanningSceneConstPtr& planning_
                           moveit_msgs::MotionPlanDetailedResponse& res)
 {
   // Load the planner-specific parameters
-  nh_.getParam("num_steps", num_steps_);
+  nh_.param("num_steps", num_steps_, 10);
 
   ros::WallTime start_time = ros::WallTime::now();
   moveit::core::RobotModelConstPtr robot_model = planning_scene->getRobotModel();
@@ -72,9 +72,9 @@ bool LERPInterface::solve(const planning_scene::PlanningSceneConstPtr& planning_
   std::vector<double> start_joint_values;
   start_state->copyJointGroupPositions(joint_model_group, start_joint_values);
 
-  // This planner only supports one goal constriant in the request
-  std::vector<moveit_msgs::Constraints> goal_constraints = req.goal_constraints;
-  std::vector<moveit_msgs::JointConstraint> goal_joint_constraint = goal_constraints[0].joint_constraints;
+  // This planner only supports one goal constraint in the request
+  const std::vector<moveit_msgs::Constraints>& goal_constraints = req.goal_constraints;
+  const std::vector<moveit_msgs::JointConstraint>& goal_joint_constraint = goal_constraints[0].joint_constraints;
 
   std::vector<double> goal_joint_values;
   goal_joint_values.reserve(goal_joint_constraint.size());

doc/creating_moveit_plugins/lerp_motion_planner/src/lerp_planner_manager.cpp

@@ -52,13 +52,12 @@ class LERPPlannerManager : public planning_interface::PlannerManager
   {
   }
 
-  bool initialize(const moveit::core::RobotModelConstPtr& model, const std::string& /*ns*/) override
+  bool initialize(const moveit::core::RobotModelConstPtr& model, const std::string& ns) override
   {
     for (const std::string& gpName : model->getJointModelGroupNames())
     {
-      std::cout << "group name " << gpName << std::endl << "robot model  " << model->getName() << std::endl;
       planning_contexts_[gpName] =
-          LERPPlanningContextPtr(new LERPPlanningContext("lerp_planning_context", gpName, model));
+          LERPPlanningContextPtr(new LERPPlanningContext("lerp_planning_context", ns, gpName, model));
     }
     return true;
   }
@@ -99,21 +98,14 @@ class LERPPlannerManager : public planning_interface::PlannerManager
       return planning_interface::PlanningContextPtr();
     }
 
-    // create PlanningScene using hybrid collision detector
-    planning_scene::PlanningScenePtr ps = planning_scene->diff();
-
-    // set FCL as the allocaotor
-    ps->setActiveCollisionDetector(collision_detection::CollisionDetectorAllocatorFCL::create(), true);
-
     // retrieve and configure existing context
     const LERPPlanningContextPtr& context = planning_contexts_.at(req.group_name);
     ROS_INFO_STREAM_NAMED("lerp_planner_manager", "===>>> context is made ");
 
-    context->setPlanningScene(ps);
+    context->setPlanningScene(planning_scene);
     context->setMotionPlanRequest(req);
 
     error_code.val = moveit_msgs::MoveItErrorCodes::SUCCESS;
-
     return context;
   }
 

doc/creating_moveit_plugins/lerp_motion_planner/src/lerp_planning_context.cpp

@@ -44,11 +44,11 @@
 
 namespace lerp_interface
 {
-LERPPlanningContext::LERPPlanningContext(const std::string& context_name, const std::string& group_name,
-                                         const moveit::core::RobotModelConstPtr& model)
+LERPPlanningContext::LERPPlanningContext(const std::string& context_name, const std::string& ns,
+                                         const std::string& group_name, const moveit::core::RobotModelConstPtr& model)
   : planning_interface::PlanningContext(context_name, group_name), robot_model_(model)
 {
-  lerp_interface_ = LERPInterfacePtr(new LERPInterface());
+  lerp_interface_ = LERPInterfacePtr(new LERPInterface(ros::NodeHandle(ns)));
 }
 
 bool LERPPlanningContext::solve(planning_interface::MotionPlanDetailedResponse& res)