Report exceptions (e.g. config issues)

This commit is contained in:
Robert Haschke 2020-09-22 10:54:51 +02:00
parent 7499f7b297
commit 88c217e9a6

View File

@ -864,14 +864,16 @@ void MergerPrivate::onNewPropagateSolution(const SolutionBase& s) {
void MergerPrivate::sendForward(SubTrajectory&& t, const InterfaceState* from) { void MergerPrivate::sendForward(SubTrajectory&& t, const InterfaceState* from) {
// generate target state // generate target state
planning_scene::PlanningScenePtr to = from->scene()->diff(); 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<SubTrajectory>(std::move(t))); StagePrivate::sendForward(*from, InterfaceState(to), std::make_shared<SubTrajectory>(std::move(t)));
} }
void MergerPrivate::sendBackward(SubTrajectory&& t, const InterfaceState* to) { void MergerPrivate::sendBackward(SubTrajectory&& t, const InterfaceState* to) {
// generate target state // generate target state
planning_scene::PlanningScenePtr from = to->scene()->diff(); 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<SubTrajectory>(std::move(t))); StagePrivate::sendBackward(InterfaceState(from), *to, std::make_shared<SubTrajectory>(std::move(t)));
} }
@ -929,14 +931,16 @@ void MergerPrivate::merge(const ChildSolutionList& sub_solutions,
try { try {
merged = task_constructor::merge(sub_trajectories, start_scene->getCurrentState(), jmg); merged = task_constructor::merge(sub_trajectories, start_scene->getCurrentState(), jmg);
} catch (const std::runtime_error& e) { } 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; return;
} }
if (jmg_merged_.get() != jmg) if (jmg_merged_.get() != jmg)
jmg_merged_.reset(jmg); jmg_merged_.reset(jmg);
if (!merged)
return;
assert(merged);
SubTrajectory t(merged); SubTrajectory t(merged);
// check merged trajectory for collisions // check merged trajectory for collisions