From c5532752764ba859bd750cb8df6420fa59b5c90f Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Tue, 10 Apr 2018 00:23:10 +0200 Subject: [PATCH] SolutionSequence::fillMessage: ignore sub solutions with same creator as parent --- core/src/stages/connect.cpp | 1 + core/src/storage.cpp | 23 +++++++++++++++++++++-- 2 files changed, 22 insertions(+), 2 deletions(-) diff --git a/core/src/stages/connect.cpp b/core/src/stages/connect.cpp index a6d7d7fd..90ec3076 100644 --- a/core/src/stages/connect.cpp +++ b/core/src/stages/connect.cpp @@ -170,6 +170,7 @@ bool Connect::compute(const InterfaceState &from, const InterfaceState &to) { if (sub_trajectories.size() != planner_.size()) { // error during sequential planning if (!storeFailures()) return false; + // push back a dummy solution to also show the target scene of the failed attempt sub_trajectories.push_back(robot_trajectory::RobotTrajectoryPtr()); solution = storeSequential(sub_trajectories, intermediate_scenes); // mark solution as failure diff --git a/core/src/storage.cpp b/core/src/storage.cpp index bd0b03d2..ae05d77c 100644 --- a/core/src/storage.cpp +++ b/core/src/storage.cpp @@ -165,16 +165,35 @@ void SolutionSequence::fillMessage(moveit_task_constructor_msgs::Solution &msg, const Introspection *ci = introspection; sub_msg.stage_id = ci ? ci->stageId(this->creator()->me()) : 0; + // Usually subsolutions originate from another stage than this solution. + // However, the Connect stage will announce sub solutions created by itself + // (only in case of merge failure). To not confuse the display's SolutionModel + // we do not publish own IDs for them, but set those IDs to zero. sub_msg.sub_solution_id.reserve(subsolutions_.size()); if (introspection) { - for (const SolutionBase* s : subsolutions_) + for (const SolutionBase* s : subsolutions_) { + // skip sub solutions with same creator as this + if (s->creator() == this->creator()) + continue; sub_msg.sub_solution_id.push_back(introspection->solutionId(*s)); + } msg.sub_solution.push_back(sub_msg); } msg.sub_trajectory.reserve(msg.sub_trajectory.size() + subsolutions_.size()); - for (const SolutionBase* s : subsolutions_) + for (const SolutionBase* s : subsolutions_) { + size_t current = msg.sub_trajectory.size(); s->fillMessage(msg, introspection); + + // zero IDs of sub solutions with same creator as this + if (s->creator() == this->creator()) { + auto it = msg.sub_trajectory.begin(); + auto end = msg.sub_trajectory.end(); + std::advance(it, current); + for (; it != end; ++it) + it->id = 0; + } + } }