SITL Phase 6: BlueBoat waypoint mission with Ardu guidance

blueboat_gazebo/CMakeLists.txt

@@ -19,6 +19,7 @@ install(DIRECTORY
   launch
   worlds
   params
+  missions
   DESTINATION share/${PROJECT_NAME})
 
 # ros_gz bridge config is GENERATED from the vehicle config plus the
@@ -121,6 +122,8 @@ if(BUILD_TESTING)
     TIMEOUT 300)
   ament_add_pytest_test(test_model_generation test/test_model_generation.py
     TIMEOUT 180)
+  ament_add_pytest_test(test_mission_file test/test_mission_file.py
+    TIMEOUT 60)
   # NOCAPTURE so the sim output printed at teardown reaches the test logs
   # even when every assertion passes; the runtime behavior tests watch the
   # vehicle for several seconds each.

blueboat_gazebo/missions/lawnmower.waypoints

@@ -0,0 +1,8 @@
+QGC WPL 110
+0	1	0	16	0	0	0	0	-35.36326100	149.16523000	584.000000	1
+1	0	3	16	0	0	0	0	-35.36326100	149.16534016	0.000000	1
+2	0	3	16	0	0	0	0	-35.36321608	149.16534016	0.000000	1
+3	0	3	16	0	0	0	0	-35.36321608	149.16523000	0.000000	1
+4	0	3	16	0	0	0	0	-35.36317117	149.16523000	0.000000	1
+5	0	3	16	0	0	0	0	-35.36317117	149.16534016	0.000000	1
+6	0	3	16	0	0	0	0	-35.36326100	149.16523000	0.000000	1

blueboat_gazebo/params/blueboat_sitl.params

@@ -9,7 +9,7 @@
 # That file is 951 lines because it is a complete dump, most of which is one
 # unit's calibration, radio setup and logging. This file carries the subset
 # that describes the *vehicle* - frame, outputs, motor limits, control gains,
-# speeds - plus the deltas simulation requires. Every value below is the
+# speeds, waypoint navigation - plus the deltas simulation requires. Every value below is the
 # shipped one unless a DELTA comment says otherwise.
 #
 # Load it LAST. `-f rover-skid` makes sim_vehicle.py load ArduPilot's own rover.parm and rover-skid.parm first: the SITL side (accelerometer calibration so pre-arm passes, SIM_PIN_MASK, mode slots), but also SERVO1/3_MIN/MAX at 1000/2000 and SERVO1_FUNCTION 73 / SERVO3_FUNCTION 74, the reverse of the shipped boat. Every one of those is set again below, and the last file wins:
@@ -73,6 +73,28 @@ WP_RADIUS        1
 TURN_RADIUS      0.1
 PILOT_STEER_TYPE 3
 
+# ---- Waypoint navigation ---------------------------------------------------
+# Shipped verbatim. Only AUTO reaches these. Without the WP_PIVOT_* lines the
+# firmware default WP_PIVOT_ANGLE of 0 disables pivot turns, and the boat arcs
+# through every corner instead of stopping and turning on the spot as the
+# shipped vehicle does. WP_ACCEL and WP_JERK at 0 take their limits from
+# ATC_ACCEL_MAX. The PSC_* gains are the position controller the waypoint
+# navigator runs through.
+WP_PIVOT_ANGLE   45
+WP_PIVOT_RATE    15
+WP_PIVOT_DELAY   0
+WP_ACCEL         0
+WP_JERK          0
+MIS_DONE_BEHAVE  1          # Loiter at the last waypoint when the mission ends.
+PSC_POS_P        0.2
+PSC_VEL_P        1
+PSC_VEL_I        0
+PSC_VEL_D        0
+PSC_VEL_FF       0
+PSC_VEL_IMAX     1
+PSC_VEL_FLTE     5
+PSC_VEL_FLTD     5
+
 # ---- Control gains --------------------------------------------------------
 # Shipped verbatim. Whether these work unmodified against our thrust model is a question for the stabilization-layer work, so they are deliberately not retuned here.
 ATC_SPEED_P      1

blueboat_gazebo/test/test_mission_file.py

@@ -0,0 +1,73 @@
+# Copyright 2026 Honu Robotics
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+"""Static checks on the installed waypoint mission (no autopilot needed)."""
+
+import math
+from pathlib import Path
+
+from ament_index_python.packages import get_package_share_directory
+import pytest
+
+MISSION = (Path(get_package_share_directory('blueboat_gazebo'))
+           / 'missions' / 'lawnmower.waypoints')
+
+# The home the walkthrough pins with sim_vehicle.py --custom-location.
+HOME = (-35.363261, 149.165230)
+WP_RADIUS = 1.0          # m, blueboat_sitl.params
+WATER_HALF_SIZE = 100.0  # m, the 200 x 200 m water plane in blueboat_sitl.sdf
+MARGIN = 10.0            # m kept clear of the plane's edge
+MAV_CMD_NAV_WAYPOINT = 16
+
+
+def _items():
+    lines = MISSION.read_text().splitlines()
+    assert lines[0].strip() == 'QGC WPL 110'
+    return [line.split('\t') for line in lines[1:] if line.strip()]
+
+
+def _north_east(lat, lon):
+    """Metres north and east of HOME, flat-earth (fine at this scale)."""
+    r = 6378137.0
+    north = math.radians(lat - HOME[0]) * r
+    east = math.radians(lon - HOME[1]) * r * math.cos(math.radians(HOME[0]))
+    return north, east
+
+
+def test_items_are_well_formed():
+    items = _items()
+    assert len(items) == 7, 'home plus six waypoints'
+    for seq, item in enumerate(items):
+        assert len(item) == 12
+        assert int(item[0]) == seq
+        assert int(item[3]) == MAV_CMD_NAV_WAYPOINT
+
+
+def test_home_is_the_pinned_location():
+    home = _items()[0]
+    assert float(home[8]) == pytest.approx(HOME[0], abs=1e-7)
+    assert float(home[9]) == pytest.approx(HOME[1], abs=1e-7)
+
+
+def test_waypoints_stay_on_the_water():
+    for item in _items()[1:]:
+        north, east = _north_east(float(item[8]), float(item[9]))
+        assert abs(north) < WATER_HALF_SIZE - MARGIN
+        assert abs(east) < WATER_HALF_SIZE - MARGIN
+
+
+def test_consecutive_waypoints_are_distinct_targets():
+    points = [(0.0, 0.0)] + [_north_east(float(i[8]), float(i[9]))
+                             for i in _items()[1:]]
+    for a, b in zip(points, points[1:]):
+        assert math.dist(a, b) > 2 * WP_RADIUS