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.
This commit is contained in:
Robert Haschke 2020-06-18 11:07:05 +02:00 committed by GitHub
parent 4fa706660e
commit ee7ebb3b96
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
2 changed files with 4 additions and 1 deletions

View File

@ -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) {

View File

@ -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;