From 60ccd744435e90393a38bb987bb398f8f9760974 Mon Sep 17 00:00:00 2001 From: Paul Gesel Date: Tue, 16 Jul 2024 09:18:16 -0600 Subject: [PATCH] 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 --- core/src/stages/move_relative.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/core/src/stages/move_relative.cpp b/core/src/stages/move_relative.cpp index 634c8b56f..7bf065f39 100644 --- a/core/src/stages/move_relative.cpp +++ b/core/src/stages/move_relative.cpp @@ -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();