Add info messages to pick and place routine (#1004)

* Add debug messages to pick and place routine The current behavior silently adds an arbitrary wait time to the pipeline. Users should define an appropriate wait time for their own gripper. This message helps them find the issue, which is otherwise not trivial to debug. * Apply clang-format * Fix auto-formatting
moveit · Jul 31, 2018 · 76058c3 · 76058c3
1 parent f650603
commit 76058c3
Show file tree

Hide file tree

Showing 2 changed files with 6 additions and 0 deletions.
diff --git a/moveit_ros/manipulation/pick_place/src/approach_and_translate_stage.cpp b/moveit_ros/manipulation/pick_place/src/approach_and_translate_stage.cpp
@@ -171,6 +171,9 @@ void addGripperTrajectory(const ManipulationPlanPtr& plan,
     }
     else
     {  // Do what was done before
+      ROS_INFO_STREAM("Adding default duration of " << PickPlace::DEFAULT_GRASP_POSTURE_COMPLETION_DURATION
+                                                    << " seconds to the grasp closure time. Assign time_from_start to "
+                                                    << "your trajectory to avoid this.");
       ee_closed_traj->addPrefixWayPoint(ee_closed_state, PickPlace::DEFAULT_GRASP_POSTURE_COMPLETION_DURATION);
     }
 

diff --git a/moveit_ros/manipulation/pick_place/src/plan_stage.cpp b/moveit_ros/manipulation/pick_place/src/plan_stage.cpp
@@ -92,6 +92,9 @@ bool PlanStage::evaluate(const ManipulationPlanPtr& plan) const
         }
         else
         {  // Do what was done before
+          ROS_INFO_STREAM("Adding default duration of " << PickPlace::DEFAULT_GRASP_POSTURE_COMPLETION_DURATION
+                                                        << " seconds to the grasp closure time. Assign time_from_start "
+                                                        << "to your trajectory to avoid this.");
           pre_approach_traj->addPrefixWayPoint(pre_approach_state,
                                                PickPlace::DEFAULT_GRASP_POSTURE_COMPLETION_DURATION);
         }