Fix Solution::fillMessage() (#432)

The scene_diff field usually describes PlanningScene changes of the end scene relative to the start scene.
For backwards planning, this direction is reversed: the start scene is derived from the end scene.
Thus, we need to generate a full planning scene message for the end scene if planning progressed backwards.

Fixes #405.

Author: rhaschke

capabilities/src/execute_task_solution_capability.cpp

@@ -170,11 +170,15 @@ bool ExecuteTaskSolutionCapability::constructMotionPlan(const moveit_task_constr
 		exec_traj.controller_names_ = sub_traj.execution_info.controller_names;
 
 		/* TODO add action feedback and markers */
-		exec_traj.effect_on_success_ = [this, sub_traj,
+		exec_traj.effect_on_success_ = [this,
+		                                &scene_diff = const_cast<::moveit_msgs::PlanningScene&>(sub_traj.scene_diff),
 		                                description](const plan_execution::ExecutableMotionPlan* /*plan*/) {
-			if (!moveit::core::isEmpty(sub_traj.scene_diff)) {
+			scene_diff.robot_state.joint_state = sensor_msgs::JointState();
+			scene_diff.robot_state.multi_dof_joint_state = sensor_msgs::MultiDOFJointState();
+
+			if (!moveit::core::isEmpty(scene_diff)) {
 				ROS_DEBUG_STREAM_NAMED("ExecuteTaskSolution", "apply effect of " << description);
-				return context_->planning_scene_monitor_->newPlanningSceneMessage(sub_traj.scene_diff);
+				return context_->planning_scene_monitor_->newPlanningSceneMessage(scene_diff);
 			}
 			return true;
 		};

core/python/test/rostest_mps.py

@@ -81,7 +81,7 @@ def test_fw_remove_object(self):
         s = self.task.solutions[0].toMsg()
         self.assertEqual(s.sub_trajectory[1].scene_diff.world.collision_objects[0].id, "box")
 
-    def DISABLED_test_bw_add_object(self):
+    def test_bw_add_object(self):
         # add object to move_group's planning scene
         self.psi.add_box("block", make_pose(0.8, 0.55, 1.25), [0.2, 0.2, 0.2])
 
@@ -104,7 +104,7 @@ def DISABLED_test_bw_add_object(self):
         objects = [o.id for o in s.sub_trajectory[1].scene_diff.world.collision_objects]
         self.assertTrue(objects == ["block", "box"])
 
-    def DISABLED_test_bw_remove_object(self):
+    def test_bw_remove_object(self):
         mps = stages.ModifyPlanningScene("removeObject(box) backwards")
         mps.removeObject("box")
         self.task.insert(mps, 0)

core/src/storage.cpp

@@ -231,10 +231,10 @@ void SubTrajectory::appendTo(moveit_task_constructor_msgs::Solution& msg, Intros
 	if (trajectory())
 		trajectory()->getRobotTrajectoryMsg(t.trajectory);
 
-	this->end()->scene()->getPlanningSceneDiffMsg(t.scene_diff);
-	// reset JointStates (joints are already handled in trajectories)
-	t.scene_diff.robot_state.joint_state = sensor_msgs::JointState();
-	t.scene_diff.robot_state.multi_dof_joint_state = sensor_msgs::MultiDOFJointState();
+	if (this->end()->scene()->getParent() == this->start()->scene())
+		this->end()->scene()->getPlanningSceneDiffMsg(t.scene_diff);
+	else
+		this->end()->scene()->getPlanningSceneMsg(t.scene_diff);
 }
 
 double SubTrajectory::computeCost(const CostTerm& f, std::string& comment) const {