Merge branch 'master' into illustrate_pick_place_task

demo/src/pick_place_task.cpp

@@ -145,12 +145,12 @@ void PickPlaceTask::loadParameters() {
 }
 
 // The init function declares all movements for the pick-and-place task. These movements are described below.
-void PickPlaceTask::init() {
+bool PickPlaceTask::init() {
 	ROS_INFO_NAMED(LOGNAME, "Initializing task pipeline");
 	const std::string object = object_name_;
 
 	// Reset ROS introspection before constructing the new object
-	// TODO(henningkayser): verify this is a bug, fix if possible
+	// TODO(v4hn): global storage for Introspection services to enable one-liner
 	// Movements are collected inside a Task object.
 	task_.reset();
 	task_.reset(new moveit::task_constructor::Task());
@@ -181,8 +181,6 @@ void PickPlaceTask::init() {
 	 *               Current State                      *
 	 *                                                  *
 	 ***************************************************/
-	// First, verify that the object is not already attached to the robot.
-	Stage* current_state_ptr = nullptr;  // Forward current_state on to grasp pose generator
 	{
 		auto current_state = std::make_unique<stages::CurrentState>("current state");
 
@@ -269,8 +267,8 @@ void PickPlaceTask::init() {
 			stage->properties().set("marker_ns", "grasp_pose");
 			stage->setPreGraspPose(hand_open_pose_);
 			stage->setObject(object);
-			stage->setAngleDelta(M_PI / 4);
-			stage->setMonitoredStage(current_state_ptr);  // Hook into current state
+			stage->setAngleDelta(M_PI / 12);
+			stage->setMonitoredStage(initial_state_ptr);  // hook into successful initial-phase solutions
 
 			// Compute joint parameters for a target frame of the end effector. Each target frame equals a grasp pose
 			// from the previous stage times the inverse of the user-defined grasp_frame_tansform.