diff --git a/README.md b/README.md index 00dfc03..4b4f8ff 100644 --- a/README.md +++ b/README.md @@ -1,145 +1,27 @@ -# Examples +# examples -## BlueROV with ArduSub +Example applications for Bumblebee simulators. -### Quickstart +## Sub-examples -We assume the ROS workspace is `~/workspaces/bluerov_ws`. Change the paths accordingly if needed. +- [BlueROV examples](bluerov/README.md) +- [Multivehicle examples](multivehicle/README.md) -Clone the repositories: +## Demo videos -```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" -``` +### BlueROV missions -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. +- 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 -### Build the workspace +## Building -Inside the container: +Build only the project you imported dependencies for: ```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: +# multivehicle +colcon build --packages-up-to multivehicle_examples -```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 +# bluerov +colcon build --packages-up-to bluerov_tasks ``` - -### 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/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..7017590 --- /dev/null +++ b/multivehicle/README.md @@ -0,0 +1,40 @@ +# multivehicle examples + +Missions and control demos built on +[`multivehicle_sim`](https://github.com/BumblebeeAS/multivehicle_sim). + +## Launch files + +- `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 +cd ~/mvsim_ws +vcs import src < src/examples/multivehicle/examples.repos --recursive + +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 +``` + +## 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/build.bash b/multivehicle/build.bash new file mode 100755 index 0000000..049eb12 --- /dev/null +++ b/multivehicle/build.bash @@ -0,0 +1,22 @@ +#!/usr/bin/env bash + +set -e + +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 src/multivehicle_sim." + 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..9f36380 --- /dev/null +++ b/multivehicle/docker/Dockerfile @@ -0,0 +1,14 @@ +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 && \ + 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..832e1a2 --- /dev/null +++ b/multivehicle/examples.repos @@ -0,0 +1,33 @@ +# examples.repos + +repositories: + multivehicle_sim: + type: git + url: https://github.com/BumblebeeAS/multivehicle_sim.git + + 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 + uav2_offboard: + type: git + url: https://github.com/BumblebeeAS/uav2_offboard.git diff --git a/multivehicle/packages/multivehicle_examples/CMakeLists.txt b/multivehicle/packages/multivehicle_examples/CMakeLists.txt new file mode 100644 index 0000000..d916da8 --- /dev/null +++ b/multivehicle/packages/multivehicle_examples/CMakeLists.txt @@ -0,0 +1,24 @@ +cmake_minimum_required(VERSION 3.12.2) +project(multivehicle_examples) + +find_package(ament_cmake REQUIRED) +find_package(ament_cmake_python REQUIRED) + +ament_python_install_package(${PROJECT_NAME}) + +install(PROGRAMS + scripts/blueboat_mission.py + scripts/blueboat_thrust_mixer.py + scripts/blueboat_waypoint_controller.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..8bfaabd --- /dev/null +++ b/multivehicle/packages/multivehicle_examples/launch/bluerov_mission.launch.py @@ -0,0 +1,25 @@ +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/boat_control.launch.py b/multivehicle/packages/multivehicle_examples/launch/boat_control.launch.py new file mode 100644 index 0000000..96795cb --- /dev/null +++ b/multivehicle/packages/multivehicle_examples/launch/boat_control.launch.py @@ -0,0 +1,78 @@ +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"), + DeclareLaunchArgument("use_mission", default_value="false"), + DeclareLaunchArgument("spawn", default_value="false"), + ] + + 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}], + ) + + 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..8492a30 --- /dev/null +++ b/multivehicle/packages/multivehicle_examples/launch/px4_offboard.launch.py @@ -0,0 +1,27 @@ +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=package, + executable=executable, + name="px4_offboard_demo", + output="screen", + parameters=[ + {"use_sim_time": True}, + {"vehicle_namespace": "x500"}, + {"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 new file mode 100644 index 0000000..f808144 --- /dev/null +++ b/multivehicle/packages/multivehicle_examples/package.xml @@ -0,0 +1,29 @@ + + + multivehicle_examples + 0.1.0 + Multivehicle demo launches and control scripts. + monkescripts + MIT + + ament_cmake + ament_cmake_python + + ament_index_python + bluerov_tasks + geometry_msgs + launch + launch_ros + multivehicle_interface + nav_msgs + rclpy + std_msgs + uav2_offboard + + 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..58e12fc --- /dev/null +++ b/multivehicle/packages/multivehicle_examples/scripts/blueboat_mission.py @@ -0,0 +1,72 @@ +#!/usr/bin/env python3 +"""BlueBoat square mission publisher. + +Provenance: Adapted from https://github.com/BumblebeeAS/bring-up/tree/main/etc/uav2_sim +""" + +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) + 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 + self.create_timer(0.2, self.try_publish) + + 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 + waypoints = [ + (x0 + s, y0), + (x0 + s, y0 + s), + (x0, y0 + s), + (x0, y0), + ] + + for wx, wy in waypoints: + 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 + + +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..2a7696c --- /dev/null +++ b/multivehicle/packages/multivehicle_examples/scripts/blueboat_thrust_mixer.py @@ -0,0 +1,110 @@ +#!/usr/bin/env python3 +"""BlueBoat differential-thrust mixer. + +Provenance: Adapted from https://github.com/BumblebeeAS/bring-up/tree/main/etc/uav2_sim +""" + +import rclpy +from rclpy.node import Node +from geometry_msgs.msg import Twist +from std_msgs.msg import Float64 + +from multivehicle_examples.math_helpers import clamp_symmetric + + +class BlueBoatThrustMixer(Node): + def __init__(self): + super().__init__("blueboat_thrust_mixer") + + 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) + + 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) + + def cmd_vel_sub(self, msg: Twist): + self._last_cmd = msg + self._last_cmd_time = self.get_clock().now() + + def publish_thrust(self): + 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_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 + + f_surge = self.surge_drag * u * abs(u) + self.surge_lin * u + 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) + + 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..64e65ee --- /dev/null +++ b/multivehicle/packages/multivehicle_examples/scripts/blueboat_waypoint_controller.py @@ -0,0 +1,254 @@ +#!/usr/bin/env python3 +"""BlueBoat LOS waypoint controller. + +Provenance: Adapted from https://github.com/BumblebeeAS/bring-up/tree/main/etc/uav2_sim +""" + +import math + +import rclpy +from rclpy.node import Node +from rclpy.qos import QoSProfile, DurabilityPolicy +from nav_msgs.msg import Odometry +from geometry_msgs.msg import PoseStamped, Twist +from std_msgs.msg import Bool + +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) + 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) + self.declare_parameter("i_max", 0.5) + 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 + + self.waypoints = [] + self.wp_idx = 0 + self.seg_start = None + self.first_leg_start = None + + self.odom = None + self.yaw_integral = 0.0 + self.finished = False + self.mode = "INIT" + self.hold_xy = None + self.target_heading = 0.0 + + 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) + + def odom_sub(self, msg: Odometry): + self.odom = msg + + def goal_sub(self, msg: PoseStamped): + self.waypoints.append((msg.pose.position.x, msg.pose.position.y)) + if self.finished: + self.finished = False + self.reached_pub.publish(Bool(data=False)) + + 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 + + if self.first_leg_start is None: + self.first_leg_start = (x, y) + + 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" + + 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" + + if self.wp_idx >= len(self.waypoints): + if not self.finished: + self.finished = True + self.reached_pub.publish(Bool(data=True)) + self.mode = "HOLD" + self.station_keep(x, y, yaw, r, allow_repoint=True) + return + + if self.mode == "ALIGN": + u_now = self.odom.twist.twist.linear.x + e_x = self.along_body_err(x, y, yaw) + u_cmd = clamp_symmetric(self.kp_pos * e_x, self.u_align_max) + if abs(u_now) > self.v_settle: + 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 + return + + 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: + psi_d = math.atan2(ty - y, tx - x) + along = 0.0 + else: + gamma = math.atan2(seg_dy, seg_dx) + e = -(x - sx) * math.sin(gamma) + (y - sy) * math.cos(gamma) + 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) + + passed = seg_len > 1e-3 and along >= seg_len + if dist_to_target < self.arrival_radius or passed: + 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 + self.wp_idx = nxt + self.yaw_integral = 0.0 + if self.turn_in_place and nxt < len(self.waypoints): + self.mode = "ALIGN" + return + + w_cmd, psi_err = self.heading_pid(psi_d, yaw, r) + turn_factor = max(0.0, math.cos(psi_err)) ** self.shaping_p + 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) + + 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): + psi_err = wrap_to_pi(psi_d - yaw) + if self.ki_yaw > 0.0: + self.yaw_integral += psi_err * self.dt + self.yaw_integral = clamp_symmetric(self.yaw_integral, self.i_max / self.ki_yaw) + else: + self.yaw_integral = 0.0 + 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): + 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): + return math.atan2(b[1] - a[1], b[0] - a[0]) + + def station_keep(self, x, y, yaw, r, allow_repoint): + 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_symmetric(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/run.bash b/multivehicle/run.bash new file mode 100755 index 0000000..d12707d --- /dev/null +++ b/multivehicle/run.bash @@ -0,0 +1,17 @@ +#!/usr/bin/env bash + +set -e + +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