SolutionSequence::fillMessage: ignore sub solutions with same creator as parent

This commit is contained in:
Robert Haschke 2018-04-10 00:23:10 +02:00
parent 556d9e1a5e
commit c553275276
2 changed files with 22 additions and 2 deletions

View File

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

View File

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