mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
MoveTo: Publish failed planning attempts
This commit is contained in:
parent
e83a5ecefa
commit
bfa2a6070d
@ -252,6 +252,11 @@ bool MoveTo::compute(const InterfaceState& state, planning_scene::PlanningSceneP
|
|||||||
}
|
}
|
||||||
|
|
||||||
// store result
|
// 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) {
|
if (robot_trajectory) {
|
||||||
scene->setCurrentState(robot_trajectory->getLastWayPoint());
|
scene->setCurrentState(robot_trajectory->getLastWayPoint());
|
||||||
if (dir == BACKWARD)
|
if (dir == BACKWARD)
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user