From ee7ebb3b9693b7314dab599adde936d933f61cc8 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Thu, 18 Jun 2020 11:07:05 +0200 Subject: [PATCH] SolutionMsg: always fill start_scene (#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. --- core/src/introspection.cpp | 3 ++- core/src/task.cpp | 2 ++ 2 files changed, 4 insertions(+), 1 deletion(-) 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;