AP_DDS: Goal topic publisher

[tizianofiorenzani]

Issue

https://github.com/ArduPilot/ardupilot/issues/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

Test

Automatic test

test_goal_lla_msg_received.py is a basic script that verify whether the topic has been published at least once.

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.

[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.