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

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.

[Ryanf55]

How did you pick this value?

[tizianofiorenzani]

all goals are usually sent at 1e-7 resolution, so this is one order of magnitude better that AP can resolve.

[Ryanf55]

That's resolution on the lat-lon value, but this is in meters, right?

[tizianofiorenzani]

no, it's degrees. Lat Lon are sent in any mavlink message to 1e-7 resolution. I don't think it's necessary to convert to meters here.

[Ryanf55]

Can you rename the variable then? Distance is measured in meters, or add some more comment.

Perhaps goal_tol_lat_lon

[tizianofiorenzani]

renamed to tolerance_lat_lon, good call.