AP_DDS: Local Pose Publisher

[pedro-fuoco]

Resolves #23278, here is the PR working on SITL:

The NED to ENU conversion seemed fine when I analysed the position coordinates of a plane flying in SITL. As you can see in the screenshot the plane went North-West after takeoff which resulted in a negative X axis (points towards east in ENU) and a positive Y axis (points towards north in ENU). Z is positive after takeoff.

[pedro-fuoco]

@Ryanf55 Figured out how to validate orientations on foxglove, they are wrong apparently lol. Will fix them soon to follow the

// In ROS REP 103, it follows this convention
        // X - Forward
        // Y - Left
        // Z - Up
        // https://www.ros.org/reps/rep-0103.html#axis-orientation

[Ryanf55]

@Ryanf55 Figured out how to validate orientations on foxglove, they are wrong apparently lol. Will fix them soon to follow the

// In ROS REP 103, it follows this convention
        // X - Forward
        // Y - Left
        // Z - Up
        // https://www.ros.org/reps/rep-0103.html#axis-orientation

In foxglove 3D, FYI I'm about 75% sure that X corresponds to red, so if the vehicle is traveling straight, red is pointing into the wind. Then green for left, and blue for up (towards sky).

XYZ = RGB

It's hard to tell from the gif, but I'll verify this on my side when you think it's good.

Glad we checked.

[pedro-fuoco]

I researched quite a bit to see if I got the orientation right.
This is what I was looking for.

We are not using a body-fixed frame on the local pose topic.
X-forward refers to a body-fixed frames, so there is no reason for X to be pointing forward in this case.
Notice how we are still following ros rep 103:

For short-range Cartesian representations of geographic locations, use the [east north up](http://en.wikipedia.org/wiki/Geodetic_datum#Local_east.2C_north.2C_up_.28ENU.29_coordinates) [[5]](https://www.ros.org/reps/rep-0103.html#id9) (ENU) convention:
X east
Y north
Z up

There should be a dicussion about using a body-fixed frame in the local velocity topic though

[Ryanf55]

Just tested in ROS 2 humble NAV2 bringup. The frame shown in RVIZ below is the base_link frame. wafflle is facing "north".

If waffle turns 45 degrees to the left, now facing northwest, it looks like this. I also zoomed out, so you can see map and odom, which are on top of each other.

base_link is in body frame, since it's attached to the robot body, so let's abide by the convention you posted above from TSC21's original clarifications, which also conveniently match the behavior in NAV2, which we hope to support easy integration with.

Expectation (In SITL+Foxglove):
If the drone is flying north, the red arrow (X) should point north, the green arrow (Y) points west, and the blue arrow (Z) points towards the sky.
If the drone is flying northwest, the red arrow (X) should point northwest, the green arrow (Y) points southwest, and the blue arrow (Z) remains towards the sky.

Hope that clears it up.

[pedro-fuoco]

@Ryanf55 Thanks a lot for the clarifications!
Just fixed it up to follow x-forward, y-left, z-up orientation! Here are my recent tests in foxglove:

Also updated the comments so that the transformations are clear. As soon as this gets reviewed I will change #23540 to follow this transformation as well