From 8152994e52f588afc82383367242d38d51486816 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Sat, 11 Jul 2020 16:54:14 +0200 Subject: [PATCH 1/4] Fix interface resolution --- core/src/container.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/core/src/container.cpp b/core/src/container.cpp index 18892591..b3c3d656 100644 --- a/core/src/container.cpp +++ b/core/src/container.cpp @@ -756,8 +756,8 @@ void Fallbacks::onNewSolution(const SolutionBase& s) { MergerPrivate::MergerPrivate(Merger* me, const std::string& name) : ParallelContainerBasePrivate(me, name) {} void MergerPrivate::resolveInterface(InterfaceFlags expected) { - ContainerBasePrivate::resolveInterface(expected); - switch (interfaceFlags()) { + ParallelContainerBasePrivate::resolveInterface(expected); + switch (requiredInterface()) { case PROPAGATE_FORWARDS: case PROPAGATE_BACKWARDS: case UNKNOWN: From 14fc3d25864f8cba1e2a028051a38c9308540228 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Sat, 11 Jul 2020 16:54:40 +0200 Subject: [PATCH 2/4] Reject failures early --- core/src/container.cpp | 18 ++++++++---------- 1 file changed, 8 insertions(+), 10 deletions(-) diff --git a/core/src/container.cpp b/core/src/container.cpp index b3c3d656..a43419f8 100644 --- a/core/src/container.cpp +++ b/core/src/container.cpp @@ -805,6 +805,9 @@ void Merger::compute() { } void Merger::onNewSolution(const SolutionBase& s) { + if (s.isFailure()) // ignore failure solutions + return; + auto impl = pimpl(); switch (impl->interfaceFlags()) { case PROPAGATE_FORWARDS: @@ -821,8 +824,8 @@ void Merger::onNewSolution(const SolutionBase& s) { void MergerPrivate::onNewPropagateSolution(const SolutionBase& s) { const SubTrajectory* trajectory = dynamic_cast(&s); - if (!trajectory) { - ROS_ERROR_NAMED("Merger", "Only simple trajectories are supported"); + if (!trajectory || !trajectory->trajectory()) { + ROS_ERROR_NAMED("Merger", "Only simple, valid trajectories are supported"); return; } @@ -872,7 +875,7 @@ void MergerPrivate::sendBackward(SubTrajectory&& t, const InterfaceState* to) { StagePrivate::sendBackward(InterfaceState(from), *to, std::make_shared(std::move(t))); } -void MergerPrivate::onNewGeneratorSolution(const SolutionBase& s) { +void MergerPrivate::onNewGeneratorSolution(const SolutionBase& /* s */) { // TODO: implement in similar fashion as onNewPropagateSolution(), but also merge start/end states } @@ -918,13 +921,8 @@ void MergerPrivate::merge(const ChildSolutionList& sub_solutions, // transform vector of SubTrajectories into vector of RobotTrajectories std::vector sub_trajectories; sub_trajectories.reserve(sub_solutions.size()); - for (const auto& sub : sub_solutions) { - // TODO: directly skip failures in mergeAnyCombination() or even earlier - if (sub->isFailure()) - return; - if (sub->trajectory()) - sub_trajectories.push_back(sub->trajectory()); - } + for (const auto& sub : sub_solutions) + sub_trajectories.push_back(sub->trajectory()); moveit::core::JointModelGroup* jmg = jmg_merged_.get(); robot_trajectory::RobotTrajectoryPtr merged; From 7499f7b297882794d037639e6785a256d8146f44 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Sat, 11 Jul 2020 16:55:18 +0200 Subject: [PATCH 3/4] Report merge failures --- core/src/container.cpp | 30 ++++++++++++++++++++---------- 1 file changed, 20 insertions(+), 10 deletions(-) diff --git a/core/src/container.cpp b/core/src/container.cpp index a43419f8..299c5ff3 100644 --- a/core/src/container.cpp +++ b/core/src/container.cpp @@ -937,18 +937,28 @@ void MergerPrivate::merge(const ChildSolutionList& sub_solutions, if (!merged) return; - // check merged trajectory for collisions - if (!start_scene->isPathValid(*merged)) - return; - SubTrajectory t(merged); - // accumulate costs and markers - double costs = 0.0; - for (const auto& sub : sub_solutions) { - costs += sub->cost(); - t.markers().insert(t.markers().end(), sub->markers().begin(), sub->markers().end()); + + // check merged trajectory for collisions + std::vector invalid_index; + if (!start_scene->isPathValid(*merged, "", true, &invalid_index)) { + t.markAsFailure(); + std::ostringstream oss; + oss << "Invalid waypoint(s): "; + if (invalid_index.size() == merged->getWayPointCount()) + oss << "all"; + else for (size_t i : invalid_index) + oss << i << ", "; + t.setComment(oss.str()); + } else { + // accumulate costs and markers + double costs = 0.0; + for (const auto& sub : sub_solutions) { + costs += sub->cost(); + t.markers().insert(t.markers().end(), sub->markers().begin(), sub->markers().end()); + } + t.setCost(costs); } - t.setCost(costs); spawner(std::move(t)); } } // namespace task_constructor From 88c217e9a6fae04eab35312a5a6e443bf41642a5 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Tue, 22 Sep 2020 10:54:51 +0200 Subject: [PATCH 4/4] Report exceptions (e.g. config issues) --- core/src/container.cpp | 14 +++++++++----- 1 file changed, 9 insertions(+), 5 deletions(-) diff --git a/core/src/container.cpp b/core/src/container.cpp index 299c5ff3..d05d10b1 100644 --- a/core/src/container.cpp +++ b/core/src/container.cpp @@ -864,14 +864,16 @@ void MergerPrivate::onNewPropagateSolution(const SolutionBase& s) { void MergerPrivate::sendForward(SubTrajectory&& t, const InterfaceState* from) { // generate target state planning_scene::PlanningScenePtr to = from->scene()->diff(); - to->setCurrentState(t.trajectory()->getLastWayPoint()); + if (t.trajectory() && !t.trajectory()->empty()) + to->setCurrentState(t.trajectory()->getLastWayPoint()); StagePrivate::sendForward(*from, InterfaceState(to), std::make_shared(std::move(t))); } void MergerPrivate::sendBackward(SubTrajectory&& t, const InterfaceState* to) { // generate target state planning_scene::PlanningScenePtr from = to->scene()->diff(); - from->setCurrentState(t.trajectory()->getFirstWayPoint()); + if (t.trajectory() && !t.trajectory()->empty()) + from->setCurrentState(t.trajectory()->getFirstWayPoint()); StagePrivate::sendBackward(InterfaceState(from), *to, std::make_shared(std::move(t))); } @@ -929,14 +931,16 @@ void MergerPrivate::merge(const ChildSolutionList& sub_solutions, try { merged = task_constructor::merge(sub_trajectories, start_scene->getCurrentState(), jmg); } catch (const std::runtime_error& e) { - ROS_INFO_STREAM_NAMED("Merger", this->name() << "Merging failed: " << e.what()); + SubTrajectory t; + t.markAsFailure(); + t.setComment(e.what()); + spawner(std::move(t)); return; } if (jmg_merged_.get() != jmg) jmg_merged_.reset(jmg); - if (!merged) - return; + assert(merged); SubTrajectory t(merged); // check merged trajectory for collisions