diff --git a/core/src/introspection.cpp b/core/src/introspection.cpp index 34822610..bc38b93c 100644 --- a/core/src/introspection.cpp +++ b/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) { diff --git a/core/src/task.cpp b/core/src/task.cpp index 78b8f5ea..02ebaa82 100644 --- a/core/src/task.cpp +++ b/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;