Issue 233-outside goal tolerance

[yai-rosejo]

This PR is to solve multiple issues dealing with goal tolerances referenced in #233

1. Received Goal tolerances can now be received out of order, the name of the tolerance is used to match it with the expected joint.
2. The final goal position is used instead of the desired position to deal with a small out of sync issue.
3. The special notations of -1.0, to ignore tolerance, and 0.0, to use default values, is now supported.

This PR does not fix the issues with the Ros_Debug_BroadcastMsg function and doubles.

Testing: tolerance_issue_04162024.zip

1. Upload the mr2_yrc1_h.out to the controller and then restart the controller
2. Run the microros client in a terminal : sudo docker run -it --net=host -v /dev:/dev --privileged microros/micro-ros-agent:humble udp4 -p 8888
3. Start the TrajMode: ros2 service call /start_traj_mode motoros2_interfaces/srv/StartTrajMode
4. Run the fjt client: python3 fjt_client.py
5. Optionally the debug_listener can also be ran to capture messages.

[gavanderhoorn]

@yai-rosejo: I've used your PR as a basis and restructured it a little. See this for the diff. All WIP right now.

It's basically the same as your changes, but split into a couple of functions. My idea is we'd be able to write some tests for the new functionality this way.

I've tested it on my YRC with GP25 and it (still) works.

High-level question: did you intentionally iterate over the maxAxes? Would limiting it to just g_Ros_Controller.totalAxesCount (or feedback_FollowJointTrajectory.feedback.joint_names.size) not work as well?

[yai-rosejo]

@yai-rosejo: I've used your PR as a basis and restructured it a little. See this for the diff. All WIP right now.

It's basically the same as your changes, but split into a couple of functions. My idea is we'd be able to write some tests for the new functionality this way.

I've tested it on my YRC with GP25 and it (still) works.

High-level question: did you intentionally iterate over the maxAxes? Would limiting it to just g_Ros_Controller.totalAxesCount (or feedback_FollowJointTrajectory.feedback.joint_names.size) not work as well?

I just did a cursory look through your solution; I think it looks much better than my current one. I iterated over all of the maxAxes due to violators being the size of maxAxes and the 'for' loop checking the differences already using maxAxes. https://github.com/Yaskawa-Global/motoros2/blob/daa47bd7275dc7907f03ecae39ef159cbf87e032/src/ActionServer_FJT.c#L498-L504

[gavanderhoorn]

Ok. Thanks.

I'll add some tests, and then push some commits to this PR if that would be OK with you.

[yai-rosejo]

Ok. Thanks.

I'll add some tests, and then push some commits to this PR if that would be OK with you.

That works for me!

[gavanderhoorn]

Added some first batch of tests for Ros_ActionServer_FJT_Parse_GoalPosTolerances(..):

Click to expand

```text === Performing unit tests ~~~ Ros_ActionServer_FJT_Parse_GoalPosTolerances: one or more inputs NULL, abort Testing Ros_Testing_Ros_ActionServer_FJT_Parse_GoalPosTolerances_null_args: goal_joint_tolerances NULL: PASS Ros_ActionServer_FJT_Parse_GoalPosTolerances: one or more inputs NULL, abort Testing Ros_Testing_Ros_ActionServer_FJT_Parse_GoalPosTolerances_null_args: joint_names NULL: PASS Ros_ActionServer_FJT_Parse_GoalPosTolerances: one or more inputs NULL, abort Testing Ros_Testing_Ros_ActionServer_FJT_Parse_GoalPosTolerances_null_args: posTolerances NULL: PASS Testing Ros_Testing_Ros_ActionServer_FJT_Parse_GoalPosTolerances_null_args: PASS ~~~ Ros_ActionServer_FJT_Parse_GoalPosTolerances: no joint tolerances specified, returning 6 defaults Testing Ros_Testing_Ros_ActionServer_FJT_Parse_GoalPosTolerances_empty_jtolerances: call: PASS Testing Ros_Testing_Ros_ActionServer_FJT_Parse_GoalPosTolerances_empty_jtolerances: all defaults: PASS Testing Ros_Testing_Ros_ActionServer_FJT_Parse_GoalPosTolerances_empty_jtolerances: PASS ~~~ Ros_ActionServer_FJT_Parse_GoalPosTolerances: pos tolerance output array too small: 1 < 2 (nr of joints) Testing Ros_Testing_Ros_ActionServer_FJT_Parse_GoalPosTolerances_out_array_too_small: out array too small: PASS Testing Ros_Testing_Ros_ActionServer_FJT_Parse_GoalPosTolerances_out_array_too_small: PASS ~~~ Ros_ActionServer_FJT_Parse_GoalPosTolerances: found position tolerance: joint5, 1.000000 Testing Ros_Testing_Ros_ActionServer_FJT_Parse_GoalPosTolerances_more_jtol_than_jnames: call: PASS Testing Ros_Testing_Ros_ActionServer_FJT_Parse_GoalPosTolerances_more_jtol_than_jnames: all defaults: PASS Testing Ros_Testing_Ros_ActionServer_FJT_Parse_GoalPosTolerances_more_jtol_than_jnames: PASS ~~~ Ros_ActionServer_FJT_Parse_GoalPosTolerances: found position tolerance: joint0, 1.000000 Ros_ActionServer_FJT_Parse_GoalPosTolerances: using position tolerance: joint0 Testing Ros_Testing_Ros_ActionServer_FJT_Parse_GoalPosTolerances_jtol_in_slot0: call: PASS Testing Ros_Testing_Ros_ActionServer_FJT_Parse_GoalPosTolerances_jtol_in_slot0: expected j0 pos tol: PASS Testing Ros_Testing_Ros_ActionServer_FJT_Parse_GoalPosTolerances_jtol_in_slot0: PASS ~~~ Ros_ActionServer_FJT_Parse_GoalPosTolerances: found position tolerance: joint0, 1.000000 Ros_ActionServer_FJT_Parse_GoalPosTolerances: using position tolerance: joint0 Testing Ros_Testing_Ros_ActionServer_FJT_Parse_GoalPosTolerances_jtol_in_slot2: call: PASS Testing Ros_Testing_Ros_ActionServer_FJT_Parse_GoalPosTolerances_jtol_in_slot2: j0 pos tol at [2]: PASS Testing Ros_Testing_Ros_ActionServer_FJT_Parse_GoalPosTolerances_jtol_in_slot2: PASS ~~~ Ros_ActionServer_FJT_Parse_GoalPosTolerances: found position tolerance: joint1, 0.123000 Ros_ActionServer_FJT_Parse_GoalPosTolerances: using position tolerance: joint1 Ros_ActionServer_FJT_Parse_GoalPosTolerances: found position tolerance: joint2, 0.234000 Ros_ActionServer_FJT_Parse_GoalPosTolerances: using position tolerance: joint2 Ros_ActionServer_FJT_Parse_GoalPosTolerances: found position tolerance: joint1, 0.345000 Ros_ActionServer_FJT_Parse_GoalPosTolerances: using position tolerance: joint1 Testing Ros_Testing_Ros_ActionServer_FJT_Parse_GoalPosTolerances_last_setting_wins: call: PASS Testing Ros_Testing_Ros_ActionServer_FJT_Parse_GoalPosTolerances_last_setting_wins: j0 pos tol at [0]: PASS Testing Ros_Testing_Ros_ActionServer_FJT_Parse_GoalPosTolerances_last_setting_wins: j2 pos tol at [1]: PASS Testing Ros_Testing_Ros_ActionServer_FJT_Parse_GoalPosTolerances_last_setting_wins: j1 pos tol at [2]: PASS Testing Ros_Testing_Ros_ActionServer_FJT_Parse_GoalPosTolerances_last_setting_wins: PASS ~~~ Testing SUCCESSFUL === ```

[gavanderhoorn]

@yai-rosejo: I've added tests for the two new functions I introduced, refactored things a bit and cleaned up the Git history. See the diff.

Your changes are now all squashed into the initial commit.

Could you take a look whether this would work for you? If you agree, I'll force-push these changes to your branch to update this PR.

[gavanderhoorn]

@yai-rosejo wrote:

@gavanderhoorn wrote:

High-level question: did you intentionally iterate over the maxAxes? Would limiting it to just g_Ros_Controller.totalAxesCount (or feedback_FollowJointTrajectory.feedback.joint_names.size) not work as well?

[..] I iterated over all of the maxAxes due to violators being the size of maxAxes and the 'for' loop checking the differences already using maxAxes.

https://github.com/Yaskawa-Global/motoros2/blob/daa47bd7275dc7907f03ecae39ef159cbf87e032/src/ActionServer_FJT.c#L498-L504

@ted-miller: would it be absolutely necessary to iterate over maxAxes, or would limiting those arrays and loops to just the configured nr of axes also work? I assume the latter, but wanted to make sure.

[ted-miller]

would it be absolutely necessary to iterate over maxAxes, or would limiting those arrays and loops to just the configured nr of axes also work? I assume the latter, but wanted to make sure.

Limiting the loops would be OK. I can't think of any justification for keeps maxaxes.

[yai-rosejo]

@yai-rosejo: I've added tests for the two new functions I introduced, refactored things a bit and cleaned up the Git history. See the diff.

Your changes are now all squashed into the initial commit.

Could you take a look whether this would work for you? If you agree, I'll force-push these changes to your branch to update this PR.

The changes look good to me. You can go ahead with the force push.

[gavanderhoorn]

Working on this a bit more and I'm beginning to wonder whether we shouldn't already parse the JointTolerance instances when first accepting the goal. If parsing fails -- due to unknown joints being referenced for instance -- we could refuse to execute the goal and provide a very clear error code and message to clients.

If we fail to parse at the end of an executed trajectory, we can't really do much anymore: failing the goal completely seems like a bit of a 'waste' of a good trajectory execution and failing so late for something almost 'unrelated' doesn't apply fail-early and least-surprise principles.

[ted-miller]

So you're suggesting separating the logic of validating the tolerance input from validating the position is within that tolerance? That makes sense to me.

We've got the Ros_MotionControl_InitTrajectory function which returns a custom Init_Trajectory_Status that could clearly identify an issue with the specified input.

My next question is how much of the goal message do we validate? So far, we've been ignoring much of the goal message, such as path_tolerance and the component_x_tolerance. Do those remain unused?

[gavanderhoorn]

So you're suggesting separating the logic of validating the tolerance input from validating the position is within that tolerance? That makes sense to me.

Indeed.

We've got the Ros_MotionControl_InitTrajectory function which returns a custom Init_Trajectory_Status that could clearly identify an issue with the specified input.

True, although the goal_tolerance field is not part of the trajectory.

My next question is how much of the goal message do we validate? So far, we've been ignoring much of the goal message, such as path_tolerance and the component_x_tolerance. Do those remain unused?

I would for now indeed suggest to fix & refactor the current implementation which only takes the goal_tolerance field into account, not extend it.

[gavanderhoorn]

Coming back to this: unknown joints in goal_tolerance will not immediately cause problems, they will simply be ignored by the current implementation.

Adding goal validation -- which would include goal_tolerance validation -- would be nice, but wouldn't be needed to resolve the current issue (ie: #233).

I'll create an issue to log the enhancement request.

@yai-rosejo: I've pushed some more commits to my version of your branch. If you could PTAL and let me know whether you'd still be ok with me force-pushing here? Thanks.

[yai-rosejo]

Coming back to this: unknown joints in goal_tolerance will not immediately cause problems, they will simply be ignored by the current implementation.

Adding goal validation -- which would include goal_tolerance validation -- would be nice, but wouldn't be needed to resolve the current issue (ie: #233).

I'll create an issue to log the enhancement request.

@yai-rosejo: I've pushed some more commits to my version of your branch. If you could PTAL and let me know whether you'd still be ok with me force-pushing here? Thanks.

I have taken a look and agree with pushing those onto this branch. This PR is focused on supporting goal functionalities, so I also agree that the goal validation should be a separate task.

[gavanderhoorn]

@yai-rosejo and/or @ted-miller: could I perhaps ask you to test this implementation on your hw?

[ted-miller]

Definitely. I can't do it today (and probably not tomorrow), but I'll get it done. I realize this PR has lingered for quite a while now.

[ted-miller]

Functionally, everything seems to be working well on hardware.