SolutionMsg: always fill start_scene (moveit#175)

So far, the start_scene field of a SolutionMsg was only filled by  Introspection::fillSolution(),
but not yet by Task::execute().

Addendum(v4hn): The previous approach was actually reasonable too (although the scene should have been marked as `is_diff`) for solutions sent for execution, but keeping the full start_scene around can facilitate debugging from recorded data.

Author: rhaschke

core/src/introspection.cpp

@@ -133,9 +133,10 @@ void Introspection::registerSolution(const SolutionBase& s) {
 
 void Introspection::fillSolution(moveit_task_constructor_msgs::Solution& msg, const SolutionBase& s) {
 	s.fillMessage(msg, this);
+	s.start()->scene()->getPlanningSceneMsg(msg.start_scene);
+
 	msg.process_id = impl->process_id_;
 	msg.task_id = impl->task_->id();
-	s.start()->scene()->getPlanningSceneMsg(msg.start_scene);
 }
 
 void Introspection::publishSolution(const SolutionBase& s) {

core/src/task.cpp

@@ -318,6 +318,8 @@ moveit_msgs::MoveItErrorCodes Task::execute(const SolutionBase& s) {
 
 	moveit_task_constructor_msgs::ExecuteTaskSolutionGoal goal;
 	s.fillMessage(goal.solution, pimpl()->introspection_.get());
+	s.start()->scene()->getPlanningSceneMsg(goal.solution.start_scene);
+
 	ac.sendGoal(goal);
 	ac.waitForResult();
 	return ac.getResult()->error_code;