Check for empty quaternion message when processing CollisionObjects

[felixvd]

Description

To avoid issues like the one @tylerjw encountered in #2031, I think we should check for empty quaternions when adding objects to the scene. This is a lightweight operation compared to the normalize that we already apply to every message, so it seems reasonable to me. Please share your opinion.

Fundamentally this is only an issue because ROS quaternion messages default to an invalid value, but that's neither here nor there.

I'll run clang-format etc. when I am at my home machine.

Checklist

• Required by CI: Code is auto formatted using clang-format
• While waiting for someone to review your request, please help review another open pull request to support the maintainers

[codecov-io]

Codecov Report

Merging #2089 into master will increase coverage by 0.45%.
The diff coverage is 100.00%.

@@            Coverage Diff             @@
##           master    #2089      +/-   ##
==========================================
+ Coverage   54.08%   54.53%   +0.45%     
==========================================
  Files         319      328       +9     
  Lines       24997    25661     +664     
==========================================
+ Hits        13519    13995     +476     
- Misses      11478    11666     +188     

Impacted Files	Coverage Δ
moveit_core/planning_scene/src/planning_scene.cpp	57.81% <100.00%> (+0.12%)	⬆️
...rimental/moveit_jog_arm/src/jog_interface_base.cpp	73.01% <0.00%> (-5.62%)	⬇️
...veit_experimental/moveit_jog_arm/src/jog_calcs.cpp	73.63% <0.00%> (-1.37%)	⬇️
moveit_core/robot_state/src/robot_state.cpp	44.40% <0.00%> (-1.22%)	⬇️
...t_setup_assistant/src/tools/moveit_config_data.cpp	20.27% <0.00%> (-0.64%)	⬇️
...cessing/src/time_optimal_trajectory_generation.cpp	75.83% <0.00%> (-0.37%)	⬇️
.../move_group_interface/src/move_group_interface.cpp	40.59% <0.00%> (-0.18%)	⬇️
...bot_state/include/moveit/robot_state/robot_state.h	91.02% <0.00%> (ø)
...veit_jog_arm/include/moveit_jog_arm/jog_arm_data.h	100.00% <0.00%> (ø)
...og_arm/include/moveit_jog_arm/jog_interface_base.h	100.00% <0.00%> (ø)
... and 21 more

---

Continue to review full report at Codecov.

Legend - Click here to learn more
Δ = absolute <relative> (impact), ø = not affected, ? = missing data
Powered by Codecov. Last update 173d6a6...676548e. Read the comment docs.

[v4hn]

Thank you for taking the initiative! I believe we had partial patches for this around already, but I can't find the corresponding request or issue right now...

I am not happy with your method to resolve this though.
In my opinion, the ApplyPlanningScene top-level ROS service and the respective PlanningScene methods should not succeed when such an object is received.

[rhaschke]

It is kind of common practice in ROS to consider uninitialized quaternions as identity, e.g. in rviz:
https://github.com/ros-visualization/rviz/blob/171eb3cfd1ec69cb6ff72da3ca41a1a4caa74eef/src/rviz/validate_quaternions.h#L50-L56

https://github.com/ros-visualization/rviz/blob/171eb3cfd1ec69cb6ff72da3ca41a1a4caa74eef/src/rviz/validate_quaternions.h#L66-L72

[v4hn]

It is kind of common practice in ROS to consider uninitialized quaternions as identity

I am aware of that but not happy with it at all 😞
You are right though, we should probably keep users happyhappier and use the warning and implicit identity.

[tylerjw]

It is kind of common practice in ROS to consider uninitialized quaternions as identity

I am aware of that but not happy with it at all
You are right though, we should probably keep users happyhappier and use the warning and implicit identity.

This does add extra code in the case where the user did the right thing and didn't give us something invalid. My pain (and other beginner users) over this comes from two places. The first is nearly useless errors in FCL when used on Kinetic and even worse, it silently somehow doesn't complain when using Melodic. The second is that I got here by copy-pasting from our tutorials (I will offer a PR to fix that shortly).

Ideally, we'd fix this upstream with default construction of values in ROS messages, which is something we have with ROS2 so hopefully as people migrate it won't matter and this is just one more case of us having to implement something downstream that should have been done upstream at compile time in the messages.

As to what the default construction value should be. I don't know enough about quaternions and if {0,0,0,0} would ever actually be valid and therefore we shouldn't stop that from being tested. If we want to be even safer we should probably test for anything that'd cause a math exception on division (nan, inf) too, but I don't think we should try to save users who give us quaternions with values set to nan.

Do you propose we set the default quaternion to something other than identity? Maybe {1, 0, 0, 0} is more reasonable, or {1, 1, 1, 1} since we normalize it after all?

We could require users to give us normalized quaternions and call abort if they didn't, that would be more performant and would fail sooner which would make it much easier to debug when the user does something invalid. That might make some users more grumpy because we stopped holding their hands by calling normalize ourselves.

In the end, thank you for merging this. I hope it saves someone else in the future from copy-pasting from our own tutorials and being greeted by an assert at runtime. 🥇

[rhaschke]

@tylerjw, ROS1 message system will not change anymore. So we need to work with such fixes.
Just for your info: The identity quaternion is (x=y=z=0, w=1).

[gavanderhoorn]

There is actually some support for adding constructors and other functionality to messages.

See Customizing generated message headers for C++.

[rhaschke]

Interesting to know, but this approach doesn't generalize across all supported languages:

IMPORTANT: be careful, as doing so might break the expected message API and/or compatibility with other languages.

[gavanderhoorn]

Adding some convenience constructors to message header does not break compatibility.

Other languages will simply not get those conveniences.

[rhaschke]

You are right. What I meant is that users will have a different experience across languages and that's probably nothing we should support.