Offboard Terrain Navigation for Plane with ROS 2 / AP_DDS

[rhys (Rhys Mainwaring)]

A guide to using the ETHZ Autonomous System Lab terrain_navigation library in ArduPilot using ROS 2 and the AP_DDS library. This is a work in progress being developed jointly with @rfriedman and Jaeyoung Lim (ETHZ ASL).

Overview

The terrain planner enables low altitude navigation in steep terrain for fixed wing vehicles. The extension to ArduPilot involves porting the terrain navigation library to ROS 2, replacing the existing mavros interface with DDS, and adding support to ArduPilot Plane for off-board path guidance.

Motivation

• Plane: Implement SmartRTL #2380
• Add trajectory tracking control for Plane #25382

Open PRs

• Plane: handle guided path requests in GUIDED mode #26328
• DDS: add guided path external control and implement for plane #26339
• AP_DDS: publish gps global origin #26409

Work in progress

The two branches below incorporate open PRs and extensions to the terrain navigation library and ArduPilot to replace all mavros nodes with calls into ArduPilot DDS. In some cases this can be done simply by remapping topics, in others it is necessary to run a small bridge node (mavros_dds_bridge.py) that carries out forwarding of services and mapping messages between the systems. Eventually the bridge should be deprecated as ROS 2 message types and services for aerial vehicles are standardised.

• GitHub - srmainwaring/ardupilot at wips/wip-terrain-nav
• GitHub - srmainwaring/terrain-navigation at wips/wip-combined

Installation

ArduPilot with ROS 2

Follow the ArduPilot Dev wiki instructions to set up an ArduPilot ROS 2 SITL environment and verify that the ArduPilot DDS node, topics and services are visible in the ROS 2 CLI. Once working, switch the ardupilot branch to GitHub - srmainwaring/ardupilot at wips/wip-terrain-nav and rebuild. There should be additional topics /ap/gps_global_origin/filtered and /ap/mode available.

% ros2 node info /ardupilot_dds
/ardupilot_dds
  Subscribers:
    /ap/cmd_gps_pose: ardupilot_msgs/msg/GlobalPosition
    /ap/cmd_vel: geometry_msgs/msg/TwistStamped
    /ap/joy: sensor_msgs/msg/Joy
    /ap/tf: tf2_msgs/msg/TFMessage
  Publishers:
    /ap/battery/battery0: sensor_msgs/msg/BatteryState
    /ap/clock: rosgraph_msgs/msg/Clock
    /ap/geopose/filtered: geographic_msgs/msg/GeoPoseStamped
    /ap/gps_global_origin/filtered: geographic_msgs/msg/GeoPointStamped
    /ap/imu/experimental/data: sensor_msgs/msg/Imu
    /ap/mode: ardupilot_msgs/msg/Mode
    /ap/navsat/navsat0: sensor_msgs/msg/NavSatFix
    /ap/pose/filtered: geometry_msgs/msg/PoseStamped
    /ap/tf_static: tf2_msgs/msg/TFMessage
    /ap/time: builtin_interfaces/msg/Time
    /ap/twist/filtered: geometry_msgs/msg/TwistStamped
  Service Servers:
    /ap/arm_motors: ardupilot_msgs/srv/ArmMotors
    /ap/mode_switch: ardupilot_msgs/srv/ModeSwitch
  Service Clients:

  Action Servers:

  Action Clients:

Terrain navigation

Follow the instructions in the ros2 branch of the terrain_navigation README to clone and build the library and dependencies in the same colcon workspace used in the step above for ArduPilot with ROS 2.

Add the remote for the work-in-progess branch of terrain_navigation: GitHub - srmainwaring/terrain-navigation at wips/wip-combined and switch to the wips/wip-combined branch, then rebuild the workspace.

Usage

A number of terminals are required to run the planner and simulation. In each one ensure to source the workspace before running any ROS commands.

Launch ArduPilot SITL with DDS and a micro-ROS agent (UDP) for a quadplane:

ros2 launch terrain_navigation_ros ardupilot.launch.py

Run an interactive MAVProxy session connected to the SITL instance:

mavproxy.py --master 127.0.0.1:14550 --console --map

Open the mission editor and load the mission:

~/ros_ws/src/terrain-navigation/terrain_navigation_ros/config/davosdorf_mission.txt

Arm the plane and switch to AUTO:

FBWA>
FBWA> arm throttle
FBWA> auto
AUTO>

The plane should takeoff and loiter over a park near the lake in Davosdorf.

Run the mavros_dds_bridge node:

python ./src/terrain-navigation/terrain_navigation_ros/scripts/mavros_dds_bridge.py

Launch the terrain planner:

ros2 launch terrain_navigation_ros terrain_planner.launch.py cruise_speed:=25.0 minimum_turn_radius:=80.0 rviz:=false

Launch rviz:

ros2 launch terrain_navigation_ros rviz.launch.py

• Click the Interact button in the top toolbar.
• Click Update Goal to set the target at the markers position.

• Drag the marker to the area near the lake where the plain is loitering.
• Click Update Start to mark the start of the path.

• Select ardupilot in the Flight Stack dropdown.
• Click Plan to run the planner.
• The path is easier to see if the terrain is hidden (uncheck GridMap in the Displays panel).

• Click Engage Planner and then NAVIGATE
• The plane should exit the loiter and start tracking the path.

Known issues, next steps

Message content

• The altitude of the plane pose and track in rviz are incorrect. This is because the GPS altitude published by AP_DDS references MSL rather than the WGS84 ellipsoid. A temporary fix is applied in the mavros_dds_bridge where the geoid height is added. This adjustment should be moved to the AP_DDS_Client.

Visualisation

• The initial and goal loiter radii shown in rviz are not matching the requested minimum turn radius.
• ideally the planner control would be run from a ground control station rather than rviz. A module for MAVProxy might be a good first step.

Guidance control

• The initial capture of the path from loiter does not always work well. This is most likely related to the loiter information from the AUTO mission not being incorporated into the planners initial state.
• In PX4 the planner uses the Nonliear Path Following Guidance (NPFG) algorithm described in one of the papers referenced in the terrain planner document. There is a draft implementation for ArduPilot here Plane: support off-board terrain navigation #26262 however it is not yet working correctly.

Terrain data

• Tools are required to manage and load the terrain data.

[khancyr]

Pretty cool !

Why not blog this ?

[rhys (Rhys Mainwaring)]

@khancyr

Why not blog this ?

Still very much in development, it’s not that easy to set up and somethings may not work properly so not yet ready for general use. The current documentation is scattered through a number of PRs so wanted to pull everything together in one place. Once it’s tested and user friendly would definitely move to the blog area.

[rfriedman]

FYI:

• The global origin support in /ap/gps_global_origin/filtered is merged
• The current mode publisher behavior of /ap/mode will be merged as part of this larger message: AP_DDS: Status Message by tizianofiorenzani · Pull Request #28337 · ArduPilot/ardupilot · GitHub

I’m hoping to keep moving this along.

[rfriedman]

Regarding The altitude of the plane pose and track in rviz are incorrect.

Have you tried enabling bit 4 of GPS_DRV_OPTIONS in SITL (which defaults to UBX GPS type)?

[rhys (Rhys Mainwaring)]

MAVProxy tools for terrain planning

One of the obstacles in adopting the terrain planner described above is that it is complicated to configure and run. There are many steps, custom terrain maps must be created and it is too easy to make inadvertent errors regarding the choice and consistency of map datum and other coordinate conventions.

As an intermediate step to a full integration of the off-board terrain navigation into ArduPilot, I’ve started to put together some tools in a MAVProxy module to make it easier to use a terrain planner. The module terrainnav uses a version of the ETHZ planner fully implemented in Python. It is not as fast as the C++ version, but fast enough for prototyping and generating waypoint missions over maps around 10km square.The other advantage is that it depends on a version of the OMPL Python bindings that may be installed with pip, so there are no C++ compile steps required to get up and running. The next version of OMPL (1.7) looks like it may contain a state space model suitable for airplane planning, and the hope is that it will be a drop-in replacement for some of the time critical sections of the current library.

Install

The terrain_nav_py Python package for the planner may be found here: GitHub - srmainwaring/terrain_nav_py. It can be installed by cloning the repo and installing with pip (using a virtual environment is recommended):

# clone and install
git clone https://github.com/srmainwaring/terrain_nav_py.git
cd terrain_nav_py/
python -m pip install .
# run the tests
pytest .

Alternatively the package can be installed directly from GitHub:

python -m pip install git+https://github.com/srmainwaring/terrain_nav_py.git

The installer should work for Python 3.10, 3.11, 3.12 on macOS and Ubuntu, either arm64 or x86_64. The CI tests run for Ubuntu 22.04 and 24.04.

The source install is useful to understand the planner in detail and experiment with various options not exposed in the MAVProxy tools.

The MAVProxy module may be found in this PR:

Add terrain planner module

It will need to be installed from source:

git clone https://github.com/srmainwaring/MAVProxy.git -b prs/pr-terrain-nav
pip install -U .

Or use the -e flag if an editable install is required for stepping through in a debugger.

Usage

Start a SITL session with a quadplane. Here’s an example that sets the home position the the Brecon Beacons National Trust car park (no real flying allowed sadly…)

sim_vehicle.py --debug -v ArduPlane -f quadplane --custom-location="51.872565163811664, -3.4788544705811075, 452, 0" --console --map

Load the terrainnav module

FBWA> module load terrainnav

Not all the buttons are functional, the navigation buttons (Hold, Navigate, etc) and Add Rally and Add Waypoint buttons are for a later version supporting an off-board controller.

Show Contours / Hide Contours toggle the display of terrain contours based on the SRTM1 or SRTM30 terrain data made available by the existing terrain module.

Click a location in the map and click Set Start to select a start location. If the circle does not clear the surrounding terrain the circle will be red:

Select an alternative location until the start circle is green.

The terrain contours help to locate a suitable start position.

Zoom out on the map and repeat with a goal location:

The planner searches within a pre-determined area which can be seen by clicking the Show Boundary button.

Both the start and goal positions must lie completely inside the boundary. The planner region (called the workspace in the OMPL documents) is initially centred on the vehicle home position, but may be re-centred by clicking the map and using the Move Boundary button. Moving the search region can sometimes help the planner find a solution if one of the end points is near the boundary edge.

Click Run Planner to solve for a valid path between the start and goal.

If the path clears the terrain it is coloured green, otherwise red. The small magenta circles along the path denote the states that are connected by the Dubins paths. If the planner cannot find a solution the states representing the best approximation are shown, but no path is generated.

The Gen Waypoint button samples the path and creates a series of waypoints that are sent to the vehicle.

To fly the mission, place the plane in QLOITER, arm and takeoff:

FBWA> QLOITER
QLOITER> arm throttle
# takeoff
QLOITER> rc 3 1800
# hover at around 60m
QLOITER> rc 3 1500

Then use the Fly To map command to place the plane in a loiter at the start circle:

Finally switch to AUTO to follow the path:

The mission has no terminal loiter, and the plane will then attempt a simple RTL, which does not usually end well in steep terrain.

These and other shortcomings will be addressed as work continues.

Update

One of the benefits of using a Python version of the planner is that extensions to the state validity checker are straightforward: for example adding exclusion polygon fences.