DDS - REP-147 Goal Interface Support for Plane (position only)

[Ryanf55]

Purpose

This adds DDS support for controlling global position control in LLH of Plane in alignment with REP-147's proposed High Level Goal Interface. It relies on GUIDED mode in plane. When supplying position, it's intended to be used for in features like "Click to go here" from the GCS. This is NOT a good interface for low-level carrot-on-stick kind of control due to the loitering behavior around the target point.

Assumptions

• map frame in ROS would be used for this type of control

Limitations

• Only plane implements the interface, other vehicles will ignore the command.
• Only position control is supported. The ROS interface supports velocity and accel, but it's ignored for now.

Demo

In ArduPilot repo:

./Tools/autotest/sim_vehicle.py -w -v ArduPlane --console  --enable-dds --map -DG

Run MicroROS Agent

cd ~/ros2_ws
. /opt/ros/humble/setup.bash
colcon build --packages-up-to ardupilot_sitl
. install/setup.bash
cd src/ardupilot/libraries/AP_DDS
ros2 run micro_ros_agent micro_ros_agent udp4 -p 2019 -r dds_xrce_profile.xml

Now, get the plane flying. Once EKF3 is initialized

arm throttle
mode takeoff

Wait a bit for it to start loitering, then switch to guided.

mode guided

Now, command the drone to head north from CMAC to the grayhound track at -35.345996, 149.159017 with an altitude of 40m above home. You only need to send it once with --once because this goal is latching on plane.

cd ~/ros2_ws
. /opt/ros/humble/setup.bash
colcon build --packages-up-to ardupilot_msgs
. install/setup.bash
ros2 topic pub /ap/cmd_gps_pose  ardupilot_msgs/msg/GlobalPosition "{header: {frame_id: map}, latitude: -35.345996, longitude: 149.159017, altitude: 40, coordinate_frame: 6}" --once

You'll see a pink and green line appear pointing to the middle of the greyhound track as soon as the first message is published.

After a while, it will do a clockwise loiter at the target location.

If you kill the terminal for publishing, the plane will continue to loiter about the same spot and stay in guided mode.

What about through a ROS node?

Good thing you asked, you can run an example waypoint follower here, which arms the plane, does the takeoff, switches to guided, commands a location, and closes when the goal is reached.

ros2 run ardupilot_dds_tests plane_waypoint_follower 

Future work

• Add a click-to-go feature to Foxglove's map panel.
• CI through ROS
• Handle stale data through timestamps

Considerations

This message is honestly poorly designed; I think this needs to have experimental topic prefix untill we can work with the ROS TSC and Aerial Working Group on a new one.

• coordinate_frame has a default value of 0 not represented in the frames of 5,6,11
• type_make is pointless, when you could just set fields to NaN.
• Even worse, you can't disable velocity.angular or acceleration_or_force.angular with type_mask
• If you are sending both target position, velocity and acceleration, but it's a plane with limited control DOF that may conflict, this is ambiguous.
• There is no documented frames for velocity and acceleration - is it body, NED, ENU, etc?
• And yea, the usual, no altitude datum concept. Absolute can't encode a datum.

[srmainwaring]

Nice to see this @Ryanf55. I've been testing the terrain planner using an extension to the SET_POSITION_TARGET_GLOBAL_INT case in Plane's GCS_MAVLINK_Plane::handleMessage. It sends the position to GCS_MAVLINK_Plane::handle_guided_request(cmd), and we should see the same behaviour.

I'll get the planner to emit both message types, then we can switch between them.

And yea, the usual, no altitude datum concept. Absolute can't encode a datum.

We do need a technical document clarifying the datum assumptions used by each library. I think AP is using MSL as the datum (which I believe is the same as the geoid). The datum for the map frame in the terrain planner, ROS 2 and Gazebo is the WGS-84 ellipsoid (again need to double check).

[srmainwaring]

Looks good. I have yet to run examples but have made a few suggestions in the interim.

Update

Tested on macOS using quadplane and davosdorf as home location. Verified that the altitude scaling needs fixing, but with that the vehicle flys to the commanded location and altitude (relative above home) then loiters.

Command used:

ros2 topic pub /ap/cmd_gps_pose  ardupilot_msgs/msg/GlobalPosition "{header: {frame_id: map}, latitude: 46.81762458, longitude: 9.85064438, altitude: 60}" --once

[Ryanf55]

Before this gets merged, I think it would be a good idea to add a colcon test, or at least a python node that replicates the README behavior.

[Ryanf55]

You can now run a full waypoint mission with a python node. It does the arming, switch to takeoff mode, switch to guided, and sending the goal. It also has timeouts for each of the behaviors, and reports progress to the goal.

1. Start AP_DDS with sim_vehicle and start the MicroROS agent like normal
2. Build ardupilot_dds_tests
3. ros2 run ardupilot_dds_tests plane_waypoint_follower

I tried folding that into a colcon test, but it hangs forever. I could use some help.
colcon test --packages-select ardupilot_dds_tests --pytest-args "-k test_plane_wp_follower" --event-handlers=console_direct+

[IamPete1]

Only really looked at the changes outside of DDS, but it all looks good.

[Ryanf55]

I've asked @snktshrma to document on the wiki. I think the script provides an excellent example for people wanting to understand how to use DDS for something useful but not too complicated.