Missing check for energy-savings mode

[ted-miller]

Related to #114
If we are already in PointQueueMode and Ros_ServiceQueueTrajPoint_Trigger is called after energy-savings mode kicks in, then it won't check for eco-mode.

[yai-rosejo]

Energy-savings mode kicks the system out of point_queue_mode. No specific change is required.

motoros2/src/MotionControl.c

Lines 1591 to 1608 in 10d8f00

	void Ros_MotionControl_ValidateMotionModeIsOk()
	{
	if (!g_messages_RobotStatus.msgRobotStatus->motion_possible.val)
	{
	if (Ros_MotionControl_IsMotionMode_PointQueue())
	{
	//If we are in a point-queue-mode and the motion_possible flag drops,
	//then we need to abort the motion mode and force them to initialize the
	//mode again. This allows the system to detect if the robot is not starting back up
	//from the expected location. (System could have been estopped during the trajectory,
	//or the user could have switched to TEACH and moved the arm around.)
	Ros_Debug_BroadcastMsg("Stopping point-queue motion mode. Please call '%s' to start a new queue.", SERVICE_NAME_START_POINT_QUEUE_MODE);
	Ros_MotionControl_StopTrajMode();
	}
	//TODO: Determine if this should be done for Trajecotry-Mode too
	}
	}