mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-09-27 00:29:13 +08:00
MoveRelative: fix segfault on empty trajectory (#595)
Check that at least one robot state exists in the robot trajectory before accessing it. Signed-off-by: Paul Gesel <paul.gesel@picknik.ai>
This commit is contained in:
parent
2e9a223827
commit
60ccd74443
@ -296,7 +296,8 @@ bool MoveRelative::compute(const InterfaceState& state, planning_scene::Planning
|
||||
if (!success)
|
||||
comment = result.message;
|
||||
|
||||
if (robot_trajectory) { // the following requires a robot_trajectory returned from planning
|
||||
if (robot_trajectory && robot_trajectory->getWayPointCount() > 0) { // the following requires a robot_trajectory
|
||||
// returned from planning
|
||||
moveit::core::RobotStatePtr& reached_state = robot_trajectory->getLastWayPointPtr();
|
||||
reached_state->updateLinkTransforms();
|
||||
const Eigen::Isometry3d& reached_pose = reached_state->getGlobalLinkTransform(link) * offset;
|
||||
@ -331,7 +332,7 @@ bool MoveRelative::compute(const InterfaceState& state, planning_scene::Planning
|
||||
}
|
||||
|
||||
// store result
|
||||
if (robot_trajectory) {
|
||||
if (robot_trajectory && robot_trajectory->getWayPointCount() > 0) {
|
||||
scene->setCurrentState(robot_trajectory->getLastWayPoint());
|
||||
if (dir == Interface::BACKWARD)
|
||||
robot_trajectory->reverse();
|
||||
|
Loading…
Reference in New Issue
Block a user