From bfa2a6070d578b852fbeab883ff34485aceab97c Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Thu, 29 Oct 2020 18:04:48 +0100 Subject: [PATCH] MoveTo: Publish failed planning attempts --- core/src/stages/move_to.cpp | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/core/src/stages/move_to.cpp b/core/src/stages/move_to.cpp index f9f915c3..ab7f052c 100644 --- a/core/src/stages/move_to.cpp +++ b/core/src/stages/move_to.cpp @@ -252,6 +252,11 @@ bool MoveTo::compute(const InterfaceState& state, planning_scene::PlanningSceneP } // store result + if (!robot_trajectory && storeFailures()) { + robot_trajectory = std::make_shared(robot_model, jmg); + robot_trajectory->addSuffixWayPoint(state.scene()->getCurrentState(), 0.0); + robot_trajectory->addSuffixWayPoint(scene->getCurrentState(), 1.0); + } if (robot_trajectory) { scene->setCurrentState(robot_trajectory->getLastWayPoint()); if (dir == BACKWARD)