From 9f094230bb628c490e4530a9b62f61c836963e3f Mon Sep 17 00:00:00 2001 From: monkescripts Date: Mon, 22 Jun 2026 22:33:02 +0800 Subject: [PATCH 1/8] Add BlueROV square mission and PX4 offboard control demo - Implemented a new Python script for BlueROV2 to execute a square mission using body-frame offsets. - Created C++ classes for PX4 offboard control, including handling vehicle commands and trajectory setpoints. - Added header file for PX4 offboard demo with mission phases and telemetry subscriptions. - Introduced a bash script to run the multivehicle examples with NVIDIA and X11 support. - Configured a tmuxp session for debugging the multivehicle examples, including launching the simulator and various missions. --- README.md | 157 ++------ bluerov/README.md | 145 ++++++++ .../bluerov_bin_mission.yaml | 0 .../bluerov_mission.yaml | 0 .../bluerov_torpedo_mission.yaml | 0 bluerov_ws.repos => bluerov/bluerov_ws.repos | 0 build.bash => bluerov/build.bash | 0 {docker => bluerov/docker}/Dockerfile | 0 {docs => bluerov/docs}/architecture.md | 0 .../bluerov_tasks/bluerov_tasks/__init__.py | 0 .../bluerov_tasks/arm_and_set_mode.py | 0 .../bluerov_tasks/bins/__init__.py | 0 .../bluerov_tasks/bluerov_tasks/bins/bins.py | 0 .../bluerov_tasks/bins/template_selector.py | 0 .../bluerov_tasks/bluerov_tasks/goto.py | 0 .../bluerov_tasks/node_registry.py | 0 .../bluerov_tasks/shared_trees/__init__.py | 0 .../bluerov_tasks/shared_trees/choice.py | 0 .../shared_trees/cluster_decision.py | 0 .../bluerov_tasks/torpedo/__init__.py | 0 .../torpedo/move_and_shoot_seq.py | 0 .../bluerov_tasks/torpedo/torpedo.py | 0 .../config/bluerov_single_tfs.yaml | 0 .../config/vision_pipeline/bin.yaml | 0 .../config/vision_pipeline/torpedo.yaml | 0 .../launch/bluerov_bin_bt.launch.py | 0 .../launch/bluerov_bin_vision.launch.py | 0 .../launch/bluerov_cluster.launch.py | 0 .../launch/bluerov_controls.launch.py | 0 .../launch/bluerov_mission.launch.py | 0 .../launch/bluerov_square_bt.launch.py | 0 .../launch/bluerov_tfs.launch.py | 0 .../launch/bluerov_torpedo_bt.launch.py | 0 .../launch/bluerov_torpedo_vision.launch.py | 0 .../packages}/bluerov_tasks/package.xml | 0 .../bluerov_tasks/resource/bluerov_tasks | 0 .../bluerov_tasks/scripts/actuators_sim.py | 0 .../scripts/bluerov_bin_mission_tree.py | 0 .../bluerov_tasks/scripts/bluerov_movement.py | 0 .../scripts/bluerov_square_mission_tree.py | 0 .../scripts/bluerov_torpedo_mission_tree.py | 0 .../scripts/choice_server_node.py | 0 .../scripts/locomotion_action_server.py | 0 .../packages}/bluerov_tasks/setup.cfg | 0 .../packages}/bluerov_tasks/setup.py | 0 run.bash => bluerov/run.bash | 2 +- multivehicle/README.md | 61 +++ multivehicle/build.bash | 27 ++ multivehicle/docker/Dockerfile | 16 + multivehicle/examples.repos | 38 ++ .../multivehicle_examples/CMakeLists.txt | 38 ++ .../config/blueboat_control.yaml | 44 +++ .../launch/bluerov_mission.launch.py | 29 ++ .../launch/boat_control.launch.py | 115 ++++++ .../launch/px4_offboard.launch.py | 25 ++ .../multivehicle_examples/package.xml | 44 +++ .../scripts/blueboat_mission.py | 80 ++++ .../scripts/blueboat_thrust_mixer.py | 152 ++++++++ .../scripts/blueboat_waypoint_controller.py | 352 ++++++++++++++++++ .../scripts/bluerov_movement.py | 270 ++++++++++++++ .../src/PX4OffboardDemo.cpp | 227 +++++++++++ .../src/PX4OffboardDemo.hpp | 102 +++++ multivehicle/run.bash | 24 ++ multivehicle/tmuxp/mvsim_debug.yaml | 113 ++++++ 64 files changed, 1924 insertions(+), 137 deletions(-) create mode 100644 bluerov/README.md rename bluerov_bin_mission.yaml => bluerov/bluerov_bin_mission.yaml (100%) rename bluerov_mission.yaml => bluerov/bluerov_mission.yaml (100%) rename bluerov_torpedo_mission.yaml => bluerov/bluerov_torpedo_mission.yaml (100%) rename bluerov_ws.repos => bluerov/bluerov_ws.repos (100%) rename build.bash => bluerov/build.bash (100%) rename {docker => bluerov/docker}/Dockerfile (100%) rename {docs => bluerov/docs}/architecture.md (100%) rename {packages => bluerov/packages}/bluerov_tasks/bluerov_tasks/__init__.py (100%) rename {packages => bluerov/packages}/bluerov_tasks/bluerov_tasks/arm_and_set_mode.py (100%) rename {packages => bluerov/packages}/bluerov_tasks/bluerov_tasks/bins/__init__.py (100%) rename {packages => bluerov/packages}/bluerov_tasks/bluerov_tasks/bins/bins.py (100%) rename {packages => bluerov/packages}/bluerov_tasks/bluerov_tasks/bins/template_selector.py (100%) rename {packages => bluerov/packages}/bluerov_tasks/bluerov_tasks/goto.py (100%) rename {packages => bluerov/packages}/bluerov_tasks/bluerov_tasks/node_registry.py (100%) rename {packages => bluerov/packages}/bluerov_tasks/bluerov_tasks/shared_trees/__init__.py (100%) rename {packages => bluerov/packages}/bluerov_tasks/bluerov_tasks/shared_trees/choice.py (100%) rename {packages => bluerov/packages}/bluerov_tasks/bluerov_tasks/shared_trees/cluster_decision.py (100%) rename {packages => bluerov/packages}/bluerov_tasks/bluerov_tasks/torpedo/__init__.py (100%) rename {packages => bluerov/packages}/bluerov_tasks/bluerov_tasks/torpedo/move_and_shoot_seq.py (100%) rename {packages => bluerov/packages}/bluerov_tasks/bluerov_tasks/torpedo/torpedo.py (100%) rename {packages => bluerov/packages}/bluerov_tasks/config/bluerov_single_tfs.yaml (100%) rename {packages => bluerov/packages}/bluerov_tasks/config/vision_pipeline/bin.yaml (100%) rename {packages => bluerov/packages}/bluerov_tasks/config/vision_pipeline/torpedo.yaml (100%) rename {packages => bluerov/packages}/bluerov_tasks/launch/bluerov_bin_bt.launch.py (100%) rename {packages => bluerov/packages}/bluerov_tasks/launch/bluerov_bin_vision.launch.py (100%) rename {packages => bluerov/packages}/bluerov_tasks/launch/bluerov_cluster.launch.py (100%) rename {packages => bluerov/packages}/bluerov_tasks/launch/bluerov_controls.launch.py (100%) rename {packages => bluerov/packages}/bluerov_tasks/launch/bluerov_mission.launch.py (100%) rename {packages => bluerov/packages}/bluerov_tasks/launch/bluerov_square_bt.launch.py (100%) rename {packages => bluerov/packages}/bluerov_tasks/launch/bluerov_tfs.launch.py (100%) rename {packages => bluerov/packages}/bluerov_tasks/launch/bluerov_torpedo_bt.launch.py (100%) rename {packages => bluerov/packages}/bluerov_tasks/launch/bluerov_torpedo_vision.launch.py (100%) rename {packages => bluerov/packages}/bluerov_tasks/package.xml (100%) rename {packages => bluerov/packages}/bluerov_tasks/resource/bluerov_tasks (100%) rename {packages => bluerov/packages}/bluerov_tasks/scripts/actuators_sim.py (100%) rename {packages => bluerov/packages}/bluerov_tasks/scripts/bluerov_bin_mission_tree.py (100%) rename {packages => bluerov/packages}/bluerov_tasks/scripts/bluerov_movement.py (100%) rename {packages => bluerov/packages}/bluerov_tasks/scripts/bluerov_square_mission_tree.py (100%) rename {packages => bluerov/packages}/bluerov_tasks/scripts/bluerov_torpedo_mission_tree.py (100%) rename {packages => bluerov/packages}/bluerov_tasks/scripts/choice_server_node.py (100%) rename {packages => bluerov/packages}/bluerov_tasks/scripts/locomotion_action_server.py (100%) rename {packages => bluerov/packages}/bluerov_tasks/setup.cfg (100%) rename {packages => bluerov/packages}/bluerov_tasks/setup.py (100%) rename run.bash => bluerov/run.bash (89%) create mode 100644 multivehicle/README.md create mode 100644 multivehicle/build.bash create mode 100644 multivehicle/docker/Dockerfile create mode 100644 multivehicle/examples.repos create mode 100644 multivehicle/packages/multivehicle_examples/CMakeLists.txt create mode 100644 multivehicle/packages/multivehicle_examples/config/blueboat_control.yaml create mode 100644 multivehicle/packages/multivehicle_examples/launch/bluerov_mission.launch.py create mode 100644 multivehicle/packages/multivehicle_examples/launch/boat_control.launch.py create mode 100644 multivehicle/packages/multivehicle_examples/launch/px4_offboard.launch.py create mode 100644 multivehicle/packages/multivehicle_examples/package.xml create mode 100755 multivehicle/packages/multivehicle_examples/scripts/blueboat_mission.py create mode 100755 multivehicle/packages/multivehicle_examples/scripts/blueboat_thrust_mixer.py create mode 100755 multivehicle/packages/multivehicle_examples/scripts/blueboat_waypoint_controller.py create mode 100755 multivehicle/packages/multivehicle_examples/scripts/bluerov_movement.py create mode 100644 multivehicle/packages/multivehicle_examples/src/PX4OffboardDemo.cpp create mode 100644 multivehicle/packages/multivehicle_examples/src/PX4OffboardDemo.hpp create mode 100644 multivehicle/run.bash create mode 100644 multivehicle/tmuxp/mvsim_debug.yaml diff --git a/README.md b/README.md index 00dfc03..36c910e 100644 --- a/README.md +++ b/README.md @@ -1,145 +1,30 @@ -# Examples +# examples -## BlueROV with ArduSub +Example applications for Bumblebee simulators, organized one subdirectory per +project. Each subdir is a self-contained sub-workspace (its own `packages/`, +`build.bash` / `run.bash`, `docker/`, and `*.repos` manifest): -### Quickstart +| Directory | Simulator base | Contents | +|-----------|----------------|----------| +| [`bluerov/`](bluerov/) | `ardusub_sim:humble` | BlueROV2 / ArduSub missions, behaviour trees, perception (`bluerov_tasks`) | +| [`multivehicle/`](multivehicle/) | `multivehicle_sim:humble` | Multivehicle missions, BlueBoat control, PX4 offboard demo (`multivehicle_examples`) | -We assume the ROS workspace is `~/workspaces/bluerov_ws`. Change the paths accordingly if needed. +Pick the project you want and follow its README. -Clone the repositories: +## Building -```bash -cd ~/workspaces/bluerov_ws/src -vcs import --recursive < examples/bluerov_ws.repos -``` - -> **Note:** `ml_models` is hosted on Hugging Face. -> Install [Git LFS](https://git-lfs.com/) **before** importing. - -Build the minimal sim image first: - -```bash -cd ~/workspaces/bluerov_ws/src/ardusub_sim -./build.bash -``` - -Build the examples image with perception and mission-tree dependencies: - -```bash -cd ~/workspaces/bluerov_ws/src/examples -./build.bash -``` - -### Start the container - -#### Native Ubuntu with NVIDIA - -Install [Rocker](https://github.com/osrf/rocker), then run: +`colcon build` discovers `package.xml` files at any depth, so the per-project +nesting (`/packages/...`) is found automatically. Because both projects' +packages live in the same repo but each pulls only its **own** dependencies (via +its `*.repos`), build the project you set up rather than the whole tree, e.g.: ```bash -cd ~/workspaces/bluerov_ws/src/examples -./run.bash bluerov_ws:humble +# multivehicle: +colcon build --packages-up-to multivehicle_examples +# bluerov: +colcon build --packages-up-to bluerov_tasks ``` -#### WSL 2 with WSLg - -```bash -docker run --rm -it \ - --gpus all \ - --device=/dev/dxg \ - --network=host \ - --ipc=host \ - -e DISPLAY="$DISPLAY" \ - -e WAYLAND_DISPLAY="$WAYLAND_DISPLAY" \ - -e XDG_RUNTIME_DIR="$XDG_RUNTIME_DIR" \ - -e PULSE_SERVER="$PULSE_SERVER" \ - -e NVIDIA_DRIVER_CAPABILITIES=all \ - -e MESA_D3D12_DEFAULT_ADAPTER_NAME=NVIDIA \ - -e LD_LIBRARY_PATH=/usr/lib/wsl/lib \ - -v /tmp/.X11-unix:/tmp/.X11-unix:rw \ - -v /mnt/wslg:/mnt/wslg \ - -v /usr/lib/wsl:/usr/lib/wsl:ro \ - -v ~/workspaces/bluerov_ws:/root/HOST/bluerov_ws \ - bluerov_ws:humble -``` - -The WSL command exposes WSLg's X11/Wayland sockets and the `/dev/dxg` virtual -GPU device, allowing Gazebo to use hardware-accelerated rendering. - -### Verify GPU rendering - -Inside the container, before launching Gazebo: - -```bash -glxinfo -B | grep -E "OpenGL vendor|OpenGL renderer" -``` - -On native Linux, the renderer should identify the NVIDIA GPU. Under WSLg, it -should mention D3D12 and the GPU. It should not report `llvmpipe`, which is -software rendering. - -### Build the workspace - -Inside the container: - -```bash -cd /root/HOST/bluerov_ws -source /opt/ros/humble/setup.bash -colcon build --symlink-install \ - --packages-up-to bluerov_tasks bluerov_sim bb_worlds -source install/setup.bash -``` - -### Demo: Square Mission - -The square mission is a control smoke test: - -1. Arm and enter `GUIDED`. -2. Move 2 m forward. -3. Move 2 m left. -4. Move 2 m backward. -5. Move 2 m right. - -Inside the container: - -```bash -cd /root/HOST/bluerov_ws -tmuxp load src/examples/bluerov_mission.yaml -``` - -### Demo: Bin Mission - -https://github.com/user-attachments/assets/6c262df8-bac6-492a-aef1-9e8cfc30d8a8 - -```bash -tmuxp load src/examples/bluerov_bin_mission.yaml -``` - -### Demo: Torpedo Mission - -https://github.com/user-attachments/assets/9a9c25c5-637a-403a-b34d-4048f9afb5e0 - -```bash -tmuxp load src/examples/bluerov_torpedo_mission.yaml -``` -### Foxglove layouts - -Prebuilt Foxglove layouts for the Bin and Torpedo missions are available at -[BumblebeeAS/controlkitv3](https://github.com/BumblebeeAS/controlkitv3/tree/main/foxglove_layouts). -Import them into Foxglove Studio for a ready-made view of the relevant topics, -making it easier to visualize and debug each mission. - -## Useful Commands - -```bash -ros2 topic echo /bluerov/odom --once -ros2 topic echo /mavros/state --once -ros2 topic echo /mavros/local_position/pose --once -ros2 topic echo /bluerov/controls/_action/feedback --once -ros2 service call /mavros/cmd/arming mavros_msgs/srv/CommandBool "{value: true}" -ros2 service call /mavros/set_mode mavros_msgs/srv/SetMode "{custom_mode: 'GUIDED'}" -``` - -## Documentation - -- [Architecture and conventions](docs/architecture.md) +`--packages-up-to` builds the chosen package plus its dependencies and skips the +other project (whose deps you have not imported). Alternatively, drop a +`COLCON_IGNORE` file in the project subdir you are not building. diff --git a/bluerov/README.md b/bluerov/README.md new file mode 100644 index 0000000..00dfc03 --- /dev/null +++ b/bluerov/README.md @@ -0,0 +1,145 @@ +# Examples + +## BlueROV with ArduSub + +### Quickstart + +We assume the ROS workspace is `~/workspaces/bluerov_ws`. Change the paths accordingly if needed. + +Clone the repositories: + +```bash +cd ~/workspaces/bluerov_ws/src +vcs import --recursive < examples/bluerov_ws.repos +``` + +> **Note:** `ml_models` is hosted on Hugging Face. +> Install [Git LFS](https://git-lfs.com/) **before** importing. + +Build the minimal sim image first: + +```bash +cd ~/workspaces/bluerov_ws/src/ardusub_sim +./build.bash +``` + +Build the examples image with perception and mission-tree dependencies: + +```bash +cd ~/workspaces/bluerov_ws/src/examples +./build.bash +``` + +### Start the container + +#### Native Ubuntu with NVIDIA + +Install [Rocker](https://github.com/osrf/rocker), then run: + +```bash +cd ~/workspaces/bluerov_ws/src/examples +./run.bash bluerov_ws:humble +``` + +#### WSL 2 with WSLg + +```bash +docker run --rm -it \ + --gpus all \ + --device=/dev/dxg \ + --network=host \ + --ipc=host \ + -e DISPLAY="$DISPLAY" \ + -e WAYLAND_DISPLAY="$WAYLAND_DISPLAY" \ + -e XDG_RUNTIME_DIR="$XDG_RUNTIME_DIR" \ + -e PULSE_SERVER="$PULSE_SERVER" \ + -e NVIDIA_DRIVER_CAPABILITIES=all \ + -e MESA_D3D12_DEFAULT_ADAPTER_NAME=NVIDIA \ + -e LD_LIBRARY_PATH=/usr/lib/wsl/lib \ + -v /tmp/.X11-unix:/tmp/.X11-unix:rw \ + -v /mnt/wslg:/mnt/wslg \ + -v /usr/lib/wsl:/usr/lib/wsl:ro \ + -v ~/workspaces/bluerov_ws:/root/HOST/bluerov_ws \ + bluerov_ws:humble +``` + +The WSL command exposes WSLg's X11/Wayland sockets and the `/dev/dxg` virtual +GPU device, allowing Gazebo to use hardware-accelerated rendering. + +### Verify GPU rendering + +Inside the container, before launching Gazebo: + +```bash +glxinfo -B | grep -E "OpenGL vendor|OpenGL renderer" +``` + +On native Linux, the renderer should identify the NVIDIA GPU. Under WSLg, it +should mention D3D12 and the GPU. It should not report `llvmpipe`, which is +software rendering. + +### Build the workspace + +Inside the container: + +```bash +cd /root/HOST/bluerov_ws +source /opt/ros/humble/setup.bash +colcon build --symlink-install \ + --packages-up-to bluerov_tasks bluerov_sim bb_worlds +source install/setup.bash +``` + +### Demo: Square Mission + +The square mission is a control smoke test: + +1. Arm and enter `GUIDED`. +2. Move 2 m forward. +3. Move 2 m left. +4. Move 2 m backward. +5. Move 2 m right. + +Inside the container: + +```bash +cd /root/HOST/bluerov_ws +tmuxp load src/examples/bluerov_mission.yaml +``` + +### Demo: Bin Mission + +https://github.com/user-attachments/assets/6c262df8-bac6-492a-aef1-9e8cfc30d8a8 + +```bash +tmuxp load src/examples/bluerov_bin_mission.yaml +``` + +### Demo: Torpedo Mission + +https://github.com/user-attachments/assets/9a9c25c5-637a-403a-b34d-4048f9afb5e0 + +```bash +tmuxp load src/examples/bluerov_torpedo_mission.yaml +``` +### Foxglove layouts + +Prebuilt Foxglove layouts for the Bin and Torpedo missions are available at +[BumblebeeAS/controlkitv3](https://github.com/BumblebeeAS/controlkitv3/tree/main/foxglove_layouts). +Import them into Foxglove Studio for a ready-made view of the relevant topics, +making it easier to visualize and debug each mission. + +## Useful Commands + +```bash +ros2 topic echo /bluerov/odom --once +ros2 topic echo /mavros/state --once +ros2 topic echo /mavros/local_position/pose --once +ros2 topic echo /bluerov/controls/_action/feedback --once +ros2 service call /mavros/cmd/arming mavros_msgs/srv/CommandBool "{value: true}" +ros2 service call /mavros/set_mode mavros_msgs/srv/SetMode "{custom_mode: 'GUIDED'}" +``` + +## Documentation + +- [Architecture and conventions](docs/architecture.md) diff --git a/bluerov_bin_mission.yaml b/bluerov/bluerov_bin_mission.yaml similarity index 100% rename from bluerov_bin_mission.yaml rename to bluerov/bluerov_bin_mission.yaml diff --git a/bluerov_mission.yaml b/bluerov/bluerov_mission.yaml similarity index 100% rename from bluerov_mission.yaml rename to bluerov/bluerov_mission.yaml diff --git a/bluerov_torpedo_mission.yaml b/bluerov/bluerov_torpedo_mission.yaml similarity index 100% rename from bluerov_torpedo_mission.yaml rename to bluerov/bluerov_torpedo_mission.yaml diff --git a/bluerov_ws.repos b/bluerov/bluerov_ws.repos similarity index 100% rename from bluerov_ws.repos rename to bluerov/bluerov_ws.repos diff --git a/build.bash b/bluerov/build.bash similarity index 100% rename from build.bash rename to bluerov/build.bash diff --git a/docker/Dockerfile b/bluerov/docker/Dockerfile similarity index 100% rename from docker/Dockerfile rename to bluerov/docker/Dockerfile diff --git a/docs/architecture.md b/bluerov/docs/architecture.md similarity index 100% rename from docs/architecture.md rename to bluerov/docs/architecture.md diff --git a/packages/bluerov_tasks/bluerov_tasks/__init__.py b/bluerov/packages/bluerov_tasks/bluerov_tasks/__init__.py similarity index 100% rename from packages/bluerov_tasks/bluerov_tasks/__init__.py rename to bluerov/packages/bluerov_tasks/bluerov_tasks/__init__.py diff --git a/packages/bluerov_tasks/bluerov_tasks/arm_and_set_mode.py b/bluerov/packages/bluerov_tasks/bluerov_tasks/arm_and_set_mode.py similarity index 100% rename from packages/bluerov_tasks/bluerov_tasks/arm_and_set_mode.py rename to bluerov/packages/bluerov_tasks/bluerov_tasks/arm_and_set_mode.py diff --git a/packages/bluerov_tasks/bluerov_tasks/bins/__init__.py b/bluerov/packages/bluerov_tasks/bluerov_tasks/bins/__init__.py similarity index 100% rename from packages/bluerov_tasks/bluerov_tasks/bins/__init__.py rename to bluerov/packages/bluerov_tasks/bluerov_tasks/bins/__init__.py diff --git a/packages/bluerov_tasks/bluerov_tasks/bins/bins.py b/bluerov/packages/bluerov_tasks/bluerov_tasks/bins/bins.py similarity index 100% rename from packages/bluerov_tasks/bluerov_tasks/bins/bins.py rename to bluerov/packages/bluerov_tasks/bluerov_tasks/bins/bins.py diff --git a/packages/bluerov_tasks/bluerov_tasks/bins/template_selector.py b/bluerov/packages/bluerov_tasks/bluerov_tasks/bins/template_selector.py similarity index 100% rename from packages/bluerov_tasks/bluerov_tasks/bins/template_selector.py rename to bluerov/packages/bluerov_tasks/bluerov_tasks/bins/template_selector.py diff --git a/packages/bluerov_tasks/bluerov_tasks/goto.py b/bluerov/packages/bluerov_tasks/bluerov_tasks/goto.py similarity index 100% rename from packages/bluerov_tasks/bluerov_tasks/goto.py rename to bluerov/packages/bluerov_tasks/bluerov_tasks/goto.py diff --git a/packages/bluerov_tasks/bluerov_tasks/node_registry.py b/bluerov/packages/bluerov_tasks/bluerov_tasks/node_registry.py similarity index 100% rename from packages/bluerov_tasks/bluerov_tasks/node_registry.py rename to bluerov/packages/bluerov_tasks/bluerov_tasks/node_registry.py diff --git a/packages/bluerov_tasks/bluerov_tasks/shared_trees/__init__.py b/bluerov/packages/bluerov_tasks/bluerov_tasks/shared_trees/__init__.py similarity index 100% rename from packages/bluerov_tasks/bluerov_tasks/shared_trees/__init__.py rename to bluerov/packages/bluerov_tasks/bluerov_tasks/shared_trees/__init__.py diff --git a/packages/bluerov_tasks/bluerov_tasks/shared_trees/choice.py b/bluerov/packages/bluerov_tasks/bluerov_tasks/shared_trees/choice.py similarity index 100% rename from packages/bluerov_tasks/bluerov_tasks/shared_trees/choice.py rename to bluerov/packages/bluerov_tasks/bluerov_tasks/shared_trees/choice.py diff --git a/packages/bluerov_tasks/bluerov_tasks/shared_trees/cluster_decision.py b/bluerov/packages/bluerov_tasks/bluerov_tasks/shared_trees/cluster_decision.py similarity index 100% rename from packages/bluerov_tasks/bluerov_tasks/shared_trees/cluster_decision.py rename to bluerov/packages/bluerov_tasks/bluerov_tasks/shared_trees/cluster_decision.py diff --git a/packages/bluerov_tasks/bluerov_tasks/torpedo/__init__.py b/bluerov/packages/bluerov_tasks/bluerov_tasks/torpedo/__init__.py similarity index 100% rename from packages/bluerov_tasks/bluerov_tasks/torpedo/__init__.py rename to bluerov/packages/bluerov_tasks/bluerov_tasks/torpedo/__init__.py diff --git a/packages/bluerov_tasks/bluerov_tasks/torpedo/move_and_shoot_seq.py b/bluerov/packages/bluerov_tasks/bluerov_tasks/torpedo/move_and_shoot_seq.py similarity index 100% rename from packages/bluerov_tasks/bluerov_tasks/torpedo/move_and_shoot_seq.py rename to bluerov/packages/bluerov_tasks/bluerov_tasks/torpedo/move_and_shoot_seq.py diff --git a/packages/bluerov_tasks/bluerov_tasks/torpedo/torpedo.py b/bluerov/packages/bluerov_tasks/bluerov_tasks/torpedo/torpedo.py similarity index 100% rename from packages/bluerov_tasks/bluerov_tasks/torpedo/torpedo.py rename to bluerov/packages/bluerov_tasks/bluerov_tasks/torpedo/torpedo.py diff --git a/packages/bluerov_tasks/config/bluerov_single_tfs.yaml b/bluerov/packages/bluerov_tasks/config/bluerov_single_tfs.yaml similarity index 100% rename from packages/bluerov_tasks/config/bluerov_single_tfs.yaml rename to bluerov/packages/bluerov_tasks/config/bluerov_single_tfs.yaml diff --git a/packages/bluerov_tasks/config/vision_pipeline/bin.yaml b/bluerov/packages/bluerov_tasks/config/vision_pipeline/bin.yaml similarity index 100% rename from packages/bluerov_tasks/config/vision_pipeline/bin.yaml rename to bluerov/packages/bluerov_tasks/config/vision_pipeline/bin.yaml diff --git a/packages/bluerov_tasks/config/vision_pipeline/torpedo.yaml b/bluerov/packages/bluerov_tasks/config/vision_pipeline/torpedo.yaml similarity index 100% rename from packages/bluerov_tasks/config/vision_pipeline/torpedo.yaml rename to bluerov/packages/bluerov_tasks/config/vision_pipeline/torpedo.yaml diff --git a/packages/bluerov_tasks/launch/bluerov_bin_bt.launch.py b/bluerov/packages/bluerov_tasks/launch/bluerov_bin_bt.launch.py similarity index 100% rename from packages/bluerov_tasks/launch/bluerov_bin_bt.launch.py rename to bluerov/packages/bluerov_tasks/launch/bluerov_bin_bt.launch.py diff --git a/packages/bluerov_tasks/launch/bluerov_bin_vision.launch.py b/bluerov/packages/bluerov_tasks/launch/bluerov_bin_vision.launch.py similarity index 100% rename from packages/bluerov_tasks/launch/bluerov_bin_vision.launch.py rename to bluerov/packages/bluerov_tasks/launch/bluerov_bin_vision.launch.py diff --git a/packages/bluerov_tasks/launch/bluerov_cluster.launch.py b/bluerov/packages/bluerov_tasks/launch/bluerov_cluster.launch.py similarity index 100% rename from packages/bluerov_tasks/launch/bluerov_cluster.launch.py rename to bluerov/packages/bluerov_tasks/launch/bluerov_cluster.launch.py diff --git a/packages/bluerov_tasks/launch/bluerov_controls.launch.py b/bluerov/packages/bluerov_tasks/launch/bluerov_controls.launch.py similarity index 100% rename from packages/bluerov_tasks/launch/bluerov_controls.launch.py rename to bluerov/packages/bluerov_tasks/launch/bluerov_controls.launch.py diff --git a/packages/bluerov_tasks/launch/bluerov_mission.launch.py b/bluerov/packages/bluerov_tasks/launch/bluerov_mission.launch.py similarity index 100% rename from packages/bluerov_tasks/launch/bluerov_mission.launch.py rename to bluerov/packages/bluerov_tasks/launch/bluerov_mission.launch.py diff --git a/packages/bluerov_tasks/launch/bluerov_square_bt.launch.py b/bluerov/packages/bluerov_tasks/launch/bluerov_square_bt.launch.py similarity index 100% rename from packages/bluerov_tasks/launch/bluerov_square_bt.launch.py rename to bluerov/packages/bluerov_tasks/launch/bluerov_square_bt.launch.py diff --git a/packages/bluerov_tasks/launch/bluerov_tfs.launch.py b/bluerov/packages/bluerov_tasks/launch/bluerov_tfs.launch.py similarity index 100% rename from packages/bluerov_tasks/launch/bluerov_tfs.launch.py rename to bluerov/packages/bluerov_tasks/launch/bluerov_tfs.launch.py diff --git a/packages/bluerov_tasks/launch/bluerov_torpedo_bt.launch.py b/bluerov/packages/bluerov_tasks/launch/bluerov_torpedo_bt.launch.py similarity index 100% rename from packages/bluerov_tasks/launch/bluerov_torpedo_bt.launch.py rename to bluerov/packages/bluerov_tasks/launch/bluerov_torpedo_bt.launch.py diff --git a/packages/bluerov_tasks/launch/bluerov_torpedo_vision.launch.py b/bluerov/packages/bluerov_tasks/launch/bluerov_torpedo_vision.launch.py similarity index 100% rename from packages/bluerov_tasks/launch/bluerov_torpedo_vision.launch.py rename to bluerov/packages/bluerov_tasks/launch/bluerov_torpedo_vision.launch.py diff --git a/packages/bluerov_tasks/package.xml b/bluerov/packages/bluerov_tasks/package.xml similarity index 100% rename from packages/bluerov_tasks/package.xml rename to bluerov/packages/bluerov_tasks/package.xml diff --git a/packages/bluerov_tasks/resource/bluerov_tasks b/bluerov/packages/bluerov_tasks/resource/bluerov_tasks similarity index 100% rename from packages/bluerov_tasks/resource/bluerov_tasks rename to bluerov/packages/bluerov_tasks/resource/bluerov_tasks diff --git a/packages/bluerov_tasks/scripts/actuators_sim.py b/bluerov/packages/bluerov_tasks/scripts/actuators_sim.py similarity index 100% rename from packages/bluerov_tasks/scripts/actuators_sim.py rename to bluerov/packages/bluerov_tasks/scripts/actuators_sim.py diff --git a/packages/bluerov_tasks/scripts/bluerov_bin_mission_tree.py b/bluerov/packages/bluerov_tasks/scripts/bluerov_bin_mission_tree.py similarity index 100% rename from packages/bluerov_tasks/scripts/bluerov_bin_mission_tree.py rename to bluerov/packages/bluerov_tasks/scripts/bluerov_bin_mission_tree.py diff --git a/packages/bluerov_tasks/scripts/bluerov_movement.py b/bluerov/packages/bluerov_tasks/scripts/bluerov_movement.py similarity index 100% rename from packages/bluerov_tasks/scripts/bluerov_movement.py rename to bluerov/packages/bluerov_tasks/scripts/bluerov_movement.py diff --git a/packages/bluerov_tasks/scripts/bluerov_square_mission_tree.py b/bluerov/packages/bluerov_tasks/scripts/bluerov_square_mission_tree.py similarity index 100% rename from packages/bluerov_tasks/scripts/bluerov_square_mission_tree.py rename to bluerov/packages/bluerov_tasks/scripts/bluerov_square_mission_tree.py diff --git a/packages/bluerov_tasks/scripts/bluerov_torpedo_mission_tree.py b/bluerov/packages/bluerov_tasks/scripts/bluerov_torpedo_mission_tree.py similarity index 100% rename from packages/bluerov_tasks/scripts/bluerov_torpedo_mission_tree.py rename to bluerov/packages/bluerov_tasks/scripts/bluerov_torpedo_mission_tree.py diff --git a/packages/bluerov_tasks/scripts/choice_server_node.py b/bluerov/packages/bluerov_tasks/scripts/choice_server_node.py similarity index 100% rename from packages/bluerov_tasks/scripts/choice_server_node.py rename to bluerov/packages/bluerov_tasks/scripts/choice_server_node.py diff --git a/packages/bluerov_tasks/scripts/locomotion_action_server.py b/bluerov/packages/bluerov_tasks/scripts/locomotion_action_server.py similarity index 100% rename from packages/bluerov_tasks/scripts/locomotion_action_server.py rename to bluerov/packages/bluerov_tasks/scripts/locomotion_action_server.py diff --git a/packages/bluerov_tasks/setup.cfg b/bluerov/packages/bluerov_tasks/setup.cfg similarity index 100% rename from packages/bluerov_tasks/setup.cfg rename to bluerov/packages/bluerov_tasks/setup.cfg diff --git a/packages/bluerov_tasks/setup.py b/bluerov/packages/bluerov_tasks/setup.py similarity index 100% rename from packages/bluerov_tasks/setup.py rename to bluerov/packages/bluerov_tasks/setup.py diff --git a/run.bash b/bluerov/run.bash similarity index 89% rename from run.bash rename to bluerov/run.bash index 00014b6..b5c75a5 100755 --- a/run.bash +++ b/bluerov/run.bash @@ -8,7 +8,7 @@ set -e IMAGE_NAME="${1:-bluerov_ws:humble}" SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)" -WORKSPACE_DIR="$(cd "${SCRIPT_DIR}/../.." && pwd)" +WORKSPACE_DIR="$(cd "${SCRIPT_DIR}/../../.." && pwd)" rocker \ --devices /dev/dri \ diff --git a/multivehicle/README.md b/multivehicle/README.md new file mode 100644 index 0000000..860dde0 --- /dev/null +++ b/multivehicle/README.md @@ -0,0 +1,61 @@ +# multivehicle examples + +Example missions, controllers and demos that run **on top of** the +[`multivehicle_sim`](https://github.com/BumblebeeAS/multivehicle_sim) simulator. + +## Contents + +``` +multivehicle/ + build.bash run.bash # multivehicle_examples:humble, FROM multivehicle_sim:humble + examples.repos # full workspace manifest (sim + deps + px4_msgs) + docker/Dockerfile # adds nav2 (BlueBoat control nav2-prep) + tmuxp/mvsim_debug.yaml # full stack: sim + examples + dashboard + packages/ + multivehicle_examples/ # ament_cmake + launch/ bluerov_mission, boat_control, px4_offboard + scripts/ bluerov_movement, blueboat_thrust_mixer, blueboat_waypoint_controller, blueboat_mission + src/ PX4OffboardDemo.cpp/.hpp (px4_msgs + Eigen3) + config/ blueboat_control.yaml +``` + +What each launch adds on top of the sim: + +| Launch | Builds on | Adds | +|--------|-----------|------| +| `bluerov_mission.launch.py` | `multivehicle_sim bluerov.launch.py` | BlueROV2 square-dive mission | +| `boat_control.launch.py` | `multivehicle_sim boat.launch.py` | thrust mixer + LOS waypoint controller (+ optional `use_mission:=true`) | +| `px4_offboard.launch.py` | `multivehicle_sim` x500 + uXRCE agent | autonomous offboard flight (`px4_offboard_demo`) | + +## Setup + +```bash +# In ~/mvsim_ws (this repo cloned at src/examples): +vcs import src < src/examples/multivehicle/examples.repos --recursive + +# Build the base sim image first, then the examples image: +cd src/multivehicle_sim && ./build.bash +cd ../examples/multivehicle && ./build.bash +./run.bash multivehicle_examples:humble +``` + +Inside the container: + +```bash +cd /root/HOST/mvsim_ws +# Scope the build to this project: --packages-up-to pulls in multivehicle_examples +# and its deps (multivehicle_sim, multivehicle_interface, px4_msgs, bb_worlds, …) +# and skips the sibling bluerov_tasks, whose vision deps aren't imported here. +colcon build --symlink-install --packages-up-to multivehicle_examples +source install/setup.bash +tmuxp load src/examples/multivehicle/tmuxp/mvsim_debug.yaml +``` + +On a hybrid-GPU (NVIDIA) host, `export __NV_PRIME_RENDER_OFFLOAD=1` and +`export __GLX_VENDOR_LIBRARY_NAME=nvidia` before launching gz (or uncomment those +lines in the tmuxp's `shell_command_before`). + +## License + +MIT (see the repo-root `LICENSE`). The `px4_offboard_demo` is a first-party, +independent implementation of the standard PX4 offboard pattern. diff --git a/multivehicle/build.bash b/multivehicle/build.bash new file mode 100644 index 0000000..2918b19 --- /dev/null +++ b/multivehicle/build.bash @@ -0,0 +1,27 @@ +#!/usr/bin/env bash + +set -e + +# Builds the multivehicle_examples Docker image FROM multivehicle_sim:humble. +# Run from this directory (src/examples/multivehicle): ./build.bash +# +# SPDX-License-Identifier: MIT +image_name=multivehicle_examples +image_tag=humble + +if [ ! -f "docker/Dockerfile" ]; then + echo "Err: docker/Dockerfile not found. Run from src/examples/multivehicle." + exit 1 +fi + +if ! docker image inspect multivehicle_sim:humble >/dev/null 2>&1; then + echo "Err: base image multivehicle_sim:humble not found." + echo "Build it first from the multivehicle_sim repo:" + echo " cd ../../multivehicle_sim && ./build.bash" + exit 1 +fi + +image_plus_tag=$image_name:$(export LC_ALL=C; date +%Y_%m_%d_%H%M) +docker build --rm -t $image_plus_tag -f docker/Dockerfile docker && \ +docker tag $image_plus_tag $image_name:$image_tag && \ +echo "Built $image_plus_tag and tagged as $image_name:$image_tag" diff --git a/multivehicle/docker/Dockerfile b/multivehicle/docker/Dockerfile new file mode 100644 index 0000000..f5f1945 --- /dev/null +++ b/multivehicle/docker/Dockerfile @@ -0,0 +1,16 @@ +FROM multivehicle_sim:humble + +SHELL ["/bin/bash", "-o", "pipefail", "-o", "errexit", "-c"] +ENV DEBIAN_FRONTEND=noninteractive +ARG HOME_DIR=/root + +RUN apt-get update && apt-get install -y --no-install-recommends \ + ros-humble-navigation2 \ + ros-humble-nav2-bringup \ + ros-humble-nav2-util \ + ros-humble-nav2-common && \ + apt-get autoremove -y && \ + apt-get clean && \ + rm -rf /var/lib/apt/lists/* + +WORKDIR ${HOME_DIR} diff --git a/multivehicle/examples.repos b/multivehicle/examples.repos new file mode 100644 index 0000000..76e9f22 --- /dev/null +++ b/multivehicle/examples.repos @@ -0,0 +1,38 @@ +# examples.repos + +repositories: + multivehicle_sim: + type: git + url: https://github.com/BumblebeeAS/multivehicle_sim.git + + # --- everything multivehicle_sim's dependencies.repos pulls --- + ardupilot_gazebo: + type: git + url: https://github.com/ArduPilot/ardupilot_gazebo.git + Micro-XRCE-DDS-Agent: + type: git + url: https://github.com/eProsima/Micro-XRCE-DDS-Agent.git + version: v2.4.2 + depth: 1 + gz_led_plugin: + type: git + url: https://github.com/BumblebeeAS/bb_led_plugin.git + version: main + bb_robotx_dashboard: + type: git + url: https://github.com/BumblebeeAS/bb_robotx_dashboard.git + bb_worlds: + type: git + url: https://github.com/BumblebeeAS/bb_worlds.git + version: main + bb_msgs: + type: git + url: https://github.com/BumblebeeAS/bb_msgs.git + version: multivehicle_sim + + # --- examples-only: PX4 uORB message types for the offboard demo --- + px4_msgs: + type: git + url: https://github.com/PX4/px4_msgs.git + version: release/1.16 + depth: 1 diff --git a/multivehicle/packages/multivehicle_examples/CMakeLists.txt b/multivehicle/packages/multivehicle_examples/CMakeLists.txt new file mode 100644 index 0000000..6054135 --- /dev/null +++ b/multivehicle/packages/multivehicle_examples/CMakeLists.txt @@ -0,0 +1,38 @@ +cmake_minimum_required(VERSION 3.12.2) +project(multivehicle_examples) + +find_package(ament_cmake REQUIRED) +find_package(ament_cmake_python REQUIRED) +find_package(rclcpp REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(px4_msgs REQUIRED) +find_package(eigen3_cmake_module REQUIRED) +find_package(Eigen3 REQUIRED) + +include_directories( + src + ${EIGEN3_INCLUDE_DIR} +) + +add_executable(px4_offboard_demo src/PX4OffboardDemo.cpp) +ament_target_dependencies(px4_offboard_demo rclcpp geometry_msgs px4_msgs Eigen3) +target_compile_features(px4_offboard_demo PUBLIC c_std_99 cxx_std_17) +install(TARGETS px4_offboard_demo DESTINATION lib/${PROJECT_NAME}) + +install(PROGRAMS + scripts/bluerov_movement.py + scripts/blueboat_thrust_mixer.py + scripts/blueboat_waypoint_controller.py + scripts/blueboat_mission.py + DESTINATION lib/${PROJECT_NAME} +) + +install(DIRECTORY launch config + DESTINATION share/${PROJECT_NAME}) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() +endif() + +ament_package() diff --git a/multivehicle/packages/multivehicle_examples/config/blueboat_control.yaml b/multivehicle/packages/multivehicle_examples/config/blueboat_control.yaml new file mode 100644 index 0000000..689b053 --- /dev/null +++ b/multivehicle/packages/multivehicle_examples/config/blueboat_control.yaml @@ -0,0 +1,44 @@ +# BlueBoat USV control-stack parameters (mixer + LOS waypoint controller). +# Single source of truth for tuning -- edit and relaunch (or `ros2 param set` +# live). See the plan's "PID tuning" section for the recommended order: +# 1) surge (straight line) 2) heading (90 deg steps at zero speed) 3) guidance. + +blueboat_thrust_mixer: + ros__parameters: + track_width: 0.59 # m, port<->stbd thruster spacing (from model.sdf) + max_thrust: 30.0 # N, per-thruster clamp (tune to the prop) + surge_drag: 58.42 # N/(m/s)^2, |xUabsU| from model.sdf -> speed feedforward + surge_lin: 0.0 # N/(m/s), optional linear surge term + v_max: 2.0 # m/s input clamp + yaw_gain: 30.0 # N*m per (rad/s) desired-yaw-rate -> moment + w_max: 1.0 # rad/s input clamp + invert_yaw: false # flip if a +angular.z command turns the wrong way + publish_rate_hz: 20.0 + cmd_timeout: 0.5 # s, watchdog -> zero thrust if cmd_vel goes stale + +blueboat_waypoint_controller: + ros__parameters: + # Guidance (LOS) + delta_min: 2.5 # m, min lookahead (larger = damped cross-track, no left-right weave) + delta_max: 8.0 # m, max lookahead + k_delta: 1.0 # lookahead growth per |cross-track error| + acceptance_radius: 0.2 # m, leg-complete radius + u_cruise: 1.0 # m/s nominal surge + turn_shaping_p: 1.5 # cos() exponent for turn-then-go speed cut + r_slow: 4.0 # m, slowdown radius approaching every waypoint + # Turn-in-place / station-keeping gait + turn_in_place: true # stop + rotate to next heading at each waypoint + arrival_radius: 0.6 # m, DRIVE->ALIGN switch distance to the waypoint + align_tol_deg: 3.0 # deg, heading-aligned threshold to exit ALIGN (crisp full pivot) + align_settle_degps: 5.0 # deg/s, yaw-rate-settled threshold (stop spinning before driving) + kp_pos: 0.7 # (m/s)/m, station-keep surge gain (along-body error) + u_align_max: 0.5 # m/s, station-keep surge clamp + repoint_radius: 1.0 # m, HOLD: point back at hold pt if drift exceeds this + v_settle: 0.1 # m/s, brake to a full stop before turning in place + # Heading PID (outputs desired yaw rate, rad/s) + kp_yaw: 2.0 # tuned: heading-loop bw (cross-track ~0.6 m, heading std ~4 deg) + ki_yaw: 0.0 # no steady-state heading bias observed -> integral not needed + kd_yaw: 0.5 # rate damping; keeps corner exit from overshooting + w_max: 1.0 # rad/s output clamp + i_max: 0.5 # integral contribution clamp (rad/s) + control_rate_hz: 20.0 # 0.05 m/tick at u_cruise -> disk accept (0.2 m) fires reliably diff --git a/multivehicle/packages/multivehicle_examples/launch/bluerov_mission.launch.py b/multivehicle/packages/multivehicle_examples/launch/bluerov_mission.launch.py new file mode 100644 index 0000000..cb6d915 --- /dev/null +++ b/multivehicle/packages/multivehicle_examples/launch/bluerov_mission.launch.py @@ -0,0 +1,29 @@ +from launch import LaunchDescription +from launch_ros.actions import Node + + +def generate_launch_description(): + use_sim_time = {"use_sim_time": True} + + ground_truth_node = Node( + package="multivehicle_interface", + executable="ground_truth_to_mavros", + name="ground_truth_to_mavros", + output="screen", + parameters=[use_sim_time], + ) + + movement_node = Node( + package="multivehicle_examples", + executable="bluerov_movement.py", + name="bluerov_movement", + output="screen", + parameters=[use_sim_time], + ) + + return LaunchDescription( + [ + ground_truth_node, + movement_node, + ] + ) diff --git a/multivehicle/packages/multivehicle_examples/launch/boat_control.launch.py b/multivehicle/packages/multivehicle_examples/launch/boat_control.launch.py new file mode 100644 index 0000000..9cff38e --- /dev/null +++ b/multivehicle/packages/multivehicle_examples/launch/boat_control.launch.py @@ -0,0 +1,115 @@ +"""Bring up the BlueBoat custom control stack (mixer + LOS controller). + +Layers (see the GNC plan): + * blueboat_thrust_mixer -- cmd_vel -> left/right thrust (ALWAYS on; the + single thruster driver and the nav2 seam). + * blueboat_odom_to_tf -- odom->base_link TF (nav2 prep; ALWAYS on). + * blueboat_waypoint_controller -- LOS guidance + heading/surge -> cmd_vel + (gated by `use_controller`, default true). + * blueboat_mission -- demo square course (gated by `use_mission`). + +This launch assumes the world and the boat are already up (world.launch.py + +boat.launch.py). Set `spawn:=true` to also include boat.launch.py here. + +nav2 migration (later): launch with `use_controller:=false` and remap nav2's +`/cmd_vel` onto `/blueboat/cmd_vel` -- the mixer is unchanged. +""" + +import os + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription +from launch.conditions import IfCondition +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration, PathJoinSubstitution +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare + + +def generate_launch_description(): + pkg = get_package_share_directory("multivehicle_examples") + control_params = os.path.join(pkg, "config", "blueboat_control.yaml") + + use_sim_time = LaunchConfiguration("use_sim_time") + use_controller = LaunchConfiguration("use_controller") + use_mission = LaunchConfiguration("use_mission") + spawn = LaunchConfiguration("spawn") + + args = [ + DeclareLaunchArgument("use_sim_time", default_value="true"), + DeclareLaunchArgument( + "use_controller", + default_value="true", + description="Run the LOS waypoint controller (set false when nav2 " + "provides /blueboat/cmd_vel instead).", + ), + DeclareLaunchArgument( + "use_mission", + default_value="false", + description="Run the demo square-course mission node.", + ), + DeclareLaunchArgument( + "spawn", + default_value="false", + description="Also include boat.launch.py to spawn the boat + bridge.", + ), + ] + + spawn_boat = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + PathJoinSubstitution([FindPackageShare("multivehicle_sim"), + "launch", "boat.launch.py"]) + ), + condition=IfCondition(spawn), + ) + + mixer = Node( + package="multivehicle_examples", + executable="blueboat_thrust_mixer.py", + name="blueboat_thrust_mixer", + output="screen", + parameters=[control_params, {"use_sim_time": use_sim_time}], + ) + + # Broadcast the boat's odom->base_link under a per-vehicle TF namespace + # (blueboat/odom -> blueboat/base_link). The gz OdometryPublisher stamps + # /blueboat/odom with the generic frames "odom"/"base_link", identical to + # the BlueROV2's — and MAVROS publishes map->base_link for the ROV. Left + # un-prefixed, both vehicles fight over one global `base_link` on /tf and + # every listener spams TF_OLD_DATA. Prefixing the boat's frames keeps the + # two TF trees disjoint. (Control/mission are topic-based, so nothing + # consumes these frames yet; this is purely nav2 prep.) + odom_tf = Node( + package="multivehicle_interface", + executable="blueboat_odom_to_tf", + name="blueboat_odom_to_tf", + output="screen", + parameters=[ + { + "use_sim_time": use_sim_time, + "frame_id": "blueboat/odom", + "child_frame_id": "blueboat/base_link", + } + ], + ) + + controller = Node( + package="multivehicle_examples", + executable="blueboat_waypoint_controller.py", + name="blueboat_waypoint_controller", + output="screen", + parameters=[control_params, {"use_sim_time": use_sim_time}], + condition=IfCondition(use_controller), + ) + + mission = Node( + package="multivehicle_examples", + executable="blueboat_mission.py", + name="blueboat_square_mission", + output="screen", + parameters=[{"use_sim_time": use_sim_time}], + condition=IfCondition(use_mission), + ) + + return LaunchDescription(args + [spawn_boat, mixer, odom_tf, controller, mission]) diff --git a/multivehicle/packages/multivehicle_examples/launch/px4_offboard.launch.py b/multivehicle/packages/multivehicle_examples/launch/px4_offboard.launch.py new file mode 100644 index 0000000..6873ebf --- /dev/null +++ b/multivehicle/packages/multivehicle_examples/launch/px4_offboard.launch.py @@ -0,0 +1,25 @@ +from launch import LaunchDescription +from launch_ros.actions import Node + + +def generate_launch_description(): + + return LaunchDescription( + [ + Node( + package="multivehicle_examples", + executable="px4_offboard_demo", + name="px4_offboard_demo", + output="screen", + parameters=[ + {"use_sim_time": True}, + # uXRCE-DDS topic namespace -> /x500/fmu/... . Must match + # PX4_UXRCE_DDS_NS=x500 set on the px4 SITL process. + {"vehicle_namespace": "x500"}, + # target_system for VehicleCommand: MAV_SYS_ID = instance+1, + # so 2 for `px4 ... -i 1`. + {"vehicle_id": 2}, + ], + ), + ] + ) diff --git a/multivehicle/packages/multivehicle_examples/package.xml b/multivehicle/packages/multivehicle_examples/package.xml new file mode 100644 index 0000000..2e04c18 --- /dev/null +++ b/multivehicle/packages/multivehicle_examples/package.xml @@ -0,0 +1,44 @@ + + + + multivehicle_examples + 0.1.0 + +

Example missions, controllers and demos that run on top of the + multivehicle_sim simulator: BlueROV2 square-dive mission, BlueBoat control + stack (thrust mixer + LOS waypoint controller + demo mission), and the PX4 + x500 offboard flight demo.

+ + MonkeScripts + MIT + + ament_cmake + ament_cmake_python + eigen3_cmake_module + eigen + + rclcpp + rclpy + geometry_msgs + std_msgs + sensor_msgs + nav_msgs + tf2 + tf2_ros + tf2_geometry_msgs + px4_msgs + mavros_msgs + + multivehicle_sim + multivehicle_interface + ament_index_python + launch_ros + tf_transformations + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/multivehicle/packages/multivehicle_examples/scripts/blueboat_mission.py b/multivehicle/packages/multivehicle_examples/scripts/blueboat_mission.py new file mode 100755 index 0000000..3f0364b --- /dev/null +++ b/multivehicle/packages/multivehicle_examples/scripts/blueboat_mission.py @@ -0,0 +1,80 @@ +#!/usr/bin/env python3 +"""Demo mission for the BlueBoat USV: drive a square course. + +Waits for the first ``/blueboat/odom``, then publishes a square of waypoints +(in the ``odom`` frame, relative to the boat's start position) to +``/blueboat/goal_pose``. The LOS waypoint controller follows the line between +consecutive waypoints; this node just lays down the course and then idles. + +""" + +import rclpy +from rclpy.node import Node +from nav_msgs.msg import Odometry +from geometry_msgs.msg import PoseStamped + + +class BlueBoatSquareMission(Node): + def __init__(self): + super().__init__("blueboat_square_mission") + + self.declare_parameter("side", 10.0) # m, square edge length + self.declare_parameter("frame_id", "odom") + self.side = self.get_parameter("side").value + self.frame_id = self.get_parameter("frame_id").value + + self.goal_pub = self.create_publisher(PoseStamped, "/blueboat/goal_pose", 10) + self.create_subscription(Odometry, "/blueboat/odom", self.save_odom, 10) + self._start = None + self._published = False + # Publish only once the controller is actually subscribed, so the + # course isn't dropped before discovery completes (and never twice, or + # the controller would queue duplicate waypoints). + self.create_timer(0.2, self.try_publish) + self.get_logger().info("Waiting for /blueboat/odom to lay down course...") + + def save_odom(self, msg: Odometry): + if self._start is None: + self._start = (msg.pose.pose.position.x, msg.pose.pose.position.y) + + def try_publish(self): + if self._published or self._start is None: + return + if self.goal_pub.get_subscription_count() == 0: + return + x0, y0 = self._start + s = self.side + # Axis-aligned square in the odom frame, returning to start. + waypoints = [ + (x0 + s, y0), + (x0 + s, y0 + s), + (x0, y0 + s), + (x0, y0), + ] + for wx, wy in waypoints: + msg_out = PoseStamped() + msg_out.header.frame_id = self.frame_id + msg_out.header.stamp = self.get_clock().now().to_msg() + msg_out.pose.position.x = wx + msg_out.pose.position.y = wy + msg_out.pose.orientation.w = 1.0 + self.goal_pub.publish(msg_out) + self.get_logger().info("Queued waypoint (%.2f, %.2f)" % (wx, wy)) + self._published = True + self.get_logger().info("Course published (%d waypoints)." % len(waypoints)) + + +def main(args=None): + rclpy.init(args=args) + node = BlueBoatSquareMission() + try: + rclpy.spin(node) + except KeyboardInterrupt: + pass + finally: + node.destroy_node() + rclpy.shutdown() + + +if __name__ == "__main__": + main() diff --git a/multivehicle/packages/multivehicle_examples/scripts/blueboat_thrust_mixer.py b/multivehicle/packages/multivehicle_examples/scripts/blueboat_thrust_mixer.py new file mode 100755 index 0000000..549f095 --- /dev/null +++ b/multivehicle/packages/multivehicle_examples/scripts/blueboat_thrust_mixer.py @@ -0,0 +1,152 @@ +#!/usr/bin/env python3 +"""Differential-thrust allocation (mixer) for the BlueBoat USV. + +It consumes ``geometry_msgs/Twist`` on ``/blueboat/cmd_vel`` and emits a per-thruster force (Newtons) on +``/blueboat/thrusters/{left,right}/thrust`` (std_msgs/Float64, bridged ROS->gz). + +Interpretation of the Twist: + * ``linear.x`` -> desired SURGE SPEED (m/s). Converted to a surge force by + inverting the quadratic hull drag from the model SDF (``xUabsU`` = -58.42), + so ``F_surge = surge_drag * u_d * |u_d|`` holds that speed in steady state + (open-loop). + * ``angular.z`` -> desired YAW RATE (rad/s). Converted to a yaw moment by a + gain ``yaw_gain`` (N*m per rad/s). + +Allocation (twin thrusters a moment arm ``d = track_width/2`` apart): + F_left = F_surge/2 - M_z/(2 d) + F_right = F_surge/2 + M_z/(2 d) +Positive ``angular.z`` (REP-103, CCW) therefore makes the right (starboard) +thruster push harder -> the boat yaws left, as expected. + +A watchdog publishes zero thrust if no ``cmd_vel`` arrives within +``cmd_timeout`` seconds, so a dead controller cannot leave the boat at full +throttle. +""" + +import rclpy +from rclpy.node import Node +from geometry_msgs.msg import Twist +from std_msgs.msg import Float64 + + +def _clamp(value, limit): + return max(-limit, min(limit, value)) + + +class BlueBoatThrustMixer(Node): + def __init__(self): + super().__init__("blueboat_thrust_mixer") + + # Geometry / actuator limits. + self.declare_parameter("track_width", 0.59) # m, port<->stbd spacing + self.declare_parameter("max_thrust", 30.0) # N, per-thruster clamp + # Surge: quadratic-drag feedforward (|xUabsU| from the model SDF). + self.declare_parameter("surge_drag", 58.42) # N per (m/s)^2 + self.declare_parameter("surge_lin", 0.0) # N per (m/s), optional + self.declare_parameter("v_max", 2.0) # m/s input clamp + # Yaw: desired yaw-rate -> yaw moment. + self.declare_parameter("yaw_gain", 30.0) # N*m per (rad/s) + self.declare_parameter("w_max", 1.0) # rad/s input clamp + self.declare_parameter("invert_yaw", False) # flip turn direction + # Topics / timing. + self.declare_parameter("cmd_vel_topic", "/blueboat/cmd_vel") + self.declare_parameter("left_thrust_topic", "/blueboat/thrusters/left/thrust") + self.declare_parameter("right_thrust_topic", "/blueboat/thrusters/right/thrust") + self.declare_parameter("publish_rate_hz", 20.0) + self.declare_parameter("cmd_timeout", 0.5) # s, watchdog + + self.track_width = self.get_parameter("track_width").value + self.max_thrust = self.get_parameter("max_thrust").value + self.surge_drag = self.get_parameter("surge_drag").value + self.surge_lin = self.get_parameter("surge_lin").value + self.v_max = self.get_parameter("v_max").value + self.yaw_gain = self.get_parameter("yaw_gain").value + self.w_max = self.get_parameter("w_max").value + self.invert_yaw = self.get_parameter("invert_yaw").value + self.cmd_timeout = self.get_parameter("cmd_timeout").value + rate = self.get_parameter("publish_rate_hz").value + + self.half_track = max(self.track_width / 2.0, 1e-3) + + self.left_pub = self.create_publisher( + Float64, self.get_parameter("left_thrust_topic").value, 10 + ) + self.right_pub = self.create_publisher( + Float64, self.get_parameter("right_thrust_topic").value, 10 + ) + self.create_subscription( + Twist, self.get_parameter("cmd_vel_topic").value, self.cmd_vel_sub, 10 + ) + + self._last_cmd = Twist() + self._last_cmd_time = None + self.create_timer(1.0 / rate, self.publish_thrust) + + self.get_logger().info( + "BlueBoat thrust mixer up: track_width=%.3f m, max_thrust=%.1f N, " + "surge_drag=%.2f, yaw_gain=%.2f, invert_yaw=%s" + % ( + self.track_width, + self.max_thrust, + self.surge_drag, + self.yaw_gain, + self.invert_yaw, + ) + ) + + def cmd_vel_sub(self, msg: Twist): + self._last_cmd = msg + self._last_cmd_time = self.get_clock().now() + + def publish_thrust(self): + # Watchdog: zero thrust if the command is stale (or never received). + stale = self._last_cmd_time is None or ( + (self.get_clock().now() - self._last_cmd_time).nanoseconds * 1e-9 + > self.cmd_timeout + ) + if stale: + self._send(0.0, 0.0) + return + + u = _clamp(self._last_cmd.linear.x, self.v_max) + w = _clamp(self._last_cmd.angular.z, self.w_max) + if self.invert_yaw: + w = -w + + # Surge speed -> force via quadratic-drag inversion (+ optional linear). + f_surge = self.surge_drag * u * abs(u) + self.surge_lin * u + # Desired yaw rate -> yaw moment. + m_z = self.yaw_gain * w + + f_left = f_surge / 2.0 - m_z / (2.0 * self.half_track) + f_right = f_surge / 2.0 + m_z / (2.0 * self.half_track) + + # Proportional clamp: scale both sides together so the turn ratio (and + # thus the commanded heading) is preserved under saturation. + peak = max(abs(f_left), abs(f_right), self.max_thrust) + if peak > self.max_thrust: + scale = self.max_thrust / peak + f_left *= scale + f_right *= scale + + self._send(f_left, f_right) + + def _send(self, left, right): + self.left_pub.publish(Float64(data=float(left))) + self.right_pub.publish(Float64(data=float(right))) + + +def main(args=None): + rclpy.init(args=args) + node = BlueBoatThrustMixer() + try: + rclpy.spin(node) + except KeyboardInterrupt: + pass + finally: + node.destroy_node() + rclpy.shutdown() + + +if __name__ == "__main__": + main() diff --git a/multivehicle/packages/multivehicle_examples/scripts/blueboat_waypoint_controller.py b/multivehicle/packages/multivehicle_examples/scripts/blueboat_waypoint_controller.py new file mode 100755 index 0000000..1a90bc8 --- /dev/null +++ b/multivehicle/packages/multivehicle_examples/scripts/blueboat_waypoint_controller.py @@ -0,0 +1,352 @@ +#!/usr/bin/env python3 +"""Line-of-Sight (LOS) waypoint controller for the BlueBoat USV. + +Guidance + control layers of the BlueBoat GNC stack. It turns a list of +waypoints into a ``geometry_msgs/Twist`` on ``/blueboat/cmd_vel`` for the thrust +mixer to allocate. + +Line-of-Sight path following (Fossen): + Track the *line between consecutive waypoints*, not the raw point, so the boat + drives the cross-track error ``e`` to zero instead of spinning toward / over- + shooting each point. For a leg from ``p_prev`` to ``p_tgt`` with path angle + ``gamma``: + e = -(x - x_prev) sin(gamma) + (y - y_prev) cos(gamma) # +ve = left + Delta = clamp(delta_min + k_delta |e|, delta_min, delta_max) # adaptive lookahead + psi_d = gamma + atan2(-e, Delta) + A leg is finished when the boat passes the target (along-track progress >= leg + length) or enters ``acceptance_radius``. + +Control (heading psi & yaw-rate r from the pose/twist; surge u from twist.linear.x): + * Heading: PID on wrap(psi_d - psi) with derivative-on-measurement (-Kd*r, + no setpoint kick on waypoint switch) and an anti-windup integrator -> + desired yaw rate ``angular.z``. + * Surge: cruise speed shaped by heading error and by the distance-to-goal slowdown -> desired ``linear.x``: + u_d = u_cruise * max(0, cos(psi_err))**p * sat(dist_to_goal / r_slow) + +Gait (``turn_in_place``) -- stop-and-turn at every waypoint: + INIT -> ALIGN -> DRIVE -> (ALIGN -> DRIVE)* -> HOLD. + * ALIGN: rotate in place to the next leg's heading while station-keeping the + waypoint (surge nulls the along-body position error, braking any arrival + momentum). Exits once the heading is aligned and the yaw rate has settled. + * DRIVE: LOS path following; slows approaching *every* waypoint so the boat + arrives slow enough to stop and turn (not just the final one). + * HOLD: actively station-keep the final pose instead of going limp -- the boat + holds position/heading rather than drifting. The USV is underactuated + (surge + yaw only, no sway), so a pure sideways offset is corrected only by + re-pointing at the hold point (``repoint_radius``); within + ``acceptance_radius`` the surge deadbands to avoid thruster jitter. + +Inputs: ``/blueboat/odom`` (nav_msgs/Odometry); ``/blueboat/goal_pose`` + (geometry_msgs/PoseStamped, appended to the path); ``waypoints`` param + (flat [x0,y0,x1,y1,...]) for a preloaded course. +Outputs: ``/blueboat/cmd_vel`` (geometry_msgs/Twist); + ``/blueboat/goal_reached`` (std_msgs/Bool, latched true when the final + waypoint is reached). +""" + +import math + +import rclpy +from rclpy.node import Node +from rclpy.qos import QoSProfile, DurabilityPolicy +from rcl_interfaces.msg import ParameterDescriptor +from nav_msgs.msg import Odometry +from geometry_msgs.msg import PoseStamped, Twist +from std_msgs.msg import Bool + + +def wrap_to_pi(angle): + """Wrap an angle to (-pi, pi].""" + return math.atan2(math.sin(angle), math.cos(angle)) + + +def yaw_from_quaternion(q): + """Extract yaw (rad) from a geometry_msgs/Quaternion.""" + siny_cosp = 2.0 * (q.w * q.z + q.x * q.y) + cosy_cosp = 1.0 - 2.0 * (q.y * q.y + q.z * q.z) + return math.atan2(siny_cosp, cosy_cosp) + + +def clamp(value, limit): + return max(-limit, min(limit, value)) + + +class BlueBoatWaypointController(Node): + def __init__(self): + super().__init__("blueboat_waypoint_controller") + + self.declare_parameter("delta_min", 2.5) # m, min lookahead + self.declare_parameter("delta_max", 8.0) # m, max lookahead + self.declare_parameter("k_delta", 1.0) # lookahead growth per |e| + self.declare_parameter("acceptance_radius", 0.2) # m, leg-done radius + self.declare_parameter("u_cruise", 1.0) # m/s nominal surge + self.declare_parameter("turn_shaping_p", 1.5) # cos() exponent + self.declare_parameter("r_slow", 4.0) # m, final-goal slowdown + # --- Turn-in-place / station-keeping gait ---------------------------- + self.declare_parameter("turn_in_place", True) # stop+rotate at each wp + self.declare_parameter("arrival_radius", 0.6) # m, DRIVE->ALIGN switch + self.declare_parameter("align_tol_deg", 3.0) # deg, heading-aligned exit + self.declare_parameter("align_settle_degps", 5.0) # deg/s, settled exit + self.declare_parameter("kp_pos", 0.7) # (m/s)/m, station-keep surge gain + self.declare_parameter("u_align_max", 0.5) # m/s, station-keep surge clamp + self.declare_parameter("repoint_radius", 1.0) # m, HOLD: drive back if drift>this + self.declare_parameter("v_settle", 0.1) # m/s, stop before turning in place + # --- Heading PID (outputs desired yaw rate, rad/s) ------------------- + self.declare_parameter("kp_yaw", 2.0) + self.declare_parameter("ki_yaw", 0.0) + self.declare_parameter("kd_yaw", 0.5) + self.declare_parameter("w_max", 1.0) # rad/s output clamp + self.declare_parameter("i_max", 0.5) # integrator clamp (rad/s) + self.declare_parameter("control_rate_hz", 20.0) + + self.declare_parameter("odom_topic", "/blueboat/odom") + self.declare_parameter("cmd_vel_topic", "/blueboat/cmd_vel") + self.declare_parameter("goal_pose_topic", "/blueboat/goal_pose") + + self.delta_min = self.get_parameter("delta_min").value + self.delta_max = self.get_parameter("delta_max").value + self.k_delta = self.get_parameter("k_delta").value + self.accept_r = self.get_parameter("acceptance_radius").value + self.u_cruise = self.get_parameter("u_cruise").value + self.shaping_p = self.get_parameter("turn_shaping_p").value + self.r_slow = self.get_parameter("r_slow").value + self.turn_in_place = self.get_parameter("turn_in_place").value + self.arrival_radius = self.get_parameter("arrival_radius").value + self.align_tol = math.radians(self.get_parameter("align_tol_deg").value) + self.align_settle = math.radians(self.get_parameter("align_settle_degps").value) + self.kp_pos = self.get_parameter("kp_pos").value + self.u_align_max = self.get_parameter("u_align_max").value + self.repoint_radius = self.get_parameter("repoint_radius").value + self.v_settle = self.get_parameter("v_settle").value + self.kp_yaw = self.get_parameter("kp_yaw").value + self.ki_yaw = self.get_parameter("ki_yaw").value + self.kd_yaw = self.get_parameter("kd_yaw").value + self.w_max = self.get_parameter("w_max").value + self.i_max = self.get_parameter("i_max").value + self.dt = 1.0 / self.get_parameter("control_rate_hz").value + + # --- Path state ------------------------------------------------------ + self.waypoints = [] + self.wp_idx = 0 + self.seg_start = None # (x, y) start of the current leg + self.first_leg_start = None # captured from odom when motion begins + + self.odom = None + self.yaw_integral = 0.0 + self.finished = False + # Gait state: INIT -> ALIGN -> DRIVE -> (ALIGN -> DRIVE)* -> HOLD. + self.mode = "INIT" + self.hold_xy = None # (x, y) pose held while aligning / station-keeping + self.target_heading = 0.0 # desired heading (rad) during ALIGN / HOLD + + # --- I/O ------------------------------------------------------------- + self.cmd_pub = self.create_publisher( + Twist, self.get_parameter("cmd_vel_topic").value, 10 + ) + latched = QoSProfile(depth=1, durability=DurabilityPolicy.TRANSIENT_LOCAL) + self.reached_pub = self.create_publisher( + Bool, "/blueboat/goal_reached", latched + ) + self.create_subscription( + Odometry, self.get_parameter("odom_topic").value, self.odom_sub, 10 + ) + self.create_subscription( + PoseStamped, self.get_parameter("goal_pose_topic").value, self.goal_sub, 10 + ) + + self.create_timer(self.dt, self.control_step) + self.get_logger().info( + "BlueBoat waypoint controller up: %d preloaded waypoint(s), " + "u_cruise=%.2f m/s, lookahead=[%.1f, %.1f] m" + % (len(self.waypoints), self.u_cruise, self.delta_min, self.delta_max) + ) + + def odom_sub(self, msg: Odometry): + self.odom = msg + + def goal_sub(self, msg: PoseStamped): + """Append a goal to the path and resume if we had finished.""" + self.waypoints.append((msg.pose.position.x, msg.pose.position.y)) + if self.finished: + self.finished = False + self.reached_pub.publish(Bool(data=False)) + self.get_logger().info( + "Goal appended (%.2f, %.2f); %d waypoint(s) queued" + % (msg.pose.position.x, msg.pose.position.y, len(self.waypoints)) + ) + + def control_step(self): + if self.odom is None or not self.waypoints: + return + + x = self.odom.pose.pose.position.x + y = self.odom.pose.pose.position.y + yaw = yaw_from_quaternion(self.odom.pose.pose.orientation) + r = self.odom.twist.twist.angular.z # yaw rate (rad/s) + + if self.first_leg_start is None: + self.first_leg_start = (x, y) + + # INIT: align to the first leg from rest (skip straight to DRIVE if the + # turn-in-place gait is disabled). + if self.mode == "INIT": + self.seg_start = (x, y) + self.hold_xy = (x, y) + self.target_heading = self.leg_heading((x, y), self.waypoints[0]) + self.mode = "ALIGN" if self.turn_in_place else "DRIVE" + + # A goal appended after finishing re-opens the course -> resume. + if self.finished and self.wp_idx < len(self.waypoints): + self.finished = False + self.reached_pub.publish(Bool(data=False)) + self.seg_start = (x, y) + self.hold_xy = (x, y) + self.target_heading = self.leg_heading((x, y), self.waypoints[self.wp_idx]) + self.mode = "ALIGN" if self.turn_in_place else "DRIVE" + + # All waypoints consumed -> actively station-keep the final pose. + if self.wp_idx >= len(self.waypoints): + if not self.finished: + self.finished = True + self.reached_pub.publish(Bool(data=True)) + self.get_logger().info("Final waypoint reached. Station-keeping.") + self.mode = "HOLD" + self.station_keep(x, y, yaw, r, allow_repoint=True) + return + + # --- ALIGN: brake to a stop, THEN rotate in place to the next leg ---- + if self.mode == "ALIGN": + u_now = self.odom.twist.twist.linear.x + e_x = self.along_body_err(x, y, yaw) + u_cmd = clamp(self.kp_pos * e_x, self.u_align_max) + if abs(u_now) > self.v_settle: + # Still coasting in from the leg: decelerate in a straight line + # (no yaw) so the boat doesn't swing its nose while moving. + # The turn only begins once it has actually stopped. + self.publish_cmd(u_cmd, 0.0) + return + w_cmd, psi_err = self.heading_pid(self.target_heading, yaw, r) + self.publish_cmd(u_cmd, w_cmd) + if abs(psi_err) < self.align_tol and abs(r) < self.align_settle: + self.mode = "DRIVE" + self.yaw_integral = 0.0 + self.get_logger().info("Leg %d aligned -> driving." % self.wp_idx) + return + + # --- DRIVE: Line-of-Sight path following ---------------------------- + sx, sy = self.seg_start if self.seg_start is not None else self.first_leg_start + tx, ty = self.waypoints[self.wp_idx] + seg_dx, seg_dy = tx - sx, ty - sy + seg_len = math.hypot(seg_dx, seg_dy) + dist_to_target = math.hypot(tx - x, ty - y) + + if seg_len < 1e-3: + # Degenerate leg: just aim straight at the target. + psi_d = math.atan2(ty - y, tx - x) + along = 0.0 + else: + gamma = math.atan2(seg_dy, seg_dx) + # Signed cross-track error (positive to the left of the path). + e = -(x - sx) * math.sin(gamma) + (y - sy) * math.cos(gamma) + # Along-track progress along the leg. + along = (x - sx) * math.cos(gamma) + (y - sy) * math.sin(gamma) + delta = max( + self.delta_min, + min(self.delta_max, self.delta_min + self.k_delta * abs(e)), + ) + psi_d = gamma + math.atan2(-e, delta) + + # Reached the waypoint: stop and (if enabled) turn in place to the next. + passed = seg_len > 1e-3 and along >= seg_len + if dist_to_target < self.arrival_radius or passed: + self.get_logger().info("Leg %d reached." % self.wp_idx) + self.seg_start = (tx, ty) + self.hold_xy = (tx, ty) + nxt = self.wp_idx + 1 + if nxt < len(self.waypoints): + self.target_heading = self.leg_heading((tx, ty), self.waypoints[nxt]) + else: + self.target_heading = yaw # final: hold the heading we arrived on + self.wp_idx = nxt + self.yaw_integral = 0.0 + if self.turn_in_place and nxt < len(self.waypoints): + self.mode = "ALIGN" + return # next tick handles the new leg / HOLD + + # --- Heading PID + surge -------------------------------------------- + w_cmd, psi_err = self.heading_pid(psi_d, yaw, r) + # Turn-then-go: cut speed while heading error is large. + turn_factor = max(0.0, math.cos(psi_err)) ** self.shaping_p + # Slow approaching EVERY waypoint so the boat arrives slow enough to stop. + slow = min(1.0, dist_to_target / self.r_slow) + u_cmd = self.u_cruise * turn_factor * slow + self.publish_cmd(u_cmd, w_cmd) + + # ---------------------------------------------------------------- helpers + def publish_cmd(self, u, w): + cmd = Twist() + cmd.linear.x = float(u) + cmd.angular.z = float(w) + self.cmd_pub.publish(cmd) + + def heading_pid(self, psi_d, yaw, r): + """Heading PID -> desired yaw rate. Returns (w_cmd, psi_err).""" + psi_err = wrap_to_pi(psi_d - yaw) + if self.ki_yaw > 0.0: + # Integrate, then clamp so the integral CONTRIBUTION stays <= i_max. + self.yaw_integral += psi_err * self.dt + self.yaw_integral = clamp(self.yaw_integral, self.i_max / self.ki_yaw) + else: + self.yaw_integral = 0.0 + # Derivative on measurement (-Kd*r) avoids a kick when psi_d jumps. + w_cmd = clamp( + self.kp_yaw * psi_err + self.ki_yaw * self.yaw_integral - self.kd_yaw * r, + self.w_max, + ) + return w_cmd, psi_err + + def along_body_err(self, x, y, yaw): + """Position error to hold_xy projected on the body x-axis (+ = ahead).""" + dx = self.hold_xy[0] - x + dy = self.hold_xy[1] - y + return dx * math.cos(yaw) + dy * math.sin(yaw) + + @staticmethod + def leg_heading(a, b): + """Heading (rad) of the line from point a to point b.""" + return math.atan2(b[1] - a[1], b[0] - a[0]) + + def station_keep(self, x, y, yaw, r, allow_repoint): + """Hold ``hold_xy``: surge nulls the along-body error while the heading + holds ``target_heading``. If drift exceeds ``repoint_radius`` the boat + points back at the hold point (the only way an underactuated USV can + correct a sideways offset). Within ``acceptance_radius`` the surge + deadbands so the thrusters don't jitter on station.""" + rng = math.hypot(self.hold_xy[0] - x, self.hold_xy[1] - y) + if rng < self.accept_r: + w_cmd, _ = self.heading_pid(self.target_heading, yaw, r) + self.publish_cmd(0.0, w_cmd) + return + if allow_repoint and rng > self.repoint_radius: + psi_d = math.atan2(self.hold_xy[1] - y, self.hold_xy[0] - x) + else: + psi_d = self.target_heading + w_cmd, _ = self.heading_pid(psi_d, yaw, r) + u_cmd = clamp(self.kp_pos * self.along_body_err(x, y, yaw), self.u_align_max) + self.publish_cmd(u_cmd, w_cmd) + + +def main(args=None): + rclpy.init(args=args) + node = BlueBoatWaypointController() + try: + rclpy.spin(node) + except KeyboardInterrupt: + pass + finally: + node.destroy_node() + rclpy.shutdown() + + +if __name__ == "__main__": + main() diff --git a/multivehicle/packages/multivehicle_examples/scripts/bluerov_movement.py b/multivehicle/packages/multivehicle_examples/scripts/bluerov_movement.py new file mode 100755 index 0000000..8bb1284 --- /dev/null +++ b/multivehicle/packages/multivehicle_examples/scripts/bluerov_movement.py @@ -0,0 +1,270 @@ +#!/usr/bin/env python3 + +import math + +import rclpy +import rclpy.duration +import rclpy.time +from rclpy.node import Node +from geometry_msgs.msg import PoseStamped +from mavros_msgs.srv import SetMode, CommandBool +from rclpy.qos import QoSProfile, ReliabilityPolicy +import tf2_ros +from tf2_geometry_msgs import do_transform_pose_stamped + + +class BlueROVSquareMission(Node): + """Drive the BlueROV2 in a square relative to its own base_link. + + Each leg is a body-frame FLU offset (forward, left) applied from the pose at + the START of that leg, while the vehicle HOLDS its initial heading — i.e. it + strafes the square (forward, left, back, right) instead of yawing to face + each waypoint. This matches how an ROV actually moves and avoids the + spinning behaviour you get from publishing absolute map setpoints / facing + each target. + + Each leg's body-frame offset is transformed into a map-frame + `setpoint_position/local` target with tf2 (`do_transform_pose_stamped` + against the live `map -> base_link` transform MAVROS publishes), so the + square is always relative to the vehicle's own base_link, never the map + origin. + """ + + # TF frames: MAVROS local_position publishes map -> base_link (tf.send=true). + MAP_FRAME = "map" + BASE_FRAME = "base_link" + + def __init__(self): + super().__init__("bluerov_square_mission") + + mavros_qos = QoSProfile(depth=10, reliability=ReliabilityPolicy.BEST_EFFORT) + + self.target_pub = self.create_publisher( + PoseStamped, "/mavros/setpoint_position/local", mavros_qos + ) + self.curr_pose_sub = self.create_subscription( + PoseStamped, "/mavros/local_position/pose", self.pose_callback, mavros_qos + ) + + # tf2 buffer/listener — used to transform base_link offsets to map. + self.tf_buffer = tf2_ros.Buffer() + self.tf_listener = tf2_ros.TransformListener(self.tf_buffer, self) + + # Service Clients + self.arming_client = self.create_client(CommandBool, "/mavros/cmd/arming") + self.mode_client = self.create_client(SetMode, "/mavros/set_mode") + + # Service state flags + self.is_armed = False + self.is_guided_mode = False + self.services_initialized = False + + # Mission Params + self.depth = -2.0 # absolute dive depth (m below surface) + self.dist_threshold = 0.2 + + # Per-leg body-frame FLU offsets (forward+, left+). Each leg is applied + # from the pose at the start of that leg, with heading held constant -> + # the vehicle strafes a square and returns to start. + self.legs = [ + (5.0, 0.0), # forward 5 m + (0.0, 5.0), # left 5 m + (-5.0, 0.0), # backward 5 m + (0.0, -5.0), # right 5 m + ] + self.leg_index = 0 + + # Heading held for the whole mission (captured at START). + self.hold_yaw = None + # Current leg's absolute map-frame target (computed when the leg begins). + self.target_x = None + self.target_y = None + + self.current_pose = None + self.state = "INIT" + + # IMPORTANT NOTE: DO NOT SET A HIGH SETPOINT PUBLISH RATE. + # ROV WOULD KEEP REPLANNING ITS TRAJECTORY, CAUSING IT TO MOVE VERY SLOWLY + self.timer = self.create_timer(1.0, self.main_loop) + + def arm(self): + """Request arming""" + if not self.arming_client.service_is_ready(): + self.get_logger().info("Waiting for arming service...") + return False + + self.get_logger().info("Requesting arm...") + request = CommandBool.Request() + request.value = True + future = self.arming_client.call_async(request) + future.add_done_callback(self.arm_callback) + return True + + def arm_callback(self, future): + """Callback for arming service""" + try: + response = future.result() + if response.success: + self.is_armed = True + self.get_logger().info("Vehicle armed successfully") + else: + self.get_logger().warn("Failed to arm vehicle") + except Exception as e: + self.get_logger().error(f"Arming service call failed: {e}") + + def set_guided_mode(self): + """Request GUIDED mode""" + if not self.mode_client.service_is_ready(): + self.get_logger().info("Waiting for SetMode service...") + return False + + self.get_logger().info("Requesting GUIDED mode...") + req = SetMode.Request() + req.custom_mode = "GUIDED" + future = self.mode_client.call_async(req) + future.add_done_callback(self.guided_mode_callback) + return True + + def guided_mode_callback(self, future): + """Callback for setting to guided mode""" + try: + response = future.result() + if response.mode_sent: + self.is_guided_mode = True + self.get_logger().info("GUIDED mode set successfully") + else: + self.get_logger().warn("Failed to set GUIDED mode") + except Exception as e: + self.get_logger().error(f"Set mode service call failed: {e}") + + def pose_callback(self, msg): + self.current_pose = msg + + def get_quaternion_from_yaw(self, yaw): + """Convert yaw (radians) to a quaternion for PoseStamped.""" + return {"x": 0.0, "y": 0.0, "z": math.sin(yaw / 2.0), "w": math.cos(yaw / 2.0)} + + def get_yaw_from_quaternion(self, quaternion): + """Extract yaw (radians) from a quaternion.""" + siny_cosp = 2.0 * (quaternion.w * quaternion.z + quaternion.x * quaternion.y) + cosy_cosp = 1.0 - 2.0 * ( + quaternion.y * quaternion.y + quaternion.z * quaternion.z + ) + return math.atan2(siny_cosp, cosy_cosp) + + def leg_target_in_map(self, fwd, left): + """Transform a body-frame FLU offset (forward, left) at the current + base_link into a map-frame (x, y) target via tf2. Returns None if the + map -> base_link transform isn't available yet. + """ + pose_in_base = PoseStamped() + pose_in_base.header.frame_id = self.BASE_FRAME + pose_in_base.pose.position.x = fwd + pose_in_base.pose.position.y = left + pose_in_base.pose.position.z = 0.0 + pose_in_base.pose.orientation.w = 1.0 + try: + tf = self.tf_buffer.lookup_transform( + self.MAP_FRAME, + self.BASE_FRAME, + rclpy.time.Time(), + timeout=rclpy.duration.Duration(seconds=1.0), + ) + except tf2_ros.TransformException as e: + self.get_logger().warn( + f"TF {self.MAP_FRAME} <- {self.BASE_FRAME} not ready: {e}" + ) + return None + target = do_transform_pose_stamped(pose_in_base, tf) + return target.pose.position.x, target.pose.position.y + + def main_loop(self): + """Main state machine""" + + if self.state == "INIT": + if not self.services_initialized: + self.is_armed or self.arm() + self.is_guided_mode or self.set_guided_mode() + + if self.is_armed and self.is_guided_mode: + self.services_initialized = True + self.state = "START" + self.get_logger().info("Initialization complete, starting mission") + + elif self.state == "START": + if self.current_pose is None: + self.get_logger().info("Waiting for pose data...") + return + # Hold the heading the vehicle starts with for the whole square. + self.hold_yaw = self.get_yaw_from_quaternion( + self.current_pose.pose.orientation + ) + self.leg_index = 0 + self.state = "PLAN_LEG" + self.get_logger().info( + f"Beginning body-relative square (hold_yaw={self.hold_yaw:.2f})" + ) + + elif self.state == "PLAN_LEG": + # Transform this leg's body-frame offset (relative to the CURRENT + # base_link) into a map-frame target via tf2. Retry next tick if the + # transform isn't available yet. + fwd, left = self.legs[self.leg_index] + target = self.leg_target_in_map(fwd, left) + if target is None: + return + self.target_x, self.target_y = target + self.state = "MOVING" + self.get_logger().info( + f"Leg {self.leg_index} (fwd={fwd}, left={left}) -> " + f"target ({self.target_x:.2f}, {self.target_y:.2f})" + ) + + elif self.state == "MOVING": + # Publish the (fixed) leg target, holding the mission heading. + q = self.get_quaternion_from_yaw(self.hold_yaw) + msg = PoseStamped() + msg.header.stamp = self.get_clock().now().to_msg() + msg.header.frame_id = "map" + msg.pose.position.x = self.target_x + msg.pose.position.y = self.target_y + msg.pose.position.z = self.depth + msg.pose.orientation.x = q["x"] + msg.pose.orientation.y = q["y"] + msg.pose.orientation.z = q["z"] + msg.pose.orientation.w = q["w"] + self.target_pub.publish(msg) + + dx = self.target_x - self.current_pose.pose.position.x + dy = self.target_y - self.current_pose.pose.position.y + dist = math.hypot(dx, dy) + self.get_logger().info( + f"Leg {self.leg_index}: dist to target {dist:.2f} m" + ) + + if dist < self.dist_threshold: + self.get_logger().info(f"Leg {self.leg_index} reached!") + self.leg_index += 1 + if self.leg_index >= len(self.legs): + self.state = "FINISHED" + else: + self.state = "PLAN_LEG" + + elif self.state == "FINISHED": + self.get_logger().info("Mission successful.") + raise SystemExit + + +def main(args=None): + rclpy.init(args=args) + node = BlueROVSquareMission() + try: + rclpy.spin(node) + except SystemExit: + pass + node.destroy_node() + rclpy.shutdown() + + +if __name__ == "__main__": + main() diff --git a/multivehicle/packages/multivehicle_examples/src/PX4OffboardDemo.cpp b/multivehicle/packages/multivehicle_examples/src/PX4OffboardDemo.cpp new file mode 100644 index 0000000..6ec8369 --- /dev/null +++ b/multivehicle/packages/multivehicle_examples/src/PX4OffboardDemo.cpp @@ -0,0 +1,227 @@ +#include "PX4OffboardDemo.hpp" + +#include +#include + +using namespace std::chrono_literals; +using std::placeholders::_1; + +using px4_msgs::msg::OffboardControlMode; +using px4_msgs::msg::TrajectorySetpoint; +using px4_msgs::msg::VehicleCommand; +using px4_msgs::msg::VehicleLandDetected; +using px4_msgs::msg::VehicleLocalPosition; +using px4_msgs::msg::VehicleStatus; + +namespace { +// Wrap an angle into (-pi, pi]. +float wrapToPi(float angle) { + return std::atan2(std::sin(angle), std::cos(angle)); +} +} // namespace + +OffboardDemo::OffboardDemo() : Node("px4_offboard_demo") { + _ns = this->declare_parameter("vehicle_namespace", ""); + _sys_id = this->declare_parameter("vehicle_id", 0); + _prefix = _ns.empty() ? std::string() : "/" + _ns; + + RCLCPP_INFO(get_logger(), "vehicle_namespace='%s' (topic prefix '%s'), target_system=%d", + _ns.c_str(), _prefix.c_str(), _sys_id); + + _cmd_pub = create_publisher(_prefix + "/fmu/in/vehicle_command", 10); + _setpoint_pub = create_publisher(_prefix + "/fmu/in/trajectory_setpoint", 10); + _offboard_mode_pub = create_publisher(_prefix + "/fmu/in/offboard_control_mode", 10); + + // PX4 publishes telemetry best-effort; match it or subscriptions stay empty. + _status_sub = create_subscription( + _prefix + "/fmu/out/vehicle_status_v1", rclcpp::QoS(1).best_effort(), + std::bind(&OffboardDemo::onStatus, this, _1)); + _position_sub = create_subscription( + _prefix + "/fmu/out/vehicle_local_position", rclcpp::QoS(10).best_effort(), + std::bind(&OffboardDemo::onLocalPosition, this, _1)); + _land_sub = create_subscription( + _prefix + "/fmu/out/vehicle_land_detected", rclcpp::QoS(1).best_effort(), + std::bind(&OffboardDemo::onLandDetected, this, _1)); + + // Stream the offboard-mode heartbeat independently of the mission tick. + _heartbeat_timer = create_wall_timer(100ms, std::bind(&OffboardDemo::streamOffboardHeartbeat, this)); + + _phase_since = now(); + + // Prime the setpoint stream with a no-op so PX4 sees a setpoint before we ask + // for offboard mode. + publishPosition(Eigen::Vector3f(NAN, NAN, NAN), NAN); + + RCLCPP_INFO(get_logger(), "OffboardDemo initialised."); +} + +void OffboardDemo::onStatus(const VehicleStatus::SharedPtr msg) { _status = *msg; } + +void OffboardDemo::onLocalPosition(const VehicleLocalPosition::SharedPtr msg) { _position = *msg; } + +void OffboardDemo::onLandDetected(const VehicleLandDetected::SharedPtr msg) { _landed = msg->landed; } + +uint64_t OffboardDemo::nowMicros() const { + return static_cast(this->now().nanoseconds() / 1000); +} + +void OffboardDemo::streamOffboardHeartbeat() { + OffboardControlMode mode{}; + mode.timestamp = nowMicros(); + mode.position = true; // we only ever command position setpoints + _offboard_mode_pub->publish(mode); +} + +void OffboardDemo::publishPosition(const Eigen::Vector3f& pos_ned, float yaw) { + TrajectorySetpoint sp{}; + sp.timestamp = nowMicros(); + sp.position = {pos_ned.x(), pos_ned.y(), pos_ned.z()}; + sp.yaw = yaw; // NAN = let PX4 hold the current heading + _setpoint_pub->publish(sp); +} + +void OffboardDemo::sendCommand(uint32_t command, float param1, float param2) { + VehicleCommand cmd{}; + cmd.timestamp = nowMicros(); + cmd.command = command; + cmd.param1 = param1; + cmd.param2 = param2; + cmd.target_system = static_cast(_sys_id); + cmd.target_component = 1; + cmd.source_system = 1; + cmd.source_component = 1; + cmd.from_external = true; + _cmd_pub->publish(cmd); +} + +void OffboardDemo::requestOffboardMode() { + // base mode = custom (1), custom main mode = OFFBOARD (6). + sendCommand(VehicleCommand::VEHICLE_CMD_DO_SET_MODE, 1.0f, 6.0f); +} + +void OffboardDemo::arm() { + sendCommand(VehicleCommand::VEHICLE_CMD_COMPONENT_ARM_DISARM, 1.0f); +} + +void OffboardDemo::disarm() { + sendCommand(VehicleCommand::VEHICLE_CMD_COMPONENT_ARM_DISARM, 0.0f); +} + +void OffboardDemo::commandLand() { + sendCommand(VehicleCommand::VEHICLE_CMD_NAV_LAND); +} + +bool OffboardDemo::phaseElapsed(double seconds) const { + return (now() - _phase_since).seconds() > seconds; +} + +bool OffboardDemo::reached(const Eigen::Vector3f& target, float tol) const { + return std::fabs(_position.x - target.x()) < tol && + std::fabs(_position.y - target.y()) < tol && + std::fabs(_position.z - target.z()) < tol; +} + +bool OffboardDemo::yawReached(float target_yaw, float tol) const { + return std::fabs(wrapToPi(target_yaw - _position.heading)) < tol; +} + +void OffboardDemo::transitionTo(Phase next, const char* note) { + _phase = next; + _phase_since = now(); + RCLCPP_INFO(get_logger(), "%s", note); +} + +void OffboardDemo::tick() { + const Eigen::Vector3f standoff(kStandoffNorth, kStandoffEast, kStandoffDown); + + switch (_phase) { + case Phase::WaitForReady: + // Let the autopilot settle, then latch the launch point as home (climbed + // up by kClimbHeight) and request offboard. + if (_status.pre_flight_checks_pass && phaseElapsed(kReadyHold)) { + _home_ned = Eigen::Vector3f(_position.x, _position.y, _position.z - kClimbHeight); + requestOffboardMode(); + transitionTo(Phase::RequestArm, "Ready -> requesting OFFBOARD"); + } + break; + + case Phase::RequestArm: + if (_status.nav_state == VehicleStatus::NAVIGATION_STATE_OFFBOARD) { + arm(); + transitionTo(Phase::AwaitArmed, "OFFBOARD -> arming"); + } + break; + + case Phase::AwaitArmed: + if (_status.arming_state == VehicleStatus::ARMING_STATE_ARMED) { + transitionTo(Phase::ClimbOut, "Armed -> climbing out"); + } + break; + + case Phase::ClimbOut: + // Hold the home setpoint; the vehicle climbs to it. Move on after a fixed + // settle so we reach altitude before transiting. + publishPosition(_home_ned, NAN); + if (phaseElapsed(kClimbHold)) { + transitionTo(Phase::GoToStandoff, "Climb-out -> standoff transit"); + } + break; + + case Phase::GoToStandoff: + publishPosition(standoff, NAN); + if (reached(standoff)) { + _scan_yaw_target = _position.heading; + _scan_yaw_swept = 0.0f; + transitionTo(Phase::ScanYaw, "At standoff -> yaw scan"); + } + break; + + case Phase::ScanYaw: + // Hold position and step the yaw target one notch each time we settle on + // the previous one, until a full revolution has been swept. + if (yawReached(_scan_yaw_target)) { + _scan_yaw_target = wrapToPi(_scan_yaw_target + kYawStep); + _scan_yaw_swept += kYawStep; + } + publishPosition(standoff, _scan_yaw_target); + if (_scan_yaw_swept >= 2.0f * static_cast(M_PI)) { + transitionTo(Phase::ReturnHome, "Yaw scan complete -> returning home"); + } + break; + + case Phase::ReturnHome: + publishPosition(_home_ned, NAN); + if (reached(_home_ned)) { + commandLand(); + transitionTo(Phase::Descend, "Home -> landing"); + } + break; + + case Phase::Descend: + if (_landed) { + disarm(); + transitionTo(Phase::Finished, "Touchdown -> disarmed"); + } + break; + + case Phase::Finished: + break; + } +} + +void OffboardDemo::run() { + while (rclcpp::ok() && _phase != Phase::Finished) { + rclcpp::spin_some(this->get_node_base_interface()); + tick(); + rclcpp::sleep_for(10ms); + } + RCLCPP_INFO(get_logger(), "Mission complete."); +} + +int main(int argc, char* argv[]) { + rclcpp::init(argc, argv); + auto node = std::make_shared(); + node->run(); + rclcpp::shutdown(); + return 0; +} diff --git a/multivehicle/packages/multivehicle_examples/src/PX4OffboardDemo.hpp b/multivehicle/packages/multivehicle_examples/src/PX4OffboardDemo.hpp new file mode 100644 index 0000000..28eadc8 --- /dev/null +++ b/multivehicle/packages/multivehicle_examples/src/PX4OffboardDemo.hpp @@ -0,0 +1,102 @@ +// PX4OffboardDemo.hpp — offboard control demo for the x500 UAV +#pragma once + +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +// Drives a single PX4 vehicle through a fixed offboard mission. All PX4 topics +// are prefixed with the vehicle namespace (e.g. "/x500/fmu/...") so the node +// can target one instance among several. +class OffboardDemo : public rclcpp::Node { + public: + OffboardDemo(); + + // Blocking: spins the node and advances the mission until it completes. + void run(); + + private: + // Mission phases, in the order they execute. + enum class Phase { + WaitForReady, // hold until pre-flight checks pass, then request offboard + RequestArm, // offboard accepted -> send the arm command + AwaitArmed, // wait for the autopilot to report ARMED + ClimbOut, // hold the climb setpoint above launch + GoToStandoff, // transit to the standoff observation point + ScanYaw, // rotate 360 degrees in place while holding position + ReturnHome, // fly back to the launch point + Descend, // land and wait for touchdown + Finished, + }; + + // Subscription callbacks (latch the latest message). + void onStatus(const px4_msgs::msg::VehicleStatus::SharedPtr msg); + void onLocalPosition(const px4_msgs::msg::VehicleLocalPosition::SharedPtr msg); + void onLandDetected(const px4_msgs::msg::VehicleLandDetected::SharedPtr msg); + + // Keepalive: PX4 only stays in offboard while this stream is fresh. + void streamOffboardHeartbeat(); + + // Outgoing commands / setpoints. + void sendCommand(uint32_t command, float param1 = NAN, float param2 = NAN); + void requestOffboardMode(); + void arm(); + void disarm(); + void commandLand(); + void publishPosition(const Eigen::Vector3f& pos_ned, float yaw); + + // Mission step + small predicates. + void tick(); + void transitionTo(Phase next, const char* note); + bool phaseElapsed(double seconds) const; + bool reached(const Eigen::Vector3f& target, float tol = kReachedTol) const; + bool yawReached(float target_yaw, float tol = kYawTol) const; + uint64_t nowMicros() const; + + // ---- parameters ---- + std::string _ns; // vehicle_namespace + std::string _prefix; // "/" (or "" when unset) + int _sys_id = 0; // vehicle_id -> MAVLink target_system + + // ---- ROS entities ---- + rclcpp::Publisher::SharedPtr _cmd_pub; + rclcpp::Publisher::SharedPtr _setpoint_pub; + rclcpp::Publisher::SharedPtr _offboard_mode_pub; + rclcpp::Subscription::SharedPtr _status_sub; + rclcpp::Subscription::SharedPtr _position_sub; + rclcpp::Subscription::SharedPtr _land_sub; + rclcpp::TimerBase::SharedPtr _heartbeat_timer; + + // ---- latest telemetry ---- + px4_msgs::msg::VehicleStatus _status{}; + px4_msgs::msg::VehicleLocalPosition _position{}; + bool _landed = false; + + // ---- mission state ---- + Phase _phase = Phase::WaitForReady; + rclcpp::Time _phase_since; + Eigen::Vector3f _home_ned = Eigen::Vector3f::Zero(); // launch point + climb (NED) + float _scan_yaw_target = 0.0f; // current yaw setpoint during ScanYaw + float _scan_yaw_swept = 0.0f; // total yaw rotated so far + + // ---- mission constants (NED; z is down-positive) ---- + static constexpr float kClimbHeight = 10.0f; // m above the launch deck + static constexpr float kStandoffNorth = -14.0f; // standoff observation point + static constexpr float kStandoffEast = -25.0f; + static constexpr float kStandoffDown = -6.0f; + static constexpr float kReachedTol = 0.1f; // m, position-reached tolerance + static constexpr float kYawStep = static_cast(M_PI / 180.0); // 1 deg + static constexpr float kYawTol = static_cast(M_PI / 180.0); // 1 deg + static constexpr double kReadyHold = 2.0; // s, settle before offboard + static constexpr double kClimbHold = 10.0; // s, hold the climb setpoint +}; diff --git a/multivehicle/run.bash b/multivehicle/run.bash new file mode 100644 index 0000000..d8f8a88 --- /dev/null +++ b/multivehicle/run.bash @@ -0,0 +1,24 @@ +#!/usr/bin/env bash + +set -e + +# Run the multivehicle_examples image with NVIDIA + X11 via rocker. +# Mounts ONLY this workspace at /root/HOST/mvsim_ws (the path the tmuxp sessions +# and setup_dashboard.sh expect). This script lives in src/examples/multivehicle, +# so the workspace root is three levels up. +# +# SPDX-License-Identifier: MIT + +IMAGE_NAME="${1:-multivehicle_examples:humble}" +SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)" +WORKSPACE_DIR="$(cd "${SCRIPT_DIR}/../../.." && pwd)" + +rocker \ + --devices /dev/dri \ + --dev-helpers \ + --nvidia \ + --x11 \ + --git \ + --volume "${WORKSPACE_DIR}:/root/HOST/mvsim_ws" \ + --network=host \ + "${IMAGE_NAME}" diff --git a/multivehicle/tmuxp/mvsim_debug.yaml b/multivehicle/tmuxp/mvsim_debug.yaml new file mode 100644 index 0000000..d173bf8 --- /dev/null +++ b/multivehicle/tmuxp/mvsim_debug.yaml @@ -0,0 +1,113 @@ +# tmuxp DEBUG session for the multivehicle examples (run INSIDE the +# multivehicle_examples container). Brings up the full stack — the simulator +# (multivehicle_sim) AND the example apps (missions, BlueBoat control, the PX4 +# offboard flight demo) — plus the dashboard. Load with: +# +# tmuxp load /root/HOST/mvsim_ws/src/examples/multivehicle/tmuxp/mvsim_debug.yaml +# +# Every command is PRE-TYPED but NOT executed (enter: false) so you start them in +# dependency order and watch each one. Recommended order: +# +# 1) world — bring up Gazebo + the course first (multivehicle_sim) +# 2) bluerov — BlueROV2 + ArduSub/MAVROS, then its square-dive mission +# 3) boat — BlueBoat spawn + control stack + square mission +# 4) drone — XRCE agent + PX4 SITL x500 + gz bridge + offboard demo +# 5) dashboard — web backend (:8080) + sim-side LED/incident bridges +# 6) debug — free shell + Foxglove bridge +# +# Workspace dir defaults to /root/HOST/mvsim_ws. Override with WS_DIR: +# WS_DIR=/root/HOST/my_ws tmuxp load .../mvsim_debug.yaml +# +# NVIDIA hosts: uncomment the two PRIME-offload exports in shell_command_before. + +session_name: mvsim + +global_options: + mouse: 'on' + history-limit: 50000 + +shell_command_before: + - 'WS_DIR="${WS_DIR:-/root/HOST/mvsim_ws}"' + # - export __NV_PRIME_RENDER_OFFLOAD=1 + # - export __GLX_VENDOR_LIBRARY_NAME=nvidia + - source /opt/ros/humble/setup.bash + - '[ -f "$WS_DIR/install/setup.bash" ] && source "$WS_DIR/install/setup.bash"' + - cd "$WS_DIR" + +windows: + # 1. Gazebo world (sim). + - window_name: world + focus: true + panes: + - shell_command: + - cmd: ros2 launch multivehicle_sim world.launch.py world_name:=robotx_2026_sg_river + enter: false + + # 2. BlueROV2 — spawn + ArduSub + MAVROS (sim, top); square-dive mission + # (examples, bottom). + - window_name: bluerov + layout: even-vertical + panes: + - shell_command: + - cmd: ros2 launch multivehicle_sim bluerov.launch.py + enter: false + - shell_command: + - cmd: ros2 launch multivehicle_examples bluerov_mission.launch.py + enter: false + + # 3. BlueBoat — spawn (sim, top); control stack + square mission (examples, + # bottom; use_mission:=true runs the demo course through the LOS controller). + - window_name: boat + layout: even-vertical + panes: + - shell_command: + - cmd: ros2 launch multivehicle_sim boat.launch.py + enter: false + - shell_command: + - cmd: ros2 launch multivehicle_examples boat_control.launch.py use_mission:=true + enter: false + + # 4. PX4 x500 drone — agent + PX4 SITL + gz bridge (sim), then the offboard + # flight demo (examples). Start top-to-bottom; offboard needs agent + PX4. + - window_name: drone + layout: even-vertical + panes: + - shell_command: + - cmd: MicroXRCEAgent udp4 -p 8888 + enter: false + - shell_command: + - cmd: PX4_GZ_STANDALONE=1 PX4_SYS_AUTOSTART=4010 PX4_UXRCE_DDS_NS=x500 PX4_PARAM_UXRCE_DDS_SYNCT=0 PX4_GZ_MODEL_POSE="47.40,-388.95,3.85,0,0.0" /root/px4/px4_sitl/bin/px4 -w /root/px4/px4_sitl/romfs -i 1 + enter: false + - shell_command: + - cmd: ros2 launch multivehicle_sim uav_gz.launch.py model_name:=x500_mono_cam_1 + enter: false + - shell_command: + - cmd: ros2 launch multivehicle_examples px4_offboard.launch.py + enter: false + + # 5. Dashboard — fake vehicle (RoboCommand :9000), web backend (:8080), and the + # sim-side LED/incident bridges (bridges_pkg=multivehicle_sim, models from + # bb_worlds) tracking all three vehicles' /…/odom. + - window_name: dashboard + layout: even-vertical + panes: + - shell_command: + - cmd: ros2 run bb_robotx_dashboard fake_vehicle_publisher --port 9000 + enter: false + - shell_command: + - cmd: ros2 launch bb_robotx_dashboard dashboard.launch.py + enter: false + - shell_command: + - cmd: ros2 launch bb_robotx_dashboard robotx_2026_sim.launch.py bridges_pkg:=multivehicle_sim models_pkg:=bb_worlds auv_name:=bluerov auv_pose_topic:=/bluerov/odom asv_name:=blueboat asv_pose_topic:=/blueboat/odom uav_name:=x500 uav_pose_topic:=/x500/odom + enter: false + + # 6. Debug — free shell + Foxglove bridge (ws://localhost:8765). + - window_name: debug + layout: even-vertical + panes: + - shell_command: + - cmd: ros2 topic list + enter: false + - shell_command: + - cmd: ros2 launch foxglove_bridge foxglove_bridge_launch.xml + enter: false From 4f9ceef60f22644b37c761113b166917853a561d Mon Sep 17 00:00:00 2001 From: monkescripts Date: Tue, 23 Jun 2026 11:54:26 +0800 Subject: [PATCH 2/8] Update build and run scripts to include additional dependencies and change file permissions --- multivehicle/README.md | 6 ++---- multivehicle/build.bash | 0 multivehicle/run.bash | 0 3 files changed, 2 insertions(+), 4 deletions(-) mode change 100644 => 100755 multivehicle/build.bash mode change 100644 => 100755 multivehicle/run.bash diff --git a/multivehicle/README.md b/multivehicle/README.md index 860dde0..5adeb1e 100644 --- a/multivehicle/README.md +++ b/multivehicle/README.md @@ -43,10 +43,8 @@ Inside the container: ```bash cd /root/HOST/mvsim_ws -# Scope the build to this project: --packages-up-to pulls in multivehicle_examples -# and its deps (multivehicle_sim, multivehicle_interface, px4_msgs, bb_worlds, …) -# and skips the sibling bluerov_tasks, whose vision deps aren't imported here. -colcon build --symlink-install --packages-up-to multivehicle_examples +colcon build --symlink-install --packages-up-to multivehicle_examples microxrcedds_agent bb_robotx_dashboard + source install/setup.bash tmuxp load src/examples/multivehicle/tmuxp/mvsim_debug.yaml ``` diff --git a/multivehicle/build.bash b/multivehicle/build.bash old mode 100644 new mode 100755 diff --git a/multivehicle/run.bash b/multivehicle/run.bash old mode 100644 new mode 100755 From d3de12a56574cd62b970e2db7d2e11c66a624257 Mon Sep 17 00:00:00 2001 From: monkescripts Date: Mon, 22 Jun 2026 22:33:02 +0800 Subject: [PATCH 3/8] Add BlueROV square mission and PX4 offboard control demo - Implemented a new Python script for BlueROV2 to execute a square mission using body-frame offsets. - Created C++ classes for PX4 offboard control, including handling vehicle commands and trajectory setpoints. - Added header file for PX4 offboard demo with mission phases and telemetry subscriptions. - Introduced a bash script to run the multivehicle examples with NVIDIA and X11 support. - Configured a tmuxp session for debugging the multivehicle examples, including launching the simulator and various missions. --- README.md | 157 ++------ bluerov/README.md | 145 ++++++++ .../bluerov_bin_mission.yaml | 0 .../bluerov_mission.yaml | 0 .../bluerov_torpedo_mission.yaml | 0 bluerov_ws.repos => bluerov/bluerov_ws.repos | 0 build.bash => bluerov/build.bash | 0 {docker => bluerov/docker}/Dockerfile | 0 {docs => bluerov/docs}/architecture.md | 0 .../bluerov_tasks/bluerov_tasks/__init__.py | 0 .../bluerov_tasks/arm_and_set_mode.py | 0 .../bluerov_tasks/bins/__init__.py | 0 .../bluerov_tasks/bluerov_tasks/bins/bins.py | 0 .../bluerov_tasks/bins/template_selector.py | 0 .../bluerov_tasks/bluerov_tasks/goto.py | 0 .../bluerov_tasks/node_registry.py | 0 .../bluerov_tasks/shared_trees/__init__.py | 0 .../bluerov_tasks/shared_trees/choice.py | 0 .../shared_trees/cluster_decision.py | 0 .../bluerov_tasks/torpedo/__init__.py | 0 .../torpedo/move_and_shoot_seq.py | 0 .../bluerov_tasks/torpedo/torpedo.py | 0 .../config/bluerov_single_tfs.yaml | 0 .../config/vision_pipeline/bin.yaml | 0 .../config/vision_pipeline/torpedo.yaml | 0 .../launch/bluerov_bin_bt.launch.py | 0 .../launch/bluerov_bin_vision.launch.py | 0 .../launch/bluerov_cluster.launch.py | 0 .../launch/bluerov_controls.launch.py | 0 .../launch/bluerov_mission.launch.py | 0 .../launch/bluerov_square_bt.launch.py | 0 .../launch/bluerov_tfs.launch.py | 0 .../launch/bluerov_torpedo_bt.launch.py | 0 .../launch/bluerov_torpedo_vision.launch.py | 0 .../packages}/bluerov_tasks/package.xml | 0 .../bluerov_tasks/resource/bluerov_tasks | 0 .../bluerov_tasks/scripts/actuators_sim.py | 0 .../scripts/bluerov_bin_mission_tree.py | 0 .../bluerov_tasks/scripts/bluerov_movement.py | 0 .../scripts/bluerov_square_mission_tree.py | 0 .../scripts/bluerov_torpedo_mission_tree.py | 0 .../scripts/choice_server_node.py | 0 .../scripts/locomotion_action_server.py | 0 .../packages}/bluerov_tasks/setup.cfg | 0 .../packages}/bluerov_tasks/setup.py | 0 run.bash => bluerov/run.bash | 2 +- multivehicle/README.md | 61 +++ multivehicle/build.bash | 27 ++ multivehicle/docker/Dockerfile | 16 + multivehicle/examples.repos | 38 ++ .../multivehicle_examples/CMakeLists.txt | 38 ++ .../config/blueboat_control.yaml | 44 +++ .../launch/bluerov_mission.launch.py | 29 ++ .../launch/boat_control.launch.py | 115 ++++++ .../launch/px4_offboard.launch.py | 25 ++ .../multivehicle_examples/package.xml | 44 +++ .../scripts/blueboat_mission.py | 80 ++++ .../scripts/blueboat_thrust_mixer.py | 152 ++++++++ .../scripts/blueboat_waypoint_controller.py | 352 ++++++++++++++++++ .../scripts/bluerov_movement.py | 270 ++++++++++++++ .../src/PX4OffboardDemo.cpp | 227 +++++++++++ .../src/PX4OffboardDemo.hpp | 102 +++++ multivehicle/run.bash | 24 ++ multivehicle/tmuxp/mvsim_debug.yaml | 113 ++++++ 64 files changed, 1924 insertions(+), 137 deletions(-) create mode 100644 bluerov/README.md rename bluerov_bin_mission.yaml => bluerov/bluerov_bin_mission.yaml (100%) rename bluerov_mission.yaml => bluerov/bluerov_mission.yaml (100%) rename bluerov_torpedo_mission.yaml => bluerov/bluerov_torpedo_mission.yaml (100%) rename bluerov_ws.repos => bluerov/bluerov_ws.repos (100%) rename build.bash => bluerov/build.bash (100%) rename {docker => bluerov/docker}/Dockerfile (100%) rename {docs => bluerov/docs}/architecture.md (100%) rename {packages => bluerov/packages}/bluerov_tasks/bluerov_tasks/__init__.py (100%) rename {packages => bluerov/packages}/bluerov_tasks/bluerov_tasks/arm_and_set_mode.py (100%) rename {packages => bluerov/packages}/bluerov_tasks/bluerov_tasks/bins/__init__.py (100%) rename {packages => bluerov/packages}/bluerov_tasks/bluerov_tasks/bins/bins.py (100%) rename {packages => bluerov/packages}/bluerov_tasks/bluerov_tasks/bins/template_selector.py (100%) rename {packages => bluerov/packages}/bluerov_tasks/bluerov_tasks/goto.py (100%) rename {packages => bluerov/packages}/bluerov_tasks/bluerov_tasks/node_registry.py (100%) rename {packages => bluerov/packages}/bluerov_tasks/bluerov_tasks/shared_trees/__init__.py (100%) rename {packages => bluerov/packages}/bluerov_tasks/bluerov_tasks/shared_trees/choice.py (100%) rename {packages => bluerov/packages}/bluerov_tasks/bluerov_tasks/shared_trees/cluster_decision.py (100%) rename {packages => bluerov/packages}/bluerov_tasks/bluerov_tasks/torpedo/__init__.py (100%) rename {packages => bluerov/packages}/bluerov_tasks/bluerov_tasks/torpedo/move_and_shoot_seq.py (100%) rename {packages => bluerov/packages}/bluerov_tasks/bluerov_tasks/torpedo/torpedo.py (100%) rename {packages => bluerov/packages}/bluerov_tasks/config/bluerov_single_tfs.yaml (100%) rename {packages => bluerov/packages}/bluerov_tasks/config/vision_pipeline/bin.yaml (100%) rename {packages => bluerov/packages}/bluerov_tasks/config/vision_pipeline/torpedo.yaml (100%) rename {packages => bluerov/packages}/bluerov_tasks/launch/bluerov_bin_bt.launch.py (100%) rename {packages => bluerov/packages}/bluerov_tasks/launch/bluerov_bin_vision.launch.py (100%) rename {packages => bluerov/packages}/bluerov_tasks/launch/bluerov_cluster.launch.py (100%) rename {packages => bluerov/packages}/bluerov_tasks/launch/bluerov_controls.launch.py (100%) rename {packages => bluerov/packages}/bluerov_tasks/launch/bluerov_mission.launch.py (100%) rename {packages => bluerov/packages}/bluerov_tasks/launch/bluerov_square_bt.launch.py (100%) rename {packages => bluerov/packages}/bluerov_tasks/launch/bluerov_tfs.launch.py (100%) rename {packages => bluerov/packages}/bluerov_tasks/launch/bluerov_torpedo_bt.launch.py (100%) rename {packages => bluerov/packages}/bluerov_tasks/launch/bluerov_torpedo_vision.launch.py (100%) rename {packages => bluerov/packages}/bluerov_tasks/package.xml (100%) rename {packages => bluerov/packages}/bluerov_tasks/resource/bluerov_tasks (100%) rename {packages => bluerov/packages}/bluerov_tasks/scripts/actuators_sim.py (100%) rename {packages => bluerov/packages}/bluerov_tasks/scripts/bluerov_bin_mission_tree.py (100%) rename {packages => bluerov/packages}/bluerov_tasks/scripts/bluerov_movement.py (100%) rename {packages => bluerov/packages}/bluerov_tasks/scripts/bluerov_square_mission_tree.py (100%) rename {packages => bluerov/packages}/bluerov_tasks/scripts/bluerov_torpedo_mission_tree.py (100%) rename {packages => bluerov/packages}/bluerov_tasks/scripts/choice_server_node.py (100%) rename {packages => bluerov/packages}/bluerov_tasks/scripts/locomotion_action_server.py (100%) rename {packages => bluerov/packages}/bluerov_tasks/setup.cfg (100%) rename {packages => bluerov/packages}/bluerov_tasks/setup.py (100%) rename run.bash => bluerov/run.bash (89%) create mode 100644 multivehicle/README.md create mode 100644 multivehicle/build.bash create mode 100644 multivehicle/docker/Dockerfile create mode 100644 multivehicle/examples.repos create mode 100644 multivehicle/packages/multivehicle_examples/CMakeLists.txt create mode 100644 multivehicle/packages/multivehicle_examples/config/blueboat_control.yaml create mode 100644 multivehicle/packages/multivehicle_examples/launch/bluerov_mission.launch.py create mode 100644 multivehicle/packages/multivehicle_examples/launch/boat_control.launch.py create mode 100644 multivehicle/packages/multivehicle_examples/launch/px4_offboard.launch.py create mode 100644 multivehicle/packages/multivehicle_examples/package.xml create mode 100755 multivehicle/packages/multivehicle_examples/scripts/blueboat_mission.py create mode 100755 multivehicle/packages/multivehicle_examples/scripts/blueboat_thrust_mixer.py create mode 100755 multivehicle/packages/multivehicle_examples/scripts/blueboat_waypoint_controller.py create mode 100755 multivehicle/packages/multivehicle_examples/scripts/bluerov_movement.py create mode 100644 multivehicle/packages/multivehicle_examples/src/PX4OffboardDemo.cpp create mode 100644 multivehicle/packages/multivehicle_examples/src/PX4OffboardDemo.hpp create mode 100644 multivehicle/run.bash create mode 100644 multivehicle/tmuxp/mvsim_debug.yaml diff --git a/README.md b/README.md index 00dfc03..36c910e 100644 --- a/README.md +++ b/README.md @@ -1,145 +1,30 @@ -# Examples +# examples -## BlueROV with ArduSub +Example applications for Bumblebee simulators, organized one subdirectory per +project. Each subdir is a self-contained sub-workspace (its own `packages/`, +`build.bash` / `run.bash`, `docker/`, and `*.repos` manifest): -### Quickstart +| Directory | Simulator base | Contents | +|-----------|----------------|----------| +| [`bluerov/`](bluerov/) | `ardusub_sim:humble` | BlueROV2 / ArduSub missions, behaviour trees, perception (`bluerov_tasks`) | +| [`multivehicle/`](multivehicle/) | `multivehicle_sim:humble` | Multivehicle missions, BlueBoat control, PX4 offboard demo (`multivehicle_examples`) | -We assume the ROS workspace is `~/workspaces/bluerov_ws`. Change the paths accordingly if needed. +Pick the project you want and follow its README. -Clone the repositories: +## Building -```bash -cd ~/workspaces/bluerov_ws/src -vcs import --recursive < examples/bluerov_ws.repos -``` - -> **Note:** `ml_models` is hosted on Hugging Face. -> Install [Git LFS](https://git-lfs.com/) **before** importing. - -Build the minimal sim image first: - -```bash -cd ~/workspaces/bluerov_ws/src/ardusub_sim -./build.bash -``` - -Build the examples image with perception and mission-tree dependencies: - -```bash -cd ~/workspaces/bluerov_ws/src/examples -./build.bash -``` - -### Start the container - -#### Native Ubuntu with NVIDIA - -Install [Rocker](https://github.com/osrf/rocker), then run: +`colcon build` discovers `package.xml` files at any depth, so the per-project +nesting (`/packages/...`) is found automatically. Because both projects' +packages live in the same repo but each pulls only its **own** dependencies (via +its `*.repos`), build the project you set up rather than the whole tree, e.g.: ```bash -cd ~/workspaces/bluerov_ws/src/examples -./run.bash bluerov_ws:humble +# multivehicle: +colcon build --packages-up-to multivehicle_examples +# bluerov: +colcon build --packages-up-to bluerov_tasks ``` -#### WSL 2 with WSLg - -```bash -docker run --rm -it \ - --gpus all \ - --device=/dev/dxg \ - --network=host \ - --ipc=host \ - -e DISPLAY="$DISPLAY" \ - -e WAYLAND_DISPLAY="$WAYLAND_DISPLAY" \ - -e XDG_RUNTIME_DIR="$XDG_RUNTIME_DIR" \ - -e PULSE_SERVER="$PULSE_SERVER" \ - -e NVIDIA_DRIVER_CAPABILITIES=all \ - -e MESA_D3D12_DEFAULT_ADAPTER_NAME=NVIDIA \ - -e LD_LIBRARY_PATH=/usr/lib/wsl/lib \ - -v /tmp/.X11-unix:/tmp/.X11-unix:rw \ - -v /mnt/wslg:/mnt/wslg \ - -v /usr/lib/wsl:/usr/lib/wsl:ro \ - -v ~/workspaces/bluerov_ws:/root/HOST/bluerov_ws \ - bluerov_ws:humble -``` - -The WSL command exposes WSLg's X11/Wayland sockets and the `/dev/dxg` virtual -GPU device, allowing Gazebo to use hardware-accelerated rendering. - -### Verify GPU rendering - -Inside the container, before launching Gazebo: - -```bash -glxinfo -B | grep -E "OpenGL vendor|OpenGL renderer" -``` - -On native Linux, the renderer should identify the NVIDIA GPU. Under WSLg, it -should mention D3D12 and the GPU. It should not report `llvmpipe`, which is -software rendering. - -### Build the workspace - -Inside the container: - -```bash -cd /root/HOST/bluerov_ws -source /opt/ros/humble/setup.bash -colcon build --symlink-install \ - --packages-up-to bluerov_tasks bluerov_sim bb_worlds -source install/setup.bash -``` - -### Demo: Square Mission - -The square mission is a control smoke test: - -1. Arm and enter `GUIDED`. -2. Move 2 m forward. -3. Move 2 m left. -4. Move 2 m backward. -5. Move 2 m right. - -Inside the container: - -```bash -cd /root/HOST/bluerov_ws -tmuxp load src/examples/bluerov_mission.yaml -``` - -### Demo: Bin Mission - -https://github.com/user-attachments/assets/6c262df8-bac6-492a-aef1-9e8cfc30d8a8 - -```bash -tmuxp load src/examples/bluerov_bin_mission.yaml -``` - -### Demo: Torpedo Mission - -https://github.com/user-attachments/assets/9a9c25c5-637a-403a-b34d-4048f9afb5e0 - -```bash -tmuxp load src/examples/bluerov_torpedo_mission.yaml -``` -### Foxglove layouts - -Prebuilt Foxglove layouts for the Bin and Torpedo missions are available at -[BumblebeeAS/controlkitv3](https://github.com/BumblebeeAS/controlkitv3/tree/main/foxglove_layouts). -Import them into Foxglove Studio for a ready-made view of the relevant topics, -making it easier to visualize and debug each mission. - -## Useful Commands - -```bash -ros2 topic echo /bluerov/odom --once -ros2 topic echo /mavros/state --once -ros2 topic echo /mavros/local_position/pose --once -ros2 topic echo /bluerov/controls/_action/feedback --once -ros2 service call /mavros/cmd/arming mavros_msgs/srv/CommandBool "{value: true}" -ros2 service call /mavros/set_mode mavros_msgs/srv/SetMode "{custom_mode: 'GUIDED'}" -``` - -## Documentation - -- [Architecture and conventions](docs/architecture.md) +`--packages-up-to` builds the chosen package plus its dependencies and skips the +other project (whose deps you have not imported). Alternatively, drop a +`COLCON_IGNORE` file in the project subdir you are not building. diff --git a/bluerov/README.md b/bluerov/README.md new file mode 100644 index 0000000..00dfc03 --- /dev/null +++ b/bluerov/README.md @@ -0,0 +1,145 @@ +# Examples + +## BlueROV with ArduSub + +### Quickstart + +We assume the ROS workspace is `~/workspaces/bluerov_ws`. Change the paths accordingly if needed. + +Clone the repositories: + +```bash +cd ~/workspaces/bluerov_ws/src +vcs import --recursive < examples/bluerov_ws.repos +``` + +> **Note:** `ml_models` is hosted on Hugging Face. +> Install [Git LFS](https://git-lfs.com/) **before** importing. + +Build the minimal sim image first: + +```bash +cd ~/workspaces/bluerov_ws/src/ardusub_sim +./build.bash +``` + +Build the examples image with perception and mission-tree dependencies: + +```bash +cd ~/workspaces/bluerov_ws/src/examples +./build.bash +``` + +### Start the container + +#### Native Ubuntu with NVIDIA + +Install [Rocker](https://github.com/osrf/rocker), then run: + +```bash +cd ~/workspaces/bluerov_ws/src/examples +./run.bash bluerov_ws:humble +``` + +#### WSL 2 with WSLg + +```bash +docker run --rm -it \ + --gpus all \ + --device=/dev/dxg \ + --network=host \ + --ipc=host \ + -e DISPLAY="$DISPLAY" \ + -e WAYLAND_DISPLAY="$WAYLAND_DISPLAY" \ + -e XDG_RUNTIME_DIR="$XDG_RUNTIME_DIR" \ + -e PULSE_SERVER="$PULSE_SERVER" \ + -e NVIDIA_DRIVER_CAPABILITIES=all \ + -e MESA_D3D12_DEFAULT_ADAPTER_NAME=NVIDIA \ + -e LD_LIBRARY_PATH=/usr/lib/wsl/lib \ + -v /tmp/.X11-unix:/tmp/.X11-unix:rw \ + -v /mnt/wslg:/mnt/wslg \ + -v /usr/lib/wsl:/usr/lib/wsl:ro \ + -v ~/workspaces/bluerov_ws:/root/HOST/bluerov_ws \ + bluerov_ws:humble +``` + +The WSL command exposes WSLg's X11/Wayland sockets and the `/dev/dxg` virtual +GPU device, allowing Gazebo to use hardware-accelerated rendering. + +### Verify GPU rendering + +Inside the container, before launching Gazebo: + +```bash +glxinfo -B | grep -E "OpenGL vendor|OpenGL renderer" +``` + +On native Linux, the renderer should identify the NVIDIA GPU. Under WSLg, it +should mention D3D12 and the GPU. It should not report `llvmpipe`, which is +software rendering. + +### Build the workspace + +Inside the container: + +```bash +cd /root/HOST/bluerov_ws +source /opt/ros/humble/setup.bash +colcon build --symlink-install \ + --packages-up-to bluerov_tasks bluerov_sim bb_worlds +source install/setup.bash +``` + +### Demo: Square Mission + +The square mission is a control smoke test: + +1. Arm and enter `GUIDED`. +2. Move 2 m forward. +3. Move 2 m left. +4. Move 2 m backward. +5. Move 2 m right. + +Inside the container: + +```bash +cd /root/HOST/bluerov_ws +tmuxp load src/examples/bluerov_mission.yaml +``` + +### Demo: Bin Mission + +https://github.com/user-attachments/assets/6c262df8-bac6-492a-aef1-9e8cfc30d8a8 + +```bash +tmuxp load src/examples/bluerov_bin_mission.yaml +``` + +### Demo: Torpedo Mission + +https://github.com/user-attachments/assets/9a9c25c5-637a-403a-b34d-4048f9afb5e0 + +```bash +tmuxp load src/examples/bluerov_torpedo_mission.yaml +``` +### Foxglove layouts + +Prebuilt Foxglove layouts for the Bin and Torpedo missions are available at +[BumblebeeAS/controlkitv3](https://github.com/BumblebeeAS/controlkitv3/tree/main/foxglove_layouts). +Import them into Foxglove Studio for a ready-made view of the relevant topics, +making it easier to visualize and debug each mission. + +## Useful Commands + +```bash +ros2 topic echo /bluerov/odom --once +ros2 topic echo /mavros/state --once +ros2 topic echo /mavros/local_position/pose --once +ros2 topic echo /bluerov/controls/_action/feedback --once +ros2 service call /mavros/cmd/arming mavros_msgs/srv/CommandBool "{value: true}" +ros2 service call /mavros/set_mode mavros_msgs/srv/SetMode "{custom_mode: 'GUIDED'}" +``` + +## Documentation + +- [Architecture and conventions](docs/architecture.md) diff --git a/bluerov_bin_mission.yaml b/bluerov/bluerov_bin_mission.yaml similarity index 100% rename from bluerov_bin_mission.yaml rename to bluerov/bluerov_bin_mission.yaml diff --git a/bluerov_mission.yaml b/bluerov/bluerov_mission.yaml similarity index 100% rename from bluerov_mission.yaml rename to bluerov/bluerov_mission.yaml diff --git a/bluerov_torpedo_mission.yaml b/bluerov/bluerov_torpedo_mission.yaml similarity index 100% rename from bluerov_torpedo_mission.yaml rename to bluerov/bluerov_torpedo_mission.yaml diff --git a/bluerov_ws.repos b/bluerov/bluerov_ws.repos similarity index 100% rename from bluerov_ws.repos rename to bluerov/bluerov_ws.repos diff --git a/build.bash b/bluerov/build.bash similarity index 100% rename from build.bash rename to bluerov/build.bash diff --git a/docker/Dockerfile b/bluerov/docker/Dockerfile similarity index 100% rename from docker/Dockerfile rename to bluerov/docker/Dockerfile diff --git a/docs/architecture.md b/bluerov/docs/architecture.md similarity index 100% rename from docs/architecture.md rename to bluerov/docs/architecture.md diff --git a/packages/bluerov_tasks/bluerov_tasks/__init__.py b/bluerov/packages/bluerov_tasks/bluerov_tasks/__init__.py similarity index 100% rename from packages/bluerov_tasks/bluerov_tasks/__init__.py rename to bluerov/packages/bluerov_tasks/bluerov_tasks/__init__.py diff --git a/packages/bluerov_tasks/bluerov_tasks/arm_and_set_mode.py b/bluerov/packages/bluerov_tasks/bluerov_tasks/arm_and_set_mode.py similarity index 100% rename from packages/bluerov_tasks/bluerov_tasks/arm_and_set_mode.py rename to bluerov/packages/bluerov_tasks/bluerov_tasks/arm_and_set_mode.py diff --git a/packages/bluerov_tasks/bluerov_tasks/bins/__init__.py b/bluerov/packages/bluerov_tasks/bluerov_tasks/bins/__init__.py similarity index 100% rename from packages/bluerov_tasks/bluerov_tasks/bins/__init__.py rename to bluerov/packages/bluerov_tasks/bluerov_tasks/bins/__init__.py diff --git a/packages/bluerov_tasks/bluerov_tasks/bins/bins.py b/bluerov/packages/bluerov_tasks/bluerov_tasks/bins/bins.py similarity index 100% rename from packages/bluerov_tasks/bluerov_tasks/bins/bins.py rename to bluerov/packages/bluerov_tasks/bluerov_tasks/bins/bins.py diff --git a/packages/bluerov_tasks/bluerov_tasks/bins/template_selector.py b/bluerov/packages/bluerov_tasks/bluerov_tasks/bins/template_selector.py similarity index 100% rename from packages/bluerov_tasks/bluerov_tasks/bins/template_selector.py rename to bluerov/packages/bluerov_tasks/bluerov_tasks/bins/template_selector.py diff --git a/packages/bluerov_tasks/bluerov_tasks/goto.py b/bluerov/packages/bluerov_tasks/bluerov_tasks/goto.py similarity index 100% rename from packages/bluerov_tasks/bluerov_tasks/goto.py rename to bluerov/packages/bluerov_tasks/bluerov_tasks/goto.py diff --git a/packages/bluerov_tasks/bluerov_tasks/node_registry.py b/bluerov/packages/bluerov_tasks/bluerov_tasks/node_registry.py similarity index 100% rename from packages/bluerov_tasks/bluerov_tasks/node_registry.py rename to bluerov/packages/bluerov_tasks/bluerov_tasks/node_registry.py diff --git a/packages/bluerov_tasks/bluerov_tasks/shared_trees/__init__.py b/bluerov/packages/bluerov_tasks/bluerov_tasks/shared_trees/__init__.py similarity index 100% rename from packages/bluerov_tasks/bluerov_tasks/shared_trees/__init__.py rename to bluerov/packages/bluerov_tasks/bluerov_tasks/shared_trees/__init__.py diff --git a/packages/bluerov_tasks/bluerov_tasks/shared_trees/choice.py b/bluerov/packages/bluerov_tasks/bluerov_tasks/shared_trees/choice.py similarity index 100% rename from packages/bluerov_tasks/bluerov_tasks/shared_trees/choice.py rename to bluerov/packages/bluerov_tasks/bluerov_tasks/shared_trees/choice.py diff --git a/packages/bluerov_tasks/bluerov_tasks/shared_trees/cluster_decision.py b/bluerov/packages/bluerov_tasks/bluerov_tasks/shared_trees/cluster_decision.py similarity index 100% rename from packages/bluerov_tasks/bluerov_tasks/shared_trees/cluster_decision.py rename to bluerov/packages/bluerov_tasks/bluerov_tasks/shared_trees/cluster_decision.py diff --git a/packages/bluerov_tasks/bluerov_tasks/torpedo/__init__.py b/bluerov/packages/bluerov_tasks/bluerov_tasks/torpedo/__init__.py similarity index 100% rename from packages/bluerov_tasks/bluerov_tasks/torpedo/__init__.py rename to bluerov/packages/bluerov_tasks/bluerov_tasks/torpedo/__init__.py diff --git a/packages/bluerov_tasks/bluerov_tasks/torpedo/move_and_shoot_seq.py b/bluerov/packages/bluerov_tasks/bluerov_tasks/torpedo/move_and_shoot_seq.py similarity index 100% rename from packages/bluerov_tasks/bluerov_tasks/torpedo/move_and_shoot_seq.py rename to bluerov/packages/bluerov_tasks/bluerov_tasks/torpedo/move_and_shoot_seq.py diff --git a/packages/bluerov_tasks/bluerov_tasks/torpedo/torpedo.py b/bluerov/packages/bluerov_tasks/bluerov_tasks/torpedo/torpedo.py similarity index 100% rename from packages/bluerov_tasks/bluerov_tasks/torpedo/torpedo.py rename to bluerov/packages/bluerov_tasks/bluerov_tasks/torpedo/torpedo.py diff --git a/packages/bluerov_tasks/config/bluerov_single_tfs.yaml b/bluerov/packages/bluerov_tasks/config/bluerov_single_tfs.yaml similarity index 100% rename from packages/bluerov_tasks/config/bluerov_single_tfs.yaml rename to bluerov/packages/bluerov_tasks/config/bluerov_single_tfs.yaml diff --git a/packages/bluerov_tasks/config/vision_pipeline/bin.yaml b/bluerov/packages/bluerov_tasks/config/vision_pipeline/bin.yaml similarity index 100% rename from packages/bluerov_tasks/config/vision_pipeline/bin.yaml rename to bluerov/packages/bluerov_tasks/config/vision_pipeline/bin.yaml diff --git a/packages/bluerov_tasks/config/vision_pipeline/torpedo.yaml b/bluerov/packages/bluerov_tasks/config/vision_pipeline/torpedo.yaml similarity index 100% rename from packages/bluerov_tasks/config/vision_pipeline/torpedo.yaml rename to bluerov/packages/bluerov_tasks/config/vision_pipeline/torpedo.yaml diff --git a/packages/bluerov_tasks/launch/bluerov_bin_bt.launch.py b/bluerov/packages/bluerov_tasks/launch/bluerov_bin_bt.launch.py similarity index 100% rename from packages/bluerov_tasks/launch/bluerov_bin_bt.launch.py rename to bluerov/packages/bluerov_tasks/launch/bluerov_bin_bt.launch.py diff --git a/packages/bluerov_tasks/launch/bluerov_bin_vision.launch.py b/bluerov/packages/bluerov_tasks/launch/bluerov_bin_vision.launch.py similarity index 100% rename from packages/bluerov_tasks/launch/bluerov_bin_vision.launch.py rename to bluerov/packages/bluerov_tasks/launch/bluerov_bin_vision.launch.py diff --git a/packages/bluerov_tasks/launch/bluerov_cluster.launch.py b/bluerov/packages/bluerov_tasks/launch/bluerov_cluster.launch.py similarity index 100% rename from packages/bluerov_tasks/launch/bluerov_cluster.launch.py rename to bluerov/packages/bluerov_tasks/launch/bluerov_cluster.launch.py diff --git a/packages/bluerov_tasks/launch/bluerov_controls.launch.py b/bluerov/packages/bluerov_tasks/launch/bluerov_controls.launch.py similarity index 100% rename from packages/bluerov_tasks/launch/bluerov_controls.launch.py rename to bluerov/packages/bluerov_tasks/launch/bluerov_controls.launch.py diff --git a/packages/bluerov_tasks/launch/bluerov_mission.launch.py b/bluerov/packages/bluerov_tasks/launch/bluerov_mission.launch.py similarity index 100% rename from packages/bluerov_tasks/launch/bluerov_mission.launch.py rename to bluerov/packages/bluerov_tasks/launch/bluerov_mission.launch.py diff --git a/packages/bluerov_tasks/launch/bluerov_square_bt.launch.py b/bluerov/packages/bluerov_tasks/launch/bluerov_square_bt.launch.py similarity index 100% rename from packages/bluerov_tasks/launch/bluerov_square_bt.launch.py rename to bluerov/packages/bluerov_tasks/launch/bluerov_square_bt.launch.py diff --git a/packages/bluerov_tasks/launch/bluerov_tfs.launch.py b/bluerov/packages/bluerov_tasks/launch/bluerov_tfs.launch.py similarity index 100% rename from packages/bluerov_tasks/launch/bluerov_tfs.launch.py rename to bluerov/packages/bluerov_tasks/launch/bluerov_tfs.launch.py diff --git a/packages/bluerov_tasks/launch/bluerov_torpedo_bt.launch.py b/bluerov/packages/bluerov_tasks/launch/bluerov_torpedo_bt.launch.py similarity index 100% rename from packages/bluerov_tasks/launch/bluerov_torpedo_bt.launch.py rename to bluerov/packages/bluerov_tasks/launch/bluerov_torpedo_bt.launch.py diff --git a/packages/bluerov_tasks/launch/bluerov_torpedo_vision.launch.py b/bluerov/packages/bluerov_tasks/launch/bluerov_torpedo_vision.launch.py similarity index 100% rename from packages/bluerov_tasks/launch/bluerov_torpedo_vision.launch.py rename to bluerov/packages/bluerov_tasks/launch/bluerov_torpedo_vision.launch.py diff --git a/packages/bluerov_tasks/package.xml b/bluerov/packages/bluerov_tasks/package.xml similarity index 100% rename from packages/bluerov_tasks/package.xml rename to bluerov/packages/bluerov_tasks/package.xml diff --git a/packages/bluerov_tasks/resource/bluerov_tasks b/bluerov/packages/bluerov_tasks/resource/bluerov_tasks similarity index 100% rename from packages/bluerov_tasks/resource/bluerov_tasks rename to bluerov/packages/bluerov_tasks/resource/bluerov_tasks diff --git a/packages/bluerov_tasks/scripts/actuators_sim.py b/bluerov/packages/bluerov_tasks/scripts/actuators_sim.py similarity index 100% rename from packages/bluerov_tasks/scripts/actuators_sim.py rename to bluerov/packages/bluerov_tasks/scripts/actuators_sim.py diff --git a/packages/bluerov_tasks/scripts/bluerov_bin_mission_tree.py b/bluerov/packages/bluerov_tasks/scripts/bluerov_bin_mission_tree.py similarity index 100% rename from packages/bluerov_tasks/scripts/bluerov_bin_mission_tree.py rename to bluerov/packages/bluerov_tasks/scripts/bluerov_bin_mission_tree.py diff --git a/packages/bluerov_tasks/scripts/bluerov_movement.py b/bluerov/packages/bluerov_tasks/scripts/bluerov_movement.py similarity index 100% rename from packages/bluerov_tasks/scripts/bluerov_movement.py rename to bluerov/packages/bluerov_tasks/scripts/bluerov_movement.py diff --git a/packages/bluerov_tasks/scripts/bluerov_square_mission_tree.py b/bluerov/packages/bluerov_tasks/scripts/bluerov_square_mission_tree.py similarity index 100% rename from packages/bluerov_tasks/scripts/bluerov_square_mission_tree.py rename to bluerov/packages/bluerov_tasks/scripts/bluerov_square_mission_tree.py diff --git a/packages/bluerov_tasks/scripts/bluerov_torpedo_mission_tree.py b/bluerov/packages/bluerov_tasks/scripts/bluerov_torpedo_mission_tree.py similarity index 100% rename from packages/bluerov_tasks/scripts/bluerov_torpedo_mission_tree.py rename to bluerov/packages/bluerov_tasks/scripts/bluerov_torpedo_mission_tree.py diff --git a/packages/bluerov_tasks/scripts/choice_server_node.py b/bluerov/packages/bluerov_tasks/scripts/choice_server_node.py similarity index 100% rename from packages/bluerov_tasks/scripts/choice_server_node.py rename to bluerov/packages/bluerov_tasks/scripts/choice_server_node.py diff --git a/packages/bluerov_tasks/scripts/locomotion_action_server.py b/bluerov/packages/bluerov_tasks/scripts/locomotion_action_server.py similarity index 100% rename from packages/bluerov_tasks/scripts/locomotion_action_server.py rename to bluerov/packages/bluerov_tasks/scripts/locomotion_action_server.py diff --git a/packages/bluerov_tasks/setup.cfg b/bluerov/packages/bluerov_tasks/setup.cfg similarity index 100% rename from packages/bluerov_tasks/setup.cfg rename to bluerov/packages/bluerov_tasks/setup.cfg diff --git a/packages/bluerov_tasks/setup.py b/bluerov/packages/bluerov_tasks/setup.py similarity index 100% rename from packages/bluerov_tasks/setup.py rename to bluerov/packages/bluerov_tasks/setup.py diff --git a/run.bash b/bluerov/run.bash similarity index 89% rename from run.bash rename to bluerov/run.bash index 00014b6..b5c75a5 100755 --- a/run.bash +++ b/bluerov/run.bash @@ -8,7 +8,7 @@ set -e IMAGE_NAME="${1:-bluerov_ws:humble}" SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)" -WORKSPACE_DIR="$(cd "${SCRIPT_DIR}/../.." && pwd)" +WORKSPACE_DIR="$(cd "${SCRIPT_DIR}/../../.." && pwd)" rocker \ --devices /dev/dri \ diff --git a/multivehicle/README.md b/multivehicle/README.md new file mode 100644 index 0000000..860dde0 --- /dev/null +++ b/multivehicle/README.md @@ -0,0 +1,61 @@ +# multivehicle examples + +Example missions, controllers and demos that run **on top of** the +[`multivehicle_sim`](https://github.com/BumblebeeAS/multivehicle_sim) simulator. + +## Contents + +``` +multivehicle/ + build.bash run.bash # multivehicle_examples:humble, FROM multivehicle_sim:humble + examples.repos # full workspace manifest (sim + deps + px4_msgs) + docker/Dockerfile # adds nav2 (BlueBoat control nav2-prep) + tmuxp/mvsim_debug.yaml # full stack: sim + examples + dashboard + packages/ + multivehicle_examples/ # ament_cmake + launch/ bluerov_mission, boat_control, px4_offboard + scripts/ bluerov_movement, blueboat_thrust_mixer, blueboat_waypoint_controller, blueboat_mission + src/ PX4OffboardDemo.cpp/.hpp (px4_msgs + Eigen3) + config/ blueboat_control.yaml +``` + +What each launch adds on top of the sim: + +| Launch | Builds on | Adds | +|--------|-----------|------| +| `bluerov_mission.launch.py` | `multivehicle_sim bluerov.launch.py` | BlueROV2 square-dive mission | +| `boat_control.launch.py` | `multivehicle_sim boat.launch.py` | thrust mixer + LOS waypoint controller (+ optional `use_mission:=true`) | +| `px4_offboard.launch.py` | `multivehicle_sim` x500 + uXRCE agent | autonomous offboard flight (`px4_offboard_demo`) | + +## Setup + +```bash +# In ~/mvsim_ws (this repo cloned at src/examples): +vcs import src < src/examples/multivehicle/examples.repos --recursive + +# Build the base sim image first, then the examples image: +cd src/multivehicle_sim && ./build.bash +cd ../examples/multivehicle && ./build.bash +./run.bash multivehicle_examples:humble +``` + +Inside the container: + +```bash +cd /root/HOST/mvsim_ws +# Scope the build to this project: --packages-up-to pulls in multivehicle_examples +# and its deps (multivehicle_sim, multivehicle_interface, px4_msgs, bb_worlds, …) +# and skips the sibling bluerov_tasks, whose vision deps aren't imported here. +colcon build --symlink-install --packages-up-to multivehicle_examples +source install/setup.bash +tmuxp load src/examples/multivehicle/tmuxp/mvsim_debug.yaml +``` + +On a hybrid-GPU (NVIDIA) host, `export __NV_PRIME_RENDER_OFFLOAD=1` and +`export __GLX_VENDOR_LIBRARY_NAME=nvidia` before launching gz (or uncomment those +lines in the tmuxp's `shell_command_before`). + +## License + +MIT (see the repo-root `LICENSE`). The `px4_offboard_demo` is a first-party, +independent implementation of the standard PX4 offboard pattern. diff --git a/multivehicle/build.bash b/multivehicle/build.bash new file mode 100644 index 0000000..2918b19 --- /dev/null +++ b/multivehicle/build.bash @@ -0,0 +1,27 @@ +#!/usr/bin/env bash + +set -e + +# Builds the multivehicle_examples Docker image FROM multivehicle_sim:humble. +# Run from this directory (src/examples/multivehicle): ./build.bash +# +# SPDX-License-Identifier: MIT +image_name=multivehicle_examples +image_tag=humble + +if [ ! -f "docker/Dockerfile" ]; then + echo "Err: docker/Dockerfile not found. Run from src/examples/multivehicle." + exit 1 +fi + +if ! docker image inspect multivehicle_sim:humble >/dev/null 2>&1; then + echo "Err: base image multivehicle_sim:humble not found." + echo "Build it first from the multivehicle_sim repo:" + echo " cd ../../multivehicle_sim && ./build.bash" + exit 1 +fi + +image_plus_tag=$image_name:$(export LC_ALL=C; date +%Y_%m_%d_%H%M) +docker build --rm -t $image_plus_tag -f docker/Dockerfile docker && \ +docker tag $image_plus_tag $image_name:$image_tag && \ +echo "Built $image_plus_tag and tagged as $image_name:$image_tag" diff --git a/multivehicle/docker/Dockerfile b/multivehicle/docker/Dockerfile new file mode 100644 index 0000000..f5f1945 --- /dev/null +++ b/multivehicle/docker/Dockerfile @@ -0,0 +1,16 @@ +FROM multivehicle_sim:humble + +SHELL ["/bin/bash", "-o", "pipefail", "-o", "errexit", "-c"] +ENV DEBIAN_FRONTEND=noninteractive +ARG HOME_DIR=/root + +RUN apt-get update && apt-get install -y --no-install-recommends \ + ros-humble-navigation2 \ + ros-humble-nav2-bringup \ + ros-humble-nav2-util \ + ros-humble-nav2-common && \ + apt-get autoremove -y && \ + apt-get clean && \ + rm -rf /var/lib/apt/lists/* + +WORKDIR ${HOME_DIR} diff --git a/multivehicle/examples.repos b/multivehicle/examples.repos new file mode 100644 index 0000000..76e9f22 --- /dev/null +++ b/multivehicle/examples.repos @@ -0,0 +1,38 @@ +# examples.repos + +repositories: + multivehicle_sim: + type: git + url: https://github.com/BumblebeeAS/multivehicle_sim.git + + # --- everything multivehicle_sim's dependencies.repos pulls --- + ardupilot_gazebo: + type: git + url: https://github.com/ArduPilot/ardupilot_gazebo.git + Micro-XRCE-DDS-Agent: + type: git + url: https://github.com/eProsima/Micro-XRCE-DDS-Agent.git + version: v2.4.2 + depth: 1 + gz_led_plugin: + type: git + url: https://github.com/BumblebeeAS/bb_led_plugin.git + version: main + bb_robotx_dashboard: + type: git + url: https://github.com/BumblebeeAS/bb_robotx_dashboard.git + bb_worlds: + type: git + url: https://github.com/BumblebeeAS/bb_worlds.git + version: main + bb_msgs: + type: git + url: https://github.com/BumblebeeAS/bb_msgs.git + version: multivehicle_sim + + # --- examples-only: PX4 uORB message types for the offboard demo --- + px4_msgs: + type: git + url: https://github.com/PX4/px4_msgs.git + version: release/1.16 + depth: 1 diff --git a/multivehicle/packages/multivehicle_examples/CMakeLists.txt b/multivehicle/packages/multivehicle_examples/CMakeLists.txt new file mode 100644 index 0000000..6054135 --- /dev/null +++ b/multivehicle/packages/multivehicle_examples/CMakeLists.txt @@ -0,0 +1,38 @@ +cmake_minimum_required(VERSION 3.12.2) +project(multivehicle_examples) + +find_package(ament_cmake REQUIRED) +find_package(ament_cmake_python REQUIRED) +find_package(rclcpp REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(px4_msgs REQUIRED) +find_package(eigen3_cmake_module REQUIRED) +find_package(Eigen3 REQUIRED) + +include_directories( + src + ${EIGEN3_INCLUDE_DIR} +) + +add_executable(px4_offboard_demo src/PX4OffboardDemo.cpp) +ament_target_dependencies(px4_offboard_demo rclcpp geometry_msgs px4_msgs Eigen3) +target_compile_features(px4_offboard_demo PUBLIC c_std_99 cxx_std_17) +install(TARGETS px4_offboard_demo DESTINATION lib/${PROJECT_NAME}) + +install(PROGRAMS + scripts/bluerov_movement.py + scripts/blueboat_thrust_mixer.py + scripts/blueboat_waypoint_controller.py + scripts/blueboat_mission.py + DESTINATION lib/${PROJECT_NAME} +) + +install(DIRECTORY launch config + DESTINATION share/${PROJECT_NAME}) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() +endif() + +ament_package() diff --git a/multivehicle/packages/multivehicle_examples/config/blueboat_control.yaml b/multivehicle/packages/multivehicle_examples/config/blueboat_control.yaml new file mode 100644 index 0000000..689b053 --- /dev/null +++ b/multivehicle/packages/multivehicle_examples/config/blueboat_control.yaml @@ -0,0 +1,44 @@ +# BlueBoat USV control-stack parameters (mixer + LOS waypoint controller). +# Single source of truth for tuning -- edit and relaunch (or `ros2 param set` +# live). See the plan's "PID tuning" section for the recommended order: +# 1) surge (straight line) 2) heading (90 deg steps at zero speed) 3) guidance. + +blueboat_thrust_mixer: + ros__parameters: + track_width: 0.59 # m, port<->stbd thruster spacing (from model.sdf) + max_thrust: 30.0 # N, per-thruster clamp (tune to the prop) + surge_drag: 58.42 # N/(m/s)^2, |xUabsU| from model.sdf -> speed feedforward + surge_lin: 0.0 # N/(m/s), optional linear surge term + v_max: 2.0 # m/s input clamp + yaw_gain: 30.0 # N*m per (rad/s) desired-yaw-rate -> moment + w_max: 1.0 # rad/s input clamp + invert_yaw: false # flip if a +angular.z command turns the wrong way + publish_rate_hz: 20.0 + cmd_timeout: 0.5 # s, watchdog -> zero thrust if cmd_vel goes stale + +blueboat_waypoint_controller: + ros__parameters: + # Guidance (LOS) + delta_min: 2.5 # m, min lookahead (larger = damped cross-track, no left-right weave) + delta_max: 8.0 # m, max lookahead + k_delta: 1.0 # lookahead growth per |cross-track error| + acceptance_radius: 0.2 # m, leg-complete radius + u_cruise: 1.0 # m/s nominal surge + turn_shaping_p: 1.5 # cos() exponent for turn-then-go speed cut + r_slow: 4.0 # m, slowdown radius approaching every waypoint + # Turn-in-place / station-keeping gait + turn_in_place: true # stop + rotate to next heading at each waypoint + arrival_radius: 0.6 # m, DRIVE->ALIGN switch distance to the waypoint + align_tol_deg: 3.0 # deg, heading-aligned threshold to exit ALIGN (crisp full pivot) + align_settle_degps: 5.0 # deg/s, yaw-rate-settled threshold (stop spinning before driving) + kp_pos: 0.7 # (m/s)/m, station-keep surge gain (along-body error) + u_align_max: 0.5 # m/s, station-keep surge clamp + repoint_radius: 1.0 # m, HOLD: point back at hold pt if drift exceeds this + v_settle: 0.1 # m/s, brake to a full stop before turning in place + # Heading PID (outputs desired yaw rate, rad/s) + kp_yaw: 2.0 # tuned: heading-loop bw (cross-track ~0.6 m, heading std ~4 deg) + ki_yaw: 0.0 # no steady-state heading bias observed -> integral not needed + kd_yaw: 0.5 # rate damping; keeps corner exit from overshooting + w_max: 1.0 # rad/s output clamp + i_max: 0.5 # integral contribution clamp (rad/s) + control_rate_hz: 20.0 # 0.05 m/tick at u_cruise -> disk accept (0.2 m) fires reliably diff --git a/multivehicle/packages/multivehicle_examples/launch/bluerov_mission.launch.py b/multivehicle/packages/multivehicle_examples/launch/bluerov_mission.launch.py new file mode 100644 index 0000000..cb6d915 --- /dev/null +++ b/multivehicle/packages/multivehicle_examples/launch/bluerov_mission.launch.py @@ -0,0 +1,29 @@ +from launch import LaunchDescription +from launch_ros.actions import Node + + +def generate_launch_description(): + use_sim_time = {"use_sim_time": True} + + ground_truth_node = Node( + package="multivehicle_interface", + executable="ground_truth_to_mavros", + name="ground_truth_to_mavros", + output="screen", + parameters=[use_sim_time], + ) + + movement_node = Node( + package="multivehicle_examples", + executable="bluerov_movement.py", + name="bluerov_movement", + output="screen", + parameters=[use_sim_time], + ) + + return LaunchDescription( + [ + ground_truth_node, + movement_node, + ] + ) diff --git a/multivehicle/packages/multivehicle_examples/launch/boat_control.launch.py b/multivehicle/packages/multivehicle_examples/launch/boat_control.launch.py new file mode 100644 index 0000000..9cff38e --- /dev/null +++ b/multivehicle/packages/multivehicle_examples/launch/boat_control.launch.py @@ -0,0 +1,115 @@ +"""Bring up the BlueBoat custom control stack (mixer + LOS controller). + +Layers (see the GNC plan): + * blueboat_thrust_mixer -- cmd_vel -> left/right thrust (ALWAYS on; the + single thruster driver and the nav2 seam). + * blueboat_odom_to_tf -- odom->base_link TF (nav2 prep; ALWAYS on). + * blueboat_waypoint_controller -- LOS guidance + heading/surge -> cmd_vel + (gated by `use_controller`, default true). + * blueboat_mission -- demo square course (gated by `use_mission`). + +This launch assumes the world and the boat are already up (world.launch.py + +boat.launch.py). Set `spawn:=true` to also include boat.launch.py here. + +nav2 migration (later): launch with `use_controller:=false` and remap nav2's +`/cmd_vel` onto `/blueboat/cmd_vel` -- the mixer is unchanged. +""" + +import os + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription +from launch.conditions import IfCondition +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration, PathJoinSubstitution +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare + + +def generate_launch_description(): + pkg = get_package_share_directory("multivehicle_examples") + control_params = os.path.join(pkg, "config", "blueboat_control.yaml") + + use_sim_time = LaunchConfiguration("use_sim_time") + use_controller = LaunchConfiguration("use_controller") + use_mission = LaunchConfiguration("use_mission") + spawn = LaunchConfiguration("spawn") + + args = [ + DeclareLaunchArgument("use_sim_time", default_value="true"), + DeclareLaunchArgument( + "use_controller", + default_value="true", + description="Run the LOS waypoint controller (set false when nav2 " + "provides /blueboat/cmd_vel instead).", + ), + DeclareLaunchArgument( + "use_mission", + default_value="false", + description="Run the demo square-course mission node.", + ), + DeclareLaunchArgument( + "spawn", + default_value="false", + description="Also include boat.launch.py to spawn the boat + bridge.", + ), + ] + + spawn_boat = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + PathJoinSubstitution([FindPackageShare("multivehicle_sim"), + "launch", "boat.launch.py"]) + ), + condition=IfCondition(spawn), + ) + + mixer = Node( + package="multivehicle_examples", + executable="blueboat_thrust_mixer.py", + name="blueboat_thrust_mixer", + output="screen", + parameters=[control_params, {"use_sim_time": use_sim_time}], + ) + + # Broadcast the boat's odom->base_link under a per-vehicle TF namespace + # (blueboat/odom -> blueboat/base_link). The gz OdometryPublisher stamps + # /blueboat/odom with the generic frames "odom"/"base_link", identical to + # the BlueROV2's — and MAVROS publishes map->base_link for the ROV. Left + # un-prefixed, both vehicles fight over one global `base_link` on /tf and + # every listener spams TF_OLD_DATA. Prefixing the boat's frames keeps the + # two TF trees disjoint. (Control/mission are topic-based, so nothing + # consumes these frames yet; this is purely nav2 prep.) + odom_tf = Node( + package="multivehicle_interface", + executable="blueboat_odom_to_tf", + name="blueboat_odom_to_tf", + output="screen", + parameters=[ + { + "use_sim_time": use_sim_time, + "frame_id": "blueboat/odom", + "child_frame_id": "blueboat/base_link", + } + ], + ) + + controller = Node( + package="multivehicle_examples", + executable="blueboat_waypoint_controller.py", + name="blueboat_waypoint_controller", + output="screen", + parameters=[control_params, {"use_sim_time": use_sim_time}], + condition=IfCondition(use_controller), + ) + + mission = Node( + package="multivehicle_examples", + executable="blueboat_mission.py", + name="blueboat_square_mission", + output="screen", + parameters=[{"use_sim_time": use_sim_time}], + condition=IfCondition(use_mission), + ) + + return LaunchDescription(args + [spawn_boat, mixer, odom_tf, controller, mission]) diff --git a/multivehicle/packages/multivehicle_examples/launch/px4_offboard.launch.py b/multivehicle/packages/multivehicle_examples/launch/px4_offboard.launch.py new file mode 100644 index 0000000..6873ebf --- /dev/null +++ b/multivehicle/packages/multivehicle_examples/launch/px4_offboard.launch.py @@ -0,0 +1,25 @@ +from launch import LaunchDescription +from launch_ros.actions import Node + + +def generate_launch_description(): + + return LaunchDescription( + [ + Node( + package="multivehicle_examples", + executable="px4_offboard_demo", + name="px4_offboard_demo", + output="screen", + parameters=[ + {"use_sim_time": True}, + # uXRCE-DDS topic namespace -> /x500/fmu/... . Must match + # PX4_UXRCE_DDS_NS=x500 set on the px4 SITL process. + {"vehicle_namespace": "x500"}, + # target_system for VehicleCommand: MAV_SYS_ID = instance+1, + # so 2 for `px4 ... -i 1`. + {"vehicle_id": 2}, + ], + ), + ] + ) diff --git a/multivehicle/packages/multivehicle_examples/package.xml b/multivehicle/packages/multivehicle_examples/package.xml new file mode 100644 index 0000000..2e04c18 --- /dev/null +++ b/multivehicle/packages/multivehicle_examples/package.xml @@ -0,0 +1,44 @@ + + + + multivehicle_examples + 0.1.0 + +

Example missions, controllers and demos that run on top of the + multivehicle_sim simulator: BlueROV2 square-dive mission, BlueBoat control + stack (thrust mixer + LOS waypoint controller + demo mission), and the PX4 + x500 offboard flight demo.

+ + MonkeScripts + MIT + + ament_cmake + ament_cmake_python + eigen3_cmake_module + eigen + + rclcpp + rclpy + geometry_msgs + std_msgs + sensor_msgs + nav_msgs + tf2 + tf2_ros + tf2_geometry_msgs + px4_msgs + mavros_msgs + + multivehicle_sim + multivehicle_interface + ament_index_python + launch_ros + tf_transformations + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/multivehicle/packages/multivehicle_examples/scripts/blueboat_mission.py b/multivehicle/packages/multivehicle_examples/scripts/blueboat_mission.py new file mode 100755 index 0000000..3f0364b --- /dev/null +++ b/multivehicle/packages/multivehicle_examples/scripts/blueboat_mission.py @@ -0,0 +1,80 @@ +#!/usr/bin/env python3 +"""Demo mission for the BlueBoat USV: drive a square course. + +Waits for the first ``/blueboat/odom``, then publishes a square of waypoints +(in the ``odom`` frame, relative to the boat's start position) to +``/blueboat/goal_pose``. The LOS waypoint controller follows the line between +consecutive waypoints; this node just lays down the course and then idles. + +""" + +import rclpy +from rclpy.node import Node +from nav_msgs.msg import Odometry +from geometry_msgs.msg import PoseStamped + + +class BlueBoatSquareMission(Node): + def __init__(self): + super().__init__("blueboat_square_mission") + + self.declare_parameter("side", 10.0) # m, square edge length + self.declare_parameter("frame_id", "odom") + self.side = self.get_parameter("side").value + self.frame_id = self.get_parameter("frame_id").value + + self.goal_pub = self.create_publisher(PoseStamped, "/blueboat/goal_pose", 10) + self.create_subscription(Odometry, "/blueboat/odom", self.save_odom, 10) + self._start = None + self._published = False + # Publish only once the controller is actually subscribed, so the + # course isn't dropped before discovery completes (and never twice, or + # the controller would queue duplicate waypoints). + self.create_timer(0.2, self.try_publish) + self.get_logger().info("Waiting for /blueboat/odom to lay down course...") + + def save_odom(self, msg: Odometry): + if self._start is None: + self._start = (msg.pose.pose.position.x, msg.pose.pose.position.y) + + def try_publish(self): + if self._published or self._start is None: + return + if self.goal_pub.get_subscription_count() == 0: + return + x0, y0 = self._start + s = self.side + # Axis-aligned square in the odom frame, returning to start. + waypoints = [ + (x0 + s, y0), + (x0 + s, y0 + s), + (x0, y0 + s), + (x0, y0), + ] + for wx, wy in waypoints: + msg_out = PoseStamped() + msg_out.header.frame_id = self.frame_id + msg_out.header.stamp = self.get_clock().now().to_msg() + msg_out.pose.position.x = wx + msg_out.pose.position.y = wy + msg_out.pose.orientation.w = 1.0 + self.goal_pub.publish(msg_out) + self.get_logger().info("Queued waypoint (%.2f, %.2f)" % (wx, wy)) + self._published = True + self.get_logger().info("Course published (%d waypoints)." % len(waypoints)) + + +def main(args=None): + rclpy.init(args=args) + node = BlueBoatSquareMission() + try: + rclpy.spin(node) + except KeyboardInterrupt: + pass + finally: + node.destroy_node() + rclpy.shutdown() + + +if __name__ == "__main__": + main() diff --git a/multivehicle/packages/multivehicle_examples/scripts/blueboat_thrust_mixer.py b/multivehicle/packages/multivehicle_examples/scripts/blueboat_thrust_mixer.py new file mode 100755 index 0000000..549f095 --- /dev/null +++ b/multivehicle/packages/multivehicle_examples/scripts/blueboat_thrust_mixer.py @@ -0,0 +1,152 @@ +#!/usr/bin/env python3 +"""Differential-thrust allocation (mixer) for the BlueBoat USV. + +It consumes ``geometry_msgs/Twist`` on ``/blueboat/cmd_vel`` and emits a per-thruster force (Newtons) on +``/blueboat/thrusters/{left,right}/thrust`` (std_msgs/Float64, bridged ROS->gz). + +Interpretation of the Twist: + * ``linear.x`` -> desired SURGE SPEED (m/s). Converted to a surge force by + inverting the quadratic hull drag from the model SDF (``xUabsU`` = -58.42), + so ``F_surge = surge_drag * u_d * |u_d|`` holds that speed in steady state + (open-loop). + * ``angular.z`` -> desired YAW RATE (rad/s). Converted to a yaw moment by a + gain ``yaw_gain`` (N*m per rad/s). + +Allocation (twin thrusters a moment arm ``d = track_width/2`` apart): + F_left = F_surge/2 - M_z/(2 d) + F_right = F_surge/2 + M_z/(2 d) +Positive ``angular.z`` (REP-103, CCW) therefore makes the right (starboard) +thruster push harder -> the boat yaws left, as expected. + +A watchdog publishes zero thrust if no ``cmd_vel`` arrives within +``cmd_timeout`` seconds, so a dead controller cannot leave the boat at full +throttle. +""" + +import rclpy +from rclpy.node import Node +from geometry_msgs.msg import Twist +from std_msgs.msg import Float64 + + +def _clamp(value, limit): + return max(-limit, min(limit, value)) + + +class BlueBoatThrustMixer(Node): + def __init__(self): + super().__init__("blueboat_thrust_mixer") + + # Geometry / actuator limits. + self.declare_parameter("track_width", 0.59) # m, port<->stbd spacing + self.declare_parameter("max_thrust", 30.0) # N, per-thruster clamp + # Surge: quadratic-drag feedforward (|xUabsU| from the model SDF). + self.declare_parameter("surge_drag", 58.42) # N per (m/s)^2 + self.declare_parameter("surge_lin", 0.0) # N per (m/s), optional + self.declare_parameter("v_max", 2.0) # m/s input clamp + # Yaw: desired yaw-rate -> yaw moment. + self.declare_parameter("yaw_gain", 30.0) # N*m per (rad/s) + self.declare_parameter("w_max", 1.0) # rad/s input clamp + self.declare_parameter("invert_yaw", False) # flip turn direction + # Topics / timing. + self.declare_parameter("cmd_vel_topic", "/blueboat/cmd_vel") + self.declare_parameter("left_thrust_topic", "/blueboat/thrusters/left/thrust") + self.declare_parameter("right_thrust_topic", "/blueboat/thrusters/right/thrust") + self.declare_parameter("publish_rate_hz", 20.0) + self.declare_parameter("cmd_timeout", 0.5) # s, watchdog + + self.track_width = self.get_parameter("track_width").value + self.max_thrust = self.get_parameter("max_thrust").value + self.surge_drag = self.get_parameter("surge_drag").value + self.surge_lin = self.get_parameter("surge_lin").value + self.v_max = self.get_parameter("v_max").value + self.yaw_gain = self.get_parameter("yaw_gain").value + self.w_max = self.get_parameter("w_max").value + self.invert_yaw = self.get_parameter("invert_yaw").value + self.cmd_timeout = self.get_parameter("cmd_timeout").value + rate = self.get_parameter("publish_rate_hz").value + + self.half_track = max(self.track_width / 2.0, 1e-3) + + self.left_pub = self.create_publisher( + Float64, self.get_parameter("left_thrust_topic").value, 10 + ) + self.right_pub = self.create_publisher( + Float64, self.get_parameter("right_thrust_topic").value, 10 + ) + self.create_subscription( + Twist, self.get_parameter("cmd_vel_topic").value, self.cmd_vel_sub, 10 + ) + + self._last_cmd = Twist() + self._last_cmd_time = None + self.create_timer(1.0 / rate, self.publish_thrust) + + self.get_logger().info( + "BlueBoat thrust mixer up: track_width=%.3f m, max_thrust=%.1f N, " + "surge_drag=%.2f, yaw_gain=%.2f, invert_yaw=%s" + % ( + self.track_width, + self.max_thrust, + self.surge_drag, + self.yaw_gain, + self.invert_yaw, + ) + ) + + def cmd_vel_sub(self, msg: Twist): + self._last_cmd = msg + self._last_cmd_time = self.get_clock().now() + + def publish_thrust(self): + # Watchdog: zero thrust if the command is stale (or never received). + stale = self._last_cmd_time is None or ( + (self.get_clock().now() - self._last_cmd_time).nanoseconds * 1e-9 + > self.cmd_timeout + ) + if stale: + self._send(0.0, 0.0) + return + + u = _clamp(self._last_cmd.linear.x, self.v_max) + w = _clamp(self._last_cmd.angular.z, self.w_max) + if self.invert_yaw: + w = -w + + # Surge speed -> force via quadratic-drag inversion (+ optional linear). + f_surge = self.surge_drag * u * abs(u) + self.surge_lin * u + # Desired yaw rate -> yaw moment. + m_z = self.yaw_gain * w + + f_left = f_surge / 2.0 - m_z / (2.0 * self.half_track) + f_right = f_surge / 2.0 + m_z / (2.0 * self.half_track) + + # Proportional clamp: scale both sides together so the turn ratio (and + # thus the commanded heading) is preserved under saturation. + peak = max(abs(f_left), abs(f_right), self.max_thrust) + if peak > self.max_thrust: + scale = self.max_thrust / peak + f_left *= scale + f_right *= scale + + self._send(f_left, f_right) + + def _send(self, left, right): + self.left_pub.publish(Float64(data=float(left))) + self.right_pub.publish(Float64(data=float(right))) + + +def main(args=None): + rclpy.init(args=args) + node = BlueBoatThrustMixer() + try: + rclpy.spin(node) + except KeyboardInterrupt: + pass + finally: + node.destroy_node() + rclpy.shutdown() + + +if __name__ == "__main__": + main() diff --git a/multivehicle/packages/multivehicle_examples/scripts/blueboat_waypoint_controller.py b/multivehicle/packages/multivehicle_examples/scripts/blueboat_waypoint_controller.py new file mode 100755 index 0000000..1a90bc8 --- /dev/null +++ b/multivehicle/packages/multivehicle_examples/scripts/blueboat_waypoint_controller.py @@ -0,0 +1,352 @@ +#!/usr/bin/env python3 +"""Line-of-Sight (LOS) waypoint controller for the BlueBoat USV. + +Guidance + control layers of the BlueBoat GNC stack. It turns a list of +waypoints into a ``geometry_msgs/Twist`` on ``/blueboat/cmd_vel`` for the thrust +mixer to allocate. + +Line-of-Sight path following (Fossen): + Track the *line between consecutive waypoints*, not the raw point, so the boat + drives the cross-track error ``e`` to zero instead of spinning toward / over- + shooting each point. For a leg from ``p_prev`` to ``p_tgt`` with path angle + ``gamma``: + e = -(x - x_prev) sin(gamma) + (y - y_prev) cos(gamma) # +ve = left + Delta = clamp(delta_min + k_delta |e|, delta_min, delta_max) # adaptive lookahead + psi_d = gamma + atan2(-e, Delta) + A leg is finished when the boat passes the target (along-track progress >= leg + length) or enters ``acceptance_radius``. + +Control (heading psi & yaw-rate r from the pose/twist; surge u from twist.linear.x): + * Heading: PID on wrap(psi_d - psi) with derivative-on-measurement (-Kd*r, + no setpoint kick on waypoint switch) and an anti-windup integrator -> + desired yaw rate ``angular.z``. + * Surge: cruise speed shaped by heading error and by the distance-to-goal slowdown -> desired ``linear.x``: + u_d = u_cruise * max(0, cos(psi_err))**p * sat(dist_to_goal / r_slow) + +Gait (``turn_in_place``) -- stop-and-turn at every waypoint: + INIT -> ALIGN -> DRIVE -> (ALIGN -> DRIVE)* -> HOLD. + * ALIGN: rotate in place to the next leg's heading while station-keeping the + waypoint (surge nulls the along-body position error, braking any arrival + momentum). Exits once the heading is aligned and the yaw rate has settled. + * DRIVE: LOS path following; slows approaching *every* waypoint so the boat + arrives slow enough to stop and turn (not just the final one). + * HOLD: actively station-keep the final pose instead of going limp -- the boat + holds position/heading rather than drifting. The USV is underactuated + (surge + yaw only, no sway), so a pure sideways offset is corrected only by + re-pointing at the hold point (``repoint_radius``); within + ``acceptance_radius`` the surge deadbands to avoid thruster jitter. + +Inputs: ``/blueboat/odom`` (nav_msgs/Odometry); ``/blueboat/goal_pose`` + (geometry_msgs/PoseStamped, appended to the path); ``waypoints`` param + (flat [x0,y0,x1,y1,...]) for a preloaded course. +Outputs: ``/blueboat/cmd_vel`` (geometry_msgs/Twist); + ``/blueboat/goal_reached`` (std_msgs/Bool, latched true when the final + waypoint is reached). +""" + +import math + +import rclpy +from rclpy.node import Node +from rclpy.qos import QoSProfile, DurabilityPolicy +from rcl_interfaces.msg import ParameterDescriptor +from nav_msgs.msg import Odometry +from geometry_msgs.msg import PoseStamped, Twist +from std_msgs.msg import Bool + + +def wrap_to_pi(angle): + """Wrap an angle to (-pi, pi].""" + return math.atan2(math.sin(angle), math.cos(angle)) + + +def yaw_from_quaternion(q): + """Extract yaw (rad) from a geometry_msgs/Quaternion.""" + siny_cosp = 2.0 * (q.w * q.z + q.x * q.y) + cosy_cosp = 1.0 - 2.0 * (q.y * q.y + q.z * q.z) + return math.atan2(siny_cosp, cosy_cosp) + + +def clamp(value, limit): + return max(-limit, min(limit, value)) + + +class BlueBoatWaypointController(Node): + def __init__(self): + super().__init__("blueboat_waypoint_controller") + + self.declare_parameter("delta_min", 2.5) # m, min lookahead + self.declare_parameter("delta_max", 8.0) # m, max lookahead + self.declare_parameter("k_delta", 1.0) # lookahead growth per |e| + self.declare_parameter("acceptance_radius", 0.2) # m, leg-done radius + self.declare_parameter("u_cruise", 1.0) # m/s nominal surge + self.declare_parameter("turn_shaping_p", 1.5) # cos() exponent + self.declare_parameter("r_slow", 4.0) # m, final-goal slowdown + # --- Turn-in-place / station-keeping gait ---------------------------- + self.declare_parameter("turn_in_place", True) # stop+rotate at each wp + self.declare_parameter("arrival_radius", 0.6) # m, DRIVE->ALIGN switch + self.declare_parameter("align_tol_deg", 3.0) # deg, heading-aligned exit + self.declare_parameter("align_settle_degps", 5.0) # deg/s, settled exit + self.declare_parameter("kp_pos", 0.7) # (m/s)/m, station-keep surge gain + self.declare_parameter("u_align_max", 0.5) # m/s, station-keep surge clamp + self.declare_parameter("repoint_radius", 1.0) # m, HOLD: drive back if drift>this + self.declare_parameter("v_settle", 0.1) # m/s, stop before turning in place + # --- Heading PID (outputs desired yaw rate, rad/s) ------------------- + self.declare_parameter("kp_yaw", 2.0) + self.declare_parameter("ki_yaw", 0.0) + self.declare_parameter("kd_yaw", 0.5) + self.declare_parameter("w_max", 1.0) # rad/s output clamp + self.declare_parameter("i_max", 0.5) # integrator clamp (rad/s) + self.declare_parameter("control_rate_hz", 20.0) + + self.declare_parameter("odom_topic", "/blueboat/odom") + self.declare_parameter("cmd_vel_topic", "/blueboat/cmd_vel") + self.declare_parameter("goal_pose_topic", "/blueboat/goal_pose") + + self.delta_min = self.get_parameter("delta_min").value + self.delta_max = self.get_parameter("delta_max").value + self.k_delta = self.get_parameter("k_delta").value + self.accept_r = self.get_parameter("acceptance_radius").value + self.u_cruise = self.get_parameter("u_cruise").value + self.shaping_p = self.get_parameter("turn_shaping_p").value + self.r_slow = self.get_parameter("r_slow").value + self.turn_in_place = self.get_parameter("turn_in_place").value + self.arrival_radius = self.get_parameter("arrival_radius").value + self.align_tol = math.radians(self.get_parameter("align_tol_deg").value) + self.align_settle = math.radians(self.get_parameter("align_settle_degps").value) + self.kp_pos = self.get_parameter("kp_pos").value + self.u_align_max = self.get_parameter("u_align_max").value + self.repoint_radius = self.get_parameter("repoint_radius").value + self.v_settle = self.get_parameter("v_settle").value + self.kp_yaw = self.get_parameter("kp_yaw").value + self.ki_yaw = self.get_parameter("ki_yaw").value + self.kd_yaw = self.get_parameter("kd_yaw").value + self.w_max = self.get_parameter("w_max").value + self.i_max = self.get_parameter("i_max").value + self.dt = 1.0 / self.get_parameter("control_rate_hz").value + + # --- Path state ------------------------------------------------------ + self.waypoints = [] + self.wp_idx = 0 + self.seg_start = None # (x, y) start of the current leg + self.first_leg_start = None # captured from odom when motion begins + + self.odom = None + self.yaw_integral = 0.0 + self.finished = False + # Gait state: INIT -> ALIGN -> DRIVE -> (ALIGN -> DRIVE)* -> HOLD. + self.mode = "INIT" + self.hold_xy = None # (x, y) pose held while aligning / station-keeping + self.target_heading = 0.0 # desired heading (rad) during ALIGN / HOLD + + # --- I/O ------------------------------------------------------------- + self.cmd_pub = self.create_publisher( + Twist, self.get_parameter("cmd_vel_topic").value, 10 + ) + latched = QoSProfile(depth=1, durability=DurabilityPolicy.TRANSIENT_LOCAL) + self.reached_pub = self.create_publisher( + Bool, "/blueboat/goal_reached", latched + ) + self.create_subscription( + Odometry, self.get_parameter("odom_topic").value, self.odom_sub, 10 + ) + self.create_subscription( + PoseStamped, self.get_parameter("goal_pose_topic").value, self.goal_sub, 10 + ) + + self.create_timer(self.dt, self.control_step) + self.get_logger().info( + "BlueBoat waypoint controller up: %d preloaded waypoint(s), " + "u_cruise=%.2f m/s, lookahead=[%.1f, %.1f] m" + % (len(self.waypoints), self.u_cruise, self.delta_min, self.delta_max) + ) + + def odom_sub(self, msg: Odometry): + self.odom = msg + + def goal_sub(self, msg: PoseStamped): + """Append a goal to the path and resume if we had finished.""" + self.waypoints.append((msg.pose.position.x, msg.pose.position.y)) + if self.finished: + self.finished = False + self.reached_pub.publish(Bool(data=False)) + self.get_logger().info( + "Goal appended (%.2f, %.2f); %d waypoint(s) queued" + % (msg.pose.position.x, msg.pose.position.y, len(self.waypoints)) + ) + + def control_step(self): + if self.odom is None or not self.waypoints: + return + + x = self.odom.pose.pose.position.x + y = self.odom.pose.pose.position.y + yaw = yaw_from_quaternion(self.odom.pose.pose.orientation) + r = self.odom.twist.twist.angular.z # yaw rate (rad/s) + + if self.first_leg_start is None: + self.first_leg_start = (x, y) + + # INIT: align to the first leg from rest (skip straight to DRIVE if the + # turn-in-place gait is disabled). + if self.mode == "INIT": + self.seg_start = (x, y) + self.hold_xy = (x, y) + self.target_heading = self.leg_heading((x, y), self.waypoints[0]) + self.mode = "ALIGN" if self.turn_in_place else "DRIVE" + + # A goal appended after finishing re-opens the course -> resume. + if self.finished and self.wp_idx < len(self.waypoints): + self.finished = False + self.reached_pub.publish(Bool(data=False)) + self.seg_start = (x, y) + self.hold_xy = (x, y) + self.target_heading = self.leg_heading((x, y), self.waypoints[self.wp_idx]) + self.mode = "ALIGN" if self.turn_in_place else "DRIVE" + + # All waypoints consumed -> actively station-keep the final pose. + if self.wp_idx >= len(self.waypoints): + if not self.finished: + self.finished = True + self.reached_pub.publish(Bool(data=True)) + self.get_logger().info("Final waypoint reached. Station-keeping.") + self.mode = "HOLD" + self.station_keep(x, y, yaw, r, allow_repoint=True) + return + + # --- ALIGN: brake to a stop, THEN rotate in place to the next leg ---- + if self.mode == "ALIGN": + u_now = self.odom.twist.twist.linear.x + e_x = self.along_body_err(x, y, yaw) + u_cmd = clamp(self.kp_pos * e_x, self.u_align_max) + if abs(u_now) > self.v_settle: + # Still coasting in from the leg: decelerate in a straight line + # (no yaw) so the boat doesn't swing its nose while moving. + # The turn only begins once it has actually stopped. + self.publish_cmd(u_cmd, 0.0) + return + w_cmd, psi_err = self.heading_pid(self.target_heading, yaw, r) + self.publish_cmd(u_cmd, w_cmd) + if abs(psi_err) < self.align_tol and abs(r) < self.align_settle: + self.mode = "DRIVE" + self.yaw_integral = 0.0 + self.get_logger().info("Leg %d aligned -> driving." % self.wp_idx) + return + + # --- DRIVE: Line-of-Sight path following ---------------------------- + sx, sy = self.seg_start if self.seg_start is not None else self.first_leg_start + tx, ty = self.waypoints[self.wp_idx] + seg_dx, seg_dy = tx - sx, ty - sy + seg_len = math.hypot(seg_dx, seg_dy) + dist_to_target = math.hypot(tx - x, ty - y) + + if seg_len < 1e-3: + # Degenerate leg: just aim straight at the target. + psi_d = math.atan2(ty - y, tx - x) + along = 0.0 + else: + gamma = math.atan2(seg_dy, seg_dx) + # Signed cross-track error (positive to the left of the path). + e = -(x - sx) * math.sin(gamma) + (y - sy) * math.cos(gamma) + # Along-track progress along the leg. + along = (x - sx) * math.cos(gamma) + (y - sy) * math.sin(gamma) + delta = max( + self.delta_min, + min(self.delta_max, self.delta_min + self.k_delta * abs(e)), + ) + psi_d = gamma + math.atan2(-e, delta) + + # Reached the waypoint: stop and (if enabled) turn in place to the next. + passed = seg_len > 1e-3 and along >= seg_len + if dist_to_target < self.arrival_radius or passed: + self.get_logger().info("Leg %d reached." % self.wp_idx) + self.seg_start = (tx, ty) + self.hold_xy = (tx, ty) + nxt = self.wp_idx + 1 + if nxt < len(self.waypoints): + self.target_heading = self.leg_heading((tx, ty), self.waypoints[nxt]) + else: + self.target_heading = yaw # final: hold the heading we arrived on + self.wp_idx = nxt + self.yaw_integral = 0.0 + if self.turn_in_place and nxt < len(self.waypoints): + self.mode = "ALIGN" + return # next tick handles the new leg / HOLD + + # --- Heading PID + surge -------------------------------------------- + w_cmd, psi_err = self.heading_pid(psi_d, yaw, r) + # Turn-then-go: cut speed while heading error is large. + turn_factor = max(0.0, math.cos(psi_err)) ** self.shaping_p + # Slow approaching EVERY waypoint so the boat arrives slow enough to stop. + slow = min(1.0, dist_to_target / self.r_slow) + u_cmd = self.u_cruise * turn_factor * slow + self.publish_cmd(u_cmd, w_cmd) + + # ---------------------------------------------------------------- helpers + def publish_cmd(self, u, w): + cmd = Twist() + cmd.linear.x = float(u) + cmd.angular.z = float(w) + self.cmd_pub.publish(cmd) + + def heading_pid(self, psi_d, yaw, r): + """Heading PID -> desired yaw rate. Returns (w_cmd, psi_err).""" + psi_err = wrap_to_pi(psi_d - yaw) + if self.ki_yaw > 0.0: + # Integrate, then clamp so the integral CONTRIBUTION stays <= i_max. + self.yaw_integral += psi_err * self.dt + self.yaw_integral = clamp(self.yaw_integral, self.i_max / self.ki_yaw) + else: + self.yaw_integral = 0.0 + # Derivative on measurement (-Kd*r) avoids a kick when psi_d jumps. + w_cmd = clamp( + self.kp_yaw * psi_err + self.ki_yaw * self.yaw_integral - self.kd_yaw * r, + self.w_max, + ) + return w_cmd, psi_err + + def along_body_err(self, x, y, yaw): + """Position error to hold_xy projected on the body x-axis (+ = ahead).""" + dx = self.hold_xy[0] - x + dy = self.hold_xy[1] - y + return dx * math.cos(yaw) + dy * math.sin(yaw) + + @staticmethod + def leg_heading(a, b): + """Heading (rad) of the line from point a to point b.""" + return math.atan2(b[1] - a[1], b[0] - a[0]) + + def station_keep(self, x, y, yaw, r, allow_repoint): + """Hold ``hold_xy``: surge nulls the along-body error while the heading + holds ``target_heading``. If drift exceeds ``repoint_radius`` the boat + points back at the hold point (the only way an underactuated USV can + correct a sideways offset). Within ``acceptance_radius`` the surge + deadbands so the thrusters don't jitter on station.""" + rng = math.hypot(self.hold_xy[0] - x, self.hold_xy[1] - y) + if rng < self.accept_r: + w_cmd, _ = self.heading_pid(self.target_heading, yaw, r) + self.publish_cmd(0.0, w_cmd) + return + if allow_repoint and rng > self.repoint_radius: + psi_d = math.atan2(self.hold_xy[1] - y, self.hold_xy[0] - x) + else: + psi_d = self.target_heading + w_cmd, _ = self.heading_pid(psi_d, yaw, r) + u_cmd = clamp(self.kp_pos * self.along_body_err(x, y, yaw), self.u_align_max) + self.publish_cmd(u_cmd, w_cmd) + + +def main(args=None): + rclpy.init(args=args) + node = BlueBoatWaypointController() + try: + rclpy.spin(node) + except KeyboardInterrupt: + pass + finally: + node.destroy_node() + rclpy.shutdown() + + +if __name__ == "__main__": + main() diff --git a/multivehicle/packages/multivehicle_examples/scripts/bluerov_movement.py b/multivehicle/packages/multivehicle_examples/scripts/bluerov_movement.py new file mode 100755 index 0000000..8bb1284 --- /dev/null +++ b/multivehicle/packages/multivehicle_examples/scripts/bluerov_movement.py @@ -0,0 +1,270 @@ +#!/usr/bin/env python3 + +import math + +import rclpy +import rclpy.duration +import rclpy.time +from rclpy.node import Node +from geometry_msgs.msg import PoseStamped +from mavros_msgs.srv import SetMode, CommandBool +from rclpy.qos import QoSProfile, ReliabilityPolicy +import tf2_ros +from tf2_geometry_msgs import do_transform_pose_stamped + + +class BlueROVSquareMission(Node): + """Drive the BlueROV2 in a square relative to its own base_link. + + Each leg is a body-frame FLU offset (forward, left) applied from the pose at + the START of that leg, while the vehicle HOLDS its initial heading — i.e. it + strafes the square (forward, left, back, right) instead of yawing to face + each waypoint. This matches how an ROV actually moves and avoids the + spinning behaviour you get from publishing absolute map setpoints / facing + each target. + + Each leg's body-frame offset is transformed into a map-frame + `setpoint_position/local` target with tf2 (`do_transform_pose_stamped` + against the live `map -> base_link` transform MAVROS publishes), so the + square is always relative to the vehicle's own base_link, never the map + origin. + """ + + # TF frames: MAVROS local_position publishes map -> base_link (tf.send=true). + MAP_FRAME = "map" + BASE_FRAME = "base_link" + + def __init__(self): + super().__init__("bluerov_square_mission") + + mavros_qos = QoSProfile(depth=10, reliability=ReliabilityPolicy.BEST_EFFORT) + + self.target_pub = self.create_publisher( + PoseStamped, "/mavros/setpoint_position/local", mavros_qos + ) + self.curr_pose_sub = self.create_subscription( + PoseStamped, "/mavros/local_position/pose", self.pose_callback, mavros_qos + ) + + # tf2 buffer/listener — used to transform base_link offsets to map. + self.tf_buffer = tf2_ros.Buffer() + self.tf_listener = tf2_ros.TransformListener(self.tf_buffer, self) + + # Service Clients + self.arming_client = self.create_client(CommandBool, "/mavros/cmd/arming") + self.mode_client = self.create_client(SetMode, "/mavros/set_mode") + + # Service state flags + self.is_armed = False + self.is_guided_mode = False + self.services_initialized = False + + # Mission Params + self.depth = -2.0 # absolute dive depth (m below surface) + self.dist_threshold = 0.2 + + # Per-leg body-frame FLU offsets (forward+, left+). Each leg is applied + # from the pose at the start of that leg, with heading held constant -> + # the vehicle strafes a square and returns to start. + self.legs = [ + (5.0, 0.0), # forward 5 m + (0.0, 5.0), # left 5 m + (-5.0, 0.0), # backward 5 m + (0.0, -5.0), # right 5 m + ] + self.leg_index = 0 + + # Heading held for the whole mission (captured at START). + self.hold_yaw = None + # Current leg's absolute map-frame target (computed when the leg begins). + self.target_x = None + self.target_y = None + + self.current_pose = None + self.state = "INIT" + + # IMPORTANT NOTE: DO NOT SET A HIGH SETPOINT PUBLISH RATE. + # ROV WOULD KEEP REPLANNING ITS TRAJECTORY, CAUSING IT TO MOVE VERY SLOWLY + self.timer = self.create_timer(1.0, self.main_loop) + + def arm(self): + """Request arming""" + if not self.arming_client.service_is_ready(): + self.get_logger().info("Waiting for arming service...") + return False + + self.get_logger().info("Requesting arm...") + request = CommandBool.Request() + request.value = True + future = self.arming_client.call_async(request) + future.add_done_callback(self.arm_callback) + return True + + def arm_callback(self, future): + """Callback for arming service""" + try: + response = future.result() + if response.success: + self.is_armed = True + self.get_logger().info("Vehicle armed successfully") + else: + self.get_logger().warn("Failed to arm vehicle") + except Exception as e: + self.get_logger().error(f"Arming service call failed: {e}") + + def set_guided_mode(self): + """Request GUIDED mode""" + if not self.mode_client.service_is_ready(): + self.get_logger().info("Waiting for SetMode service...") + return False + + self.get_logger().info("Requesting GUIDED mode...") + req = SetMode.Request() + req.custom_mode = "GUIDED" + future = self.mode_client.call_async(req) + future.add_done_callback(self.guided_mode_callback) + return True + + def guided_mode_callback(self, future): + """Callback for setting to guided mode""" + try: + response = future.result() + if response.mode_sent: + self.is_guided_mode = True + self.get_logger().info("GUIDED mode set successfully") + else: + self.get_logger().warn("Failed to set GUIDED mode") + except Exception as e: + self.get_logger().error(f"Set mode service call failed: {e}") + + def pose_callback(self, msg): + self.current_pose = msg + + def get_quaternion_from_yaw(self, yaw): + """Convert yaw (radians) to a quaternion for PoseStamped.""" + return {"x": 0.0, "y": 0.0, "z": math.sin(yaw / 2.0), "w": math.cos(yaw / 2.0)} + + def get_yaw_from_quaternion(self, quaternion): + """Extract yaw (radians) from a quaternion.""" + siny_cosp = 2.0 * (quaternion.w * quaternion.z + quaternion.x * quaternion.y) + cosy_cosp = 1.0 - 2.0 * ( + quaternion.y * quaternion.y + quaternion.z * quaternion.z + ) + return math.atan2(siny_cosp, cosy_cosp) + + def leg_target_in_map(self, fwd, left): + """Transform a body-frame FLU offset (forward, left) at the current + base_link into a map-frame (x, y) target via tf2. Returns None if the + map -> base_link transform isn't available yet. + """ + pose_in_base = PoseStamped() + pose_in_base.header.frame_id = self.BASE_FRAME + pose_in_base.pose.position.x = fwd + pose_in_base.pose.position.y = left + pose_in_base.pose.position.z = 0.0 + pose_in_base.pose.orientation.w = 1.0 + try: + tf = self.tf_buffer.lookup_transform( + self.MAP_FRAME, + self.BASE_FRAME, + rclpy.time.Time(), + timeout=rclpy.duration.Duration(seconds=1.0), + ) + except tf2_ros.TransformException as e: + self.get_logger().warn( + f"TF {self.MAP_FRAME} <- {self.BASE_FRAME} not ready: {e}" + ) + return None + target = do_transform_pose_stamped(pose_in_base, tf) + return target.pose.position.x, target.pose.position.y + + def main_loop(self): + """Main state machine""" + + if self.state == "INIT": + if not self.services_initialized: + self.is_armed or self.arm() + self.is_guided_mode or self.set_guided_mode() + + if self.is_armed and self.is_guided_mode: + self.services_initialized = True + self.state = "START" + self.get_logger().info("Initialization complete, starting mission") + + elif self.state == "START": + if self.current_pose is None: + self.get_logger().info("Waiting for pose data...") + return + # Hold the heading the vehicle starts with for the whole square. + self.hold_yaw = self.get_yaw_from_quaternion( + self.current_pose.pose.orientation + ) + self.leg_index = 0 + self.state = "PLAN_LEG" + self.get_logger().info( + f"Beginning body-relative square (hold_yaw={self.hold_yaw:.2f})" + ) + + elif self.state == "PLAN_LEG": + # Transform this leg's body-frame offset (relative to the CURRENT + # base_link) into a map-frame target via tf2. Retry next tick if the + # transform isn't available yet. + fwd, left = self.legs[self.leg_index] + target = self.leg_target_in_map(fwd, left) + if target is None: + return + self.target_x, self.target_y = target + self.state = "MOVING" + self.get_logger().info( + f"Leg {self.leg_index} (fwd={fwd}, left={left}) -> " + f"target ({self.target_x:.2f}, {self.target_y:.2f})" + ) + + elif self.state == "MOVING": + # Publish the (fixed) leg target, holding the mission heading. + q = self.get_quaternion_from_yaw(self.hold_yaw) + msg = PoseStamped() + msg.header.stamp = self.get_clock().now().to_msg() + msg.header.frame_id = "map" + msg.pose.position.x = self.target_x + msg.pose.position.y = self.target_y + msg.pose.position.z = self.depth + msg.pose.orientation.x = q["x"] + msg.pose.orientation.y = q["y"] + msg.pose.orientation.z = q["z"] + msg.pose.orientation.w = q["w"] + self.target_pub.publish(msg) + + dx = self.target_x - self.current_pose.pose.position.x + dy = self.target_y - self.current_pose.pose.position.y + dist = math.hypot(dx, dy) + self.get_logger().info( + f"Leg {self.leg_index}: dist to target {dist:.2f} m" + ) + + if dist < self.dist_threshold: + self.get_logger().info(f"Leg {self.leg_index} reached!") + self.leg_index += 1 + if self.leg_index >= len(self.legs): + self.state = "FINISHED" + else: + self.state = "PLAN_LEG" + + elif self.state == "FINISHED": + self.get_logger().info("Mission successful.") + raise SystemExit + + +def main(args=None): + rclpy.init(args=args) + node = BlueROVSquareMission() + try: + rclpy.spin(node) + except SystemExit: + pass + node.destroy_node() + rclpy.shutdown() + + +if __name__ == "__main__": + main() diff --git a/multivehicle/packages/multivehicle_examples/src/PX4OffboardDemo.cpp b/multivehicle/packages/multivehicle_examples/src/PX4OffboardDemo.cpp new file mode 100644 index 0000000..6ec8369 --- /dev/null +++ b/multivehicle/packages/multivehicle_examples/src/PX4OffboardDemo.cpp @@ -0,0 +1,227 @@ +#include "PX4OffboardDemo.hpp" + +#include +#include + +using namespace std::chrono_literals; +using std::placeholders::_1; + +using px4_msgs::msg::OffboardControlMode; +using px4_msgs::msg::TrajectorySetpoint; +using px4_msgs::msg::VehicleCommand; +using px4_msgs::msg::VehicleLandDetected; +using px4_msgs::msg::VehicleLocalPosition; +using px4_msgs::msg::VehicleStatus; + +namespace { +// Wrap an angle into (-pi, pi]. +float wrapToPi(float angle) { + return std::atan2(std::sin(angle), std::cos(angle)); +} +} // namespace + +OffboardDemo::OffboardDemo() : Node("px4_offboard_demo") { + _ns = this->declare_parameter("vehicle_namespace", ""); + _sys_id = this->declare_parameter("vehicle_id", 0); + _prefix = _ns.empty() ? std::string() : "/" + _ns; + + RCLCPP_INFO(get_logger(), "vehicle_namespace='%s' (topic prefix '%s'), target_system=%d", + _ns.c_str(), _prefix.c_str(), _sys_id); + + _cmd_pub = create_publisher(_prefix + "/fmu/in/vehicle_command", 10); + _setpoint_pub = create_publisher(_prefix + "/fmu/in/trajectory_setpoint", 10); + _offboard_mode_pub = create_publisher(_prefix + "/fmu/in/offboard_control_mode", 10); + + // PX4 publishes telemetry best-effort; match it or subscriptions stay empty. + _status_sub = create_subscription( + _prefix + "/fmu/out/vehicle_status_v1", rclcpp::QoS(1).best_effort(), + std::bind(&OffboardDemo::onStatus, this, _1)); + _position_sub = create_subscription( + _prefix + "/fmu/out/vehicle_local_position", rclcpp::QoS(10).best_effort(), + std::bind(&OffboardDemo::onLocalPosition, this, _1)); + _land_sub = create_subscription( + _prefix + "/fmu/out/vehicle_land_detected", rclcpp::QoS(1).best_effort(), + std::bind(&OffboardDemo::onLandDetected, this, _1)); + + // Stream the offboard-mode heartbeat independently of the mission tick. + _heartbeat_timer = create_wall_timer(100ms, std::bind(&OffboardDemo::streamOffboardHeartbeat, this)); + + _phase_since = now(); + + // Prime the setpoint stream with a no-op so PX4 sees a setpoint before we ask + // for offboard mode. + publishPosition(Eigen::Vector3f(NAN, NAN, NAN), NAN); + + RCLCPP_INFO(get_logger(), "OffboardDemo initialised."); +} + +void OffboardDemo::onStatus(const VehicleStatus::SharedPtr msg) { _status = *msg; } + +void OffboardDemo::onLocalPosition(const VehicleLocalPosition::SharedPtr msg) { _position = *msg; } + +void OffboardDemo::onLandDetected(const VehicleLandDetected::SharedPtr msg) { _landed = msg->landed; } + +uint64_t OffboardDemo::nowMicros() const { + return static_cast(this->now().nanoseconds() / 1000); +} + +void OffboardDemo::streamOffboardHeartbeat() { + OffboardControlMode mode{}; + mode.timestamp = nowMicros(); + mode.position = true; // we only ever command position setpoints + _offboard_mode_pub->publish(mode); +} + +void OffboardDemo::publishPosition(const Eigen::Vector3f& pos_ned, float yaw) { + TrajectorySetpoint sp{}; + sp.timestamp = nowMicros(); + sp.position = {pos_ned.x(), pos_ned.y(), pos_ned.z()}; + sp.yaw = yaw; // NAN = let PX4 hold the current heading + _setpoint_pub->publish(sp); +} + +void OffboardDemo::sendCommand(uint32_t command, float param1, float param2) { + VehicleCommand cmd{}; + cmd.timestamp = nowMicros(); + cmd.command = command; + cmd.param1 = param1; + cmd.param2 = param2; + cmd.target_system = static_cast(_sys_id); + cmd.target_component = 1; + cmd.source_system = 1; + cmd.source_component = 1; + cmd.from_external = true; + _cmd_pub->publish(cmd); +} + +void OffboardDemo::requestOffboardMode() { + // base mode = custom (1), custom main mode = OFFBOARD (6). + sendCommand(VehicleCommand::VEHICLE_CMD_DO_SET_MODE, 1.0f, 6.0f); +} + +void OffboardDemo::arm() { + sendCommand(VehicleCommand::VEHICLE_CMD_COMPONENT_ARM_DISARM, 1.0f); +} + +void OffboardDemo::disarm() { + sendCommand(VehicleCommand::VEHICLE_CMD_COMPONENT_ARM_DISARM, 0.0f); +} + +void OffboardDemo::commandLand() { + sendCommand(VehicleCommand::VEHICLE_CMD_NAV_LAND); +} + +bool OffboardDemo::phaseElapsed(double seconds) const { + return (now() - _phase_since).seconds() > seconds; +} + +bool OffboardDemo::reached(const Eigen::Vector3f& target, float tol) const { + return std::fabs(_position.x - target.x()) < tol && + std::fabs(_position.y - target.y()) < tol && + std::fabs(_position.z - target.z()) < tol; +} + +bool OffboardDemo::yawReached(float target_yaw, float tol) const { + return std::fabs(wrapToPi(target_yaw - _position.heading)) < tol; +} + +void OffboardDemo::transitionTo(Phase next, const char* note) { + _phase = next; + _phase_since = now(); + RCLCPP_INFO(get_logger(), "%s", note); +} + +void OffboardDemo::tick() { + const Eigen::Vector3f standoff(kStandoffNorth, kStandoffEast, kStandoffDown); + + switch (_phase) { + case Phase::WaitForReady: + // Let the autopilot settle, then latch the launch point as home (climbed + // up by kClimbHeight) and request offboard. + if (_status.pre_flight_checks_pass && phaseElapsed(kReadyHold)) { + _home_ned = Eigen::Vector3f(_position.x, _position.y, _position.z - kClimbHeight); + requestOffboardMode(); + transitionTo(Phase::RequestArm, "Ready -> requesting OFFBOARD"); + } + break; + + case Phase::RequestArm: + if (_status.nav_state == VehicleStatus::NAVIGATION_STATE_OFFBOARD) { + arm(); + transitionTo(Phase::AwaitArmed, "OFFBOARD -> arming"); + } + break; + + case Phase::AwaitArmed: + if (_status.arming_state == VehicleStatus::ARMING_STATE_ARMED) { + transitionTo(Phase::ClimbOut, "Armed -> climbing out"); + } + break; + + case Phase::ClimbOut: + // Hold the home setpoint; the vehicle climbs to it. Move on after a fixed + // settle so we reach altitude before transiting. + publishPosition(_home_ned, NAN); + if (phaseElapsed(kClimbHold)) { + transitionTo(Phase::GoToStandoff, "Climb-out -> standoff transit"); + } + break; + + case Phase::GoToStandoff: + publishPosition(standoff, NAN); + if (reached(standoff)) { + _scan_yaw_target = _position.heading; + _scan_yaw_swept = 0.0f; + transitionTo(Phase::ScanYaw, "At standoff -> yaw scan"); + } + break; + + case Phase::ScanYaw: + // Hold position and step the yaw target one notch each time we settle on + // the previous one, until a full revolution has been swept. + if (yawReached(_scan_yaw_target)) { + _scan_yaw_target = wrapToPi(_scan_yaw_target + kYawStep); + _scan_yaw_swept += kYawStep; + } + publishPosition(standoff, _scan_yaw_target); + if (_scan_yaw_swept >= 2.0f * static_cast(M_PI)) { + transitionTo(Phase::ReturnHome, "Yaw scan complete -> returning home"); + } + break; + + case Phase::ReturnHome: + publishPosition(_home_ned, NAN); + if (reached(_home_ned)) { + commandLand(); + transitionTo(Phase::Descend, "Home -> landing"); + } + break; + + case Phase::Descend: + if (_landed) { + disarm(); + transitionTo(Phase::Finished, "Touchdown -> disarmed"); + } + break; + + case Phase::Finished: + break; + } +} + +void OffboardDemo::run() { + while (rclcpp::ok() && _phase != Phase::Finished) { + rclcpp::spin_some(this->get_node_base_interface()); + tick(); + rclcpp::sleep_for(10ms); + } + RCLCPP_INFO(get_logger(), "Mission complete."); +} + +int main(int argc, char* argv[]) { + rclcpp::init(argc, argv); + auto node = std::make_shared(); + node->run(); + rclcpp::shutdown(); + return 0; +} diff --git a/multivehicle/packages/multivehicle_examples/src/PX4OffboardDemo.hpp b/multivehicle/packages/multivehicle_examples/src/PX4OffboardDemo.hpp new file mode 100644 index 0000000..28eadc8 --- /dev/null +++ b/multivehicle/packages/multivehicle_examples/src/PX4OffboardDemo.hpp @@ -0,0 +1,102 @@ +// PX4OffboardDemo.hpp — offboard control demo for the x500 UAV +#pragma once + +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +// Drives a single PX4 vehicle through a fixed offboard mission. All PX4 topics +// are prefixed with the vehicle namespace (e.g. "/x500/fmu/...") so the node +// can target one instance among several. +class OffboardDemo : public rclcpp::Node { + public: + OffboardDemo(); + + // Blocking: spins the node and advances the mission until it completes. + void run(); + + private: + // Mission phases, in the order they execute. + enum class Phase { + WaitForReady, // hold until pre-flight checks pass, then request offboard + RequestArm, // offboard accepted -> send the arm command + AwaitArmed, // wait for the autopilot to report ARMED + ClimbOut, // hold the climb setpoint above launch + GoToStandoff, // transit to the standoff observation point + ScanYaw, // rotate 360 degrees in place while holding position + ReturnHome, // fly back to the launch point + Descend, // land and wait for touchdown + Finished, + }; + + // Subscription callbacks (latch the latest message). + void onStatus(const px4_msgs::msg::VehicleStatus::SharedPtr msg); + void onLocalPosition(const px4_msgs::msg::VehicleLocalPosition::SharedPtr msg); + void onLandDetected(const px4_msgs::msg::VehicleLandDetected::SharedPtr msg); + + // Keepalive: PX4 only stays in offboard while this stream is fresh. + void streamOffboardHeartbeat(); + + // Outgoing commands / setpoints. + void sendCommand(uint32_t command, float param1 = NAN, float param2 = NAN); + void requestOffboardMode(); + void arm(); + void disarm(); + void commandLand(); + void publishPosition(const Eigen::Vector3f& pos_ned, float yaw); + + // Mission step + small predicates. + void tick(); + void transitionTo(Phase next, const char* note); + bool phaseElapsed(double seconds) const; + bool reached(const Eigen::Vector3f& target, float tol = kReachedTol) const; + bool yawReached(float target_yaw, float tol = kYawTol) const; + uint64_t nowMicros() const; + + // ---- parameters ---- + std::string _ns; // vehicle_namespace + std::string _prefix; // "/" (or "" when unset) + int _sys_id = 0; // vehicle_id -> MAVLink target_system + + // ---- ROS entities ---- + rclcpp::Publisher::SharedPtr _cmd_pub; + rclcpp::Publisher::SharedPtr _setpoint_pub; + rclcpp::Publisher::SharedPtr _offboard_mode_pub; + rclcpp::Subscription::SharedPtr _status_sub; + rclcpp::Subscription::SharedPtr _position_sub; + rclcpp::Subscription::SharedPtr _land_sub; + rclcpp::TimerBase::SharedPtr _heartbeat_timer; + + // ---- latest telemetry ---- + px4_msgs::msg::VehicleStatus _status{}; + px4_msgs::msg::VehicleLocalPosition _position{}; + bool _landed = false; + + // ---- mission state ---- + Phase _phase = Phase::WaitForReady; + rclcpp::Time _phase_since; + Eigen::Vector3f _home_ned = Eigen::Vector3f::Zero(); // launch point + climb (NED) + float _scan_yaw_target = 0.0f; // current yaw setpoint during ScanYaw + float _scan_yaw_swept = 0.0f; // total yaw rotated so far + + // ---- mission constants (NED; z is down-positive) ---- + static constexpr float kClimbHeight = 10.0f; // m above the launch deck + static constexpr float kStandoffNorth = -14.0f; // standoff observation point + static constexpr float kStandoffEast = -25.0f; + static constexpr float kStandoffDown = -6.0f; + static constexpr float kReachedTol = 0.1f; // m, position-reached tolerance + static constexpr float kYawStep = static_cast(M_PI / 180.0); // 1 deg + static constexpr float kYawTol = static_cast(M_PI / 180.0); // 1 deg + static constexpr double kReadyHold = 2.0; // s, settle before offboard + static constexpr double kClimbHold = 10.0; // s, hold the climb setpoint +}; diff --git a/multivehicle/run.bash b/multivehicle/run.bash new file mode 100644 index 0000000..d8f8a88 --- /dev/null +++ b/multivehicle/run.bash @@ -0,0 +1,24 @@ +#!/usr/bin/env bash + +set -e + +# Run the multivehicle_examples image with NVIDIA + X11 via rocker. +# Mounts ONLY this workspace at /root/HOST/mvsim_ws (the path the tmuxp sessions +# and setup_dashboard.sh expect). This script lives in src/examples/multivehicle, +# so the workspace root is three levels up. +# +# SPDX-License-Identifier: MIT + +IMAGE_NAME="${1:-multivehicle_examples:humble}" +SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)" +WORKSPACE_DIR="$(cd "${SCRIPT_DIR}/../../.." && pwd)" + +rocker \ + --devices /dev/dri \ + --dev-helpers \ + --nvidia \ + --x11 \ + --git \ + --volume "${WORKSPACE_DIR}:/root/HOST/mvsim_ws" \ + --network=host \ + "${IMAGE_NAME}" diff --git a/multivehicle/tmuxp/mvsim_debug.yaml b/multivehicle/tmuxp/mvsim_debug.yaml new file mode 100644 index 0000000..d173bf8 --- /dev/null +++ b/multivehicle/tmuxp/mvsim_debug.yaml @@ -0,0 +1,113 @@ +# tmuxp DEBUG session for the multivehicle examples (run INSIDE the +# multivehicle_examples container). Brings up the full stack — the simulator +# (multivehicle_sim) AND the example apps (missions, BlueBoat control, the PX4 +# offboard flight demo) — plus the dashboard. Load with: +# +# tmuxp load /root/HOST/mvsim_ws/src/examples/multivehicle/tmuxp/mvsim_debug.yaml +# +# Every command is PRE-TYPED but NOT executed (enter: false) so you start them in +# dependency order and watch each one. Recommended order: +# +# 1) world — bring up Gazebo + the course first (multivehicle_sim) +# 2) bluerov — BlueROV2 + ArduSub/MAVROS, then its square-dive mission +# 3) boat — BlueBoat spawn + control stack + square mission +# 4) drone — XRCE agent + PX4 SITL x500 + gz bridge + offboard demo +# 5) dashboard — web backend (:8080) + sim-side LED/incident bridges +# 6) debug — free shell + Foxglove bridge +# +# Workspace dir defaults to /root/HOST/mvsim_ws. Override with WS_DIR: +# WS_DIR=/root/HOST/my_ws tmuxp load .../mvsim_debug.yaml +# +# NVIDIA hosts: uncomment the two PRIME-offload exports in shell_command_before. + +session_name: mvsim + +global_options: + mouse: 'on' + history-limit: 50000 + +shell_command_before: + - 'WS_DIR="${WS_DIR:-/root/HOST/mvsim_ws}"' + # - export __NV_PRIME_RENDER_OFFLOAD=1 + # - export __GLX_VENDOR_LIBRARY_NAME=nvidia + - source /opt/ros/humble/setup.bash + - '[ -f "$WS_DIR/install/setup.bash" ] && source "$WS_DIR/install/setup.bash"' + - cd "$WS_DIR" + +windows: + # 1. Gazebo world (sim). + - window_name: world + focus: true + panes: + - shell_command: + - cmd: ros2 launch multivehicle_sim world.launch.py world_name:=robotx_2026_sg_river + enter: false + + # 2. BlueROV2 — spawn + ArduSub + MAVROS (sim, top); square-dive mission + # (examples, bottom). + - window_name: bluerov + layout: even-vertical + panes: + - shell_command: + - cmd: ros2 launch multivehicle_sim bluerov.launch.py + enter: false + - shell_command: + - cmd: ros2 launch multivehicle_examples bluerov_mission.launch.py + enter: false + + # 3. BlueBoat — spawn (sim, top); control stack + square mission (examples, + # bottom; use_mission:=true runs the demo course through the LOS controller). + - window_name: boat + layout: even-vertical + panes: + - shell_command: + - cmd: ros2 launch multivehicle_sim boat.launch.py + enter: false + - shell_command: + - cmd: ros2 launch multivehicle_examples boat_control.launch.py use_mission:=true + enter: false + + # 4. PX4 x500 drone — agent + PX4 SITL + gz bridge (sim), then the offboard + # flight demo (examples). Start top-to-bottom; offboard needs agent + PX4. + - window_name: drone + layout: even-vertical + panes: + - shell_command: + - cmd: MicroXRCEAgent udp4 -p 8888 + enter: false + - shell_command: + - cmd: PX4_GZ_STANDALONE=1 PX4_SYS_AUTOSTART=4010 PX4_UXRCE_DDS_NS=x500 PX4_PARAM_UXRCE_DDS_SYNCT=0 PX4_GZ_MODEL_POSE="47.40,-388.95,3.85,0,0.0" /root/px4/px4_sitl/bin/px4 -w /root/px4/px4_sitl/romfs -i 1 + enter: false + - shell_command: + - cmd: ros2 launch multivehicle_sim uav_gz.launch.py model_name:=x500_mono_cam_1 + enter: false + - shell_command: + - cmd: ros2 launch multivehicle_examples px4_offboard.launch.py + enter: false + + # 5. Dashboard — fake vehicle (RoboCommand :9000), web backend (:8080), and the + # sim-side LED/incident bridges (bridges_pkg=multivehicle_sim, models from + # bb_worlds) tracking all three vehicles' /…/odom. + - window_name: dashboard + layout: even-vertical + panes: + - shell_command: + - cmd: ros2 run bb_robotx_dashboard fake_vehicle_publisher --port 9000 + enter: false + - shell_command: + - cmd: ros2 launch bb_robotx_dashboard dashboard.launch.py + enter: false + - shell_command: + - cmd: ros2 launch bb_robotx_dashboard robotx_2026_sim.launch.py bridges_pkg:=multivehicle_sim models_pkg:=bb_worlds auv_name:=bluerov auv_pose_topic:=/bluerov/odom asv_name:=blueboat asv_pose_topic:=/blueboat/odom uav_name:=x500 uav_pose_topic:=/x500/odom + enter: false + + # 6. Debug — free shell + Foxglove bridge (ws://localhost:8765). + - window_name: debug + layout: even-vertical + panes: + - shell_command: + - cmd: ros2 topic list + enter: false + - shell_command: + - cmd: ros2 launch foxglove_bridge foxglove_bridge_launch.xml + enter: false From e9f03b59d9ff36d7c3b4d9c3d2dcb2d491f95e85 Mon Sep 17 00:00:00 2001 From: monkescripts Date: Tue, 23 Jun 2026 11:54:26 +0800 Subject: [PATCH 4/8] Update build and run scripts to include additional dependencies and change file permissions --- multivehicle/README.md | 6 ++---- multivehicle/build.bash | 0 multivehicle/run.bash | 0 3 files changed, 2 insertions(+), 4 deletions(-) mode change 100644 => 100755 multivehicle/build.bash mode change 100644 => 100755 multivehicle/run.bash diff --git a/multivehicle/README.md b/multivehicle/README.md index 860dde0..5adeb1e 100644 --- a/multivehicle/README.md +++ b/multivehicle/README.md @@ -43,10 +43,8 @@ Inside the container: ```bash cd /root/HOST/mvsim_ws -# Scope the build to this project: --packages-up-to pulls in multivehicle_examples -# and its deps (multivehicle_sim, multivehicle_interface, px4_msgs, bb_worlds, …) -# and skips the sibling bluerov_tasks, whose vision deps aren't imported here. -colcon build --symlink-install --packages-up-to multivehicle_examples +colcon build --symlink-install --packages-up-to multivehicle_examples microxrcedds_agent bb_robotx_dashboard + source install/setup.bash tmuxp load src/examples/multivehicle/tmuxp/mvsim_debug.yaml ``` diff --git a/multivehicle/build.bash b/multivehicle/build.bash old mode 100644 new mode 100755 diff --git a/multivehicle/run.bash b/multivehicle/run.bash old mode 100644 new mode 100755 From 8684c9255758e5ee8333cbd23ca4df563fd4efb3 Mon Sep 17 00:00:00 2001 From: "copilot-swe-agent[bot]" <198982749+Copilot@users.noreply.github.com> Date: Wed, 24 Jun 2026 17:41:41 +0000 Subject: [PATCH 5/8] Refactor multivehicle examples to reuse shared packages --- README.md | 33 +-- multivehicle/README.md | 53 ++-- multivehicle/build.bash | 7 +- multivehicle/docker/Dockerfile | 4 +- multivehicle/examples.repos | 9 +- .../multivehicle_examples/CMakeLists.txt | 18 +- .../launch/bluerov_mission.launch.py | 32 +-- .../launch/boat_control.launch.py | 49 +--- .../launch/px4_offboard.launch.py | 14 +- .../multivehicle_examples/__init__.py | 1 + .../multivehicle_examples/math_helpers.py | 21 ++ .../multivehicle_examples/package.xml | 37 +-- .../scripts/blueboat_mission.py | 34 +-- .../scripts/blueboat_thrust_mixer.py | 72 +---- .../scripts/blueboat_waypoint_controller.py | 172 +++-------- .../scripts/bluerov_movement.py | 270 ------------------ .../src/PX4OffboardDemo.cpp | 227 --------------- .../src/PX4OffboardDemo.hpp | 102 ------- multivehicle/run.bash | 7 - 19 files changed, 164 insertions(+), 998 deletions(-) create mode 100644 multivehicle/packages/multivehicle_examples/multivehicle_examples/__init__.py create mode 100644 multivehicle/packages/multivehicle_examples/multivehicle_examples/math_helpers.py delete mode 100755 multivehicle/packages/multivehicle_examples/scripts/bluerov_movement.py delete mode 100644 multivehicle/packages/multivehicle_examples/src/PX4OffboardDemo.cpp delete mode 100644 multivehicle/packages/multivehicle_examples/src/PX4OffboardDemo.hpp diff --git a/README.md b/README.md index 36c910e..4b4f8ff 100644 --- a/README.md +++ b/README.md @@ -1,30 +1,27 @@ # examples -Example applications for Bumblebee simulators, organized one subdirectory per -project. Each subdir is a self-contained sub-workspace (its own `packages/`, -`build.bash` / `run.bash`, `docker/`, and `*.repos` manifest): +Example applications for Bumblebee simulators. -| Directory | Simulator base | Contents | -|-----------|----------------|----------| -| [`bluerov/`](bluerov/) | `ardusub_sim:humble` | BlueROV2 / ArduSub missions, behaviour trees, perception (`bluerov_tasks`) | -| [`multivehicle/`](multivehicle/) | `multivehicle_sim:humble` | Multivehicle missions, BlueBoat control, PX4 offboard demo (`multivehicle_examples`) | +## Sub-examples -Pick the project you want and follow its README. +- [BlueROV examples](bluerov/README.md) +- [Multivehicle examples](multivehicle/README.md) + +## Demo videos + +### BlueROV missions + +- Bin mission: https://github.com/user-attachments/assets/6c262df8-bac6-492a-aef1-9e8cfc30d8a8 +- Torpedo mission: https://github.com/user-attachments/assets/9a9c25c5-637a-403a-b34d-4048f9afb5e0 ## Building -`colcon build` discovers `package.xml` files at any depth, so the per-project -nesting (`/packages/...`) is found automatically. Because both projects' -packages live in the same repo but each pulls only its **own** dependencies (via -its `*.repos`), build the project you set up rather than the whole tree, e.g.: +Build only the project you imported dependencies for: ```bash -# multivehicle: +# multivehicle colcon build --packages-up-to multivehicle_examples -# bluerov: + +# bluerov colcon build --packages-up-to bluerov_tasks ``` - -`--packages-up-to` builds the chosen package plus its dependencies and skips the -other project (whose deps you have not imported). Alternatively, drop a -`COLCON_IGNORE` file in the project subdir you are not building. diff --git a/multivehicle/README.md b/multivehicle/README.md index 5adeb1e..7017590 100644 --- a/multivehicle/README.md +++ b/multivehicle/README.md @@ -1,41 +1,25 @@ # multivehicle examples -Example missions, controllers and demos that run **on top of** the -[`multivehicle_sim`](https://github.com/BumblebeeAS/multivehicle_sim) simulator. +Missions and control demos built on +[`multivehicle_sim`](https://github.com/BumblebeeAS/multivehicle_sim). -## Contents +## Launch files -``` -multivehicle/ - build.bash run.bash # multivehicle_examples:humble, FROM multivehicle_sim:humble - examples.repos # full workspace manifest (sim + deps + px4_msgs) - docker/Dockerfile # adds nav2 (BlueBoat control nav2-prep) - tmuxp/mvsim_debug.yaml # full stack: sim + examples + dashboard - packages/ - multivehicle_examples/ # ament_cmake - launch/ bluerov_mission, boat_control, px4_offboard - scripts/ bluerov_movement, blueboat_thrust_mixer, blueboat_waypoint_controller, blueboat_mission - src/ PX4OffboardDemo.cpp/.hpp (px4_msgs + Eigen3) - config/ blueboat_control.yaml -``` - -What each launch adds on top of the sim: - -| Launch | Builds on | Adds | -|--------|-----------|------| -| `bluerov_mission.launch.py` | `multivehicle_sim bluerov.launch.py` | BlueROV2 square-dive mission | -| `boat_control.launch.py` | `multivehicle_sim boat.launch.py` | thrust mixer + LOS waypoint controller (+ optional `use_mission:=true`) | -| `px4_offboard.launch.py` | `multivehicle_sim` x500 + uXRCE agent | autonomous offboard flight (`px4_offboard_demo`) | +- `bluerov_mission.launch.py`: `multivehicle_sim` + `ground_truth_to_mavros` + `bluerov_tasks/bluerov_movement.py` +- `boat_control.launch.py`: BlueBoat thrust mixer + LOS controller (+ optional mission node) +- `px4_offboard.launch.py`: PX4 offboard demo from `uav2_offboard` ## Setup ```bash -# In ~/mvsim_ws (this repo cloned at src/examples): +cd ~/mvsim_ws vcs import src < src/examples/multivehicle/examples.repos --recursive -# Build the base sim image first, then the examples image: -cd src/multivehicle_sim && ./build.bash -cd ../examples/multivehicle && ./build.bash +cd src/multivehicle_sim +./build.bash + +cd ../examples/multivehicle +./build.bash ./run.bash multivehicle_examples:humble ``` @@ -44,16 +28,13 @@ Inside the container: ```bash cd /root/HOST/mvsim_ws colcon build --symlink-install --packages-up-to multivehicle_examples microxrcedds_agent bb_robotx_dashboard - source install/setup.bash tmuxp load src/examples/multivehicle/tmuxp/mvsim_debug.yaml ``` -On a hybrid-GPU (NVIDIA) host, `export __NV_PRIME_RENDER_OFFLOAD=1` and -`export __GLX_VENDOR_LIBRARY_NAME=nvidia` before launching gz (or uncomment those -lines in the tmuxp's `shell_command_before`). - -## License +## Provenance -MIT (see the repo-root `LICENSE`). The `px4_offboard_demo` is a first-party, -independent implementation of the standard PX4 offboard pattern. +- `packages/multivehicle_examples/scripts/blueboat_mission.py` adapted from `bring-up/etc/uav2_sim` +- `packages/multivehicle_examples/scripts/blueboat_thrust_mixer.py` adapted from `bring-up/etc/uav2_sim` +- `packages/multivehicle_examples/scripts/blueboat_waypoint_controller.py` adapted from `bring-up/etc/uav2_sim` +- PX4 offboard demo is provided by `https://github.com/BumblebeeAS/uav2_offboard` diff --git a/multivehicle/build.bash b/multivehicle/build.bash index 2918b19..049eb12 100755 --- a/multivehicle/build.bash +++ b/multivehicle/build.bash @@ -2,10 +2,6 @@ set -e -# Builds the multivehicle_examples Docker image FROM multivehicle_sim:humble. -# Run from this directory (src/examples/multivehicle): ./build.bash -# -# SPDX-License-Identifier: MIT image_name=multivehicle_examples image_tag=humble @@ -16,8 +12,7 @@ fi if ! docker image inspect multivehicle_sim:humble >/dev/null 2>&1; then echo "Err: base image multivehicle_sim:humble not found." - echo "Build it first from the multivehicle_sim repo:" - echo " cd ../../multivehicle_sim && ./build.bash" + echo "Build it first from src/multivehicle_sim." exit 1 fi diff --git a/multivehicle/docker/Dockerfile b/multivehicle/docker/Dockerfile index f5f1945..9f36380 100644 --- a/multivehicle/docker/Dockerfile +++ b/multivehicle/docker/Dockerfile @@ -6,9 +6,7 @@ ARG HOME_DIR=/root RUN apt-get update && apt-get install -y --no-install-recommends \ ros-humble-navigation2 \ - ros-humble-nav2-bringup \ - ros-humble-nav2-util \ - ros-humble-nav2-common && \ + ros-humble-nav2-bringup && \ apt-get autoremove -y && \ apt-get clean && \ rm -rf /var/lib/apt/lists/* diff --git a/multivehicle/examples.repos b/multivehicle/examples.repos index 76e9f22..832e1a2 100644 --- a/multivehicle/examples.repos +++ b/multivehicle/examples.repos @@ -5,7 +5,6 @@ repositories: type: git url: https://github.com/BumblebeeAS/multivehicle_sim.git - # --- everything multivehicle_sim's dependencies.repos pulls --- ardupilot_gazebo: type: git url: https://github.com/ArduPilot/ardupilot_gazebo.git @@ -29,10 +28,6 @@ repositories: type: git url: https://github.com/BumblebeeAS/bb_msgs.git version: multivehicle_sim - - # --- examples-only: PX4 uORB message types for the offboard demo --- - px4_msgs: + uav2_offboard: type: git - url: https://github.com/PX4/px4_msgs.git - version: release/1.16 - depth: 1 + url: https://github.com/BumblebeeAS/uav2_offboard.git diff --git a/multivehicle/packages/multivehicle_examples/CMakeLists.txt b/multivehicle/packages/multivehicle_examples/CMakeLists.txt index 6054135..d916da8 100644 --- a/multivehicle/packages/multivehicle_examples/CMakeLists.txt +++ b/multivehicle/packages/multivehicle_examples/CMakeLists.txt @@ -3,27 +3,13 @@ project(multivehicle_examples) find_package(ament_cmake REQUIRED) find_package(ament_cmake_python REQUIRED) -find_package(rclcpp REQUIRED) -find_package(geometry_msgs REQUIRED) -find_package(px4_msgs REQUIRED) -find_package(eigen3_cmake_module REQUIRED) -find_package(Eigen3 REQUIRED) -include_directories( - src - ${EIGEN3_INCLUDE_DIR} -) - -add_executable(px4_offboard_demo src/PX4OffboardDemo.cpp) -ament_target_dependencies(px4_offboard_demo rclcpp geometry_msgs px4_msgs Eigen3) -target_compile_features(px4_offboard_demo PUBLIC c_std_99 cxx_std_17) -install(TARGETS px4_offboard_demo DESTINATION lib/${PROJECT_NAME}) +ament_python_install_package(${PROJECT_NAME}) install(PROGRAMS - scripts/bluerov_movement.py + scripts/blueboat_mission.py scripts/blueboat_thrust_mixer.py scripts/blueboat_waypoint_controller.py - scripts/blueboat_mission.py DESTINATION lib/${PROJECT_NAME} ) diff --git a/multivehicle/packages/multivehicle_examples/launch/bluerov_mission.launch.py b/multivehicle/packages/multivehicle_examples/launch/bluerov_mission.launch.py index cb6d915..8bfaabd 100644 --- a/multivehicle/packages/multivehicle_examples/launch/bluerov_mission.launch.py +++ b/multivehicle/packages/multivehicle_examples/launch/bluerov_mission.launch.py @@ -5,25 +5,21 @@ def generate_launch_description(): use_sim_time = {"use_sim_time": True} - ground_truth_node = Node( - package="multivehicle_interface", - executable="ground_truth_to_mavros", - name="ground_truth_to_mavros", - output="screen", - parameters=[use_sim_time], - ) - - movement_node = Node( - package="multivehicle_examples", - executable="bluerov_movement.py", - name="bluerov_movement", - output="screen", - parameters=[use_sim_time], - ) - return LaunchDescription( [ - ground_truth_node, - movement_node, + Node( + package="multivehicle_interface", + executable="ground_truth_to_mavros", + name="ground_truth_to_mavros", + output="screen", + parameters=[use_sim_time], + ), + Node( + package="bluerov_tasks", + executable="bluerov_movement.py", + name="bluerov_movement", + output="screen", + parameters=[use_sim_time], + ), ] ) diff --git a/multivehicle/packages/multivehicle_examples/launch/boat_control.launch.py b/multivehicle/packages/multivehicle_examples/launch/boat_control.launch.py index 9cff38e..96795cb 100644 --- a/multivehicle/packages/multivehicle_examples/launch/boat_control.launch.py +++ b/multivehicle/packages/multivehicle_examples/launch/boat_control.launch.py @@ -1,20 +1,3 @@ -"""Bring up the BlueBoat custom control stack (mixer + LOS controller). - -Layers (see the GNC plan): - * blueboat_thrust_mixer -- cmd_vel -> left/right thrust (ALWAYS on; the - single thruster driver and the nav2 seam). - * blueboat_odom_to_tf -- odom->base_link TF (nav2 prep; ALWAYS on). - * blueboat_waypoint_controller -- LOS guidance + heading/surge -> cmd_vel - (gated by `use_controller`, default true). - * blueboat_mission -- demo square course (gated by `use_mission`). - -This launch assumes the world and the boat are already up (world.launch.py + -boat.launch.py). Set `spawn:=true` to also include boat.launch.py here. - -nav2 migration (later): launch with `use_controller:=false` and remap nav2's -`/cmd_vel` onto `/blueboat/cmd_vel` -- the mixer is unchanged. -""" - import os from ament_index_python.packages import get_package_share_directory @@ -38,28 +21,16 @@ def generate_launch_description(): args = [ DeclareLaunchArgument("use_sim_time", default_value="true"), - DeclareLaunchArgument( - "use_controller", - default_value="true", - description="Run the LOS waypoint controller (set false when nav2 " - "provides /blueboat/cmd_vel instead).", - ), - DeclareLaunchArgument( - "use_mission", - default_value="false", - description="Run the demo square-course mission node.", - ), - DeclareLaunchArgument( - "spawn", - default_value="false", - description="Also include boat.launch.py to spawn the boat + bridge.", - ), + DeclareLaunchArgument("use_controller", default_value="true"), + DeclareLaunchArgument("use_mission", default_value="false"), + DeclareLaunchArgument("spawn", default_value="false"), ] spawn_boat = IncludeLaunchDescription( PythonLaunchDescriptionSource( - PathJoinSubstitution([FindPackageShare("multivehicle_sim"), - "launch", "boat.launch.py"]) + PathJoinSubstitution( + [FindPackageShare("multivehicle_sim"), "launch", "boat.launch.py"] + ) ), condition=IfCondition(spawn), ) @@ -72,14 +43,6 @@ def generate_launch_description(): parameters=[control_params, {"use_sim_time": use_sim_time}], ) - # Broadcast the boat's odom->base_link under a per-vehicle TF namespace - # (blueboat/odom -> blueboat/base_link). The gz OdometryPublisher stamps - # /blueboat/odom with the generic frames "odom"/"base_link", identical to - # the BlueROV2's — and MAVROS publishes map->base_link for the ROV. Left - # un-prefixed, both vehicles fight over one global `base_link` on /tf and - # every listener spams TF_OLD_DATA. Prefixing the boat's frames keeps the - # two TF trees disjoint. (Control/mission are topic-based, so nothing - # consumes these frames yet; this is purely nav2 prep.) odom_tf = Node( package="multivehicle_interface", executable="blueboat_odom_to_tf", diff --git a/multivehicle/packages/multivehicle_examples/launch/px4_offboard.launch.py b/multivehicle/packages/multivehicle_examples/launch/px4_offboard.launch.py index 6873ebf..8492a30 100644 --- a/multivehicle/packages/multivehicle_examples/launch/px4_offboard.launch.py +++ b/multivehicle/packages/multivehicle_examples/launch/px4_offboard.launch.py @@ -1,23 +1,25 @@ from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration from launch_ros.actions import Node def generate_launch_description(): + package = LaunchConfiguration("offboard_package") + executable = LaunchConfiguration("offboard_executable") return LaunchDescription( [ + DeclareLaunchArgument("offboard_package", default_value="uav2_offboard"), + DeclareLaunchArgument("offboard_executable", default_value="px4_offboard_demo"), Node( - package="multivehicle_examples", - executable="px4_offboard_demo", + package=package, + executable=executable, name="px4_offboard_demo", output="screen", parameters=[ {"use_sim_time": True}, - # uXRCE-DDS topic namespace -> /x500/fmu/... . Must match - # PX4_UXRCE_DDS_NS=x500 set on the px4 SITL process. {"vehicle_namespace": "x500"}, - # target_system for VehicleCommand: MAV_SYS_ID = instance+1, - # so 2 for `px4 ... -i 1`. {"vehicle_id": 2}, ], ), diff --git a/multivehicle/packages/multivehicle_examples/multivehicle_examples/__init__.py b/multivehicle/packages/multivehicle_examples/multivehicle_examples/__init__.py new file mode 100644 index 0000000..150c5d3 --- /dev/null +++ b/multivehicle/packages/multivehicle_examples/multivehicle_examples/__init__.py @@ -0,0 +1 @@ +"""Shared helpers for multivehicle example nodes.""" diff --git a/multivehicle/packages/multivehicle_examples/multivehicle_examples/math_helpers.py b/multivehicle/packages/multivehicle_examples/multivehicle_examples/math_helpers.py new file mode 100644 index 0000000..3de12f3 --- /dev/null +++ b/multivehicle/packages/multivehicle_examples/multivehicle_examples/math_helpers.py @@ -0,0 +1,21 @@ +"""Common math helpers for control scripts.""" + +import math + + +def clamp_symmetric(value, limit): + return max(-limit, min(limit, value)) + + +def wrap_to_pi(angle): + return math.atan2(math.sin(angle), math.cos(angle)) + + +def yaw_from_quaternion(quaternion): + siny_cosp = 2.0 * ( + quaternion.w * quaternion.z + quaternion.x * quaternion.y + ) + cosy_cosp = 1.0 - 2.0 * ( + quaternion.y * quaternion.y + quaternion.z * quaternion.z + ) + return math.atan2(siny_cosp, cosy_cosp) diff --git a/multivehicle/packages/multivehicle_examples/package.xml b/multivehicle/packages/multivehicle_examples/package.xml index 2e04c18..f808144 100644 --- a/multivehicle/packages/multivehicle_examples/package.xml +++ b/multivehicle/packages/multivehicle_examples/package.xml @@ -1,39 +1,24 @@ - - + multivehicle_examples 0.1.0 - -

Example missions, controllers and demos that run on top of the - multivehicle_sim simulator: BlueROV2 square-dive mission, BlueBoat control - stack (thrust mixer + LOS waypoint controller + demo mission), and the PX4 - x500 offboard flight demo.

- - MonkeScripts + Multivehicle demo launches and control scripts. + monkescripts MIT ament_cmake ament_cmake_python - eigen3_cmake_module - eigen - rclcpp - rclpy - geometry_msgs - std_msgs - sensor_msgs - nav_msgs - tf2 - tf2_ros - tf2_geometry_msgs - px4_msgs - mavros_msgs - - multivehicle_sim - multivehicle_interface ament_index_python + bluerov_tasks + geometry_msgs + launch launch_ros - tf_transformations + multivehicle_interface + nav_msgs + rclpy + std_msgs + uav2_offboard ament_lint_auto ament_lint_common diff --git a/multivehicle/packages/multivehicle_examples/scripts/blueboat_mission.py b/multivehicle/packages/multivehicle_examples/scripts/blueboat_mission.py index 3f0364b..58e12fc 100755 --- a/multivehicle/packages/multivehicle_examples/scripts/blueboat_mission.py +++ b/multivehicle/packages/multivehicle_examples/scripts/blueboat_mission.py @@ -1,11 +1,7 @@ #!/usr/bin/env python3 -"""Demo mission for the BlueBoat USV: drive a square course. - -Waits for the first ``/blueboat/odom``, then publishes a square of waypoints -(in the ``odom`` frame, relative to the boat's start position) to -``/blueboat/goal_pose``. The LOS waypoint controller follows the line between -consecutive waypoints; this node just lays down the course and then idles. +"""BlueBoat square mission publisher. +Provenance: Adapted from https://github.com/BumblebeeAS/bring-up/tree/main/etc/uav2_sim """ import rclpy @@ -18,7 +14,7 @@ class BlueBoatSquareMission(Node): def __init__(self): super().__init__("blueboat_square_mission") - self.declare_parameter("side", 10.0) # m, square edge length + self.declare_parameter("side", 10.0) self.declare_parameter("frame_id", "odom") self.side = self.get_parameter("side").value self.frame_id = self.get_parameter("frame_id").value @@ -27,11 +23,7 @@ def __init__(self): self.create_subscription(Odometry, "/blueboat/odom", self.save_odom, 10) self._start = None self._published = False - # Publish only once the controller is actually subscribed, so the - # course isn't dropped before discovery completes (and never twice, or - # the controller would queue duplicate waypoints). self.create_timer(0.2, self.try_publish) - self.get_logger().info("Waiting for /blueboat/odom to lay down course...") def save_odom(self, msg: Odometry): if self._start is None: @@ -42,26 +34,26 @@ def try_publish(self): return if self.goal_pub.get_subscription_count() == 0: return + x0, y0 = self._start s = self.side - # Axis-aligned square in the odom frame, returning to start. waypoints = [ (x0 + s, y0), (x0 + s, y0 + s), (x0, y0 + s), (x0, y0), ] + for wx, wy in waypoints: - msg_out = PoseStamped() - msg_out.header.frame_id = self.frame_id - msg_out.header.stamp = self.get_clock().now().to_msg() - msg_out.pose.position.x = wx - msg_out.pose.position.y = wy - msg_out.pose.orientation.w = 1.0 - self.goal_pub.publish(msg_out) - self.get_logger().info("Queued waypoint (%.2f, %.2f)" % (wx, wy)) + msg = PoseStamped() + msg.header.frame_id = self.frame_id + msg.header.stamp = self.get_clock().now().to_msg() + msg.pose.position.x = wx + msg.pose.position.y = wy + msg.pose.orientation.w = 1.0 + self.goal_pub.publish(msg) + self._published = True - self.get_logger().info("Course published (%d waypoints)." % len(waypoints)) def main(args=None): diff --git a/multivehicle/packages/multivehicle_examples/scripts/blueboat_thrust_mixer.py b/multivehicle/packages/multivehicle_examples/scripts/blueboat_thrust_mixer.py index 549f095..2a7696c 100755 --- a/multivehicle/packages/multivehicle_examples/scripts/blueboat_thrust_mixer.py +++ b/multivehicle/packages/multivehicle_examples/scripts/blueboat_thrust_mixer.py @@ -1,26 +1,7 @@ #!/usr/bin/env python3 -"""Differential-thrust allocation (mixer) for the BlueBoat USV. - -It consumes ``geometry_msgs/Twist`` on ``/blueboat/cmd_vel`` and emits a per-thruster force (Newtons) on -``/blueboat/thrusters/{left,right}/thrust`` (std_msgs/Float64, bridged ROS->gz). - -Interpretation of the Twist: - * ``linear.x`` -> desired SURGE SPEED (m/s). Converted to a surge force by - inverting the quadratic hull drag from the model SDF (``xUabsU`` = -58.42), - so ``F_surge = surge_drag * u_d * |u_d|`` holds that speed in steady state - (open-loop). - * ``angular.z`` -> desired YAW RATE (rad/s). Converted to a yaw moment by a - gain ``yaw_gain`` (N*m per rad/s). - -Allocation (twin thrusters a moment arm ``d = track_width/2`` apart): - F_left = F_surge/2 - M_z/(2 d) - F_right = F_surge/2 + M_z/(2 d) -Positive ``angular.z`` (REP-103, CCW) therefore makes the right (starboard) -thruster push harder -> the boat yaws left, as expected. - -A watchdog publishes zero thrust if no ``cmd_vel`` arrives within -``cmd_timeout`` seconds, so a dead controller cannot leave the boat at full -throttle. +"""BlueBoat differential-thrust mixer. + +Provenance: Adapted from https://github.com/BumblebeeAS/bring-up/tree/main/etc/uav2_sim """ import rclpy @@ -28,32 +9,26 @@ from geometry_msgs.msg import Twist from std_msgs.msg import Float64 - -def _clamp(value, limit): - return max(-limit, min(limit, value)) +from multivehicle_examples.math_helpers import clamp_symmetric class BlueBoatThrustMixer(Node): def __init__(self): super().__init__("blueboat_thrust_mixer") - # Geometry / actuator limits. - self.declare_parameter("track_width", 0.59) # m, port<->stbd spacing - self.declare_parameter("max_thrust", 30.0) # N, per-thruster clamp - # Surge: quadratic-drag feedforward (|xUabsU| from the model SDF). - self.declare_parameter("surge_drag", 58.42) # N per (m/s)^2 - self.declare_parameter("surge_lin", 0.0) # N per (m/s), optional - self.declare_parameter("v_max", 2.0) # m/s input clamp - # Yaw: desired yaw-rate -> yaw moment. - self.declare_parameter("yaw_gain", 30.0) # N*m per (rad/s) - self.declare_parameter("w_max", 1.0) # rad/s input clamp - self.declare_parameter("invert_yaw", False) # flip turn direction - # Topics / timing. + self.declare_parameter("track_width", 0.59) + self.declare_parameter("max_thrust", 30.0) + self.declare_parameter("surge_drag", 58.42) + self.declare_parameter("surge_lin", 0.0) + self.declare_parameter("v_max", 2.0) + self.declare_parameter("yaw_gain", 30.0) + self.declare_parameter("w_max", 1.0) + self.declare_parameter("invert_yaw", False) self.declare_parameter("cmd_vel_topic", "/blueboat/cmd_vel") self.declare_parameter("left_thrust_topic", "/blueboat/thrusters/left/thrust") self.declare_parameter("right_thrust_topic", "/blueboat/thrusters/right/thrust") self.declare_parameter("publish_rate_hz", 20.0) - self.declare_parameter("cmd_timeout", 0.5) # s, watchdog + self.declare_parameter("cmd_timeout", 0.5) self.track_width = self.get_parameter("track_width").value self.max_thrust = self.get_parameter("max_thrust").value @@ -82,24 +57,11 @@ def __init__(self): self._last_cmd_time = None self.create_timer(1.0 / rate, self.publish_thrust) - self.get_logger().info( - "BlueBoat thrust mixer up: track_width=%.3f m, max_thrust=%.1f N, " - "surge_drag=%.2f, yaw_gain=%.2f, invert_yaw=%s" - % ( - self.track_width, - self.max_thrust, - self.surge_drag, - self.yaw_gain, - self.invert_yaw, - ) - ) - def cmd_vel_sub(self, msg: Twist): self._last_cmd = msg self._last_cmd_time = self.get_clock().now() def publish_thrust(self): - # Watchdog: zero thrust if the command is stale (or never received). stale = self._last_cmd_time is None or ( (self.get_clock().now() - self._last_cmd_time).nanoseconds * 1e-9 > self.cmd_timeout @@ -108,21 +70,17 @@ def publish_thrust(self): self._send(0.0, 0.0) return - u = _clamp(self._last_cmd.linear.x, self.v_max) - w = _clamp(self._last_cmd.angular.z, self.w_max) + u = clamp_symmetric(self._last_cmd.linear.x, self.v_max) + w = clamp_symmetric(self._last_cmd.angular.z, self.w_max) if self.invert_yaw: w = -w - # Surge speed -> force via quadratic-drag inversion (+ optional linear). f_surge = self.surge_drag * u * abs(u) + self.surge_lin * u - # Desired yaw rate -> yaw moment. m_z = self.yaw_gain * w f_left = f_surge / 2.0 - m_z / (2.0 * self.half_track) f_right = f_surge / 2.0 + m_z / (2.0 * self.half_track) - # Proportional clamp: scale both sides together so the turn ratio (and - # thus the commanded heading) is preserved under saturation. peak = max(abs(f_left), abs(f_right), self.max_thrust) if peak > self.max_thrust: scale = self.max_thrust / peak diff --git a/multivehicle/packages/multivehicle_examples/scripts/blueboat_waypoint_controller.py b/multivehicle/packages/multivehicle_examples/scripts/blueboat_waypoint_controller.py index 1a90bc8..64e65ee 100755 --- a/multivehicle/packages/multivehicle_examples/scripts/blueboat_waypoint_controller.py +++ b/multivehicle/packages/multivehicle_examples/scripts/blueboat_waypoint_controller.py @@ -1,47 +1,7 @@ #!/usr/bin/env python3 -"""Line-of-Sight (LOS) waypoint controller for the BlueBoat USV. - -Guidance + control layers of the BlueBoat GNC stack. It turns a list of -waypoints into a ``geometry_msgs/Twist`` on ``/blueboat/cmd_vel`` for the thrust -mixer to allocate. - -Line-of-Sight path following (Fossen): - Track the *line between consecutive waypoints*, not the raw point, so the boat - drives the cross-track error ``e`` to zero instead of spinning toward / over- - shooting each point. For a leg from ``p_prev`` to ``p_tgt`` with path angle - ``gamma``: - e = -(x - x_prev) sin(gamma) + (y - y_prev) cos(gamma) # +ve = left - Delta = clamp(delta_min + k_delta |e|, delta_min, delta_max) # adaptive lookahead - psi_d = gamma + atan2(-e, Delta) - A leg is finished when the boat passes the target (along-track progress >= leg - length) or enters ``acceptance_radius``. - -Control (heading psi & yaw-rate r from the pose/twist; surge u from twist.linear.x): - * Heading: PID on wrap(psi_d - psi) with derivative-on-measurement (-Kd*r, - no setpoint kick on waypoint switch) and an anti-windup integrator -> - desired yaw rate ``angular.z``. - * Surge: cruise speed shaped by heading error and by the distance-to-goal slowdown -> desired ``linear.x``: - u_d = u_cruise * max(0, cos(psi_err))**p * sat(dist_to_goal / r_slow) - -Gait (``turn_in_place``) -- stop-and-turn at every waypoint: - INIT -> ALIGN -> DRIVE -> (ALIGN -> DRIVE)* -> HOLD. - * ALIGN: rotate in place to the next leg's heading while station-keeping the - waypoint (surge nulls the along-body position error, braking any arrival - momentum). Exits once the heading is aligned and the yaw rate has settled. - * DRIVE: LOS path following; slows approaching *every* waypoint so the boat - arrives slow enough to stop and turn (not just the final one). - * HOLD: actively station-keep the final pose instead of going limp -- the boat - holds position/heading rather than drifting. The USV is underactuated - (surge + yaw only, no sway), so a pure sideways offset is corrected only by - re-pointing at the hold point (``repoint_radius``); within - ``acceptance_radius`` the surge deadbands to avoid thruster jitter. - -Inputs: ``/blueboat/odom`` (nav_msgs/Odometry); ``/blueboat/goal_pose`` - (geometry_msgs/PoseStamped, appended to the path); ``waypoints`` param - (flat [x0,y0,x1,y1,...]) for a preloaded course. -Outputs: ``/blueboat/cmd_vel`` (geometry_msgs/Twist); - ``/blueboat/goal_reached`` (std_msgs/Bool, latched true when the final - waypoint is reached). +"""BlueBoat LOS waypoint controller. + +Provenance: Adapted from https://github.com/BumblebeeAS/bring-up/tree/main/etc/uav2_sim """ import math @@ -49,54 +9,41 @@ import rclpy from rclpy.node import Node from rclpy.qos import QoSProfile, DurabilityPolicy -from rcl_interfaces.msg import ParameterDescriptor from nav_msgs.msg import Odometry from geometry_msgs.msg import PoseStamped, Twist from std_msgs.msg import Bool - -def wrap_to_pi(angle): - """Wrap an angle to (-pi, pi].""" - return math.atan2(math.sin(angle), math.cos(angle)) - - -def yaw_from_quaternion(q): - """Extract yaw (rad) from a geometry_msgs/Quaternion.""" - siny_cosp = 2.0 * (q.w * q.z + q.x * q.y) - cosy_cosp = 1.0 - 2.0 * (q.y * q.y + q.z * q.z) - return math.atan2(siny_cosp, cosy_cosp) - - -def clamp(value, limit): - return max(-limit, min(limit, value)) +from multivehicle_examples.math_helpers import ( + clamp_symmetric, + wrap_to_pi, + yaw_from_quaternion, +) class BlueBoatWaypointController(Node): def __init__(self): super().__init__("blueboat_waypoint_controller") - self.declare_parameter("delta_min", 2.5) # m, min lookahead - self.declare_parameter("delta_max", 8.0) # m, max lookahead - self.declare_parameter("k_delta", 1.0) # lookahead growth per |e| - self.declare_parameter("acceptance_radius", 0.2) # m, leg-done radius - self.declare_parameter("u_cruise", 1.0) # m/s nominal surge - self.declare_parameter("turn_shaping_p", 1.5) # cos() exponent - self.declare_parameter("r_slow", 4.0) # m, final-goal slowdown - # --- Turn-in-place / station-keeping gait ---------------------------- - self.declare_parameter("turn_in_place", True) # stop+rotate at each wp - self.declare_parameter("arrival_radius", 0.6) # m, DRIVE->ALIGN switch - self.declare_parameter("align_tol_deg", 3.0) # deg, heading-aligned exit - self.declare_parameter("align_settle_degps", 5.0) # deg/s, settled exit - self.declare_parameter("kp_pos", 0.7) # (m/s)/m, station-keep surge gain - self.declare_parameter("u_align_max", 0.5) # m/s, station-keep surge clamp - self.declare_parameter("repoint_radius", 1.0) # m, HOLD: drive back if drift>this - self.declare_parameter("v_settle", 0.1) # m/s, stop before turning in place - # --- Heading PID (outputs desired yaw rate, rad/s) ------------------- + self.declare_parameter("delta_min", 2.5) + self.declare_parameter("delta_max", 8.0) + self.declare_parameter("k_delta", 1.0) + self.declare_parameter("acceptance_radius", 0.2) + self.declare_parameter("u_cruise", 1.0) + self.declare_parameter("turn_shaping_p", 1.5) + self.declare_parameter("r_slow", 4.0) + self.declare_parameter("turn_in_place", True) + self.declare_parameter("arrival_radius", 0.6) + self.declare_parameter("align_tol_deg", 3.0) + self.declare_parameter("align_settle_degps", 5.0) + self.declare_parameter("kp_pos", 0.7) + self.declare_parameter("u_align_max", 0.5) + self.declare_parameter("repoint_radius", 1.0) + self.declare_parameter("v_settle", 0.1) self.declare_parameter("kp_yaw", 2.0) self.declare_parameter("ki_yaw", 0.0) self.declare_parameter("kd_yaw", 0.5) - self.declare_parameter("w_max", 1.0) # rad/s output clamp - self.declare_parameter("i_max", 0.5) # integrator clamp (rad/s) + self.declare_parameter("w_max", 1.0) + self.declare_parameter("i_max", 0.5) self.declare_parameter("control_rate_hz", 20.0) self.declare_parameter("odom_topic", "/blueboat/odom") @@ -125,28 +72,23 @@ def __init__(self): self.i_max = self.get_parameter("i_max").value self.dt = 1.0 / self.get_parameter("control_rate_hz").value - # --- Path state ------------------------------------------------------ self.waypoints = [] self.wp_idx = 0 - self.seg_start = None # (x, y) start of the current leg - self.first_leg_start = None # captured from odom when motion begins + self.seg_start = None + self.first_leg_start = None self.odom = None self.yaw_integral = 0.0 self.finished = False - # Gait state: INIT -> ALIGN -> DRIVE -> (ALIGN -> DRIVE)* -> HOLD. self.mode = "INIT" - self.hold_xy = None # (x, y) pose held while aligning / station-keeping - self.target_heading = 0.0 # desired heading (rad) during ALIGN / HOLD + self.hold_xy = None + self.target_heading = 0.0 - # --- I/O ------------------------------------------------------------- self.cmd_pub = self.create_publisher( Twist, self.get_parameter("cmd_vel_topic").value, 10 ) latched = QoSProfile(depth=1, durability=DurabilityPolicy.TRANSIENT_LOCAL) - self.reached_pub = self.create_publisher( - Bool, "/blueboat/goal_reached", latched - ) + self.reached_pub = self.create_publisher(Bool, "/blueboat/goal_reached", latched) self.create_subscription( Odometry, self.get_parameter("odom_topic").value, self.odom_sub, 10 ) @@ -155,25 +97,15 @@ def __init__(self): ) self.create_timer(self.dt, self.control_step) - self.get_logger().info( - "BlueBoat waypoint controller up: %d preloaded waypoint(s), " - "u_cruise=%.2f m/s, lookahead=[%.1f, %.1f] m" - % (len(self.waypoints), self.u_cruise, self.delta_min, self.delta_max) - ) def odom_sub(self, msg: Odometry): self.odom = msg def goal_sub(self, msg: PoseStamped): - """Append a goal to the path and resume if we had finished.""" self.waypoints.append((msg.pose.position.x, msg.pose.position.y)) if self.finished: self.finished = False self.reached_pub.publish(Bool(data=False)) - self.get_logger().info( - "Goal appended (%.2f, %.2f); %d waypoint(s) queued" - % (msg.pose.position.x, msg.pose.position.y, len(self.waypoints)) - ) def control_step(self): if self.odom is None or not self.waypoints: @@ -182,20 +114,17 @@ def control_step(self): x = self.odom.pose.pose.position.x y = self.odom.pose.pose.position.y yaw = yaw_from_quaternion(self.odom.pose.pose.orientation) - r = self.odom.twist.twist.angular.z # yaw rate (rad/s) + r = self.odom.twist.twist.angular.z if self.first_leg_start is None: self.first_leg_start = (x, y) - # INIT: align to the first leg from rest (skip straight to DRIVE if the - # turn-in-place gait is disabled). if self.mode == "INIT": self.seg_start = (x, y) self.hold_xy = (x, y) self.target_heading = self.leg_heading((x, y), self.waypoints[0]) self.mode = "ALIGN" if self.turn_in_place else "DRIVE" - # A goal appended after finishing re-opens the course -> resume. if self.finished and self.wp_idx < len(self.waypoints): self.finished = False self.reached_pub.publish(Bool(data=False)) @@ -204,25 +133,19 @@ def control_step(self): self.target_heading = self.leg_heading((x, y), self.waypoints[self.wp_idx]) self.mode = "ALIGN" if self.turn_in_place else "DRIVE" - # All waypoints consumed -> actively station-keep the final pose. if self.wp_idx >= len(self.waypoints): if not self.finished: self.finished = True self.reached_pub.publish(Bool(data=True)) - self.get_logger().info("Final waypoint reached. Station-keeping.") self.mode = "HOLD" self.station_keep(x, y, yaw, r, allow_repoint=True) return - # --- ALIGN: brake to a stop, THEN rotate in place to the next leg ---- if self.mode == "ALIGN": u_now = self.odom.twist.twist.linear.x e_x = self.along_body_err(x, y, yaw) - u_cmd = clamp(self.kp_pos * e_x, self.u_align_max) + u_cmd = clamp_symmetric(self.kp_pos * e_x, self.u_align_max) if abs(u_now) > self.v_settle: - # Still coasting in from the leg: decelerate in a straight line - # (no yaw) so the boat doesn't swing its nose while moving. - # The turn only begins once it has actually stopped. self.publish_cmd(u_cmd, 0.0) return w_cmd, psi_err = self.heading_pid(self.target_heading, yaw, r) @@ -230,10 +153,8 @@ def control_step(self): if abs(psi_err) < self.align_tol and abs(r) < self.align_settle: self.mode = "DRIVE" self.yaw_integral = 0.0 - self.get_logger().info("Leg %d aligned -> driving." % self.wp_idx) return - # --- DRIVE: Line-of-Sight path following ---------------------------- sx, sy = self.seg_start if self.seg_start is not None else self.first_leg_start tx, ty = self.waypoints[self.wp_idx] seg_dx, seg_dy = tx - sx, ty - sy @@ -241,14 +162,11 @@ def control_step(self): dist_to_target = math.hypot(tx - x, ty - y) if seg_len < 1e-3: - # Degenerate leg: just aim straight at the target. psi_d = math.atan2(ty - y, tx - x) along = 0.0 else: gamma = math.atan2(seg_dy, seg_dx) - # Signed cross-track error (positive to the left of the path). e = -(x - sx) * math.sin(gamma) + (y - sy) * math.cos(gamma) - # Along-track progress along the leg. along = (x - sx) * math.cos(gamma) + (y - sy) * math.sin(gamma) delta = max( self.delta_min, @@ -256,33 +174,27 @@ def control_step(self): ) psi_d = gamma + math.atan2(-e, delta) - # Reached the waypoint: stop and (if enabled) turn in place to the next. passed = seg_len > 1e-3 and along >= seg_len if dist_to_target < self.arrival_radius or passed: - self.get_logger().info("Leg %d reached." % self.wp_idx) self.seg_start = (tx, ty) self.hold_xy = (tx, ty) nxt = self.wp_idx + 1 if nxt < len(self.waypoints): self.target_heading = self.leg_heading((tx, ty), self.waypoints[nxt]) else: - self.target_heading = yaw # final: hold the heading we arrived on + self.target_heading = yaw self.wp_idx = nxt self.yaw_integral = 0.0 if self.turn_in_place and nxt < len(self.waypoints): self.mode = "ALIGN" - return # next tick handles the new leg / HOLD + return - # --- Heading PID + surge -------------------------------------------- w_cmd, psi_err = self.heading_pid(psi_d, yaw, r) - # Turn-then-go: cut speed while heading error is large. turn_factor = max(0.0, math.cos(psi_err)) ** self.shaping_p - # Slow approaching EVERY waypoint so the boat arrives slow enough to stop. slow = min(1.0, dist_to_target / self.r_slow) u_cmd = self.u_cruise * turn_factor * slow self.publish_cmd(u_cmd, w_cmd) - # ---------------------------------------------------------------- helpers def publish_cmd(self, u, w): cmd = Twist() cmd.linear.x = float(u) @@ -290,38 +202,28 @@ def publish_cmd(self, u, w): self.cmd_pub.publish(cmd) def heading_pid(self, psi_d, yaw, r): - """Heading PID -> desired yaw rate. Returns (w_cmd, psi_err).""" psi_err = wrap_to_pi(psi_d - yaw) if self.ki_yaw > 0.0: - # Integrate, then clamp so the integral CONTRIBUTION stays <= i_max. self.yaw_integral += psi_err * self.dt - self.yaw_integral = clamp(self.yaw_integral, self.i_max / self.ki_yaw) + self.yaw_integral = clamp_symmetric(self.yaw_integral, self.i_max / self.ki_yaw) else: self.yaw_integral = 0.0 - # Derivative on measurement (-Kd*r) avoids a kick when psi_d jumps. - w_cmd = clamp( + w_cmd = clamp_symmetric( self.kp_yaw * psi_err + self.ki_yaw * self.yaw_integral - self.kd_yaw * r, self.w_max, ) return w_cmd, psi_err def along_body_err(self, x, y, yaw): - """Position error to hold_xy projected on the body x-axis (+ = ahead).""" dx = self.hold_xy[0] - x dy = self.hold_xy[1] - y return dx * math.cos(yaw) + dy * math.sin(yaw) @staticmethod def leg_heading(a, b): - """Heading (rad) of the line from point a to point b.""" return math.atan2(b[1] - a[1], b[0] - a[0]) def station_keep(self, x, y, yaw, r, allow_repoint): - """Hold ``hold_xy``: surge nulls the along-body error while the heading - holds ``target_heading``. If drift exceeds ``repoint_radius`` the boat - points back at the hold point (the only way an underactuated USV can - correct a sideways offset). Within ``acceptance_radius`` the surge - deadbands so the thrusters don't jitter on station.""" rng = math.hypot(self.hold_xy[0] - x, self.hold_xy[1] - y) if rng < self.accept_r: w_cmd, _ = self.heading_pid(self.target_heading, yaw, r) @@ -332,7 +234,7 @@ def station_keep(self, x, y, yaw, r, allow_repoint): else: psi_d = self.target_heading w_cmd, _ = self.heading_pid(psi_d, yaw, r) - u_cmd = clamp(self.kp_pos * self.along_body_err(x, y, yaw), self.u_align_max) + u_cmd = clamp_symmetric(self.kp_pos * self.along_body_err(x, y, yaw), self.u_align_max) self.publish_cmd(u_cmd, w_cmd) diff --git a/multivehicle/packages/multivehicle_examples/scripts/bluerov_movement.py b/multivehicle/packages/multivehicle_examples/scripts/bluerov_movement.py deleted file mode 100755 index 8bb1284..0000000 --- a/multivehicle/packages/multivehicle_examples/scripts/bluerov_movement.py +++ /dev/null @@ -1,270 +0,0 @@ -#!/usr/bin/env python3 - -import math - -import rclpy -import rclpy.duration -import rclpy.time -from rclpy.node import Node -from geometry_msgs.msg import PoseStamped -from mavros_msgs.srv import SetMode, CommandBool -from rclpy.qos import QoSProfile, ReliabilityPolicy -import tf2_ros -from tf2_geometry_msgs import do_transform_pose_stamped - - -class BlueROVSquareMission(Node): - """Drive the BlueROV2 in a square relative to its own base_link. - - Each leg is a body-frame FLU offset (forward, left) applied from the pose at - the START of that leg, while the vehicle HOLDS its initial heading — i.e. it - strafes the square (forward, left, back, right) instead of yawing to face - each waypoint. This matches how an ROV actually moves and avoids the - spinning behaviour you get from publishing absolute map setpoints / facing - each target. - - Each leg's body-frame offset is transformed into a map-frame - `setpoint_position/local` target with tf2 (`do_transform_pose_stamped` - against the live `map -> base_link` transform MAVROS publishes), so the - square is always relative to the vehicle's own base_link, never the map - origin. - """ - - # TF frames: MAVROS local_position publishes map -> base_link (tf.send=true). - MAP_FRAME = "map" - BASE_FRAME = "base_link" - - def __init__(self): - super().__init__("bluerov_square_mission") - - mavros_qos = QoSProfile(depth=10, reliability=ReliabilityPolicy.BEST_EFFORT) - - self.target_pub = self.create_publisher( - PoseStamped, "/mavros/setpoint_position/local", mavros_qos - ) - self.curr_pose_sub = self.create_subscription( - PoseStamped, "/mavros/local_position/pose", self.pose_callback, mavros_qos - ) - - # tf2 buffer/listener — used to transform base_link offsets to map. - self.tf_buffer = tf2_ros.Buffer() - self.tf_listener = tf2_ros.TransformListener(self.tf_buffer, self) - - # Service Clients - self.arming_client = self.create_client(CommandBool, "/mavros/cmd/arming") - self.mode_client = self.create_client(SetMode, "/mavros/set_mode") - - # Service state flags - self.is_armed = False - self.is_guided_mode = False - self.services_initialized = False - - # Mission Params - self.depth = -2.0 # absolute dive depth (m below surface) - self.dist_threshold = 0.2 - - # Per-leg body-frame FLU offsets (forward+, left+). Each leg is applied - # from the pose at the start of that leg, with heading held constant -> - # the vehicle strafes a square and returns to start. - self.legs = [ - (5.0, 0.0), # forward 5 m - (0.0, 5.0), # left 5 m - (-5.0, 0.0), # backward 5 m - (0.0, -5.0), # right 5 m - ] - self.leg_index = 0 - - # Heading held for the whole mission (captured at START). - self.hold_yaw = None - # Current leg's absolute map-frame target (computed when the leg begins). - self.target_x = None - self.target_y = None - - self.current_pose = None - self.state = "INIT" - - # IMPORTANT NOTE: DO NOT SET A HIGH SETPOINT PUBLISH RATE. - # ROV WOULD KEEP REPLANNING ITS TRAJECTORY, CAUSING IT TO MOVE VERY SLOWLY - self.timer = self.create_timer(1.0, self.main_loop) - - def arm(self): - """Request arming""" - if not self.arming_client.service_is_ready(): - self.get_logger().info("Waiting for arming service...") - return False - - self.get_logger().info("Requesting arm...") - request = CommandBool.Request() - request.value = True - future = self.arming_client.call_async(request) - future.add_done_callback(self.arm_callback) - return True - - def arm_callback(self, future): - """Callback for arming service""" - try: - response = future.result() - if response.success: - self.is_armed = True - self.get_logger().info("Vehicle armed successfully") - else: - self.get_logger().warn("Failed to arm vehicle") - except Exception as e: - self.get_logger().error(f"Arming service call failed: {e}") - - def set_guided_mode(self): - """Request GUIDED mode""" - if not self.mode_client.service_is_ready(): - self.get_logger().info("Waiting for SetMode service...") - return False - - self.get_logger().info("Requesting GUIDED mode...") - req = SetMode.Request() - req.custom_mode = "GUIDED" - future = self.mode_client.call_async(req) - future.add_done_callback(self.guided_mode_callback) - return True - - def guided_mode_callback(self, future): - """Callback for setting to guided mode""" - try: - response = future.result() - if response.mode_sent: - self.is_guided_mode = True - self.get_logger().info("GUIDED mode set successfully") - else: - self.get_logger().warn("Failed to set GUIDED mode") - except Exception as e: - self.get_logger().error(f"Set mode service call failed: {e}") - - def pose_callback(self, msg): - self.current_pose = msg - - def get_quaternion_from_yaw(self, yaw): - """Convert yaw (radians) to a quaternion for PoseStamped.""" - return {"x": 0.0, "y": 0.0, "z": math.sin(yaw / 2.0), "w": math.cos(yaw / 2.0)} - - def get_yaw_from_quaternion(self, quaternion): - """Extract yaw (radians) from a quaternion.""" - siny_cosp = 2.0 * (quaternion.w * quaternion.z + quaternion.x * quaternion.y) - cosy_cosp = 1.0 - 2.0 * ( - quaternion.y * quaternion.y + quaternion.z * quaternion.z - ) - return math.atan2(siny_cosp, cosy_cosp) - - def leg_target_in_map(self, fwd, left): - """Transform a body-frame FLU offset (forward, left) at the current - base_link into a map-frame (x, y) target via tf2. Returns None if the - map -> base_link transform isn't available yet. - """ - pose_in_base = PoseStamped() - pose_in_base.header.frame_id = self.BASE_FRAME - pose_in_base.pose.position.x = fwd - pose_in_base.pose.position.y = left - pose_in_base.pose.position.z = 0.0 - pose_in_base.pose.orientation.w = 1.0 - try: - tf = self.tf_buffer.lookup_transform( - self.MAP_FRAME, - self.BASE_FRAME, - rclpy.time.Time(), - timeout=rclpy.duration.Duration(seconds=1.0), - ) - except tf2_ros.TransformException as e: - self.get_logger().warn( - f"TF {self.MAP_FRAME} <- {self.BASE_FRAME} not ready: {e}" - ) - return None - target = do_transform_pose_stamped(pose_in_base, tf) - return target.pose.position.x, target.pose.position.y - - def main_loop(self): - """Main state machine""" - - if self.state == "INIT": - if not self.services_initialized: - self.is_armed or self.arm() - self.is_guided_mode or self.set_guided_mode() - - if self.is_armed and self.is_guided_mode: - self.services_initialized = True - self.state = "START" - self.get_logger().info("Initialization complete, starting mission") - - elif self.state == "START": - if self.current_pose is None: - self.get_logger().info("Waiting for pose data...") - return - # Hold the heading the vehicle starts with for the whole square. - self.hold_yaw = self.get_yaw_from_quaternion( - self.current_pose.pose.orientation - ) - self.leg_index = 0 - self.state = "PLAN_LEG" - self.get_logger().info( - f"Beginning body-relative square (hold_yaw={self.hold_yaw:.2f})" - ) - - elif self.state == "PLAN_LEG": - # Transform this leg's body-frame offset (relative to the CURRENT - # base_link) into a map-frame target via tf2. Retry next tick if the - # transform isn't available yet. - fwd, left = self.legs[self.leg_index] - target = self.leg_target_in_map(fwd, left) - if target is None: - return - self.target_x, self.target_y = target - self.state = "MOVING" - self.get_logger().info( - f"Leg {self.leg_index} (fwd={fwd}, left={left}) -> " - f"target ({self.target_x:.2f}, {self.target_y:.2f})" - ) - - elif self.state == "MOVING": - # Publish the (fixed) leg target, holding the mission heading. - q = self.get_quaternion_from_yaw(self.hold_yaw) - msg = PoseStamped() - msg.header.stamp = self.get_clock().now().to_msg() - msg.header.frame_id = "map" - msg.pose.position.x = self.target_x - msg.pose.position.y = self.target_y - msg.pose.position.z = self.depth - msg.pose.orientation.x = q["x"] - msg.pose.orientation.y = q["y"] - msg.pose.orientation.z = q["z"] - msg.pose.orientation.w = q["w"] - self.target_pub.publish(msg) - - dx = self.target_x - self.current_pose.pose.position.x - dy = self.target_y - self.current_pose.pose.position.y - dist = math.hypot(dx, dy) - self.get_logger().info( - f"Leg {self.leg_index}: dist to target {dist:.2f} m" - ) - - if dist < self.dist_threshold: - self.get_logger().info(f"Leg {self.leg_index} reached!") - self.leg_index += 1 - if self.leg_index >= len(self.legs): - self.state = "FINISHED" - else: - self.state = "PLAN_LEG" - - elif self.state == "FINISHED": - self.get_logger().info("Mission successful.") - raise SystemExit - - -def main(args=None): - rclpy.init(args=args) - node = BlueROVSquareMission() - try: - rclpy.spin(node) - except SystemExit: - pass - node.destroy_node() - rclpy.shutdown() - - -if __name__ == "__main__": - main() diff --git a/multivehicle/packages/multivehicle_examples/src/PX4OffboardDemo.cpp b/multivehicle/packages/multivehicle_examples/src/PX4OffboardDemo.cpp deleted file mode 100644 index 6ec8369..0000000 --- a/multivehicle/packages/multivehicle_examples/src/PX4OffboardDemo.cpp +++ /dev/null @@ -1,227 +0,0 @@ -#include "PX4OffboardDemo.hpp" - -#include -#include - -using namespace std::chrono_literals; -using std::placeholders::_1; - -using px4_msgs::msg::OffboardControlMode; -using px4_msgs::msg::TrajectorySetpoint; -using px4_msgs::msg::VehicleCommand; -using px4_msgs::msg::VehicleLandDetected; -using px4_msgs::msg::VehicleLocalPosition; -using px4_msgs::msg::VehicleStatus; - -namespace { -// Wrap an angle into (-pi, pi]. -float wrapToPi(float angle) { - return std::atan2(std::sin(angle), std::cos(angle)); -} -} // namespace - -OffboardDemo::OffboardDemo() : Node("px4_offboard_demo") { - _ns = this->declare_parameter("vehicle_namespace", ""); - _sys_id = this->declare_parameter("vehicle_id", 0); - _prefix = _ns.empty() ? std::string() : "/" + _ns; - - RCLCPP_INFO(get_logger(), "vehicle_namespace='%s' (topic prefix '%s'), target_system=%d", - _ns.c_str(), _prefix.c_str(), _sys_id); - - _cmd_pub = create_publisher(_prefix + "/fmu/in/vehicle_command", 10); - _setpoint_pub = create_publisher(_prefix + "/fmu/in/trajectory_setpoint", 10); - _offboard_mode_pub = create_publisher(_prefix + "/fmu/in/offboard_control_mode", 10); - - // PX4 publishes telemetry best-effort; match it or subscriptions stay empty. - _status_sub = create_subscription( - _prefix + "/fmu/out/vehicle_status_v1", rclcpp::QoS(1).best_effort(), - std::bind(&OffboardDemo::onStatus, this, _1)); - _position_sub = create_subscription( - _prefix + "/fmu/out/vehicle_local_position", rclcpp::QoS(10).best_effort(), - std::bind(&OffboardDemo::onLocalPosition, this, _1)); - _land_sub = create_subscription( - _prefix + "/fmu/out/vehicle_land_detected", rclcpp::QoS(1).best_effort(), - std::bind(&OffboardDemo::onLandDetected, this, _1)); - - // Stream the offboard-mode heartbeat independently of the mission tick. - _heartbeat_timer = create_wall_timer(100ms, std::bind(&OffboardDemo::streamOffboardHeartbeat, this)); - - _phase_since = now(); - - // Prime the setpoint stream with a no-op so PX4 sees a setpoint before we ask - // for offboard mode. - publishPosition(Eigen::Vector3f(NAN, NAN, NAN), NAN); - - RCLCPP_INFO(get_logger(), "OffboardDemo initialised."); -} - -void OffboardDemo::onStatus(const VehicleStatus::SharedPtr msg) { _status = *msg; } - -void OffboardDemo::onLocalPosition(const VehicleLocalPosition::SharedPtr msg) { _position = *msg; } - -void OffboardDemo::onLandDetected(const VehicleLandDetected::SharedPtr msg) { _landed = msg->landed; } - -uint64_t OffboardDemo::nowMicros() const { - return static_cast(this->now().nanoseconds() / 1000); -} - -void OffboardDemo::streamOffboardHeartbeat() { - OffboardControlMode mode{}; - mode.timestamp = nowMicros(); - mode.position = true; // we only ever command position setpoints - _offboard_mode_pub->publish(mode); -} - -void OffboardDemo::publishPosition(const Eigen::Vector3f& pos_ned, float yaw) { - TrajectorySetpoint sp{}; - sp.timestamp = nowMicros(); - sp.position = {pos_ned.x(), pos_ned.y(), pos_ned.z()}; - sp.yaw = yaw; // NAN = let PX4 hold the current heading - _setpoint_pub->publish(sp); -} - -void OffboardDemo::sendCommand(uint32_t command, float param1, float param2) { - VehicleCommand cmd{}; - cmd.timestamp = nowMicros(); - cmd.command = command; - cmd.param1 = param1; - cmd.param2 = param2; - cmd.target_system = static_cast(_sys_id); - cmd.target_component = 1; - cmd.source_system = 1; - cmd.source_component = 1; - cmd.from_external = true; - _cmd_pub->publish(cmd); -} - -void OffboardDemo::requestOffboardMode() { - // base mode = custom (1), custom main mode = OFFBOARD (6). - sendCommand(VehicleCommand::VEHICLE_CMD_DO_SET_MODE, 1.0f, 6.0f); -} - -void OffboardDemo::arm() { - sendCommand(VehicleCommand::VEHICLE_CMD_COMPONENT_ARM_DISARM, 1.0f); -} - -void OffboardDemo::disarm() { - sendCommand(VehicleCommand::VEHICLE_CMD_COMPONENT_ARM_DISARM, 0.0f); -} - -void OffboardDemo::commandLand() { - sendCommand(VehicleCommand::VEHICLE_CMD_NAV_LAND); -} - -bool OffboardDemo::phaseElapsed(double seconds) const { - return (now() - _phase_since).seconds() > seconds; -} - -bool OffboardDemo::reached(const Eigen::Vector3f& target, float tol) const { - return std::fabs(_position.x - target.x()) < tol && - std::fabs(_position.y - target.y()) < tol && - std::fabs(_position.z - target.z()) < tol; -} - -bool OffboardDemo::yawReached(float target_yaw, float tol) const { - return std::fabs(wrapToPi(target_yaw - _position.heading)) < tol; -} - -void OffboardDemo::transitionTo(Phase next, const char* note) { - _phase = next; - _phase_since = now(); - RCLCPP_INFO(get_logger(), "%s", note); -} - -void OffboardDemo::tick() { - const Eigen::Vector3f standoff(kStandoffNorth, kStandoffEast, kStandoffDown); - - switch (_phase) { - case Phase::WaitForReady: - // Let the autopilot settle, then latch the launch point as home (climbed - // up by kClimbHeight) and request offboard. - if (_status.pre_flight_checks_pass && phaseElapsed(kReadyHold)) { - _home_ned = Eigen::Vector3f(_position.x, _position.y, _position.z - kClimbHeight); - requestOffboardMode(); - transitionTo(Phase::RequestArm, "Ready -> requesting OFFBOARD"); - } - break; - - case Phase::RequestArm: - if (_status.nav_state == VehicleStatus::NAVIGATION_STATE_OFFBOARD) { - arm(); - transitionTo(Phase::AwaitArmed, "OFFBOARD -> arming"); - } - break; - - case Phase::AwaitArmed: - if (_status.arming_state == VehicleStatus::ARMING_STATE_ARMED) { - transitionTo(Phase::ClimbOut, "Armed -> climbing out"); - } - break; - - case Phase::ClimbOut: - // Hold the home setpoint; the vehicle climbs to it. Move on after a fixed - // settle so we reach altitude before transiting. - publishPosition(_home_ned, NAN); - if (phaseElapsed(kClimbHold)) { - transitionTo(Phase::GoToStandoff, "Climb-out -> standoff transit"); - } - break; - - case Phase::GoToStandoff: - publishPosition(standoff, NAN); - if (reached(standoff)) { - _scan_yaw_target = _position.heading; - _scan_yaw_swept = 0.0f; - transitionTo(Phase::ScanYaw, "At standoff -> yaw scan"); - } - break; - - case Phase::ScanYaw: - // Hold position and step the yaw target one notch each time we settle on - // the previous one, until a full revolution has been swept. - if (yawReached(_scan_yaw_target)) { - _scan_yaw_target = wrapToPi(_scan_yaw_target + kYawStep); - _scan_yaw_swept += kYawStep; - } - publishPosition(standoff, _scan_yaw_target); - if (_scan_yaw_swept >= 2.0f * static_cast(M_PI)) { - transitionTo(Phase::ReturnHome, "Yaw scan complete -> returning home"); - } - break; - - case Phase::ReturnHome: - publishPosition(_home_ned, NAN); - if (reached(_home_ned)) { - commandLand(); - transitionTo(Phase::Descend, "Home -> landing"); - } - break; - - case Phase::Descend: - if (_landed) { - disarm(); - transitionTo(Phase::Finished, "Touchdown -> disarmed"); - } - break; - - case Phase::Finished: - break; - } -} - -void OffboardDemo::run() { - while (rclcpp::ok() && _phase != Phase::Finished) { - rclcpp::spin_some(this->get_node_base_interface()); - tick(); - rclcpp::sleep_for(10ms); - } - RCLCPP_INFO(get_logger(), "Mission complete."); -} - -int main(int argc, char* argv[]) { - rclcpp::init(argc, argv); - auto node = std::make_shared(); - node->run(); - rclcpp::shutdown(); - return 0; -} diff --git a/multivehicle/packages/multivehicle_examples/src/PX4OffboardDemo.hpp b/multivehicle/packages/multivehicle_examples/src/PX4OffboardDemo.hpp deleted file mode 100644 index 28eadc8..0000000 --- a/multivehicle/packages/multivehicle_examples/src/PX4OffboardDemo.hpp +++ /dev/null @@ -1,102 +0,0 @@ -// PX4OffboardDemo.hpp — offboard control demo for the x500 UAV -#pragma once - -#include -#include -#include -#include -#include - -#include -#include -#include -#include -#include -#include -#include - -// Drives a single PX4 vehicle through a fixed offboard mission. All PX4 topics -// are prefixed with the vehicle namespace (e.g. "/x500/fmu/...") so the node -// can target one instance among several. -class OffboardDemo : public rclcpp::Node { - public: - OffboardDemo(); - - // Blocking: spins the node and advances the mission until it completes. - void run(); - - private: - // Mission phases, in the order they execute. - enum class Phase { - WaitForReady, // hold until pre-flight checks pass, then request offboard - RequestArm, // offboard accepted -> send the arm command - AwaitArmed, // wait for the autopilot to report ARMED - ClimbOut, // hold the climb setpoint above launch - GoToStandoff, // transit to the standoff observation point - ScanYaw, // rotate 360 degrees in place while holding position - ReturnHome, // fly back to the launch point - Descend, // land and wait for touchdown - Finished, - }; - - // Subscription callbacks (latch the latest message). - void onStatus(const px4_msgs::msg::VehicleStatus::SharedPtr msg); - void onLocalPosition(const px4_msgs::msg::VehicleLocalPosition::SharedPtr msg); - void onLandDetected(const px4_msgs::msg::VehicleLandDetected::SharedPtr msg); - - // Keepalive: PX4 only stays in offboard while this stream is fresh. - void streamOffboardHeartbeat(); - - // Outgoing commands / setpoints. - void sendCommand(uint32_t command, float param1 = NAN, float param2 = NAN); - void requestOffboardMode(); - void arm(); - void disarm(); - void commandLand(); - void publishPosition(const Eigen::Vector3f& pos_ned, float yaw); - - // Mission step + small predicates. - void tick(); - void transitionTo(Phase next, const char* note); - bool phaseElapsed(double seconds) const; - bool reached(const Eigen::Vector3f& target, float tol = kReachedTol) const; - bool yawReached(float target_yaw, float tol = kYawTol) const; - uint64_t nowMicros() const; - - // ---- parameters ---- - std::string _ns; // vehicle_namespace - std::string _prefix; // "/" (or "" when unset) - int _sys_id = 0; // vehicle_id -> MAVLink target_system - - // ---- ROS entities ---- - rclcpp::Publisher::SharedPtr _cmd_pub; - rclcpp::Publisher::SharedPtr _setpoint_pub; - rclcpp::Publisher::SharedPtr _offboard_mode_pub; - rclcpp::Subscription::SharedPtr _status_sub; - rclcpp::Subscription::SharedPtr _position_sub; - rclcpp::Subscription::SharedPtr _land_sub; - rclcpp::TimerBase::SharedPtr _heartbeat_timer; - - // ---- latest telemetry ---- - px4_msgs::msg::VehicleStatus _status{}; - px4_msgs::msg::VehicleLocalPosition _position{}; - bool _landed = false; - - // ---- mission state ---- - Phase _phase = Phase::WaitForReady; - rclcpp::Time _phase_since; - Eigen::Vector3f _home_ned = Eigen::Vector3f::Zero(); // launch point + climb (NED) - float _scan_yaw_target = 0.0f; // current yaw setpoint during ScanYaw - float _scan_yaw_swept = 0.0f; // total yaw rotated so far - - // ---- mission constants (NED; z is down-positive) ---- - static constexpr float kClimbHeight = 10.0f; // m above the launch deck - static constexpr float kStandoffNorth = -14.0f; // standoff observation point - static constexpr float kStandoffEast = -25.0f; - static constexpr float kStandoffDown = -6.0f; - static constexpr float kReachedTol = 0.1f; // m, position-reached tolerance - static constexpr float kYawStep = static_cast(M_PI / 180.0); // 1 deg - static constexpr float kYawTol = static_cast(M_PI / 180.0); // 1 deg - static constexpr double kReadyHold = 2.0; // s, settle before offboard - static constexpr double kClimbHold = 10.0; // s, hold the climb setpoint -}; diff --git a/multivehicle/run.bash b/multivehicle/run.bash index d8f8a88..d12707d 100755 --- a/multivehicle/run.bash +++ b/multivehicle/run.bash @@ -2,13 +2,6 @@ set -e -# Run the multivehicle_examples image with NVIDIA + X11 via rocker. -# Mounts ONLY this workspace at /root/HOST/mvsim_ws (the path the tmuxp sessions -# and setup_dashboard.sh expect). This script lives in src/examples/multivehicle, -# so the workspace root is three levels up. -# -# SPDX-License-Identifier: MIT - IMAGE_NAME="${1:-multivehicle_examples:humble}" SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)" WORKSPACE_DIR="$(cd "${SCRIPT_DIR}/../../.." && pwd)" From 36618cd17f5bcb72e2397df703caef1c7cb9bc3d Mon Sep 17 00:00:00 2001 From: monkescripts Date: Fri, 26 Jun 2026 15:04:24 +0800 Subject: [PATCH 6/8] Remove deprecated BlueROV launch and movement scripts; update README and Dockerfile for new dependencies and configurations; add new YAML config for UAV2 offboard control; modify tmuxp session for improved clarity. --- .../launch/bluerov_mission.launch.py | 23 -- .../bluerov_tasks/scripts/bluerov_movement.py | 209 ------------------ multivehicle/README.md | 14 +- multivehicle/docker/Dockerfile | 8 +- multivehicle/examples.repos | 8 + .../config/uav2_offboard_x500.yaml | 21 ++ .../launch/bluerov_mission.launch.py | 25 --- .../launch/px4_offboard.launch.py | 37 ++-- .../multivehicle_examples/package.xml | 2 + multivehicle/tmuxp/mvsim_debug.yaml | 37 ++-- 10 files changed, 80 insertions(+), 304 deletions(-) delete mode 100644 bluerov/packages/bluerov_tasks/launch/bluerov_mission.launch.py delete mode 100755 bluerov/packages/bluerov_tasks/scripts/bluerov_movement.py create mode 100644 multivehicle/packages/multivehicle_examples/config/uav2_offboard_x500.yaml delete mode 100644 multivehicle/packages/multivehicle_examples/launch/bluerov_mission.launch.py diff --git a/bluerov/packages/bluerov_tasks/launch/bluerov_mission.launch.py b/bluerov/packages/bluerov_tasks/launch/bluerov_mission.launch.py deleted file mode 100644 index 63e772f..0000000 --- a/bluerov/packages/bluerov_tasks/launch/bluerov_mission.launch.py +++ /dev/null @@ -1,23 +0,0 @@ -from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument -from launch.substitutions import LaunchConfiguration -from launch_ros.actions import Node - - -def generate_launch_description(): - use_sim_time = LaunchConfiguration("use_sim_time") - - movement_node = Node( - package="bluerov_tasks", - executable="bluerov_movement.py", - name="bluerov_movement", - output="screen", - parameters=[{"use_sim_time": use_sim_time}], - ) - - return LaunchDescription( - [ - DeclareLaunchArgument("use_sim_time", default_value="true"), - movement_node, - ] - ) diff --git a/bluerov/packages/bluerov_tasks/scripts/bluerov_movement.py b/bluerov/packages/bluerov_tasks/scripts/bluerov_movement.py deleted file mode 100755 index 825fb6f..0000000 --- a/bluerov/packages/bluerov_tasks/scripts/bluerov_movement.py +++ /dev/null @@ -1,209 +0,0 @@ -#!/usr/bin/env python3 - -import rclpy -from rclpy.node import Node -from geometry_msgs.msg import PoseStamped -from mavros_msgs.srv import SetMode, CommandBool -from rclpy.qos import QoSProfile, ReliabilityPolicy -import math - - -class BlueROVSquareMission(Node): - def __init__(self): - super().__init__("bluerov_square_mission") - - mavros_qos = QoSProfile(depth=10, reliability=ReliabilityPolicy.BEST_EFFORT) - - self.target_pub = self.create_publisher( - PoseStamped, "/mavros/setpoint_position/local", mavros_qos - ) - self.curr_pose_sub = self.create_subscription( - PoseStamped, "/mavros/local_position/pose", self.pose_callback, mavros_qos - ) - - # Service Clients - self.arming_client = self.create_client(CommandBool, "/mavros/cmd/arming") - self.mode_client = self.create_client(SetMode, "/mavros/set_mode") - - # Service state flags - self.is_armed = False - self.is_guided_mode = False - self.services_initialized = False - - # Mission Params - self.depth = -2.0 - self.dist_threshold = 0.2 - self.yaw_threshold = 0.1 - - # State & Waypoints in FLU - self.current_pose = None - self.state = "INIT" - self.waypoints = [ - (5.0, 0.0), # Forward - (5.0, 5.0), # Left - (0.0, 5.0), # Backward - (0.0, 0.0), # Back to start - ] - self.wp_index = 0 - - # IMPORTANT NOTE: DO NOT SET A HIGH SETPOINT PUBLISH RATE. - # ROV WOULD KEEP REPLANNING ITS TRAJECTORY, CAUSING IT TO MOVE VERY SLOWLY - self.timer = self.create_timer(1.0, self.main_loop) - - def arm(self): - """Request arming""" - if not self.arming_client.service_is_ready(): - self.get_logger().info("Waiting for arming service...") - return False - - self.get_logger().info("Requesting arm...") - request = CommandBool.Request() - request.value = True - future = self.arming_client.call_async(request) - future.add_done_callback(self.arm_callback) - return True - - def arm_callback(self, future): - """Callback for arming service""" - try: - response = future.result() - if response.success: - self.is_armed = True - self.get_logger().info("Vehicle armed successfully") - else: - self.get_logger().warn("Failed to arm vehicle") - except Exception as e: - self.get_logger().error(f"Arming service call failed: {e}") - - def set_guided_mode(self): - """Request GUIDED mode""" - if not self.mode_client.service_is_ready(): - self.get_logger().info("Waiting for SetMode service...") - return False - - self.get_logger().info("Requesting GUIDED mode...") - req = SetMode.Request() - req.custom_mode = "GUIDED" - future = self.mode_client.call_async(req) - future.add_done_callback(self.guided_mode_callback) - return True - - def guided_mode_callback(self, future): - """Callback for setting to guided mode""" - try: - response = future.result() - if response.mode_sent: - self.is_guided_mode = True - self.get_logger().info("GUIDED mode set successfully") - else: - self.get_logger().warn("Failed to set GUIDED mode") - except Exception as e: - self.get_logger().error(f"Set mode service call failed: {e}") - - def pose_callback(self, msg): - self.current_pose = msg - - def get_quaternion_from_yaw(self, yaw): - """Convert yaw (radians) to a quaternion for PoseStamped.""" - return {"x": 0.0, "y": 0.0, "z": math.sin(yaw / 2.0), "w": math.cos(yaw / 2.0)} - - def get_yaw_from_quaternion(self, quaternion): - """Extract yaw (radians) from a quaternion.""" - siny_cosp = 2.0 * (quaternion.w * quaternion.z + quaternion.x * quaternion.y) - cosy_cosp = 1.0 - 2.0 * ( - quaternion.y * quaternion.y + quaternion.z * quaternion.z - ) - return math.atan2(siny_cosp, cosy_cosp) - - def normalize_angle(self, angle): - """Normalize angle to [-pi, pi]""" - while angle > math.pi: - angle -= 2.0 * math.pi - while angle < -math.pi: - angle += 2.0 * math.pi - return angle - - def main_loop(self): - """Main state machine""" - - if self.state == "INIT": - # Try to initialize services - if not self.services_initialized: - if not self.is_armed: - self.arm() - if not self.is_guided_mode: - self.set_guided_mode() - - if self.is_armed and self.is_guided_mode: - self.services_initialized = True - self.state = "START" - self.get_logger().info("Initialization complete, starting mission") - - elif self.state == "START": - if self.current_pose is None: - self.get_logger().info("Waiting for pose data...") - return - self.state = "MOVING" - self.get_logger().info("Beginning square mission") - - elif self.state == "MOVING": - tx, ty = self.waypoints[self.wp_index] - - # Calculate Yaw to face the target - dx = tx - self.current_pose.pose.position.x - dy = ty - self.current_pose.pose.position.y - yaw = math.atan2(dy, dx) - q = self.get_quaternion_from_yaw(yaw) - - # Publish Pose - msg = PoseStamped() - msg.header.stamp = self.get_clock().now().to_msg() - msg.header.frame_id = "map" - msg.pose.position.x = tx - msg.pose.position.y = ty - msg.pose.position.z = self.depth - msg.pose.orientation.x = q["x"] - msg.pose.orientation.y = q["y"] - msg.pose.orientation.z = q["z"] - msg.pose.orientation.w = q["w"] - self.target_pub.publish(msg) - self.get_logger().info(f"Publishing waypoint: {msg.pose.position}") - - # Check distance - dist = math.sqrt(dx**2 + dy**2) - target_yaw = self.get_yaw_from_quaternion(msg.pose.orientation) - current_yaw = self.get_yaw_from_quaternion( - self.current_pose.pose.orientation - ) - yaw_error = abs(self.normalize_angle(target_yaw - current_yaw)) - reach_threshold = ( - dist < self.dist_threshold and yaw_error < self.yaw_threshold - ) - self.get_logger().info( - f"dist to target: {dist}, current yaw: {current_yaw}, target_yaw: {target_yaw} reach_threshold: {reach_threshold}" - ) - - if dist < self.dist_threshold: - self.get_logger().info(f"Waypoint {self.wp_index} reached!") - self.wp_index += 1 - if self.wp_index >= len(self.waypoints): - self.state = "FINISHED" - - elif self.state == "FINISHED": - self.get_logger().info("Mission successful.") - raise SystemExit - - -def main(args=None): - rclpy.init(args=args) - node = BlueROVSquareMission() - try: - rclpy.spin(node) - except SystemExit: - pass - node.destroy_node() - rclpy.shutdown() - - -if __name__ == "__main__": - main() diff --git a/multivehicle/README.md b/multivehicle/README.md index 7017590..4eb7a26 100644 --- a/multivehicle/README.md +++ b/multivehicle/README.md @@ -5,9 +5,12 @@ Missions and control demos built on ## Launch files -- `bluerov_mission.launch.py`: `multivehicle_sim` + `ground_truth_to_mavros` + `bluerov_tasks/bluerov_movement.py` +- BlueROV square mission: run `bluerov_tasks`'s tree launch directly + `ros2 launch bluerov_tasks bluerov_square_bt.launch.py` - `boat_control.launch.py`: BlueBoat thrust mixer + LOS controller (+ optional mission node) -- `px4_offboard.launch.py`: PX4 offboard demo from `uav2_offboard` +- `px4_offboard.launch.py`: the `uav2_offboard` `offboard_node` action backend + the + `mission_planner_2` UAV2 offboard demo behaviour tree (`uav2_offboard_demo_main.py`) + driving it (takeoff → standoff → return → land). ## Setup @@ -31,10 +34,3 @@ colcon build --symlink-install --packages-up-to multivehicle_examples microxrced source install/setup.bash tmuxp load src/examples/multivehicle/tmuxp/mvsim_debug.yaml ``` - -## Provenance - -- `packages/multivehicle_examples/scripts/blueboat_mission.py` adapted from `bring-up/etc/uav2_sim` -- `packages/multivehicle_examples/scripts/blueboat_thrust_mixer.py` adapted from `bring-up/etc/uav2_sim` -- `packages/multivehicle_examples/scripts/blueboat_waypoint_controller.py` adapted from `bring-up/etc/uav2_sim` -- PX4 offboard demo is provided by `https://github.com/BumblebeeAS/uav2_offboard` diff --git a/multivehicle/docker/Dockerfile b/multivehicle/docker/Dockerfile index 9f36380..dce6149 100644 --- a/multivehicle/docker/Dockerfile +++ b/multivehicle/docker/Dockerfile @@ -6,9 +6,15 @@ ARG HOME_DIR=/root RUN apt-get update && apt-get install -y --no-install-recommends \ ros-humble-navigation2 \ - ros-humble-nav2-bringup && \ + ros-humble-nav2-bringup \ + ros-humble-py-trees \ + ros-humble-py-trees-ros \ + ros-humble-tf-transformations && \ apt-get autoremove -y && \ apt-get clean && \ rm -rf /var/lib/apt/lists/* +RUN pip3 install --no-cache-dir --ignore-installed "transforms3d>=0.4" "numpy>=1.25,<2" +RUN pip3 install --no-cache-dir mavsdk + WORKDIR ${HOME_DIR} diff --git a/multivehicle/examples.repos b/multivehicle/examples.repos index 832e1a2..043495b 100644 --- a/multivehicle/examples.repos +++ b/multivehicle/examples.repos @@ -31,3 +31,11 @@ repositories: uav2_offboard: type: git url: https://github.com/BumblebeeAS/uav2_offboard.git + mission_planner_release: + type: git + url: https://github.com/BumblebeeAS/mission_planner_release.git + version: main + frames: + type: git + url: https://github.com/BumblebeeAS/frames.git + version: humble diff --git a/multivehicle/packages/multivehicle_examples/config/uav2_offboard_x500.yaml b/multivehicle/packages/multivehicle_examples/config/uav2_offboard_x500.yaml new file mode 100644 index 0000000..482c6a0 --- /dev/null +++ b/multivehicle/packages/multivehicle_examples/config/uav2_offboard_x500.yaml @@ -0,0 +1,21 @@ +/**/offboard_node: + ros__parameters: + mav_sys_id: 2 + offboard_control_mode_topic: /x500/fmu/in/offboard_control_mode + trajectory_setpoint_topic: /x500/fmu/in/trajectory_setpoint + vehicle_command_topic: /x500/fmu/in/vehicle_command + vehicle_local_position_topic: /x500/fmu/out/vehicle_local_position + vehicle_status_topic: /x500/fmu/out/vehicle_status_v1 + acceleration_control_topic: /uav2/acceleration_control + +/**/landing_target_pose_node: + ros__parameters: + landing_target_pose_topic: /x500/fmu/in/landing_target_pose + vehicle_status_topic: /x500/fmu/out/vehicle_status_v1 + +/**/actuator_control_node: + ros__parameters: + actuation_server_name: /uav2/tins/actuation + px4_address: udpin://0.0.0.0:14540 + timeout: 5.0 + actuation_indexes: [1] diff --git a/multivehicle/packages/multivehicle_examples/launch/bluerov_mission.launch.py b/multivehicle/packages/multivehicle_examples/launch/bluerov_mission.launch.py deleted file mode 100644 index 8bfaabd..0000000 --- a/multivehicle/packages/multivehicle_examples/launch/bluerov_mission.launch.py +++ /dev/null @@ -1,25 +0,0 @@ -from launch import LaunchDescription -from launch_ros.actions import Node - - -def generate_launch_description(): - use_sim_time = {"use_sim_time": True} - - return LaunchDescription( - [ - Node( - package="multivehicle_interface", - executable="ground_truth_to_mavros", - name="ground_truth_to_mavros", - output="screen", - parameters=[use_sim_time], - ), - Node( - package="bluerov_tasks", - executable="bluerov_movement.py", - name="bluerov_movement", - output="screen", - parameters=[use_sim_time], - ), - ] - ) diff --git a/multivehicle/packages/multivehicle_examples/launch/px4_offboard.launch.py b/multivehicle/packages/multivehicle_examples/launch/px4_offboard.launch.py index 8492a30..0766c26 100644 --- a/multivehicle/packages/multivehicle_examples/launch/px4_offboard.launch.py +++ b/multivehicle/packages/multivehicle_examples/launch/px4_offboard.launch.py @@ -1,27 +1,38 @@ +import os + +from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource from launch.substitutions import LaunchConfiguration from launch_ros.actions import Node def generate_launch_description(): - package = LaunchConfiguration("offboard_package") - executable = LaunchConfiguration("offboard_executable") + params_file = LaunchConfiguration("params_file") + + offboard_launch = os.path.join( + get_package_share_directory("uav2_offboard"), "launch", "launch.py" + ) + default_params = os.path.join( + get_package_share_directory("multivehicle_examples"), + "config", + "uav2_offboard_x500.yaml", + ) return LaunchDescription( [ - DeclareLaunchArgument("offboard_package", default_value="uav2_offboard"), - DeclareLaunchArgument("offboard_executable", default_value="px4_offboard_demo"), + DeclareLaunchArgument("params_file", default_value=default_params), + IncludeLaunchDescription( + PythonLaunchDescriptionSource(offboard_launch), + launch_arguments={"params_file": params_file}.items(), + ), Node( - package=package, - executable=executable, - name="px4_offboard_demo", + package="mission_planner_2", + executable="uav2_offboard_demo_main.py", + name="uav2_offboard_demo", output="screen", - parameters=[ - {"use_sim_time": True}, - {"vehicle_namespace": "x500"}, - {"vehicle_id": 2}, - ], + parameters=[{"use_sim_time": True}], ), ] ) diff --git a/multivehicle/packages/multivehicle_examples/package.xml b/multivehicle/packages/multivehicle_examples/package.xml index f808144..48dee0e 100644 --- a/multivehicle/packages/multivehicle_examples/package.xml +++ b/multivehicle/packages/multivehicle_examples/package.xml @@ -11,9 +11,11 @@ ament_index_python bluerov_tasks + frames geometry_msgs launch launch_ros + mission_planner_2 multivehicle_interface nav_msgs rclpy diff --git a/multivehicle/tmuxp/mvsim_debug.yaml b/multivehicle/tmuxp/mvsim_debug.yaml index d173bf8..544acb8 100644 --- a/multivehicle/tmuxp/mvsim_debug.yaml +++ b/multivehicle/tmuxp/mvsim_debug.yaml @@ -1,7 +1,5 @@ -# tmuxp DEBUG session for the multivehicle examples (run INSIDE the -# multivehicle_examples container). Brings up the full stack — the simulator -# (multivehicle_sim) AND the example apps (missions, BlueBoat control, the PX4 -# offboard flight demo) — plus the dashboard. Load with: +# tmuxp DEBUG session for the multivehicle examples +# Load with: # # tmuxp load /root/HOST/mvsim_ws/src/examples/multivehicle/tmuxp/mvsim_debug.yaml # @@ -9,16 +7,11 @@ # dependency order and watch each one. Recommended order: # # 1) world — bring up Gazebo + the course first (multivehicle_sim) -# 2) bluerov — BlueROV2 + ArduSub/MAVROS, then its square-dive mission +# 2) bluerov — BlueROV2 + ArduSub/MAVROS, then its square mission BT # 3) boat — BlueBoat spawn + control stack + square mission -# 4) drone — XRCE agent + PX4 SITL x500 + gz bridge + offboard demo +# 4) drone — XRCE agent + PX4 SITL x500 + gz bridge + offboard demo BT # 5) dashboard — web backend (:8080) + sim-side LED/incident bridges # 6) debug — free shell + Foxglove bridge -# -# Workspace dir defaults to /root/HOST/mvsim_ws. Override with WS_DIR: -# WS_DIR=/root/HOST/my_ws tmuxp load .../mvsim_debug.yaml -# -# NVIDIA hosts: uncomment the two PRIME-offload exports in shell_command_before. session_name: mvsim @@ -28,8 +21,9 @@ global_options: shell_command_before: - 'WS_DIR="${WS_DIR:-/root/HOST/mvsim_ws}"' - # - export __NV_PRIME_RENDER_OFFLOAD=1 - # - export __GLX_VENDOR_LIBRARY_NAME=nvidia + # NVIDIA PRIME render offload — auto-enabled when an NVIDIA GPU is passed through + # (rocker --nvidia mounts /dev/nvidia0). Harmless / skipped on non-NVIDIA hosts. + - '[ -e /dev/nvidia0 ] && export __NV_PRIME_RENDER_OFFLOAD=1 __GLX_VENDOR_LIBRARY_NAME=nvidia || true' - source /opt/ros/humble/setup.bash - '[ -f "$WS_DIR/install/setup.bash" ] && source "$WS_DIR/install/setup.bash"' - cd "$WS_DIR" @@ -43,8 +37,7 @@ windows: - cmd: ros2 launch multivehicle_sim world.launch.py world_name:=robotx_2026_sg_river enter: false - # 2. BlueROV2 — spawn + ArduSub + MAVROS (sim, top); square-dive mission - # (examples, bottom). + # 2. BlueROV2 - window_name: bluerov layout: even-vertical panes: @@ -52,11 +45,10 @@ windows: - cmd: ros2 launch multivehicle_sim bluerov.launch.py enter: false - shell_command: - - cmd: ros2 launch multivehicle_examples bluerov_mission.launch.py + - cmd: ros2 launch bluerov_tasks bluerov_square_bt.launch.py enter: false - # 3. BlueBoat — spawn (sim, top); control stack + square mission (examples, - # bottom; use_mission:=true runs the demo course through the LOS controller). + # 3. BlueBoat - window_name: boat layout: even-vertical panes: @@ -67,8 +59,7 @@ windows: - cmd: ros2 launch multivehicle_examples boat_control.launch.py use_mission:=true enter: false - # 4. PX4 x500 drone — agent + PX4 SITL + gz bridge (sim), then the offboard - # flight demo (examples). Start top-to-bottom; offboard needs agent + PX4. + # 4. PX4 x500 drone - window_name: drone layout: even-vertical panes: @@ -85,9 +76,7 @@ windows: - cmd: ros2 launch multivehicle_examples px4_offboard.launch.py enter: false - # 5. Dashboard — fake vehicle (RoboCommand :9000), web backend (:8080), and the - # sim-side LED/incident bridges (bridges_pkg=multivehicle_sim, models from - # bb_worlds) tracking all three vehicles' /…/odom. + # 5. Dashboard - window_name: dashboard layout: even-vertical panes: @@ -101,7 +90,7 @@ windows: - cmd: ros2 launch bb_robotx_dashboard robotx_2026_sim.launch.py bridges_pkg:=multivehicle_sim models_pkg:=bb_worlds auv_name:=bluerov auv_pose_topic:=/bluerov/odom asv_name:=blueboat asv_pose_topic:=/blueboat/odom uav_name:=x500 uav_pose_topic:=/x500/odom enter: false - # 6. Debug — free shell + Foxglove bridge (ws://localhost:8765). + # 6. Debug - window_name: debug layout: even-vertical panes: From b68edb4bd95b5aef908e3bcacef5be1fea38de1f Mon Sep 17 00:00:00 2001 From: SamuelFoo Date: Wed, 1 Jul 2026 02:42:46 +0200 Subject: [PATCH 7/8] Update READMEs --- README.md | 29 +++++++---------------------- bluerov/README.md | 29 ++++++++++++----------------- multivehicle/README.md | 2 +- 3 files changed, 20 insertions(+), 40 deletions(-) diff --git a/README.md b/README.md index 4b4f8ff..bda7670 100644 --- a/README.md +++ b/README.md @@ -1,27 +1,12 @@ -# examples +# Examples -Example applications for Bumblebee simulators. +- [BlueROV with ArduSub](bluerov/README.md) +- [Multi-vehicle simulation](multivehicle/README.md) -## Sub-examples +## BlueROV -- [BlueROV examples](bluerov/README.md) -- [Multivehicle examples](multivehicle/README.md) +https://github.com/user-attachments/assets/6c262df8-bac6-492a-aef1-9e8cfc30d8a8 -## Demo videos +https://github.com/user-attachments/assets/9a9c25c5-637a-403a-b34d-4048f9afb5e0 -### BlueROV missions - -- Bin mission: https://github.com/user-attachments/assets/6c262df8-bac6-492a-aef1-9e8cfc30d8a8 -- Torpedo mission: https://github.com/user-attachments/assets/9a9c25c5-637a-403a-b34d-4048f9afb5e0 - -## Building - -Build only the project you imported dependencies for: - -```bash -# multivehicle -colcon build --packages-up-to multivehicle_examples - -# bluerov -colcon build --packages-up-to bluerov_tasks -``` +## Multi-vehicle diff --git a/bluerov/README.md b/bluerov/README.md index 00dfc03..d832626 100644 --- a/bluerov/README.md +++ b/bluerov/README.md @@ -1,8 +1,6 @@ -# Examples +# BlueROV with ArduSub -## BlueROV with ArduSub - -### Quickstart +## Quickstart We assume the ROS workspace is `~/workspaces/bluerov_ws`. Change the paths accordingly if needed. @@ -30,9 +28,9 @@ cd ~/workspaces/bluerov_ws/src/examples ./build.bash ``` -### Start the container +## Start the container -#### Native Ubuntu with NVIDIA +### Native Ubuntu with NVIDIA Install [Rocker](https://github.com/osrf/rocker), then run: @@ -41,7 +39,7 @@ cd ~/workspaces/bluerov_ws/src/examples ./run.bash bluerov_ws:humble ``` -#### WSL 2 with WSLg +### WSL 2 with WSLg ```bash docker run --rm -it \ @@ -66,7 +64,7 @@ docker run --rm -it \ The WSL command exposes WSLg's X11/Wayland sockets and the `/dev/dxg` virtual GPU device, allowing Gazebo to use hardware-accelerated rendering. -### Verify GPU rendering +## Verify GPU rendering Inside the container, before launching Gazebo: @@ -78,7 +76,7 @@ On native Linux, the renderer should identify the NVIDIA GPU. Under WSLg, it should mention D3D12 and the GPU. It should not report `llvmpipe`, which is software rendering. -### Build the workspace +## Build the workspace Inside the container: @@ -90,7 +88,7 @@ colcon build --symlink-install \ source install/setup.bash ``` -### Demo: Square Mission +## Demo: Square Mission The square mission is a control smoke test: @@ -107,7 +105,7 @@ cd /root/HOST/bluerov_ws tmuxp load src/examples/bluerov_mission.yaml ``` -### Demo: Bin Mission +## Demo: Bin Mission https://github.com/user-attachments/assets/6c262df8-bac6-492a-aef1-9e8cfc30d8a8 @@ -115,19 +113,16 @@ https://github.com/user-attachments/assets/6c262df8-bac6-492a-aef1-9e8cfc30d8a8 tmuxp load src/examples/bluerov_bin_mission.yaml ``` -### Demo: Torpedo Mission +## Demo: Torpedo Mission https://github.com/user-attachments/assets/9a9c25c5-637a-403a-b34d-4048f9afb5e0 ```bash tmuxp load src/examples/bluerov_torpedo_mission.yaml ``` -### Foxglove layouts +## Foxglove Layouts -Prebuilt Foxglove layouts for the Bin and Torpedo missions are available at -[BumblebeeAS/controlkitv3](https://github.com/BumblebeeAS/controlkitv3/tree/main/foxglove_layouts). -Import them into Foxglove Studio for a ready-made view of the relevant topics, -making it easier to visualize and debug each mission. +Import our [Foxglove layouts](https://github.com/BumblebeeAS/controlkitv3/tree/main/foxglove_layouts) for a ready-made view of the relevant topics and services, making it easier to visualize and debug each mission. ## Useful Commands diff --git a/multivehicle/README.md b/multivehicle/README.md index 4eb7a26..cb69a89 100644 --- a/multivehicle/README.md +++ b/multivehicle/README.md @@ -1,4 +1,4 @@ -# multivehicle examples +# Multi-vehicle examples Missions and control demos built on [`multivehicle_sim`](https://github.com/BumblebeeAS/multivehicle_sim). From 0c3113fbb745c22943aec7d67beaf8b264ce3a23 Mon Sep 17 00:00:00 2001 From: SamuelFoo Date: Wed, 1 Jul 2026 02:46:40 +0200 Subject: [PATCH 8/8] Move Foxglove layouts section to main README --- README.md | 4 ++++ bluerov/README.md | 3 --- 2 files changed, 4 insertions(+), 3 deletions(-) diff --git a/README.md b/README.md index bda7670..a1c327d 100644 --- a/README.md +++ b/README.md @@ -10,3 +10,7 @@ https://github.com/user-attachments/assets/6c262df8-bac6-492a-aef1-9e8cfc30d8a8 https://github.com/user-attachments/assets/9a9c25c5-637a-403a-b34d-4048f9afb5e0 ## Multi-vehicle + +## Foxglove Layouts + +Import our [Foxglove layouts](https://github.com/BumblebeeAS/controlkitv3/tree/main/foxglove_layouts) for a ready-made view of the relevant topics and services, making it easier to visualize and debug each mission. diff --git a/bluerov/README.md b/bluerov/README.md index d832626..c56b7fc 100644 --- a/bluerov/README.md +++ b/bluerov/README.md @@ -120,9 +120,6 @@ https://github.com/user-attachments/assets/9a9c25c5-637a-403a-b34d-4048f9afb5e0 ```bash tmuxp load src/examples/bluerov_torpedo_mission.yaml ``` -## Foxglove Layouts - -Import our [Foxglove layouts](https://github.com/BumblebeeAS/controlkitv3/tree/main/foxglove_layouts) for a ready-made view of the relevant topics and services, making it easier to visualize and debug each mission. ## Useful Commands