MoveTo: Publish failed planning attempts

This commit is contained in:
Robert Haschke 2020-10-29 18:04:48 +01:00
parent e83a5ecefa
commit bfa2a6070d

View File

@ -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_trajectory::RobotTrajectory>(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)