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..5adeb1e --- /dev/null +++ b/multivehicle/README.md @@ -0,0 +1,59 @@ +# 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 +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 + +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 100755 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 100755 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