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 <[email protected]>

Author: pac48

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