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:
Paul Gesel 2024-07-16 09:18:16 -06:00 committed by GitHub
parent 2e9a223827
commit 60ccd74443
No known key found for this signature in database
GPG Key ID: B5690EEEBB952194

View File

@ -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();