terrain_nav: add alt waypoint generator and settings dialog

- Add WP generator that uses MAV_CMD_NAV_LOITER_TO_ALT
- Update window size
- Use wxsettings module for dialog.
- Ensure dialog retrieves settings from correct thread.

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

Author: srmainwaring

MAVProxy/modules/mavproxy_terrainnav/terrainnav.py

@@ -12,6 +12,7 @@
 from MAVProxy.modules.lib import multiproc
 from MAVProxy.modules.lib import mp_module
 from MAVProxy.modules.lib import mp_settings
+from MAVProxy.modules.lib.mp_settings import MPSetting
 from MAVProxy.modules.lib import mp_util
 
 from MAVProxy.modules.mavproxy_map import mp_slipmap
@@ -21,39 +22,54 @@
 
 if mp_util.has_wxpython:
     from MAVProxy.modules.lib.mp_menu import MPMenuSubMenu
+    from MAVProxy.modules.lib.wxsettings import WXSettings
 
 from pymavlink import mavutil
+from pymavlink.rotmat import Vector3
 
 # terrain navigation
 from terrain_nav_py.dubins_airplane import DubinsAirplaneStateSpace
 from terrain_nav_py.grid_map import GridMapSRTM
 from terrain_nav_py.path import Path
+from terrain_nav_py.path_segment import PathSegment
 from terrain_nav_py.terrain_map import TerrainMap
 from terrain_nav_py.terrain_ompl_rrt import TerrainOmplRrt
 
 
 class TerrainNavModule(mp_module.MPModule):
-    def __init__(self, mpstate: MPState) -> None:
-        super().__init__(mpstate, "terrainnav", "terrain navigation module")
 
-        # TODO: some of these settings should be extracted from params
-        # *** planner settings ***
-        self.terrainnav_settings = mp_settings.MPSettings(
+    # TODO: some of these settings should be extracted from params
+    @staticmethod
+    def default_settings():
+        return mp_settings.MPSettings(
             [
-                ("loiter_agl_alt", float, 60.0),
-                ("loiter_radius", float, 60.0),
-                ("turning_radius", float, 60.0),
-                ("climb_angle_deg", float, 8.0),
-                ("max_agl_alt", float, 100.0),
-                ("min_agl_alt", float, 50.0),
-                ("grid_spacing", float, 30.0),
-                ("grid_length", float, 10000.0),
-                ("time_budget", float, 20.0),
-                ("resolution", float, 100.0),
-                ("wp_spacing", float, 60.0),
+                MPSetting("loiter_agl_alt", float, 60.0),
+                MPSetting("loiter_radius", float, 60.0),  # WP_LOITER_RADIUS
+                MPSetting("turning_radius", float, 60.0),  # WP_LOITER_RADIUS
+                MPSetting("climb_angle_deg", float, 8.0),  # TECS_CLMB_MAX
+                MPSetting("max_agl_alt", float, 100.0),
+                MPSetting("min_agl_alt", float, 50.0),
+                MPSetting("grid_spacing", float, 30.0),
+                MPSetting("grid_length", float, 10000.0),
+                MPSetting("time_budget", float, 20.0),
+                MPSetting("resolution", float, 100.0),
+                MPSetting(
+                    "wp_generator",
+                    str,
+                    "SimpleWaypoints",
+                    choice=["SimpleWaypoints", "UseLoiterToAlt"],
+                ),
+                MPSetting("wp_spacing", float, 60.0),
+                MPSetting("wp_min_loiter_angle_deg", float, 45.0),
             ]
         )
 
+    def __init__(self, mpstate: MPState) -> None:
+        super().__init__(mpstate, "terrainnav", "terrain navigation module")
+
+        # *** planner settings ***
+        self.terrainnav_settings = TerrainNavModule.default_settings()
+
         # *** commands ***
         cmdname = "terrainnav"
         # TODO: do not support multi-instance or multi-vehicle yet
@@ -119,7 +135,7 @@ def __init__(self, mpstate: MPState) -> None:
         # *** fence state ***
         self._fence_change_time = 0
 
-        # *** multiprocessing ***
+        # *** planner multiprocessing ***
         self._planner_process = None
         self._parent_pipe_recv, self._planner_pipe_send = multiproc.Pipe(duplex=False)
         self._planner_pipe_recv, self._parent_pipe_send = multiproc.Pipe(duplex=False)
@@ -251,7 +267,7 @@ def process_ui_msgs(self):
             elif isinstance(msg, terrainnav_msgs.RunPlanner):
                 self._parent_pipe_send.send(PlannerCmdRunPlanner())
             elif isinstance(msg, terrainnav_msgs.GenWaypoints):
-                self.gen_waypoints()
+                self.generate_waypoints()
             elif isinstance(msg, terrainnav_msgs.ClearPath):
                 self.clear_path()
             elif isinstance(msg, terrainnav_msgs.ClearWaypoints):
@@ -287,6 +303,8 @@ def process_ui_msgs(self):
                 self.hide_planner_boundary()
             elif isinstance(msg, terrainnav_msgs.MoveBoundary):
                 self.move_planner_boundary()
+            elif isinstance(msg, terrainnav_msgs.Settings):
+                self.edit_settings()
             else:
                 # TODO: raise an exception
                 if self.is_debug:
@@ -408,6 +426,12 @@ def move_planner_boundary(self):
 
         self._parent_pipe_send.send(PlannerGridLatLon((lat, lon)))
 
+    def edit_settings(self):
+        """
+        Open the settings dialog
+        """
+        WXSettings(self.terrainnav_settings)
+
     def draw_circle(self, id, lat, lon, radius, colour):
         map_module = self.module("map")
         if map_module is None:
@@ -680,7 +704,16 @@ def draw_states(self, states):
             )
             map_module.map.add_object(slip_polygon)
 
-    def gen_waypoints(self):
+    def generate_waypoints(self):
+        wp_gen = self.terrainnav_settings.wp_generator
+        if wp_gen == "SimpleWaypoints":
+            self.wp_gen_simple_waypoints()
+        elif wp_gen =="UseLoiterToAlt":
+            self.wp_gen_use_loiter_to_alt()
+        else:
+            print(f"[terrainnav] invalid WP generator: {wp_gen}")
+
+    def wp_gen_simple_waypoints(self):
         path = self._candidate_path
         map_lat = self._grid_map_lat
         map_lon = self._grid_map_lon
@@ -692,7 +725,7 @@ def gen_waypoints(self):
         if wp_module is None:
             return
 
-        # TODO: provide accessors on Path  - fix upstream
+        # TODO: provide accessors on Path - fix upstream
         # TODO: dt is not set - fix upstream
         wp_spacing = self.terrainnav_settings.wp_spacing
         wp_num_total = 0
@@ -770,6 +803,135 @@ def gen_waypoints(self):
             # tell the wp module to expect some waypoints
             wp_module.loading_waypoints = True
 
+    def wp_gen_use_loiter_to_alt(self):
+        path = self._candidate_path
+        map_lat = self._grid_map_lat
+        map_lon = self._grid_map_lon
+
+        if path is None or map_lat is None or map_lon is None:
+            return
+
+        wp_module = self.module("wp")
+        if wp_module is None:
+            return
+
+        # TODO: provide accessors on Path
+        mission_items = []
+        wp_num = 0
+        for i, segment in enumerate(path._segments):
+            position3 = segment.first_state().position
+            tangent3 = (segment.first_state().velocity).normalized()
+            curvature = segment.curvature
+            position2 = Vector3(position3.x, position3.y, 0.0)
+            tangent2 = Vector3(tangent3.x, tangent3.y, 0.0)
+
+            start_alt = segment.first_state().position.z
+
+            (end_lat, end_lon) = mp_util.gps_offset(
+                map_lat,
+                map_lon,
+                segment.last_state().position.x,
+                segment.last_state().position.y,
+            )
+            end_alt = segment.last_state().position.z
+            end_yaw_deg = math.degrees(segment.last_state().attitude.euler[2])
+
+            segment_delta_alt = end_alt - start_alt
+            segment_length = segment.get_length()
+            segment_gamma = math.asin(segment_delta_alt / segment_length)
+            segment_gamma_deg = math.degrees(segment_gamma)
+            # TODO: remove debug print
+            # print(
+            #     f"[{wp_num}] delta_alt: {segment_delta_alt}, "
+            #     f"length: {segment_length}, gamma_deg: {segment_gamma_deg}"
+            # )
+
+            # waypoint number - should also include home position
+            sys_id = self.mpstate.settings.target_system
+            cmp_id = self.mpstate.settings.target_component
+            pass_radius = 0.0
+            if curvature != 0.0:
+                # curvature in ENU, CCW is negative, CW is positive
+                pass_radius = 1.0 if curvature < 0 else -1.0
+
+                # do not add a loiter for short turns
+                radius = 1.0 / math.fabs(curvature)
+                phi = segment_length / radius
+                min_phi = math.radians(self.terrainnav_settings.wp_min_loiter_angle_deg)
+
+                if phi >= min_phi:
+                    arc_centre = PathSegment.get_arc_centre(
+                        position2, tangent2, curvature
+                    )
+                    (cen_lat, cen_lon) = mp_util.gps_offset(
+                        map_lat,
+                        map_lon,
+                        arc_centre.x,
+                        arc_centre.y,
+                    )
+
+                    # MAV_CMD_NAV_LOITER_TO_ALT (31)
+                    p1 = 1.0  # heading required: 0: no, 1: yes
+                    p2 = -1.0 / curvature  # radius: > 0 loiter CW, < 0 loiter CCW
+                    p3 = 0.0
+                    p4 = 1.0  # loiter exit location: 1: line between exit and next wp.
+                    mission_item = mavutil.mavlink.MAVLink_mission_item_message(
+                        target_system=sys_id,
+                        target_component=cmp_id,
+                        seq=wp_num,
+                        frame=mavutil.mavlink.MAV_FRAME_GLOBAL,
+                        command=mavutil.mavlink.MAV_CMD_NAV_LOITER_TO_ALT,
+                        current=0,
+                        autocontinue=1,
+                        param1=p1,
+                        param2=p2,
+                        param3=p3,
+                        param4=p4,
+                        x=cen_lat,
+                        y=cen_lon,
+                        z=end_alt,
+                    )
+                    mission_items.append(mission_item)
+                    wp_num += 1
+
+            # mission item MAV_CMD_NAV_WAYPOINT (16)
+            p1 = 0.0  # hold
+            p2 = 0.0  # accept radius
+            p3 = 0.0  # pass_radius # pass radius - not working?
+            p4 = 0.0  # end_yaw_deg # yaw at waypoint - not working?
+            mission_item = mavutil.mavlink.MAVLink_mission_item_message(
+                target_system=sys_id,
+                target_component=cmp_id,
+                seq=wp_num,
+                frame=mavutil.mavlink.MAV_FRAME_GLOBAL,
+                command=mavutil.mavlink.MAV_CMD_NAV_WAYPOINT,
+                current=0,
+                autocontinue=1,
+                param1=p1,
+                param2=p2,
+                param3=p3,
+                param4=p4,
+                x=end_lat,
+                y=end_lon,
+                z=end_alt,
+            )
+            mission_items.append(mission_item)
+            wp_num += 1
+
+        # prepare waypoints for load
+        wp_module.wploader.clear()
+        wp_module.wploader.expected_count = len(mission_items)
+        self.mpstate.master().waypoint_count_send(len(mission_items))
+        for w in mission_items:
+            wp_module.wploader.add(w)
+            wsend = wp_module.wploader.wp(w.seq)
+            if self.mpstate.settings.wp_use_mission_int:
+                wsend = wp_module.wp_to_mission_item_int(w)
+            self.mpstate.master().mav.send(wsend)
+
+            # tell the wp module to expect some waypoints
+            wp_module.loading_waypoints = True
+
     @property
     def is_debug(self):
         return self.mpstate.settings.moddebug > 1
@@ -1196,8 +1358,6 @@ def init_terrain_map(self):
         # if self.is_debug:
         #     print(f"[TerrainPlanner] calculating distance-surface...", end="")
         # self._grid_map.addLayerDistanceTransform(surface_distance=self.terrainnav_settings.min_agl_alt)
-        # if self.is_debug:
-        #     print(f"done.")
 
         self._terrain_map = TerrainMap()
         self._terrain_map.setGridMap(self._grid_map)
@@ -1276,12 +1436,6 @@ def init_planner(self):
         resolution_requested = self._resolution / self._grid_length
         problem.setStateValidityCheckingResolution(resolution_requested)
 
-        # TODO: enable debug - will need message from mddule
-        # if self.is_debug:
-        #     si = problem.getSpaceInformation()
-        #     resolution_used = si.getStateValidityCheckingResolution()
-        #     print(f"[TerrainPlanner] resolution used: {resolution_used}")
-
         self._lock.release()
 
     def run_planner(self):

MAVProxy/modules/mavproxy_terrainnav/terrainnav_app.py

@@ -44,7 +44,7 @@ def ui_task(self):
         # create the wx application and pass self as the state
         app = wx.App()
         app.frame = terrainnav_ui.TerrainNavFrame(
-            state=self, title=self.title, size=(360, 300)
+            state=self, title=self.title, size=(460, 340)
         )
         app.frame.SetDoubleBuffered(True)
         app.frame.Show()
@@ -111,6 +111,8 @@ def ui_is_alive(self):
     """
     multiproc.freeze_support()
     app = TerrainNavApp(title="Terrain Navigation")
+    app.start_ui()
+
     while app.ui_is_alive():
         print("terrain navigation app is alive")
-        time.sleep(0.5)
+        time.sleep(0.1)

MAVProxy/modules/mavproxy_terrainnav/terrainnav_msgs.py

@@ -91,3 +91,7 @@ def __init__(self):
 class ClearWaypoints:
     def __init__(self):
         pass
+
+class Settings:
+    def __init__(self):
+        pass