Fix Servo JointJog Crash

[mjforan]

Description

Currently the realtime servo node crashes if a JointJog message is received without velocity commands. I added a check for this condition and a warning if the message contains displacement commands.

Checklist

• Required by CI: Code is auto formatted using clang-format
• Create tests, which fail without this PR

[sea-bass]

Thanks for finding this!

I think by putting all these checks at the ROS node level, it may be making servo even harder to test than it already is.

If I may offer a suggestion, a few lines down where you modified things there is a line that actually creates a JointJogCommand struct:

JointJogCommand command{ latest_joint_jog_.joint_names, latest_joint_jog_.velocities };

As you have correctly noticed, this is just a plain struct and has no validation on the joint name vs. velocity inputs. I think what would make this stuff much more sustainable to test would be to create either free functions that convert directly from the ROS JointJog message to the servo specific JointJogCommand type, or otherwise make non-default constructors for these structs.

If we go the free functions route, these can have their own validation / failure handling with things like std::optional or tl::expected.

If you'd be up for giving this a try, it would definitely help to take some of the logic in the servo_node.cpp and encapsulate it a little better.

But I'll defer to you on how far you want to go here vs. just fix a bug and move on :)

[codecov-commenter]

⚠️ Please install the to ensure uploads and comments are reliably processed by Codecov.

Codecov Report

Attention: Patch coverage is 70.51282% with 23 lines in your changes missing coverage. Please review.

Project coverage is 45.91%. Comparing base (50b4337) to head (211cbe9).
Report is 5 commits behind head on main.

Files with missing lines	Patch %	Lines
moveit_ros/moveit_servo/src/utils/command.cpp	68.97%	18 Missing ⚠️
moveit_ros/moveit_servo/src/servo_node.cpp	0.00%	5 Missing ⚠️

❗ Your organization needs to install the Codecov GitHub app to enable full functionality.

Additional details and impacted files

@@            Coverage Diff             @@
##             main    #3351      +/-   ##
==========================================
+ Coverage   45.85%   45.91%   +0.06%     
==========================================
  Files         717      717              
  Lines       62572    62579       +7     
  Branches     7564     7566       +2     
==========================================
+ Hits        28689    28724      +35     
+ Misses      33717    33688      -29     
- Partials      166      167       +1     

☔ View full report in Codecov by Sentry.
📢 Have feedback on the report? Share it here.

🚀 New features to boost your workflow:

• ❄ Test Analytics: Detect flaky tests, report on failures, and find test suite problems.

[mjforan]

@sea-bass what do you think of this approach? Instead of adding extra functions on the struct initialization, I moved the check to the section where velocities are actually processed in command.cpp. This also protects against the case where a C++ user creates a JointJogCommand and alters the vector size afterwards.

[sea-bass]

This definitely is better! Still 2 comments though

1. I think the amount of conditionals/nesting can still be simplified significantly
2. I'm a bit worried about relying on a subscription to the /rosout topic for tests to pass, and may lead to (even more) flaky tests. Have you tried to use the stress command to stress your system and run this new test repeatedly?

[mjforan]

I'm a bit worried about relying on a subscription to the /rosout topic for tests to pass, and may lead to (even more) flaky tests. Have you tried to use the stress command to stress your system and run this new test repeatedly?

I tried running with stress multiple times and saw no failures. The tests were already relying on a ROS topic (publishing commands) so I don't think my change significantly increases the flakiness.

[sea-bass]

Looks good -- thanks! Just one last comment from me.

Now that I see this all again, would it make sense to put the warning about displacements not being supported in jointDeltaFromJointJog() instead of in the ROS node itself?

[mjforan]

The JointJogCommand struct does not have a displacements field so command.cpp is unaware of such input. If you think displacement functionality will be restored, we can add the struct field and move the warning.

[mjforan]

You were right, CI is failing on the /rosout log check. Maybe I should add a longer delay before that check? Or is there something else unusual about the environment? So far I have only tested on Raspberry Pi 5 in Jazzy Docker.

[sea-bass]

You were right, CI is failing on the /rosout log check. Maybe I should add a longer delay before that check? Or is there something else unusual about the environment? So far I have only tested on Raspberry Pi 5 in Jazzy Docker.

Hah, never underestimate how crappy CI runners can be. Honestly, if it were up to me I'd just verify the value of the status coming out of the function and not worry about the logs.

Also, understood re: the displacement warning. 👍🏻

[mjforan]

@sea-bass ready to merge?

[sea-bass]

@sea-bass ready to merge?

Sorry, I thought I had asked other active maintainers for a 2nd approval. Tagged a few.