stop_traj_mode: "Couldn't stop trajectory mode: unknown error"

[gavanderhoorn]

While testing the node described in #66, I ran into the following problem.

Invoking the stop_traj_mode service returned:

user@host:~$ ros2 service call /yrc1000/stop_traj_mode std_srvs/srv/Trigger 
requester: making request: std_srvs.srv.Trigger_Request()

response:
std_srvs.srv.Trigger_Response(success=False, message="Couldn't stop trajectory mode: unknown error")

debug log:

[2023-06-26 17:23:33.703843] [192.168.255.1:58950]: stop_traj_mode: attempting to stop trajectory mode
[2023-06-26 17:23:33.777264] [192.168.255.1:58950]: WARNING: Message processing not stopped before clearing queue
[2023-06-26 17:23:33.777611] [192.168.255.1:58950]: stop_traj_mode: StopMotion failed (unknown error)
[2023-06-26 17:23:33.783029] [192.168.255.1:58950]: Stopping point-queue motion mode. Please call 'start_point_queue_mode' to start a new queue.

This was after queuing points for a couple of trajectories using MoveIt.

---

Edit: INIT_ROS was terminated, and servos eventually also de-energised. Not sure what the error cause really was.

[ted-miller]

If the internal queue is full and there's pending point to be processed, we're only giving it 20 ms to finish processing.

motoros2/src/MotionControl.c

Lines 1012 to 1028 in 2d2b62d

	for (checkCnt = 0; checkCnt < MOTION_STOP_TIMEOUT; checkCnt++)
	{
	BOOL bAtLeastOne = FALSE;
	for (int i = 0; i < g_Ros_Controller.numGroup; i += 1)
	{
	if (g_Ros_Controller.ctrlGroups[i]->hasDataToProcess)
	bAtLeastOne = TRUE;
	}
	if (!bAtLeastOne)
	{
	bStopped = TRUE;
	break;
	}
	else
	Ros_Sleep(1);
	}

Immediately following that though, Ros_MotionControl_ClearQ_All sets hasDataToProcess to FALSE. So really, that timeout is rather pointless and leads to an erroneous error message.

I think we should remove that timeout loop and remove the bStopped field.

[gavanderhoorn]

I believe what you write, but in the test the robot had already come to a complete stop.

[zakjeens]

I ran into the same issue as described originally. I had set the start_point_queue_mode but needed to switch back to the trajectory mode, so I called stop_traj_mode (since there isn't a stop_point_queue_mode ) and got the same error

Couldn't stop trajectory mode: unknown error

I am guessing this is because stop_traj_mode is meant for the trajectory mode and not point queue mode? However other than getting this error message and success=False in the response, it seems to work as expected as I was able to call the start_traj_mode without errors.

I confirm that the trajectory was finished and the robot was not moving, we actually waited for an extra minute just in case when verifying this behavior.

[Yolnan]

I ran into a similar issue running an HC10DT and YRC1000micro. While in start_point_queue_mode, the 1st service call to stop_traj_mode failed with "Couldn't stop trajectory mode: unknown error". However, the 2nd service call succeeded. The 1st service call to stop_traj_mode would fail with unknown error even when the robot_status showed that in_motion was false (0).

[yai-rosejo]

I did a small code trace of where this issue could be occurring:
ServiceStopTrajMode.c/Ros_ServiceStopTrajMode_Trigger():

motoros2/src/ServiceStopTrajMode.c

Lines 58 to 65 in b066b5f

	// Stop motion
	if(!Ros_MotionControl_StopMotion(/*bKeepJobRunning = */ FALSE))
	{
	Ros_Debug_BroadcastMsg("stop_traj_mode: StopMotion failed (unknown error)");
	rosidl_runtime_c__String__assign(&response->message, "Couldn't stop trajectory mode: unknown error");
	response->success = FALSE;
	return;
	}

Inside MotionControl.c:
Ros_MotionControl_StopMotion() does no early exiting, so the function will complete even if a single part of its process runs into issues. The function returns (bStopped && bRet)
bStopped is checking if the control groups have any data left.
bRet is the return value of ROS_MotionControl_ClearQ_ALL(), this value is always TRUE.
So the error is being generated from this loop in Ros_MotionControl_StopMotion():

motoros2/src/MotionControl.c

Lines 1225 to 1243 in b066b5f

	bStopped = FALSE;
	// Check that background processing of message has been stopped
	for (checkCnt = 0; checkCnt < MOTION_STOP_TIMEOUT; checkCnt++)
	{
	BOOL bAtLeastOne = FALSE;
	for (int i = 0; i < g_Ros_Controller.numGroup; i += 1)
	{
	if (g_Ros_Controller.ctrlGroups[i]->hasDataToProcess)
	bAtLeastOne = TRUE;
	}
	if (!bAtLeastOne)
	{
	bStopped = TRUE;
	break;
	}
	else
	Ros_Sleep(1);
	}

I believe g_Ros_Controller.ctrlGroups[i]->hasDataToProcess would be the incorrect field to use due to hasDataToProcess is actually just a flag saying if a control group should be using the queue system, it isn't checking if there is any data available when in PointQueueMode

[gavanderhoorn]

Thanks for looking into this @yai-rosejo.

I believe g_Ros_Controller.ctrlGroups[i]->hasDataToProcess would be the incorrect field to use due to hasDataToProcess is actually just a flag saying if a control group should be using the queue system, it isn't checking if there is any data available when in PointQueueMode

this makes sense to me.

I believe we just tried to reuse the already existing stop_traj_mode for both types of motion interface. We may need to specialise / conditionally check other parts of internal state instead.

[yai-rosejo]

@gavanderhoorn Would we want to add a Ros_MotionControl_IsMotionMode_Trajectory conditional before the for loop in Ros_MotionControl_StopMotion()?