diff --git a/blueboat_gazebo/CMakeLists.txt b/blueboat_gazebo/CMakeLists.txt index e68df6d..4ddd988 100644 --- a/blueboat_gazebo/CMakeLists.txt +++ b/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. diff --git a/blueboat_gazebo/missions/lawnmower.waypoints b/blueboat_gazebo/missions/lawnmower.waypoints new file mode 100644 index 0000000..96b53cb --- /dev/null +++ b/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 diff --git a/blueboat_gazebo/params/blueboat_sitl.params b/blueboat_gazebo/params/blueboat_sitl.params index b8f2204..15af2fd 100644 --- a/blueboat_gazebo/params/blueboat_sitl.params +++ b/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 diff --git a/blueboat_gazebo/test/test_mission_file.py b/blueboat_gazebo/test/test_mission_file.py new file mode 100644 index 0000000..f97e366 --- /dev/null +++ b/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 diff --git a/docs/specs/SITL_SPEC.md b/docs/specs/SITL_SPEC.md index 66fcddf..1ac4728 100644 --- a/docs/specs/SITL_SPEC.md +++ b/docs/specs/SITL_SPEC.md @@ -20,31 +20,29 @@ Types of PRs: The unit of work and of review is a phase: one phase, one pull request. -Status values: **done** (merged), **in review**, **next**, **later**. - This spec is the only document that is current about the plan. [SITL_SPEC_CONTEXT.md](SITL_SPEC_CONTEXT.md) is the original static roadmap, kept for the reasoning behind these decisions. ## Phases -| Phase | Deliverable | Repository | Status | -|---|---|---|---| -| 0 | Toolchain and a setup document | `bluerobotics_models`, `drydock` | done — [#43](https://github.com/HonuRobotics/bluerobotics_models/pull/43) | -| 1 | BlueBoat under ArduRover, newtons interface | `bluerobotics_models` | done — [#44](https://github.com/HonuRobotics/bluerobotics_models/pull/44) | -| 2 | Normalized command mode on the thruster | `gz-maritime`, `bluerobotics_models` | done — [gz-maritime#18](https://github.com/HonuRobotics/gz-maritime/pull/18), [#53](https://github.com/HonuRobotics/bluerobotics_models/pull/53) | -| 2.999 | Pin the ArduPilot versions | `bluerobotics_models` | next | -| 3 | BlueROV2 under ArduSub | `bluerobotics_models` | later | -| 4 | Stabilization layer, boat | `bluerobotics_models` | later | -| 5 | Stabilization layer, ROV | `bluerobotics_models` | in review | -| 6 | Waypoint mission, boat only | `bluerobotics_models` | later | -| 7 | Waves, boat only | `bluerobotics_models` | later | -| 8 | Documentation and CI | `bluerobotics_models` | later | -| 9 | Upstream the thruster mode | `gz-maritime`, then `gz-sim` | later | +| Phase | Deliverable | Repository | +|---|---|---| +| 0 | Toolchain and a setup document | `bluerobotics_models`, `drydock` | +| 1 | BlueBoat under ArduRover, newtons interface | `bluerobotics_models` | +| 2 | Normalized command mode on the thruster | `gz-maritime`, `bluerobotics_models` | +| 2.999 | Pin the ArduPilot versions | `bluerobotics_models` | +| 3 | BlueROV2 under ArduSub | `bluerobotics_models` | +| 4 | Stabilization layer, boat | `bluerobotics_models` | +| 5 | Stabilization layer, ROV | `bluerobotics_models` | +| 6 | Waypoint mission, boat only | `bluerobotics_models` | +| 7 | ~~Waves, boat only~~ | — | +| 8 | Documentation and CI | `bluerobotics_models` | +| 9 | Upstream the thruster mode | `gz-maritime`, then `gz-sim` | Each phase states an objective and how it is verified met. The objective is the clarification of intent — what this phase is for and what it deliberately leaves to a later one — and the verification is the checks actually run to decide it is done, automated where they can be and by hand where they cannot. From phase 4 on, each phase states its verification in two parts. **Automated** is what CI and an agent can run unattended: which existing tests become load-bearing for the change, and which new ones the phase writes. **Human** is the instruction to a PR reviewer, and it is usually "follow this page and confirm what it says will happen" — which only works if the page states the development environment, the exact run steps and what a correct result looks like. A phase whose human verification cannot be written as a walkthrough has a documentation gap to close before it is done. -Both vehicles are in scope up to and including phase 5. Phases 0 to 2 were BlueBoat-first because the BlueBoat is the simpler platform and had the published parameter sets to be faithful to; phase 3 brings the BlueROV2 up to the same line, then phase 4 does the stabilization layer for the boat and phase 5 repeats it for the ROV. Phases 6 and 7 are the boat alone, for reasons each states. Where a phase claims both vehicles, a walkthrough that covers only one is an incomplete phase. +Both vehicles are in scope up to and including phase 5. Phases 0 to 2 were BlueBoat-first because the BlueBoat is the simpler platform and had the published parameter sets to be faithful to; phase 3 brings the BlueROV2 up to the same line, then phase 4 does the stabilization layer for the boat and phase 5 repeats it for the ROV. Phase 6 is the boat alone, for the reasons it states. Where a phase claims both vehicles, a walkthrough that covers only one is an incomplete phase. ### Prerequisites, assumptions and contraints @@ -211,52 +209,37 @@ The reviewer follows the walkthroughs: the open-loop spot check on the parameter ### Phase 6 — Waypoint mission, boat only -**Objective:** demonstrate one standard waypoint mission on the BlueBoat, flown unattended in `AUTO` with the failsafes in the loop. This is the autopilot layer above the phase 4 stabilization layer. It is also the broader test of the transfer claim, because the parameters governing it were tuned on the water. +**Objective:** demonstrate one standard waypoint mission on the BlueBoat, flown unattended in `AUTO`. This is the autopilot layer above the phase 4 stabilization layer, kept to the minimum that shows it works: one small mission, completed, with the track judged against figures stated beforehand. It is also the broader test of the transfer claim, because the parameters governing it were tuned on the water. + +The inner loops are inherited unchanged: the waypoint controller produces a turn rate and a speed, and those go to the same `ATC_STR_RAT_*` and `ATC_SPEED_*` controllers phase 4 validated. One loop is new — `ATC_STR_ANG_P`, the heading controller, which a boat reaches on pivot turns at waypoints. That is the only control gain this phase puts under test for the first time, and it is worth naming so it does not arrive as a surprise. -The inner loops are inherited unchanged: the waypoint controller produces a turn rate and a speed, and those go to the same `ATC_STR_RAT_*` and `ATC_SPEED_*` controllers phase 4 validated. One loop is new — `ATC_STR_ANG_P`, the heading controller, which a boat reaches on arriving at a waypoint and on pivot turns between legs. That is the only control gain this phase puts under test for the first time, and it is worth naming so it does not arrive as a surprise. +- One mission, held in the repository rather than typed in per run: a small lawnmower — 10 m legs, 5 m steps, back to the start — in `blueboat_gazebo/missions/` and installed with the package, in the QGC WPL format both MAVProxy's `wp load` and QGroundControl read. +- A pinned SITL home, passed to `sim_vehicle.py` with `--custom-location`, because geodetic waypoints only land where intended against a fixed home. +- The waypoint navigation parameters from the factory dump: `WP_SPEED`, `WP_RADIUS`, `TURN_RADIUS`, and also `WP_PIVOT_*`, `WP_ACCEL`, `WP_JERK`, `MIS_DONE_BEHAVE` and the `PSC_*` position controller, which ArduRover's waypoint navigator runs through. Without the `WP_PIVOT_*` lines the firmware default disables pivot turns and the boat arcs through corners it would stop and turn at. -- One mission, held in the repository rather than typed in per run: a lawnmower survey pattern, in `blueboat_gazebo/missions/` and installed with the package, in the QGC WPL format both MAVProxy's `wp load` and QGroundControl read. -- `WP_SPEED`, `WP_RADIUS` and `TURN_RADIUS` from the factory parameters. +One mission, not a family of them. It is a fixed artifact so that the walkthrough, a later automated run and any regression all exercise the same waypoints — a mission that changes between runs is not a regression test. Anything beyond it is more mission rather than more evidence. -One mission, not a family of them. It is a fixed artifact so that the automated run, the walkthrough and any later regression all exercise the same waypoints — a mission that changes between runs is not a regression test. Anything beyond it is more mission rather than more evidence. +Failsafes (GCS loss, low battery, a failing arming check) and a headless mission test are deferred, not dropped. Both need a MAVLink client the workspace does not declare, and phase 4 made the same call for the same reason. The run used to write the walkthrough's figures was scripted with ArduPilot's own pymavlink and is the starting point for that test when it is picked up. **The BlueROV2 is not in this phase.** Geodetic waypoint navigation asks for something the vehicle does not have: underwater there is no GNSS, so a position fix has to come from DVL dead reckoning or an acoustic system, and the BlueROV2 as configured is not set up for that. It is not where ArduSub is strongest either, and it is not what the VRX application needs. The ROV's closed-loop behavior is fully exercised one layer down, in phase 5. If a guidance result disappoints, the first question is whether phase 4 is actually finished. Splitting the phases exists to make that question answerable: with the inner loops validated and the hydrodynamics identified separately, a bad track points at the navigation parameters rather than at everything at once. -Other closed-loop behaviors are out of scope, as listed below — `GUIDED`, `LOITER`, `HOLD` and station keeping, follow and line- or path-following modes. They are more capability on the same loops rather than a different thing to prove. - -#### Automated verification - -New: a headless run of the repository's mission file — upload it over MAVLink, fly it, and assert each waypoint is reached within a radius and the mission completes. Failsafes scripted the same way: trigger GCS loss, low battery and a failing arming check, and assert the response rather than the parameter. +Found in this phase, and an example of exactly that: pivots drift. A steering-only command does not cancel in surge on this model — the published forward/reverse thrust ratio is 2.0 and the shipped `MOT_THST_ASYM` compensates for 1.6 — so the boat moves forward at a few tenths of a metre per second while it pivots and starts each leg 1 to 2 m off the line. That is the phase 4 `MOT_THST_ASYM` cross-check showing up one layer higher. It is stated in the walkthrough as a result, and neither the thrust numbers nor the navigation gains are moved to hide it. -The test loads the same file the walkthrough tells a reviewer to load. This is the one place where the automated check and the human one are the same procedure, only unattended — which is the argument for the mission being a versioned artifact rather than a set of instructions. - -#### Human verification - -The reviewer loads the mission file from the installed package, then watches it run on the MAVProxy map or in QGroundControl, touching nothing. Checked against `CRUISE_SPEED` 1 m/s and `TURN_RADIUS` 0.1 m. Cross-track error and achieved turn radius are the quantities to judge against, and the page states the expected figures beforehand. - -Then each failsafe triggered deliberately, with the expected response stated in the page before the reviewer triggers it. - -### Phase 7 — Waves, boat only - -**Objective:** This is a regression test and demonstration for marketing and raising awareness. We'll repeat the tests of phases 4 and 6 within an active wavefield. Previous tests did not exercise the added wave enfironment effects. In theory this should work out of the box, but important to test end-to-end to identify regressions. This is only pertinent to the Boat. Testing the ROV in near surface waves is out of scope. - -Since this is a regression check rather than new capability, the work is mostly in the world and the run rather than in the vehicle: a wave world for the BlueBoat, and the phase 4 and 6 checks pointed at it. If something has to change in the boat to survive a sea state, then we fix. +Other closed-loop behaviors are out of scope, as listed below — `GUIDED`, `LOITER`, `HOLD` and station keeping, follow and line- or path-following modes. They are more capability on the same loops rather than a different thing to prove. #### Automated verification -Existing: `gz_waves` carries its own suite in `gz-maritime`, and `blueboat_gazebo`'s launch tests must keep passing once a wave world is an option rather than always flat water. - -New: phase 6's headless mission run, repeated in a wave world, asserting the same waypoints are reached and the mission completes — with widened bounds, stated and justified, rather than the flat-water ones. Plus a station-hold run asserting the pose stays bounded and the EKF's innovations stay inside a threshold, which is the check worth most here: the failure mode is slow divergence that a human watching for two minutes will not see. +New: a static check on the installed mission file in `blueboat_gazebo` — well-formed, home at the pinned location, every waypoint on the water, consecutive waypoints further apart than twice `WP_RADIUS`. No autopilot is involved. Existing: `blueboat_gazebo`'s tests keep passing with the waypoint parameters added. #### Human verification -The reviewer runs both wave providers, Gerstner and FFT, and watches the boat in the viewport — the buoyancy lag between the two is a visual finding before it is a numerical one, and it is the known issue this phase is most likely to surface. +The reviewer follows [Waypoint mission for BlueBoat](../vehicles/blueboat/mission-sitl-boat.md): load the mission file from the installed package, arm, `mode auto`, and watch it run on the MAVProxy map, touching nothing. SITL in lockstep with Gazebo is deterministic, so the page states the figures exactly: each waypoint's arrival time, peak and arrival cross-track error per leg, speed, total mission time and the hold at home. The check is that the reviewer sees those numbers and that track, and that the corner drift reads as the stated finding rather than a surprise. -Then phase 4's step responses and phase 6's survey mission re-run in a stated sea state, confirming the boat still completes them and degrades gracefully rather than diverging. The sea state and the widened bounds are numbers to pick before the run and write into the page, so the reviewer is checking a claim rather than forming an impression. +### Phase 7 — Removed -This phase also produces the material worth showing: a wave-field run is the demonstration, so capturing video or stills while the reviewer is there costs nothing extra. +This phase was waves, boat only: repeat phase 6's mission in a wave field. It is removed because the boat cannot respond to waves yet. Buoyancy in gz-maritime acts at the flat z = 0 plane and the waves are only drawn; coupling buoyancy to the wave surface is the planned `gz_waves_buoyancy` consumer, which does not exist yet. Building it is wave modeling, not SITL integration, so it is out of scope here (see below). The number is kept so that references to phases 8 and 9 stay valid. ### Phase 8 — Documentation and Announce @@ -312,7 +295,7 @@ Closed-loop behaviors beyond the phases above are out of scope: `GUIDED`, `LOITE Waypoint missions for the BlueROV2 are out of scope, for the reason phase 6 gives: geodetic navigation underwater needs a position source the vehicle is not configured for, and the ROV's closed-loop behavior is fully exercised one layer down in phase 5. -The BlueROV2 in waves is out of scope. Near-surface wave interaction for a submerged vehicle is its own modeling problem, and phase 7 is a regression check on the boat rather than new capability for either vehicle. +Waves are out of scope for both vehicles. Gazebo vehicles in this workspace float on the flat water level, and the wave field is only drawn; buoyancy that follows the waves is the planned `gz_waves_buoyancy` consumer in gz-maritime, its own wave-modeling effort with its own owner. A SITL run in a wave world today would exercise nothing the flat-water phases do not. For the BlueROV2, near-surface wave interaction for a submerged vehicle is a further modeling problem on top of that. The Holybro X500 and any other UAV is out of scope for this plan. It is a different vehicle class with a different autopilot binding, it lives in `holybro_models` rather than here, and nothing in these phases is blocked on it. Rebinding it from PX4 is its own effort with its own plan. diff --git a/docs/vehicles/blueboat/images/ardu_map_wps.png b/docs/vehicles/blueboat/images/ardu_map_wps.png new file mode 100644 index 0000000..65707b6 Binary files /dev/null and b/docs/vehicles/blueboat/images/ardu_map_wps.png differ diff --git a/docs/vehicles/blueboat/images/mav_console_EKF.png b/docs/vehicles/blueboat/images/mav_console_EKF.png new file mode 100644 index 0000000..2e182f4 Binary files /dev/null and b/docs/vehicles/blueboat/images/mav_console_EKF.png differ diff --git a/docs/vehicles/blueboat/images/mav_console_wp.png b/docs/vehicles/blueboat/images/mav_console_wp.png new file mode 100644 index 0000000..c3d76a7 Binary files /dev/null and b/docs/vehicles/blueboat/images/mav_console_wp.png differ diff --git a/docs/vehicles/blueboat/images/mission-boat-5win.png b/docs/vehicles/blueboat/images/mission-boat-5win.png new file mode 100644 index 0000000..076f298 Binary files /dev/null and b/docs/vehicles/blueboat/images/mission-boat-5win.png differ diff --git a/docs/vehicles/blueboat/images/mission_speed.png b/docs/vehicles/blueboat/images/mission_speed.png new file mode 100644 index 0000000..887f91c Binary files /dev/null and b/docs/vehicles/blueboat/images/mission_speed.png differ diff --git a/docs/vehicles/blueboat/images/mission_track.png b/docs/vehicles/blueboat/images/mission_track.png new file mode 100644 index 0000000..6eade65 Binary files /dev/null and b/docs/vehicles/blueboat/images/mission_track.png differ diff --git a/docs/vehicles/blueboat/images/mission_xtrack.png b/docs/vehicles/blueboat/images/mission_xtrack.png new file mode 100644 index 0000000..adb7d6c Binary files /dev/null and b/docs/vehicles/blueboat/images/mission_xtrack.png differ diff --git a/docs/vehicles/blueboat/index.md b/docs/vehicles/blueboat/index.md index c680003..44926da 100644 --- a/docs/vehicles/blueboat/index.md +++ b/docs/vehicles/blueboat/index.md @@ -14,6 +14,7 @@ configuration parameters verify-sitl-boat tune-sitl-boat +mission-sitl-boat visual_asset_commission ``` diff --git a/docs/vehicles/blueboat/mission-sitl-boat.md b/docs/vehicles/blueboat/mission-sitl-boat.md new file mode 100644 index 0000000..040d75e --- /dev/null +++ b/docs/vehicles/blueboat/mission-sitl-boat.md @@ -0,0 +1,136 @@ +# Waypoint mission for BlueBoat + +This walkthrough flies one waypoint mission on the simulated BlueBoat in ArduRover's `AUTO` mode. + +`AUTO` is the layer above the stabilization layer exercised in [Tuning of closed-loop response](tune-sitl-boat.md). The waypoint navigator produces a speed and a turn rate, and those go to the same `ATC_SPEED_*` and `ATC_STR_RAT_*` loops. Two things are new. One is the heading controller `ATC_STR_ANG_P`, which the boat reaches when it pivots at a waypoint. The other is the waypoint navigation parameters (`WP_*`, `WP_PIVOT_*`, `PSC_*`), all at Blue Robotics' shipped values. + +## Prerequisites + +* Setup and run the drydock + source development environment: [installation_drydock.md](../../getting-started/installation_drydock.md) +* [ArduPilot SITL setup](../../getting-started/ardupilot_setup.md); +* [Verify the SITL connection (Boat)](verify-sitl-boat.md) and [Tuning of closed-loop response](tune-sitl-boat.md) pass. If a mission does not run correctly, it is likely issues with the lower feedback control loops. + +## The mission + +The mission is a fixed file installed with the package, `blueboat_gazebo/missions/lawnmower.waypoints`, in the QGC WPL format that both MAVProxy and QGroundControl read. It is a small lawnmower pattern: 10 m legs east and west, 5 m steps north between them, then a diagonal back to the start. + +| Item | North [m] | East [m] | Leg into it | +|---|---|---|---| +| 0 (home) | 0 | 0 | | +| 1 | 0 | 10 | 10 m east, the direction the boat spawns facing | +| 2 | 5 | 10 | 5 m north, after a 90° pivot | +| 3 | 5 | 0 | 10 m west, after a 90° pivot | +| 4 | 10 | 0 | 5 m north, after a 90° pivot | +| 5 | 10 | 10 | 10 m east, after a 90° pivot | +| 6 | 0 | 0 | 14 m diagonal home, after a 135° pivot | + +Here is what the waypoints look like in the map. +![alt text](images/ardu_map_wps.png) + +The waypoints are geodetic (lat/lon/alt), so they make sense where intended against a fixed home location. The walkthrough sets this it with `--custom-location` to ArduPilot's default SITL location, written out so that a change upstream cannot move the mission. Gazebo's +X is east here (`gazeboXYZToNED` in the model), and the boat spawns with its bow along +X, so the first leg starts dead ahead. + +Every turn in this mission is sharper than `WP_PIVOT_ANGLE` (45°), so at each waypoint the boat stops and pivots at up to `WP_PIVOT_RATE` (15 deg/s) before setting off down the next leg. + +## Walkthrough + +All commands are issued within the drydock dev container. Two shells, as in the verification walkthrough. + +First, the simulation: + +```bash +source ~/maritime_ws/install/setup.bash +source ~/maritime_ws/thirdparty/setup-ardupilot.sh +gz sim -v3 -r $(ros2 pkg prefix --share blueboat_gazebo)/worlds/blueboat_sitl.sdf +``` + +Then the autopilot, with the map and the pinned home: + +```bash +source ~/maritime_ws/install/setup.bash +source ~/maritime_ws/thirdparty/setup-ardupilot.sh +AP=$HOME/maritime_ws/thirdparty/ardupilot/Tools/autotest +sim_vehicle.py -v Rover -f rover-skid --model JSON --console --map -w \ + --custom-location=-35.363261,149.165230,584,90 \ + --add-param-file=$AP/default_params/rover.parm \ + --add-param-file=$(ros2 pkg prefix --share blueboat_gazebo)/params/blueboat_sitl.params +``` + +Wait until the console reports `EKF3 IMU0 is using GPS`, about 40 s after start. If you try to send a mission earlier is refused. + +![alt text](images/mav_console_EKF.png) + +### Load the mission + +At the MAVProxy prompt (note, you will need to adjust the absolute path to your repo location!): + +``` +wp load /home/bsb/maritime_ws/install/share/blueboat_gazebo/missions/lawnmower.waypoints +wp list +``` +Expect `Loaded 7 waypoints`, then `Flight plan received` in the console, and the six legs drawn on the map. + +![alt text](images/mav_console_wp.png) + + +### Graph the navigation + +``` +module load graph +graph NAV_CONTROLLER_OUTPUT.xtrack_error +graph VFR_HUD.groundspeed +``` + +### Fly it + +``` +arm throttle +mode auto +``` + +Then touch nothing. The console reports `Mission: 1 WP`, then `Reached waypoint #1` through `#6`, then `Mission Complete`. The boat then loiters at home (`MIS_DONE_BEHAVE 1`). To stop, `mode manual` and `disarm`. + +When running, you should see these windows. +![alt text](images/mission-boat-5win.png) + +**Video** of the full mission is here: [YouTube](https://youtu.be/kcPEiLzBE0k) + +## What a correct run looks like + +SITL steps in lockstep with Gazebo, so the run is deterministic. Repeated runs here agreed to 0.01 m and 0.1 s, and a reviewer should see the same figures, not just similar ones. Times are from `mode auto`. + +| Leg | Reached at [s] | Leg time [s] | Peak \|xtrack\| [m] | \|xtrack\| at arrival [m] | +|---|---|---|---|---| +| home → WP1 | 13.9 | 13.9 | 0.01 | 0.00 | +| WP1 → WP2 | 28.8 | 14.9 | 1.2 | 0.25 | +| WP2 → WP3 | 47.3 | 18.5 | 1.6 | 0.09 | +| WP3 → WP4 | 63.2 | 15.9 | 1.5 | 0.26 | +| WP4 → WP5 | 81.5 | 18.3 | 1.7 | 0.10 | +| WP5 → WP6 | 108.0 | 26.5 | 2.3 | 0.06 | + +* **Speed.** 1.0 m/s on the first leg, which is the target `WP_SPEED`. dropping to about 0 at each waypoint for the pivot, then 0.8–0.95 m/s on the later legs before slowing for the next waypoint. +* **Mission time.** 108 s, all six waypoints reached, `Mission Complete`. +* **End.** The boat holds within 0.4 m of home - loitering + +The plots below visualize the data. The graphs are illustrative; the project does not contain the post-processing source. + +![Planned legs and flown track](images/mission_track.png) + +![Cross-track error through the mission](images/mission_xtrack.png) + +![Speed through the mission](images/mission_speed.png) + +### Assessment + +The controller stack can obviously be optimized - as it always can. However, that is not the point. The goal is a "digital cousin", simulated closed-loop dyanmics that mimic the actual vessel with the SILT Ardu controller and guidance running. Because the closed-loop behavior is similar that of the hardware, and SITL uses that real-time code and parameter set, we have reasoanble confidnece that any modifications to the controller in sim, can be evaluted in sim and then seemlessly transitioned to hardware. + +## Troubleshooting + +| Observation | What it means | +|---|---| +| `Only 0 items are supported` on `wp load` | sent before ArduPilot was ready; wait for `is using GPS` and load again | +| `No Mission. Can't set AUTO.` | the mission did not load; repeat `wp load` and check `wp list` | +| the boat turns before the first leg | the boat is not spawned facing east, or `--custom-location` was dropped; check the command | +| the boat circles a waypoint and never reaches it | it cannot get inside `WP_RADIUS` (1 m); check the turn-rate loop in [Tuning](tune-sitl-boat.md) | +| the boat arcs through corners at speed instead of stopping | the `WP_PIVOT_*` lines are missing from the parameter file; the firmware default disables pivot turns | +| large, steady cross-track on a leg | the speed or heading loop; re-run [Tuning](tune-sitl-boat.md) before suspecting the navigator | +| times or errors differ from the table | something in the vehicle, the parameters or the firmware version changed; the run is deterministic, so a difference is a change, not noise | diff --git a/docs/vehicles/blueboat/parameters.md b/docs/vehicles/blueboat/parameters.md index 4079395..4bc7535 100644 --- a/docs/vehicles/blueboat/parameters.md +++ b/docs/vehicles/blueboat/parameters.md @@ -109,7 +109,7 @@ rc 1 1900 This section is about the *autopilot's* configuration. -The file is `blueboat_gazebo/params/blueboat_sitl.params`. It is derived from Blue Robotics' dump off a vehicle, `params/ardupilot/ArduRover/4.7/Navigator/BlueBoat120.params` in [Blueos-Parameter-Repository](https://github.com/bluerobotics/Blueos-Parameter-Repository), whose header reads *Vehicle: Surface Boat / Platform: navigator / Version: 4.7.0-BETA*. That dump is 951 lines, most of it one unit's accelerometer calibration, radio setup and logging. Ours carries only the subset that describes the vehicle — frame, outputs, motor limits, control gains, speeds — plus the deltas simulation requires. Every value is the shipped one unless a `DELTA` comment beside it says otherwise. +The file is `blueboat_gazebo/params/blueboat_sitl.params`. It is derived from Blue Robotics' dump off a vehicle, `params/ardupilot/ArduRover/4.7/Navigator/BlueBoat120.params` in [Blueos-Parameter-Repository](https://github.com/bluerobotics/Blueos-Parameter-Repository), whose header reads *Vehicle: Surface Boat / Platform: navigator / Version: 4.7.0-BETA*. That dump is 951 lines, most of it one unit's accelerometer calibration, radio setup and logging. Ours carries only the subset that describes the vehicle — frame, outputs, motor limits, control gains, speeds, waypoint navigation — plus the deltas simulation requires. Every value is the shipped one unless a `DELTA` comment beside it says otherwise. It is loaded last. `-f rover-skid` makes `sim_vehicle.py` load ArduPilot's own `rover.parm` and `rover-skid.parm` first, which supply the SITL side (fake accelerometer calibration so pre-arm passes, `SIM_PIN_MASK`, the mode slots) but also set `SERVO1/3_MIN/MAX` to 1000/2000 and `SERVO1_FUNCTION 73` / `SERVO3_FUNCTION 74` — the reverse of the shipped boat. Our file sets each of those again, and the last file wins.