AP_DDS: Rally Get and Set #28449

tizianofiorenzani · 2024-10-22T22:15:56Z

Issue

What Changed

Service /ap/rally_set appends a Rally Point to the list or clears the list (using the clear flag).
- point: geolocation
- altitude_frame mandatory, follows enum of AltFrame.
Replies with:
success is the action is successful;
size is the list's size.
Service /ap/rally_get gets the Rally Point at a given index. Returns:
- success
- rally the rally point as ardupilot_msgs/Rally type.
New ardupilot_msgs/Rally type is defined, that copies the structure definition in AP_Rally.

Test

Auto Test

colcon test --packages-select ardupilot_dds_tests --event-handlers=console_cohesion+ --pytest-args -k test_rally_point

Manual Test

run the SITL
Call the service to clear the rally list ros2 service call /ap/rally_set ardupilot_msgs/srv/RallySet "{clear: true}"
Open up Mavproxy with map and make sure the rally list is clear.
Append a new Rally Point: ros2 service call /ap/rally_set ardupilot_msgs/srv/RallySet "{rally: {point: {latitude: -35.36, longitude: 149.17, altitude: 400}}}"
In mavproxy, update the rally list and make sure the rally point appears on the map

use the service call to get the Rally at index 0: ros2 service call /ap/rally_get ardupilot_msgs/srv/RallyGet "index: 0"

Ryanf55

Nice addition, great to see some more interfaces like this.

Tools/ros2/ardupilot_msgs/msg/Rally.msg

Tools/ros2/ardupilot_msgs/srv/RallySet.srv

libraries/AP_DDS/AP_DDS_Client.cpp

timtuxworth · 2024-10-22T22:51:28Z

The altitude needs a frame.

tizianofiorenzani · 2024-10-22T23:51:01Z

@timtuxworth I have set the altitude frame explicitly now.
I don't have a clear answer for what to do with the break_altitude: RallyLocation expects that in Above Home level.

tizianofiorenzani · 2024-11-05T00:30:28Z

As there is no code associated with the Rally flags, the DDS interface has been limited to only the Geopoint and the altitude frame

Tools/ros2/ardupilot_msgs/msg/Rally.msg

Tools/ros2/ardupilot_dds_tests/test/ardupilot_dds_tests/test_rally_point.py

libraries/AP_DDS/AP_DDS_Client.cpp

tizianofiorenzani · 2024-11-13T17:03:16Z

Rebased and pushed again.

libraries/AP_DDS/AP_DDS_Service_Table.h

libraries/AP_DDS/AP_DDS_Client.cpp

Ryanf55

Functionally, looks good. Just needs code cleanup and removal of potential link-flooding.

Tools/ros2/ardupilot_dds_tests/test/ardupilot_dds_tests/test_rally_point.py

Tools/ros2/ardupilot_msgs/CMakeLists.txt

Tools/ros2/ardupilot_msgs/msg/Rally.msg

Tools/ros2/ardupilot_msgs/srv/RallyGet.srv

libraries/AP_DDS/AP_DDS_Client.cpp

tizianofiorenzani · 2024-11-21T16:10:36Z

@Ryanf55 suggestions applied, rebased and tested.

timtuxworth · 2024-11-23T12:46:28Z

libraries/AP_DDS/AP_DDS_Client.cpp

+        } else {
+            rally_location.lat = static_cast<int32_t>(rally_set_request.rally.point.latitude * 1e7);
+            rally_location.lng = static_cast<int32_t>(rally_set_request.rally.point.longitude * 1e7);
+            rally_location.alt = static_cast<int16_t>(rally_set_request.rally.point.altitude);


We are implicitly losing precision and accuracy here. What should we do if the altitude is > 16 bits? Just silently truncate the value?

Location is set to be integer, I need to pass values like this, the same as you do if it was the GCS to provide it.

timtuxworth · 2024-11-23T12:49:42Z

libraries/AP_DDS/AP_DDS_Client.cpp

+            rally_location.flags |= (rally_set_request.rally.altitude_frame << 3);
+
+            if (rally_location.lat != 0 && rally_location.lng != 0
+                && rally_set_request.rally.altitude_frame <= static_cast<uint8_t>(Location::AltFrame::ABOVE_TERRAIN)) {


I don't understand this test for altitude_frame being valid. Could you code it in a way that's clearer to understand what you are actually doing?

That means the altitude enum is valid. I can re write it to check for each individual value, but I don't think it makes it more readable.

@timtuxworth check it out now: I have explicitly compared it against every value in the enum.

timtuxworth · 2024-11-23T12:55:11Z

libraries/AP_DDS/README.md

+Append a Rally Point:
+
+```bash
+ros2 service call /ap/rally_set ardupilot_msgs/srv/RallySet "{rally: {point: {latitude: -35, longitude: 151, altitude: 400}}}"


It would be good to have an example with a frame supplied.

timtuxworth · 2024-11-23T12:57:01Z

libraries/AP_DDS/README.md

+
+```bash
+ros2 service call /ap/rally_get ardupilot_msgs/srv/RallyGet "index: 0"
+requester: making request: ardupilot_msgs.srv.RallyGet_Request(index=0)


Maybe it's something I need to understand about ROS, but how would you know if there is more than one rally point, there doesn't seem to be a request rally count message, do you just keep requesting indexes until you get a fail?

When you set a rally, it responds with the length of the Rally list. When you request a particular index, it fails if it does not exist. You can also use the service set call with an invalid point to obtain the length of the Rally list without adding any.

An AP might have previously been configured with zero, one or more rally points. After booting, the only way to find out if there are any rally points would be to start requesting them at the first and then incrementally keep requesting until you get a fail, or to request some really large index number (what the largest valid number +1?) to get the number.

I'm not sure I understand how that's ok.

I agree that we should ensure this API works if there are pre-existing rally points. A sequence diagram could be helpful

timtuxworth · 2024-11-23T12:58:27Z

libraries/AP_DDS/README.md

+ardupilot_msgs.srv.RallySet_Response(success=True, size=0)
+```
+
+Append a Rally Point:


It would probably be good to show fractional lat/long and be clear that the altitude is meters.

Do we have to show units of measure when in SI?

I think so because unfortunately some ArduPilot metrics are not SI (it's a work in progress).

no problem, I can update the readme.

I think this README needs an update now that you removed some of the flags and optional features.

Regarding units, the DDS interface follows ROS REP-103. Units are SI unless otherwise stated.

@Ryanf55 the Readme has been updated.

timtuxworth · 2024-11-23T13:01:33Z

libraries/AP_DDS/Idl/ardupilot_msgs/msg/Rally.idl

+      geographic_msgs::msg::GeoPoint point;
+
+      uint8 altitude_frame;
+    };


Where are break_altitude, land_dir, flag_favorable_winds, flag_do_auto_land?

They have been removed, as not used. It was agreed in the dev call.

timtuxworth · 2024-11-23T13:03:19Z

libraries/AP_DDS/AP_DDS_Client.cpp

+                rally_get_response.rally.altitude_frame = (rally_location.flags >> 3) & ((1 << 2) - 1);
+            } else {
+                rally_get_response.rally.altitude_frame = static_cast<uint8_t>(Location::AltFrame::ABOVE_HOME);
+            }


Where are break_altitude, land_dir, flag_favorable_winds, flag_do_auto_land?

They are not used anywhere in the code and have been removed. We discussed about that in the dev call.

timtuxworth · 2024-11-23T13:03:36Z

libraries/AP_DDS/AP_DDS_Client.cpp

+                } else {
+                    rally_set_response.success = false;
+                }
+            } else {


Where are break_altitude, land_dir, flag_favorable_winds, flag_do_auto_land?

They are no where to be used in the code and have been removed.

tizianofiorenzani · 2024-12-03T21:41:23Z

Rebased, ready for another round.

Ryanf55 requested changes Oct 22, 2024

View reviewed changes

Ryanf55 added the ROS label Oct 22, 2024

tizianofiorenzani requested a review from Ryanf55 October 22, 2024 23:52

tizianofiorenzani force-pushed the wips/rally branch from 8fb305b to 7b5774b Compare October 29, 2024 15:03

Ryanf55 added the DevCallTopic label Oct 31, 2024

CraigElder removed the DevCallTopic label Nov 5, 2024

tizianofiorenzani force-pushed the wips/rally branch from 0c549f9 to e735057 Compare November 5, 2024 00:37

Ryanf55 requested changes Nov 11, 2024

View reviewed changes

Tools/ros2/ardupilot_msgs/msg/Rally.msg Outdated Show resolved Hide resolved

Tools/ros2/ardupilot_dds_tests/test/ardupilot_dds_tests/test_rally_point.py Outdated Show resolved Hide resolved

Ryanf55 reviewed Nov 11, 2024

View reviewed changes

libraries/AP_DDS/AP_DDS_Client.cpp Show resolved Hide resolved

tizianofiorenzani force-pushed the wips/rally branch 2 times, most recently from 420c0d1 to 6dca61b Compare November 13, 2024 17:03

Ryanf55 added the Enhancement label Nov 13, 2024

timtuxworth reviewed Nov 14, 2024

View reviewed changes

libraries/AP_DDS/AP_DDS_Service_Table.h Show resolved Hide resolved

timtuxworth reviewed Nov 14, 2024

View reviewed changes

libraries/AP_DDS/AP_DDS_Client.cpp Outdated Show resolved Hide resolved

tizianofiorenzani force-pushed the wips/rally branch from 6dca61b to 9fdff44 Compare November 14, 2024 16:35

tizianofiorenzani requested a review from Ryanf55 November 14, 2024 16:49

tizianofiorenzani force-pushed the wips/rally branch from df289c2 to 76e08d0 Compare November 18, 2024 16:29

tizianofiorenzani requested a review from timtuxworth November 20, 2024 16:24

Ryanf55 requested changes Nov 21, 2024

View reviewed changes

tizianofiorenzani force-pushed the wips/rally branch from 7bcf60a to bee945e Compare November 21, 2024 16:10

tizianofiorenzani force-pushed the wips/rally branch 4 times, most recently from 08d2a04 to 3d943b9 Compare November 22, 2024 18:31

timtuxworth reviewed Nov 23, 2024

View reviewed changes

tizianofiorenzani requested a review from timtuxworth December 2, 2024 16:07

tizianofiorenzani force-pushed the wips/rally branch from ba5d645 to 0b91c49 Compare December 3, 2024 21:40

AP_DDS: Rally Interface to get-set rally points.

dd3576f

tizianofiorenzani force-pushed the wips/rally branch from 0b91c49 to dd3576f Compare December 3, 2024 21:48

tizianofiorenzani requested a review from Ryanf55 December 3, 2024 21:48

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

AP_DDS: Rally Get and Set #28449

AP_DDS: Rally Get and Set #28449

tizianofiorenzani commented Oct 22, 2024 •

edited

Loading

Ryanf55 left a comment

timtuxworth commented Oct 22, 2024

tizianofiorenzani commented Oct 22, 2024 •

edited

Loading

tizianofiorenzani commented Nov 5, 2024

tizianofiorenzani commented Nov 13, 2024

Ryanf55 left a comment

tizianofiorenzani commented Nov 21, 2024

timtuxworth Nov 23, 2024

tizianofiorenzani Dec 2, 2024

timtuxworth Nov 23, 2024

tizianofiorenzani Dec 2, 2024

tizianofiorenzani Dec 3, 2024

timtuxworth Nov 23, 2024

timtuxworth Nov 23, 2024

tizianofiorenzani Dec 2, 2024

timtuxworth Dec 2, 2024 •

edited

Loading

Ryanf55 Dec 2, 2024

timtuxworth Nov 23, 2024

tizianofiorenzani Dec 2, 2024

timtuxworth Dec 2, 2024

tizianofiorenzani Dec 2, 2024

Ryanf55 Dec 2, 2024

tizianofiorenzani Dec 3, 2024

timtuxworth Nov 23, 2024

tizianofiorenzani Dec 2, 2024

timtuxworth Nov 23, 2024

tizianofiorenzani Dec 2, 2024

timtuxworth Nov 23, 2024

tizianofiorenzani Dec 2, 2024

tizianofiorenzani commented Dec 3, 2024

AP_DDS: Rally Get and Set #28449

Are you sure you want to change the base?

AP_DDS: Rally Get and Set #28449

Conversation

tizianofiorenzani commented Oct 22, 2024 • edited Loading

Issue

What Changed

Test

Auto Test

Manual Test

Ryanf55 left a comment

Choose a reason for hiding this comment

timtuxworth commented Oct 22, 2024

tizianofiorenzani commented Oct 22, 2024 • edited Loading

tizianofiorenzani commented Nov 5, 2024

tizianofiorenzani commented Nov 13, 2024

Ryanf55 left a comment

Choose a reason for hiding this comment

tizianofiorenzani commented Nov 21, 2024

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

timtuxworth Dec 2, 2024 • edited Loading

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

tizianofiorenzani commented Dec 3, 2024

tizianofiorenzani commented Oct 22, 2024 •

edited

Loading

tizianofiorenzani commented Oct 22, 2024 •

edited

Loading

timtuxworth Dec 2, 2024 •

edited

Loading