Gracefully handle NULL robot_trajectory (moveit#469)

Author: rhaschke

core/src/stages/move_relative.cpp

@@ -281,35 +281,37 @@ bool MoveRelative::compute(const InterfaceState& state, planning_scene::Planning
 		success =
 		    planner_->plan(state.scene(), *link, offset, target_eigen, jmg, timeout, robot_trajectory, path_constraints);
 
-		robot_state::RobotStatePtr& reached_state = robot_trajectory->getLastWayPointPtr();
-		reached_state->updateLinkTransforms();
-		const Eigen::Isometry3d& reached_pose = reached_state->getGlobalLinkTransform(link) * offset;
-
-		double distance = 0.0;
-		if (use_rotation_distance) {
-			Eigen::AngleAxisd rotation(reached_pose.linear() * ik_pose_world.linear().transpose());
-			distance = rotation.angle();
-		} else
-			distance = (reached_pose.translation() - ik_pose_world.translation()).norm();
-
-		// min_distance reached?
-		if (min_distance > 0.0) {
-			success = distance >= min_distance;
-			if (!success) {
-				char msg[100];
-				snprintf(msg, sizeof(msg), "min_distance not reached (%.3g < %.3g)", distance, min_distance);
-				solution.setComment(msg);
+		if (robot_trajectory) {  // the following requires a robot_trajectory returned from planning
+			robot_state::RobotStatePtr& reached_state = robot_trajectory->getLastWayPointPtr();
+			reached_state->updateLinkTransforms();
+			const Eigen::Isometry3d& reached_pose = reached_state->getGlobalLinkTransform(link) * offset;
+
+			double distance = 0.0;
+			if (use_rotation_distance) {
+				Eigen::AngleAxisd rotation(reached_pose.linear() * ik_pose_world.linear().transpose());
+				distance = rotation.angle();
+			} else
+				distance = (reached_pose.translation() - ik_pose_world.translation()).norm();
+
+			// min_distance reached?
+			if (min_distance > 0.0) {
+				success = distance >= min_distance;
+				if (!success) {
+					char msg[100];
+					snprintf(msg, sizeof(msg), "min_distance not reached (%.3g < %.3g)", distance, min_distance);
+					solution.setComment(msg);
+				}
+			} else if (min_distance == 0.0) {  // if min_distance is zero, we succeed in any case
+				success = true;
+			} else if (!success)
+				solution.setComment("failed to move full distance");
+
+			// visualize plan
+			auto ns = props.get<std::string>("marker_ns");
+			if (!ns.empty() && linear_norm > 0) {  // ensures that 'distance' is the norm of the reached distance
+				visualizePlan(solution.markers(), dir, success, ns, scene->getPlanningFrame(), ik_pose_world, reached_pose,
+				              linear, distance);
 			}
-		} else if (min_distance == 0.0) {  // if min_distance is zero, we succeed in any case
-			success = true;
-		} else if (!success)
-			solution.setComment("failed to move full distance");
-
-		// visualize plan
-		auto ns = props.get<std::string>("marker_ns");
-		if (!ns.empty() && linear_norm > 0) {  // ensures that 'distance' is the norm of the reached distance
-			visualizePlan(solution.markers(), dir, success, ns, scene->getPlanningFrame(), ik_pose_world, reached_pose,
-			              linear, distance);
 		}
 	}