`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.

https://github.com/Yaskawa-Global/motoros2/blob/2d2b62df3655078b93cfe77c1c133ff5df8670fd/src/MotionControl.c#L1012-L1028

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(): https://github.com/Yaskawa-Global/motoros2/blob/b066b5fe6ac0f7e750254e986153d236ada6052e/src/ServiceStopTrajMode.c#L58-L65 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(): https://github.com/Yaskawa-Global/motoros2/blob/b066b5fe6ac0f7e750254e986153d236ada6052e/src/MotionControl.c#L1225-L1243 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()?