terrainnav: add module for terrain navigation

- Add module folder.
- Add module and app backend.
- Add wx ui frame.
- Add buttons and send commands to backend.

Signed-off-by: Rhys Mainwaring <rhys.mainwaring@me.com>

terrainnav: enable control of map contours

Signed-off-by: Rhys Mainwaring <rhys.mainwaring@me.com>

terrainnav: add start and goal to map

Signed-off-by: Rhys Mainwaring <rhys.mainwaring@me.com>

terrainnav: initial integration of terrain planner

- Add dependency on (py-)ompl and terrain_nav_py.

Signed-off-by: Rhys Mainwaring <rhys.mainwaring@me.com>

terrainnav: use updated GridMap interface

- Named args have changed home_lat -> map_lat etc.

Signed-off-by: Rhys Mainwaring <rhys.mainwaring@me.com>

terrainnav: add initial version of waypoint generator

Signed-off-by: Rhys Mainwaring <rhys.mainwaring@me.com>

terrainnav: formatting and remove commented code

Signed-off-by: Rhys Mainwaring <rhys.mainwaring@me.com>

terrainnav: add gen waypoints button

Signed-off-by: Rhys Mainwaring <rhys.mainwaring@me.com>

terrainnav: add gen waypoints button

Signed-off-by: Rhys Mainwaring <rhys.mainwaring@me.com>

terrainnav: draw the solution states as polygon circles

Signed-off-by: Rhys Mainwaring <rhys.mainwaring@me.com>

terrainnav: path segment states use Vector3

terrainnav: run planner on thread

Signed-off-by: Rhys Mainwaring <rhys.mainwaring@me.com>

terrainnav: update waypoint generator

- Calculate strides for each segment.
- Skip duplicates at ends of each Dubins curve.

Signed-off-by: Rhys Mainwaring <rhys.mainwaring@me.com>

terrainnav: rename init_map_layer to init_slip_map_layer

Signed-off-by: Rhys Mainwaring <rhys.mainwaring@me.com>

terrainnav: validate start and goal positions when set

- Move map initialisation out of planner thread.
- Add validation to start and goal positions before drawing.
- Consolidate conversion from (lat, lon) to (east, north) in static method.

Signed-off-by: Rhys Mainwaring <rhys.mainwaring@me.com>

terrainnav: correct module variable name

Signed-off-by: Rhys Mainwaring <rhys.mainwaring@me.com>

terrainnav: enable show, hide, move planner boundary

- Add buttons to show or hide the current planner boundary.
- Ensure start and goal ENU positions in terrain map frame are updated.

Signed-off-by: Rhys Mainwaring <rhys.mainwaring@me.com>

terrainnav: clean up handling of planner thread

Signed-off-by: Rhys Mainwaring <rhys.mainwaring@me.com>

terrainnav: only print messages if moddebug > 1

Signed-off-by: Rhys Mainwaring <rhys.mainwaring@me.com>

terrainnav: add buttons to clear paths and waypoints

Signed-off-by: Rhys Mainwaring <rhys.mainwaring@me.com>

terrainnav: fix bug in planner thread

- Thread was not set to None on early return.

Signed-off-by: Rhys Mainwaring <rhys.mainwaring@me.com>

terrainnav: check path is above terrain and colour code in map

Signed-off-by: Rhys Mainwaring <rhys.mainwaring@me.com>

terrainnav: add exception handling round planner calls that may raise runtime errors

Signed-off-by: Rhys Mainwaring <rhys.mainwaring@me.com>

terrainnav: recreate planner each time

- Recreate as inputs may have changed between planner runs.

Signed-off-by: Rhys Mainwaring <rhys.mainwaring@me.com>

terrainnav: add support for exclusion polygons

Signed-off-by: Rhys Mainwaring <rhys.mainwaring@me.com>

terrainnav: use UnclosedSlipPolygon for drawing planner boundary

Signed-off-by: Rhys Mainwaring <rhys.mainwaring@me.com>

terrainnav: add support for (ex/in)clusion polygons and circles

Signed-off-by: Rhys Mainwaring <rhys.mainwaring@me.com>

terrainnav: use planner validity checker on start and goal circles

- Move planner initialisation and fence setup to separate function
- Initialise planner early so the state validity checker is available.
- Start and goal circles are now checked they satisfy fence conditions.

Signed-off-by: Rhys Mainwaring <rhys.mainwaring@me.com>

terain_ompl_rrt: ensure planner is re-initialised if boundary moves

Signed-off-by: Rhys Mainwaring <rhys.mainwaring@me.com>

terrainnav: monitor fence for updates

- Re-initialise the planner if fence points change.

Signed-off-by: Rhys Mainwaring <rhys.mainwaring@me.com>

terrainnav: use MPSettings for planner parameters

Signed-off-by: Rhys Mainwaring <rhys.mainwaring@me.com>

terrainnav: set default time budget to 20s

- Inadvertently set to 60s previously.

Signed-off-by: Rhys Mainwaring <rhys.mainwaring@me.com>

terrainnav: add commands to set and clear

- Set start circle to use loiter radius.
- Set goal circle to use turning radius (current default for planner).
- Pass radius as arg to draw circle command.

Signed-off-by: Rhys Mainwaring <rhys.mainwaring@me.com>

terrainnav: add setting for spacing waypoints

Signed-off-by: Rhys Mainwaring <rhys.mainwaring@me.com>

terrainnav: update start and goal positions if loiter alt changes

Signed-off-by: Rhys Mainwaring <rhys.mainwaring@me.com>

terrainnav: move planner to separate process

- Add class TerrainPlanner(Process).
- Set up pipes for communication.
- Set up process control methods: start, stop, kill.
- Add message types for inter-proc comms.
- Implement planner functionality (no fences).

Signed-off-by: Rhys Mainwaring <rhys.mainwaring@me.com>

terrainnav: remove unused imports

Signed-off-by: Rhys Mainwaring <rhys.mainwaring@me.com>

terrainnav: prefix print messages with module name

Signed-off-by: Rhys Mainwaring <rhys.mainwaring@me.com>

terrainnav: process messages from planner

- Add validation state to start and goal lat, lon messages.
- Draw validated start and goal states.

Signed-off-by: Rhys Mainwaring <rhys.mainwaring@me.com>

terrainnav: display path from planner process

- Remove code moved to the planner process.
- Comment code to be removed.
- Process plans published by the planner process.

Signed-off-by: Rhys Mainwaring <rhys.mainwaring@me.com>

terrainnav: move fence handling to planner process

- Remove code moved to the planner process.
- Handle fences on planner process.

Signed-off-by: Rhys Mainwaring <rhys.mainwaring@me.com>

terrainnav: send Dubins states from planner process

- Convert ompl states to list of tuples(x, y, z, yaw).
- Fix bug in naming of poly fences properties.
- Update draw_states method.
- Remove stale comments and imports.

Signed-off-by: Rhys Mainwaring <rhys.mainwaring@me.com>

terrainnav: ensure all settings are sent to planner process

Signed-off-by: Rhys Mainwaring <rhys.mainwaring@me.com>

terrainnav: remove unused variables

Signed-off-by: Rhys Mainwaring <rhys.mainwaring@me.com>

terrainnav: remove debug prints

Signed-off-by: Rhys Mainwaring <rhys.mainwaring@me.com>

terrainnav: catch ValueError when retrieving terrain elevation data

Signed-off-by: Rhys Mainwaring <rhys.mainwaring@me.com>

terrainnav: catch ValueError in event loop and release lock

Signed-off-by: Rhys Mainwaring <rhys.mainwaring@me.com>

Author: srmainwaring

MAVProxy/modules/mavproxy_terrainnav/__init__.py

@@ -0,0 +1,14 @@
+"""
+MAVProxy terrain navigation module
+"""
+
+from MAVProxy.mavproxy import MPState
+from MAVProxy.modules.lib.mp_module import MPModule
+from MAVProxy.modules.mavproxy_terrainnav.terrainnav import TerrainNavModule
+
+
+def init(mpstate: MPState) -> MPModule:
+    """
+    Initialise module
+    """
+    return TerrainNavModule(mpstate)