From 88c217e9a6fae04eab35312a5a6e443bf41642a5 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Tue, 22 Sep 2020 10:54:51 +0200 Subject: [PATCH] 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