terrainnav: add settings for wp generation

- Add settings for waypoint generation.
- Add option to use relative alt in waypoint generation.
- Reorganise terrain nav screen.

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

Author: srmainwaring

MAVProxy/modules/mavproxy_terrainnav/terrainnav.py

@@ -49,6 +49,12 @@ def default_settings():
                 MPSetting("grid_length", float, 10000.0),
                 MPSetting("time_budget", float, 20.0),
                 MPSetting("resolution", float, 100.0),
+                MPSetting(
+                    "terrain_source",
+                    str,
+                    "SRTM1",
+                    choice=["SRTM1", "SRTM3"],
+                ),
                 MPSetting(
                     "wp_generator",
                     str,
@@ -57,12 +63,10 @@ def default_settings():
                 ),
                 MPSetting("wp_spacing", float, 60.0),
                 MPSetting("wp_min_loiter_angle_deg", float, 45.0),
-                MPSetting(
-                    "terrain_source",
-                    str,
-                    "SRTM1",
-                    choice=["SRTM1", "SRTM3"],
-                ),
+                MPSetting("wp_use_relative_alt", bool, True),
+                MPSetting("wp_add_home", bool, True),
+                MPSetting("wp_add_start_loiter", bool, True),
+                MPSetting("wp_add_goal_loiter", bool, True),
             ]
         )
 
@@ -249,8 +253,6 @@ def process_ui_msgs(self):
                 self.clear_path()
             elif isinstance(msg, terrainnav_msgs.ClearWaypoints):
                 self.clear_waypoints()
-            elif isinstance(msg, terrainnav_msgs.ClearAll):
-                self.clear_all()
             elif isinstance(msg, terrainnav_msgs.Hold):
                 if self.is_debug:
                     print("[terrainnav] Hold")
@@ -552,11 +554,6 @@ def clear_start_goal(self):
         map_module.map.remove_object(self._map_start_id)
         map_module.map.remove_object(self._map_goal_id)
 
-    def clear_all(self):
-        self.clear_path()
-        self.clear_waypoints()
-        self.clear_start_goal()
-
     def start_planner(self):
         if self.is_planner_alive():
             return
@@ -829,8 +826,7 @@ def _wp_gen_start_loiter(self, seq):
             state.position, state.velocity, Vector3(start_x, start_y, 0.0)
         )
 
-        # TODO: add to settings
-        use_relative_alt = True
+        use_relative_alt = self.terrainnav_settings.wp_use_relative_alt
 
         # TODO: check home.frame
         home_alt_amsl = home.z
@@ -908,8 +904,7 @@ def _wp_gen_goal_loiter(self, seq):
             state.position, state.velocity, Vector3(start_x, start_y, 0.0)
         )
 
-        # TODO: add to settings
-        use_relative_alt = True
+        use_relative_alt = self.terrainnav_settings.wp_use_relative_alt
 
         # TODO: check home.frame
         home_alt_amsl = home.z
@@ -969,28 +964,33 @@ def _wp_gen_simple_waypoints(self):
         mission_items = []
         wp_num = 0
 
-        # TODO: add to settings
-        use_relative_alt = True
-        add_home = True
-        add_start_loiter = True
-        add_goal_loiter = True
+        use_relative_alt = self.terrainnav_settings.wp_use_relative_alt
+        add_home = self.terrainnav_settings.wp_add_home
+        add_start_loiter = self.terrainnav_settings.wp_add_start_loiter
+        add_goal_loiter = self.terrainnav_settings.wp_add_goal_loiter
 
         # TODO: check home.frame
         home_alt_amsl = home.z
 
         if add_home:
+            if self.is_debug:
+                print("[terrainnav] adding home")
             mission_item = self._wp_gen_home(wp_num, home)
             if mission_item is not None:
                 mission_items.append(mission_item)
                 wp_num += 1
 
         if add_start_loiter:
+            if self.is_debug:
+                print("[terrainnav] adding start loiter")
             mission_item = self._wp_gen_start_loiter(wp_num)
             if mission_item is not None:
                 mission_items.append(mission_item)
                 wp_num += 1
 
         # sample waypoints along the path
+        if self.is_debug:
+            print("[terrainnav] adding waypoints")
         wp_spacing = self.terrainnav_settings.wp_spacing
         wp_num_total = 0
         wp_positions = []
@@ -1033,7 +1033,7 @@ def _wp_gen_simple_waypoints(self):
                     f"lat: {wp_lat:.6f}, lon: {wp_lon:.6f}, wp_alt_amsl: {wp_alt_amsl:.2f}, "
                     f"ter_alt_amsl: {ter_alt_amsl:.2f}, wp_alt_agl: {wp_alt_agl:.2f}"
                 )
-        
+
             if use_relative_alt:
                 wp_frame = mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT
                 wp_alt = wp_alt_amsl - home_alt_amsl
@@ -1068,6 +1068,8 @@ def _wp_gen_simple_waypoints(self):
             wp_num += 1
 
         if add_goal_loiter:
+            if self.is_debug:
+                print("[terrainnav] adding goal loiter")
             mission_item = self._wp_gen_goal_loiter(wp_num)
             if mission_item is not None:
                 mission_items.append(mission_item)
@@ -1110,30 +1112,33 @@ def _wp_gen_use_loiter_to_alt(self):
         mission_items = []
         wp_num = 0
 
-        # TODO: add wp generation settings to module
-        # MAV_FRAME_GLOBAL                amsl in WGS84 coordinate (WGS84 geoid)
-        # MAV_FRAME_GLOBAL_RELATIVE_ALT   relaive to HOME (WGS84 reference)
-        use_relative_alt = True
-        add_home = True
-        add_start_loiter = True
-        add_goal_loiter = True
+        use_relative_alt = self.terrainnav_settings.wp_use_relative_alt
+        add_home = self.terrainnav_settings.wp_add_home
+        add_start_loiter = self.terrainnav_settings.wp_add_start_loiter
+        add_goal_loiter = self.terrainnav_settings.wp_add_goal_loiter
 
         # TODO: check home.frame
         home_alt_amsl = home.z
 
         if add_home:
+            if self.is_debug:
+                print("[terrainnav] adding home")
             mission_item = self._wp_gen_home(wp_num, home)
             if mission_item is not None:
                 mission_items.append(mission_item)
                 wp_num += 1
 
         if add_start_loiter:
+            if self.is_debug:
+                print("[terrainnav] adding start loiter")
             mission_item = self._wp_gen_start_loiter(wp_num)
             if mission_item is not None:
                 mission_items.append(mission_item)
                 wp_num += 1
 
         # add path mission items
+        if self.is_debug:
+            print("[terrainnav] adding waypoints")
         for i, segment in enumerate(path._segments):
             position3 = segment.first_state().position
             tangent3 = (segment.first_state().velocity).normalized()
@@ -1242,6 +1247,8 @@ def _wp_gen_use_loiter_to_alt(self):
                 wp_num += 1
 
         if add_goal_loiter:
+            if self.is_debug:
+                print("[terrainnav] adding goal loiter")
             mission_item = self._wp_gen_goal_loiter(wp_num)
             if mission_item is not None:
                 mission_items.append(mission_item)

MAVProxy/modules/mavproxy_terrainnav/terrainnav_msgs.py

@@ -85,11 +85,6 @@ class MoveBoundary:
     pass
 
 
-@dataclass
-class ClearAll:
-    pass
-
-
 @dataclass
 class ClearPath:
     pass

MAVProxy/modules/mavproxy_terrainnav/terrainnav_ui.py

@@ -22,7 +22,9 @@ def __init__(self, state, title, size):
         self._vert_sizer4 = wx.StaticBoxSizer(wx.VERTICAL, self)
         self._vert_sizer5 = wx.StaticBoxSizer(wx.VERTICAL, self)
         self._vert_sizer6 = wx.StaticBoxSizer(wx.VERTICAL, self)
-        self._vert_sizer7 = wx.StaticBoxSizer(wx.VERTICAL, self)
+
+        self._vert_sizer_pad1 = wx.StaticBoxSizer(wx.VERTICAL, self)
+        self._vert_sizer_pad2 = wx.StaticBoxSizer(wx.VERTICAL, self)
 
         # buttons
         button_size = (120, 25)
@@ -41,23 +43,15 @@ def __init__(self, state, title, size):
         self._button_add_wp = wx.Button(
             location_box, id=wx.ID_ANY, label="Add Waypoint", size=button_size
         )
+
         # planning
         planning_box = self._vert_sizer2.GetStaticBox()
         self._button_run_planner = wx.Button(
             planning_box, id=wx.ID_ANY, label="Run Planner", size=button_size
         )
-        self._button_gen_waypoints = wx.Button(
-            planning_box, id=wx.ID_ANY, label="Gen Waypoints", size=button_size
-        )
         self._button_clear_path = wx.Button(
             planning_box, id=wx.ID_ANY, label="Clear Path", size=button_size
         )
-        self._button_clear_waypoints = wx.Button(
-            planning_box, id=wx.ID_ANY, label="Clear Waypoints", size=button_size
-        )
-        self._button_clear_all = wx.Button(
-            planning_box, id=wx.ID_ANY, label="Clear All", size=button_size
-        )
 
         # navigation actions
         navigation_box = self._vert_sizer3.GetStaticBox()
@@ -77,38 +71,45 @@ def __init__(self, state, title, size):
             navigation_box, id=wx.ID_ANY, label="Return", size=button_size
         )
 
-        # terrain visualisation
-        terrain_box = self._vert_sizer4.GetStaticBox()
+        # visualisation
+        visualisation_box = self._vert_sizer4.GetStaticBox()
         self._button_show_contours = wx.Button(
-            terrain_box, id=wx.ID_ANY, label="Show Contours", size=button_size
+            visualisation_box, id=wx.ID_ANY, label="Show Contours", size=button_size
         )
         self._button_hide_contours = wx.Button(
-            terrain_box, id=wx.ID_ANY, label="Hide Contours", size=button_size
+            visualisation_box, id=wx.ID_ANY, label="Hide Contours", size=button_size
         )
-
-        # planner boundary (map extents)
         self._button_show_boundary = wx.Button(
-            terrain_box, id=wx.ID_ANY, label="Show Boundary", size=button_size
+            visualisation_box, id=wx.ID_ANY, label="Show Boundary", size=button_size
         )
         self._button_hide_boundary = wx.Button(
-            terrain_box, id=wx.ID_ANY, label="Hide Boundary", size=button_size
+            visualisation_box, id=wx.ID_ANY, label="Hide Boundary", size=button_size
         )
         self._button_move_boundary = wx.Button(
-            terrain_box, id=wx.ID_ANY, label="Move Boundary", size=button_size
+            visualisation_box, id=wx.ID_ANY, label="Move Boundary", size=button_size
         )
 
-        # setting entry boxes
-        settings_box = self._vert_sizer5.GetStaticBox()
+        # waypoint generation
+        wp_gen_box = self._vert_sizer5.GetStaticBox()
+        self._button_gen_waypoints = wx.Button(
+            wp_gen_box, id=wx.ID_ANY, label="Gen Waypoints", size=button_size
+        )
+        self._button_clear_waypoints = wx.Button(
+            wp_gen_box, id=wx.ID_ANY, label="Clear Waypoints", size=button_size
+        )
+
+        # settings
+        settings_box = self._vert_sizer6.GetStaticBox()
         self._button_settings = wx.Button(
             settings_box, id=wx.ID_ANY, label="Settings", size=button_size
         )
 
         # main panel horiz padding
-        horz_pad_1_box = self._vert_sizer6.GetStaticBox()
-        self._panel_horz_pad_1 = wx.Panel(horz_pad_1_box, id=wx.ID_ANY)
+        horz_pad1_box = self._vert_sizer_pad1.GetStaticBox()
+        self._panel_horz_pad1 = wx.Panel(horz_pad1_box, id=wx.ID_ANY)
 
-        horz_pad_2_box = self._vert_sizer7.GetStaticBox()
-        self._panel_horz_pad_2 = wx.Panel(horz_pad_2_box, id=wx.ID_ANY)
+        horz_pad2_box = self._vert_sizer_pad2.GetStaticBox()
+        self._panel_horz_pad2 = wx.Panel(horz_pad2_box, id=wx.ID_ANY)
 
         # disabled buttons - for future use
         self._button_add_rally.Disable()
@@ -127,12 +128,7 @@ def __init__(self, state, title, size):
 
         # planning layout
         self._vert_sizer2.Add(self._button_run_planner, proportion=0, flag=wx.EXPAND)
-        self._vert_sizer2.Add(self._button_gen_waypoints, proportion=0, flag=wx.EXPAND)
         self._vert_sizer2.Add(self._button_clear_path, proportion=0, flag=wx.EXPAND)
-        self._vert_sizer2.Add(
-            self._button_clear_waypoints, proportion=0, flag=wx.EXPAND
-        )
-        self._vert_sizer2.Add(self._button_clear_all, proportion=0, flag=wx.EXPAND)
 
         # navigation layout
         self._vert_sizer3.Add(self._button_hold, proportion=0, flag=wx.EXPAND)
@@ -148,20 +144,27 @@ def __init__(self, state, title, size):
         self._vert_sizer4.Add(self._button_hide_boundary, proportion=0, flag=wx.EXPAND)
         self._vert_sizer4.Add(self._button_move_boundary, proportion=0, flag=wx.EXPAND)
 
+        # waypoint generation layout
+        self._vert_sizer5.Add(self._button_gen_waypoints, proportion=0, flag=wx.EXPAND)
+        self._vert_sizer5.Add(
+            self._button_clear_waypoints, proportion=0, flag=wx.EXPAND
+        )
+
         # settings layout
-        self._vert_sizer5.Add(self._button_settings, proportion=0, flag=wx.EXPAND)
+        self._vert_sizer6.Add(self._button_settings, proportion=0, flag=wx.EXPAND)
 
         # main panel
         self._horz_sizer1 = wx.BoxSizer(wx.HORIZONTAL)
         self._horz_sizer1.Add(self._vert_sizer1, proportion=0, flag=wx.EXPAND)
         self._horz_sizer1.Add(self._vert_sizer2, proportion=0, flag=wx.EXPAND)
         self._horz_sizer1.Add(self._vert_sizer3, proportion=0, flag=wx.EXPAND)
-        self._horz_sizer1.Add(self._vert_sizer6, proportion=1, flag=wx.EXPAND)
+        self._horz_sizer1.Add(self._vert_sizer_pad1, proportion=1, flag=wx.EXPAND)
 
         self._horz_sizer2 = wx.BoxSizer(wx.HORIZONTAL)
         self._horz_sizer2.Add(self._vert_sizer4, proportion=0, flag=wx.EXPAND)
         self._horz_sizer2.Add(self._vert_sizer5, proportion=0, flag=wx.EXPAND)
-        self._horz_sizer2.Add(self._vert_sizer7, proportion=1, flag=wx.EXPAND)
+        self._horz_sizer2.Add(self._vert_sizer6, proportion=0, flag=wx.EXPAND)
+        self._horz_sizer2.Add(self._vert_sizer_pad2, proportion=1, flag=wx.EXPAND)
 
         # stack rows of buttons
         self._vert_sizer0 = wx.BoxSizer(wx.VERTICAL)
@@ -176,14 +179,14 @@ def __init__(self, state, title, size):
         self.Bind(wx.EVT_BUTTON, self.on_add_wp_pushed, self._button_add_wp)
 
         self.Bind(wx.EVT_BUTTON, self.on_run_planner_pushed, self._button_run_planner)
+        self.Bind(wx.EVT_BUTTON, self.on_clear_path_pushed, self._button_clear_path)
+
         self.Bind(
             wx.EVT_BUTTON, self.on_gen_waypoints_pushed, self._button_gen_waypoints
         )
-        self.Bind(wx.EVT_BUTTON, self.on_clear_path_pushed, self._button_clear_path)
         self.Bind(
             wx.EVT_BUTTON, self.on_clear_waypoints_pushed, self._button_clear_waypoints
         )
-        self.Bind(wx.EVT_BUTTON, self.on_clear_all_pushed, self._button_clear_all)
 
         self.Bind(wx.EVT_BUTTON, self.on_hold_pushed, self._button_hold)
         self.Bind(wx.EVT_BUTTON, self.on_navigate_pushed, self._button_navigate)
@@ -320,10 +323,6 @@ def on_clear_waypoints_pushed(self, event):
         msg = terrainnav_msgs.ClearWaypoints()
         self._state.ui_pipe_send.send(msg)
 
-    def on_clear_all_pushed(self, event):
-        msg = terrainnav_msgs.ClearAll()
-        self._state.ui_pipe_send.send(msg)
-
     def on_settings_pushed(self, event):
         msg = terrainnav_msgs.Settings()
         self._state.ui_pipe_send.send(msg)