AP_DDS: Goal topic publisher

[tizianofiorenzani]

Issue

#28299

What Changed

• New topic /ap/goal_lla to provide current goal as GeopointStamped.
• The topic is published only when changed, reliability set to UXR_RELIABILITY_RELIABLE

Semi automatic test

Run the plane SITL

ros2 launch ardupilot_sitl sitl_dds_udp.launch.py model:=plane command:=arduplane defaults:=$(ros2 pkg prefix ardupilot_sitl)/share/ardupilot_sitl/config/models/plane.parm

and then run the test as:

ros2 run ardupilot_dds_tests plane_waypoint_follower

SITL

Tested in SITL: sending copter to a waypoint:

ros2 topic echo /ap/goal_lla

[tizianofiorenzani]

@Ryanf55 I set the topic to be published only when changed.

[tizianofiorenzani]

After applying suggestions, rally point is in Absolute altitude.

[Ryanf55]

Looking great, just need to move the rate to be user configurable like the rest and then I'll approve.

[peterbarker]

Needs to pass CI; formatting problems.

[tizianofiorenzani]

@peterbarker fixed, rebased and pushed

[Ryanf55]

Looks good. This doesn't have any of the GCS logging or sprintf problems of other PR's, so it should be safe to go in.