Add terrain planner module

[srmainwaring]

Add a module for airplane terrain planning. The module integrates a Python version of the ETHZ terrain navigation package that uses a Dubins airplane state space model with the OMPL RRT planner.

Main features are:

• Integration with SRTM1 terrain data supplied by the MAVProxy mp_elevation module.
• Command line settings for the main planner parameters.
• Support for exclusion and inclusion polyfences (polygons and circles).
• Generate flight plans that stay within min and max altitude bands given a climb angle.
• Generate and upload waypoints sampled from the planned path.
• Multiprocessing support for the planner to avoid Python GIL contention issues.

A guide on usage and further discussion on the planner used can be found here: https://discuss.ardupilot.org/t/offboard-terrain-navigation-for-plane-with-ros-2-ap-dds/114418/6?u=rhys

Related

• Add terrain navigation module #1513

Dependencies

• https://github.com/srmainwaring/terrain_nav_py
• https://ompl.kavrakilab.org/

The ompl dependency for recent Python versions running on either Linux or macOS are resolved by the terrain_nav_py installer (via the projects pyproject.toml).

Installing the terrain_nav_py package should pull in all dependencies:

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

Usage

Ensure the SRTM1 terrain data is available:

FBWA> terrain set source SRTM1

Load the module:

FBWA> module load terrainnav

Display settings:

FBWA> terrainnav set
     climb_angle_deg 8.0
         grid_length 10000.0
        grid_spacing 30.0
      loiter_agl_alt 60.0
       loiter_radius 60.0
         max_agl_alt 100.0
         min_agl_alt 50.0
          resolution 100.0
         time_budget 20.0
      turning_radius 60.0
          wp_spacing 60.0

Tasks

• replace prints in upstream library with a Python logger to prevent flooding the console
• remove or grey out currently unused buttons from UI
• squash commits

[Georacer]

Oh my god, this PR description has all the right keywords to make me scream with joy!

[srmainwaring]

Oh my god, this PR description has all the right keywords to make me scream with joy!

Testers welcome @Georacer! I found a possible issue with installing the ompl Python bindings on an Ubuntu VM that did not show up in CI and local docker machines. The ompl wheel may be trying to install ompl system libraries as well, which needs sudo, and this caused complaints in a Python virtual environment. It can be forced, but I need to try and replicate the issue on a clean build.

Various other niggles to do with clashing numpy and matplotlib versions should now be resolved, as well as an issue where terrain tiles were not being loaded because I'd marked the planner process as a daemon, and the terrain loader needs to run multiproc children which a daemon process could not do.

[Ryanf55]

First test result, not running SITL, just loading the module after running mavproxy.py.

MAV> module load terrainnav
MAV> /usr/lib/python3/dist-packages/scipy/__init__.py:146: UserWarning: A NumPy version >=1.17.3 and <1.25.0 is required for this version of SciPy (detected version 1.26.4
  warnings.warn(f"A NumPy version >={np_minversion} and <{np_maxversion}"
ERROR in command ['load', 'terrainnav']: 'NoneType' object has no attribute 'messages'
Traceback (most recent call last):
  File "/home/ryan/Dev/ardu_ws/src/MAVProxy/MAVProxy/mavproxy.py", line 825, in process_stdin
    fn(args[1:])
  File "/home/ryan/Dev/ardu_ws/src/MAVProxy/MAVProxy/mavproxy.py", line 623, in cmd_module
    mpstate.load_module(modname, **kwargs)
  File "/home/ryan/Dev/ardu_ws/src/MAVProxy/MAVProxy/mavproxy.py", line 387, in load_module
    module = m.init(mpstate, **kwargs)
  File "/home/ryan/Dev/ardu_ws/src/MAVProxy/MAVProxy/modules/mavproxy_terrainnav/__init__.py", line 14, in init
    return TerrainNavModule(mpstate)
  File "/home/ryan/Dev/ardu_ws/src/MAVProxy/MAVProxy/modules/mavproxy_terrainnav/terrainnav.py", line 129, in __init__
    self.start_planner()
  File "/home/ryan/Dev/ardu_ws/src/MAVProxy/MAVProxy/modules/mavproxy_terrainnav/terrainnav.py", line 520, in start_planner
    home = wp_module.get_home()
  File "/home/ryan/Dev/ardu_ws/src/MAVProxy/MAVProxy/modules/mavproxy_wp.py", line 269, in get_home
    return self.get_WP0(home_only=True)
  File "/home/ryan/Dev/ardu_ws/src/MAVProxy/MAVProxy/modules/mavproxy_wp.py", line 244, in get_WP0
    if 'HOME_POSITION' in self.master.messages:
AttributeError: 'NoneType' object has no attribute 'messages'

[Georacer]

@srmainwaring it F*****G works!

But, I refused to alter my system MAVProxy or other python packages. I really wanted to make this work in a Python virtual environment.
So here's what I had to do:

1. Clone your branch.
2. Create a new virtual environment:

mkdir ~/deleteme && cd deleteme
uv init
uv python pin 3.10 (my system Python version, perhaps unneded)

3. Install the necessary dependencies. In the end they look like this:

❯ uv tree
Resolved 42 packages in 5ms
mavproxy-terrain-planner v0.1.0
├── mavproxy v1.8.71
│   ├── numpy v2.2.4
│   ├── pymavlink v2.4.43
│   │   ├── future v1.0.0
│   │   └── lxml v5.3.2
│   ├── pynmeagps v1.0.49
│   └── pyserial v3.5
├── opencv-python v4.11.0.86
│   └── numpy v2.2.4
├── pygame v2.6.1
├── terrain-nav-py v0.0.1
│   ├── matplotlib v3.10.1
│   │   ├── contourpy v1.3.1
│   │   │   └── numpy v2.2.4
│   │   ├── cycler v0.12.1
│   │   ├── fonttools v4.57.0
│   │   ├── kiwisolver v1.4.8
│   │   ├── numpy v2.2.4
│   │   ├── packaging v24.2
│   │   ├── pillow v11.1.0
│   │   ├── pyparsing v3.2.3
│   │   └── python-dateutil v2.9.0.post0
│   │       └── six v1.17.0
│   ├── mavproxy v1.8.71 (*)
│   ├── numpy v2.2.4
│   ├── ompl v1.7.0
│   ├── pymavlink v2.4.43 (*)
│   ├── pytest v8.3.5
│   │   ├── exceptiongroup v1.2.2
│   │   ├── iniconfig v2.1.0
│   │   ├── packaging v24.2
│   │   ├── pluggy v1.5.0
│   │   └── tomli v2.2.1
│   ├── scipy v1.15.2
│   │   └── numpy v2.2.4
│   └── shapely v2.1.0
│       └── numpy v2.2.4
└── wxpython v4.2.2
    ├── numpy v2.2.4
    └── six v1.17.0
(*) Package tree already displayed

Note that your custom MAVProxy branch is installed from local sources.
wxPython tool about 10mins to build. But ChatGPT suggests I could have done

uv pip install -f https://wxpython.org/Phoenix/snapshot-builds/ wxPython

Didn't try that.

4. Then I had to open a fresh terminal and shadow mavproxy:

ln -s ~/deleteme/.venv/bin/mavproxy.py mavproxy.py

5. Finally I could run the command:

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

Some visual artifacts didn't work seamlessly. E.g. the contours didn't appear until I zoomed out. Then I could zoom back in and see them still. Or perhaps they just took a long time to load?

In general, I was always unhappy with our lack of venv support for MAVProxy, but perhaps this workflow kind of works.
Now I'll be able to tinker with MAVProxy with peace of mind.

[Georacer]

I see 3 loiters towards the end

but the actual waypoints are more sparse and actually simple waypoints, not loiters:

(Sorry, closed the sim, had to use QGC.)

Is this a shortcoming or is it intentional?

[srmainwaring]

@Georacer thanks for testing.

I use the venv module for virtual environments, which seems to work well for me on macOS and Ubuntu docker images and VMs. Setting up a new environment should be quick, the steps I use are outlined below:

• Using the '—system-site-packages` flag when creating the venv should save having to rebuild wxPython and reinstall most existing packages.

• this mavproxy fork needs to be cloned and installed. Do this before installing terrain_nav_py as the latter includes mavproxy in its pyproject.toml dependencies and will install it if not already present.

• cloning terrain_nav_py and running pip install . in the project root directory should install all its dependencies.

• the contours may take a little while to display, depending on whether the terrain tiles are present locally or not. The contours are a separate feature to this PR, but I think they can be made faster by using a different opencv object to display them.

• the three spirals at the end of the path may have been sampled too sparsely. The setting wp_spacing can be reduced to increase the number of waypoints generated. Alternatively the turning_radius can be increased.

I think the generator needs a different spacing for curved sections to straight ones.

The most recent version of OMPL includes an OwenStateSpace model whose Python bindings can be used to replace much of the performance critical Python code in terrain_nav_py. This makes significant difference (about 10x faster in the planner). There are some bugs to resolve and testing to complete before that can be used, but it should not change the use or behaviour of the module (aside from speed).

[Georacer]

I think the generator needs a different spacing for curved sections to straight ones.

Ideally you would convert the turns to NAV_LOITER_ALT (perhaps with a CONDITION_YAW if necessary, I don't remember well).
But I understand that's not plug and play with the planner, it requires post-processing of the path.

[srmainwaring]

The circles are placed to gain or lose altitude, so will need to understand the mission commands for that. The raw output of the planner is a set of points that may be connected by Dubins curves. These are then post processed to construct a trajectory, in which the position, altitude, path tangent and current curvature are deduced. From that the options are to generate an input for a path following controller (eg offboard control using DDS or MAVLink), or generate a mission. Only the latter is provided at the moment, and it’s pretty basic. I’d like to get that improved and the path guidance is for a bit later, there are PRs to support this in the Plane library already and this tool will help generate examples to complete the autotests needed.

[srmainwaring]

@Georacer I've added an alternative waypoint generator that uses NAV_LOITER_TO_ALT for turns, where the turn angle is greater than some configurable angle.

There is a dialog to facilitate editing the settings:

The choice of WP generator can now be toggled - the planner does not need to be re-run in order to regenerate the mission using a different method.

Figure: simple waypoints sampled along the path

Figure: waypoints generated using NAV_LOITER_TO_ALT for turns

Figure: mission details

For the most case, the plane follows the mission with NAV_LOITER_TO_ALT well. However I have noticed that the plane may fail to achieve the required altitude gain for some turns and then completes a full circle before continuing. This is not ideal, as the planner does not check that such circles are safe (unless a spiral is requested by the planner).

I am still not sure why the plane is not achieving the alt change, the climb rate demanded is consistent with the planner settings (climb_angle_deg), so I'd have thought that provided TECS_CLMB_MAX was greater than this, it should be satisfied.

[Ryanf55]

Seems like planner start/goal are hard to see over large plans. Over a large area, when you zoom out, they disappear. Meanwhile, for a mission, the circles are fixed pixel size so they are always visible. Not critical, just a small enhancement opportunity.

[srmainwaring]

Seems like planner start/goal are hard to see over large plans.

This is because they are sized at the loiter radius, rather than as general markers. The reasoning is that this lets you see how the initial and final loiters will fit in between terrain contours. We could add an additional symbol that scales with the map zoom?