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
|
||||
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)
|
||||
|
||||
Loading…
Reference in New Issue
Block a user