AP_DDS: ensure zero rotation quaternions are normalised

[srmainwaring]

ROS expects quaternions to be normalised and the default message constructor does not enforce this.

Testing

Quick check is to run the copter launch example:

ros2 launch ardupilot_sitl sitl_dds_udp.launch.py transport:=udp4 refs:=$(ros2 pkg prefix ardupilot_sitl)/share/ardupilot_sitl/config/dds_xrce_profile.xml synthetic_clock:=True wipe:=False model:=quad speedup:=1 slave:=0 instance:=0 defaults:=$(ros2 pkg prefix ardupilot_sitl)/share/ardupilot_sitl/config/default_params/copter.parm,$(ros2 pkg prefix ardupilot_sitl)/share/ardupilot_sitl/config/default_params/dds_udp.parm sim_address:=127.0.0.1 master:=tcp:127.0.0.1:5760 sitl:=127.0.0.1:5501

Then run rviz with the ArduPilot TF topics remapped:

ros2 run rviz2 rviz2 --ros-args -r tf:=/ap/tf -r tf_static:=/ap/tf_static

Without this change there is a warning about Quaternion normalisation

$ ros2 run rviz2 rviz2 --ros-args -r tf:=/ap/tf -r tf_static:=/ap/tf_static
[INFO] [1716465954.524460000] [rviz2]: Stereo is NOT SUPPORTED
[INFO] [1716465954.524511000] [rviz2]: OpenGl version: 2.1 (GLSL 1.2)
[INFO] [1716465954.618444000] [rviz2]: Stereo is NOT SUPPORTED
[ERROR] [1716465956.356920000] []: TF_NAN_INPUT: Ignoring transform for child_frame_id "GPS_0" from authority "Authority undetectable" because of a nan value in the transform (0.000000 -0.000000 -0.000000) (nan nan nan nan)
[ERROR] [1716465956.357087000] []: TF_DENORMALIZED_QUATERNION: Ignoring transform for child_frame_id "GPS_0" from authority "Authority undetectable" because of an invalid quaternion in the transform (nan nan nan nan)
[ERROR] [1716465959.054706000] []: TF_NAN_INPUT: Ignoring transform for child_frame_id "GPS_0" from authority "Authority undetectable" because of a nan value in the transform (0.000000 -0.000000 -0.000000) (nan nan nan nan)
[ERROR] [1716465959.054789000] []: TF_DENORMALIZED_QUATERNION: Ignoring transform for child_frame_id "GPS_0" from authority "Authority undetectable" because of an invalid quaternion in the transform (nan nan nan nan)

and there is no tf data for the Fixed Frame.

With the change the tf data is good, and the TF tree can be viewed.

[Ryanf55]

Thanks!!!

[peterbarker]

Is this correct?!

If this is a quaternion, should you just be calling .initialise() on it?

[Ryanf55]

This quaternion is of type geometry_msgs::msg::rotation. It is not an AP quaternion.

ryan@B650-970:~/Dev/ardu_ws/src/ardupilot$ ros2 interface show tf2_msgs/msg/TFMessage 
geometry_msgs/TransformStamped[] transforms
        #
        #
        std_msgs/Header header
                builtin_interfaces/Time stamp
                        int32 sec
                        uint32 nanosec
                string frame_id
        string child_frame_id
        Transform transform
                Vector3 translation
                        float64 x
                        float64 y
                        float64 z
                Quaternion rotation
                        float64 x 0
                        float64 y 0
                        float64 z 0
                        float64 w 1

In build/sitl/libraries/AP_DDS/generated/geographic_msgs/msg/Quaternion.h, it generates the following code:

  typedef struct geometry_msgs_msg_Quaternion
{
    double x;

    double y;

    double z;

    double w;

} geometry_msgs_msg_Quaternion;

So no, the default generated messages do not use normalized quaternions. Note - this is only a problem with MicroXRCEDDSGen not supporting default fields, not just quaternions. Same problem with all the messages that should be defaulting certain fields to NaN's.

In the normally used ROSIDL tooling that all the C++ applications on the companion computer side, it generates the following struct in /opt/ros/humble/include/geometry_msgs/geometry_msgs/msg/detail/quaternion__struct.hpp

namespace geometry_msgs
{

namespace msg
{

// message struct
template<class ContainerAllocator>
struct Quaternion_
{
  using Type = Quaternion_<ContainerAllocator>;

  explicit Quaternion_(rosidl_runtime_cpp::MessageInitialization _init = rosidl_runtime_cpp::MessageInitialization::ALL)
  {
    if (rosidl_runtime_cpp::MessageInitialization::ALL == _init ||
      rosidl_runtime_cpp::MessageInitialization::DEFAULTS_ONLY == _init)
    {
      this->x = 0.0;
      this->y = 0.0;
      this->z = 0.0;
      this->w = 1.0;
    } else if (rosidl_runtime_cpp::MessageInitialization::ZERO == _init) {
      this->x = 0.0;
      this->y = 0.0;
      this->z = 0.0;
      this->w = 0.0;
    }
  }

I've asked in the upstream generator. I agree Peter, regardless of which Quaternion type we use, whether it's an AP quaternion or another tool, the default constructor should be normalized.

eProsima/Micro-XRCE-DDS-Gen#76 (comment)

For now, I believe the changes that Rhys proposed are correct, hence the approval, but in an ideal world, these would not be necessary. Once upstream can fix the issue (or I have some spare time), we can bump the generator version and remove this code.

[srmainwaring]

While we are waiting for upstream fixes, we could reduce duplication by consolidating initialisation functions in a AP_DDS_Utils class (as a follow up PR?). This won't be the last time we need to initialise DDS quaternion msgs, and I'm sure we'll come across other msg types that need specific initialisation as well.

[srmainwaring]

Is this correct?!

Fixed in fa537a1. Good catch thanks @peterbarker.

[peterbarker]

normalized/initialised can be fixed by a good function name....

[rmackay9]

I tried to include this in 4.5.6-beta1 but was unable to due to a merge conflict. If someone is keen on including this in 4.5 then a backport PR would be greatly appreciated!