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]

Codecov Report

Attention: Patch coverage is 75.60976% with 20 lines in your changes missing coverage. Please review.

Project coverage is 46.27%. Comparing base (5946a4e) to head (3d23efb).
Report is 1 commits behind head on main.

Files with missing lines	Patch %	Lines
moveit_ros/moveit_servo/src/utils/command.cpp	72.42%	16 Missing ⚠️
moveit_ros/moveit_servo/src/servo_node.cpp	55.56%	4 Missing ⚠️

Additional details and impacted files

@@            Coverage Diff             @@
##             main    #3351      +/-   ##
==========================================
+ Coverage   46.25%   46.27%   +0.03%     
==========================================
  Files         717      717              
  Lines       62601    62614      +13     
  Branches     7571     7571              
==========================================
+ Hits        28948    28967      +19     
+ Misses      33485    33480       -5     
+ Partials      168      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.

[sea-bass]

Came here to stamp the PR since nobody else had looked at it and found a few lingering minor things.

But after that, I think I'll just merge this. Thanks @mjforan !

[sea-bass]

Thank you!

[mjforan]

Thanks. Frustrating that the CI has different formatting rules from the colcon tests.