mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
SolutionSequence::fillMessage: ignore sub solutions with same creator as parent
This commit is contained in:
parent
556d9e1a5e
commit
c553275276
@ -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
|
||||
|
||||
@ -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;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
Loading…
Reference in New Issue
Block a user