Fix generation of Solution msg: consider backward operation

Need to generate full planning scene message if planning progressed backwards:
The scene_diff field describes PlanningScene changes of the end scene relative to the start scene,
while during planning it might be inversed.

Author: rhaschke

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 {