diff --git a/README.md b/README.md index 00dfc03..a1c327d 100644 --- a/README.md +++ b/README.md @@ -1,145 +1,16 @@ # Examples -## BlueROV with ArduSub +- [BlueROV with ArduSub](bluerov/README.md) +- [Multi-vehicle simulation](multivehicle/README.md) -### 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 +## BlueROV 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'}" -``` +## Multi-vehicle -## Documentation +## Foxglove Layouts -- [Architecture and conventions](docs/architecture.md) +Import our [Foxglove layouts](https://github.com/BumblebeeAS/controlkitv3/tree/main/foxglove_layouts) for a ready-made view of the relevant topics and services, making it easier to visualize and debug each mission. diff --git a/bluerov/README.md b/bluerov/README.md new file mode 100644 index 0000000..c56b7fc --- /dev/null +++ b/bluerov/README.md @@ -0,0 +1,137 @@ +# 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 +``` + +## 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_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_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..cb69a89 --- /dev/null +++ b/multivehicle/README.md @@ -0,0 +1,36 @@ +# Multi-vehicle examples + +Missions and control demos built on +[`multivehicle_sim`](https://github.com/BumblebeeAS/multivehicle_sim). + +## Launch files + +- BlueROV square mission: run `bluerov_tasks`'s tree launch directly + `ros2 launch bluerov_tasks bluerov_square_bt.launch.py` +- `boat_control.launch.py`: BlueBoat thrust mixer + LOS controller (+ optional mission node) +- `px4_offboard.launch.py`: the `uav2_offboard` `offboard_node` action backend + the + `mission_planner_2` UAV2 offboard demo behaviour tree (`uav2_offboard_demo_main.py`) + driving it (takeoff → standoff → return → land). + +## Setup + +```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 +``` 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..dce6149 --- /dev/null +++ b/multivehicle/docker/Dockerfile @@ -0,0 +1,20 @@ +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-py-trees \ + ros-humble-py-trees-ros \ + ros-humble-tf-transformations && \ + apt-get autoremove -y && \ + apt-get clean && \ + rm -rf /var/lib/apt/lists/* + +RUN pip3 install --no-cache-dir --ignore-installed "transforms3d>=0.4" "numpy>=1.25,<2" +RUN pip3 install --no-cache-dir mavsdk + +WORKDIR ${HOME_DIR} diff --git a/multivehicle/examples.repos b/multivehicle/examples.repos new file mode 100644 index 0000000..043495b --- /dev/null +++ b/multivehicle/examples.repos @@ -0,0 +1,41 @@ +# 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 + mission_planner_release: + type: git + url: https://github.com/BumblebeeAS/mission_planner_release.git + version: main + frames: + type: git + url: https://github.com/BumblebeeAS/frames.git + version: humble diff --git a/multivehicle/packages/multivehicle_examples/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/config/uav2_offboard_x500.yaml b/multivehicle/packages/multivehicle_examples/config/uav2_offboard_x500.yaml new file mode 100644 index 0000000..482c6a0 --- /dev/null +++ b/multivehicle/packages/multivehicle_examples/config/uav2_offboard_x500.yaml @@ -0,0 +1,21 @@ +/**/offboard_node: + ros__parameters: + mav_sys_id: 2 + offboard_control_mode_topic: /x500/fmu/in/offboard_control_mode + trajectory_setpoint_topic: /x500/fmu/in/trajectory_setpoint + vehicle_command_topic: /x500/fmu/in/vehicle_command + vehicle_local_position_topic: /x500/fmu/out/vehicle_local_position + vehicle_status_topic: /x500/fmu/out/vehicle_status_v1 + acceleration_control_topic: /uav2/acceleration_control + +/**/landing_target_pose_node: + ros__parameters: + landing_target_pose_topic: /x500/fmu/in/landing_target_pose + vehicle_status_topic: /x500/fmu/out/vehicle_status_v1 + +/**/actuator_control_node: + ros__parameters: + actuation_server_name: /uav2/tins/actuation + px4_address: udpin://0.0.0.0:14540 + timeout: 5.0 + actuation_indexes: [1] diff --git a/multivehicle/packages/multivehicle_examples/launch/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..0766c26 --- /dev/null +++ b/multivehicle/packages/multivehicle_examples/launch/px4_offboard.launch.py @@ -0,0 +1,38 @@ +import os + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node + + +def generate_launch_description(): + params_file = LaunchConfiguration("params_file") + + offboard_launch = os.path.join( + get_package_share_directory("uav2_offboard"), "launch", "launch.py" + ) + default_params = os.path.join( + get_package_share_directory("multivehicle_examples"), + "config", + "uav2_offboard_x500.yaml", + ) + + return LaunchDescription( + [ + DeclareLaunchArgument("params_file", default_value=default_params), + IncludeLaunchDescription( + PythonLaunchDescriptionSource(offboard_launch), + launch_arguments={"params_file": params_file}.items(), + ), + Node( + package="mission_planner_2", + executable="uav2_offboard_demo_main.py", + name="uav2_offboard_demo", + output="screen", + parameters=[{"use_sim_time": True}], + ), + ] + ) 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..48dee0e --- /dev/null +++ b/multivehicle/packages/multivehicle_examples/package.xml @@ -0,0 +1,31 @@ + + + multivehicle_examples + 0.1.0 + Multivehicle demo launches and control scripts. + monkescripts + MIT + + ament_cmake + ament_cmake_python + + ament_index_python + bluerov_tasks + frames + geometry_msgs + launch + launch_ros + mission_planner_2 + 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..544acb8 --- /dev/null +++ b/multivehicle/tmuxp/mvsim_debug.yaml @@ -0,0 +1,102 @@ +# tmuxp DEBUG session for the multivehicle examples +# 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 mission BT +# 3) boat — BlueBoat spawn + control stack + square mission +# 4) drone — XRCE agent + PX4 SITL x500 + gz bridge + offboard demo BT +# 5) dashboard — web backend (:8080) + sim-side LED/incident bridges +# 6) debug — free shell + Foxglove bridge + +session_name: mvsim + +global_options: + mouse: 'on' + history-limit: 50000 + +shell_command_before: + - 'WS_DIR="${WS_DIR:-/root/HOST/mvsim_ws}"' + # NVIDIA PRIME render offload — auto-enabled when an NVIDIA GPU is passed through + # (rocker --nvidia mounts /dev/nvidia0). Harmless / skipped on non-NVIDIA hosts. + - '[ -e /dev/nvidia0 ] && export __NV_PRIME_RENDER_OFFLOAD=1 __GLX_VENDOR_LIBRARY_NAME=nvidia || true' + - source /opt/ros/humble/setup.bash + - '[ -f "$WS_DIR/install/setup.bash" ] && source "$WS_DIR/install/setup.bash"' + - cd "$WS_DIR" + +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 + - window_name: bluerov + layout: even-vertical + panes: + - shell_command: + - cmd: ros2 launch multivehicle_sim bluerov.launch.py + enter: false + - shell_command: + - cmd: ros2 launch bluerov_tasks bluerov_square_bt.launch.py + enter: false + + # 3. BlueBoat + - 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 + - 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 + - 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 + - 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 diff --git a/packages/bluerov_tasks/launch/bluerov_mission.launch.py b/packages/bluerov_tasks/launch/bluerov_mission.launch.py deleted file mode 100644 index 63e772f..0000000 --- a/packages/bluerov_tasks/launch/bluerov_mission.launch.py +++ /dev/null @@ -1,23 +0,0 @@ -from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument -from launch.substitutions import LaunchConfiguration -from launch_ros.actions import Node - - -def generate_launch_description(): - use_sim_time = LaunchConfiguration("use_sim_time") - - movement_node = Node( - package="bluerov_tasks", - executable="bluerov_movement.py", - name="bluerov_movement", - output="screen", - parameters=[{"use_sim_time": use_sim_time}], - ) - - return LaunchDescription( - [ - DeclareLaunchArgument("use_sim_time", default_value="true"), - movement_node, - ] - ) diff --git a/packages/bluerov_tasks/scripts/bluerov_movement.py b/packages/bluerov_tasks/scripts/bluerov_movement.py deleted file mode 100755 index 825fb6f..0000000 --- a/packages/bluerov_tasks/scripts/bluerov_movement.py +++ /dev/null @@ -1,209 +0,0 @@ -#!/usr/bin/env python3 - -import rclpy -from rclpy.node import Node -from geometry_msgs.msg import PoseStamped -from mavros_msgs.srv import SetMode, CommandBool -from rclpy.qos import QoSProfile, ReliabilityPolicy -import math - - -class BlueROVSquareMission(Node): - def __init__(self): - super().__init__("bluerov_square_mission") - - mavros_qos = QoSProfile(depth=10, reliability=ReliabilityPolicy.BEST_EFFORT) - - self.target_pub = self.create_publisher( - PoseStamped, "/mavros/setpoint_position/local", mavros_qos - ) - self.curr_pose_sub = self.create_subscription( - PoseStamped, "/mavros/local_position/pose", self.pose_callback, mavros_qos - ) - - # Service Clients - self.arming_client = self.create_client(CommandBool, "/mavros/cmd/arming") - self.mode_client = self.create_client(SetMode, "/mavros/set_mode") - - # Service state flags - self.is_armed = False - self.is_guided_mode = False - self.services_initialized = False - - # Mission Params - self.depth = -2.0 - self.dist_threshold = 0.2 - self.yaw_threshold = 0.1 - - # State & Waypoints in FLU - self.current_pose = None - self.state = "INIT" - self.waypoints = [ - (5.0, 0.0), # Forward - (5.0, 5.0), # Left - (0.0, 5.0), # Backward - (0.0, 0.0), # Back to start - ] - self.wp_index = 0 - - # IMPORTANT NOTE: DO NOT SET A HIGH SETPOINT PUBLISH RATE. - # ROV WOULD KEEP REPLANNING ITS TRAJECTORY, CAUSING IT TO MOVE VERY SLOWLY - self.timer = self.create_timer(1.0, self.main_loop) - - def arm(self): - """Request arming""" - if not self.arming_client.service_is_ready(): - self.get_logger().info("Waiting for arming service...") - return False - - self.get_logger().info("Requesting arm...") - request = CommandBool.Request() - request.value = True - future = self.arming_client.call_async(request) - future.add_done_callback(self.arm_callback) - return True - - def arm_callback(self, future): - """Callback for arming service""" - try: - response = future.result() - if response.success: - self.is_armed = True - self.get_logger().info("Vehicle armed successfully") - else: - self.get_logger().warn("Failed to arm vehicle") - except Exception as e: - self.get_logger().error(f"Arming service call failed: {e}") - - def set_guided_mode(self): - """Request GUIDED mode""" - if not self.mode_client.service_is_ready(): - self.get_logger().info("Waiting for SetMode service...") - return False - - self.get_logger().info("Requesting GUIDED mode...") - req = SetMode.Request() - req.custom_mode = "GUIDED" - future = self.mode_client.call_async(req) - future.add_done_callback(self.guided_mode_callback) - return True - - def guided_mode_callback(self, future): - """Callback for setting to guided mode""" - try: - response = future.result() - if response.mode_sent: - self.is_guided_mode = True - self.get_logger().info("GUIDED mode set successfully") - else: - self.get_logger().warn("Failed to set GUIDED mode") - except Exception as e: - self.get_logger().error(f"Set mode service call failed: {e}") - - def pose_callback(self, msg): - self.current_pose = msg - - def get_quaternion_from_yaw(self, yaw): - """Convert yaw (radians) to a quaternion for PoseStamped.""" - return {"x": 0.0, "y": 0.0, "z": math.sin(yaw / 2.0), "w": math.cos(yaw / 2.0)} - - def get_yaw_from_quaternion(self, quaternion): - """Extract yaw (radians) from a quaternion.""" - siny_cosp = 2.0 * (quaternion.w * quaternion.z + quaternion.x * quaternion.y) - cosy_cosp = 1.0 - 2.0 * ( - quaternion.y * quaternion.y + quaternion.z * quaternion.z - ) - return math.atan2(siny_cosp, cosy_cosp) - - def normalize_angle(self, angle): - """Normalize angle to [-pi, pi]""" - while angle > math.pi: - angle -= 2.0 * math.pi - while angle < -math.pi: - angle += 2.0 * math.pi - return angle - - def main_loop(self): - """Main state machine""" - - if self.state == "INIT": - # Try to initialize services - if not self.services_initialized: - if not self.is_armed: - self.arm() - if not self.is_guided_mode: - self.set_guided_mode() - - if self.is_armed and self.is_guided_mode: - self.services_initialized = True - self.state = "START" - self.get_logger().info("Initialization complete, starting mission") - - elif self.state == "START": - if self.current_pose is None: - self.get_logger().info("Waiting for pose data...") - return - self.state = "MOVING" - self.get_logger().info("Beginning square mission") - - elif self.state == "MOVING": - tx, ty = self.waypoints[self.wp_index] - - # Calculate Yaw to face the target - dx = tx - self.current_pose.pose.position.x - dy = ty - self.current_pose.pose.position.y - yaw = math.atan2(dy, dx) - q = self.get_quaternion_from_yaw(yaw) - - # Publish Pose - msg = PoseStamped() - msg.header.stamp = self.get_clock().now().to_msg() - msg.header.frame_id = "map" - msg.pose.position.x = tx - msg.pose.position.y = ty - msg.pose.position.z = self.depth - msg.pose.orientation.x = q["x"] - msg.pose.orientation.y = q["y"] - msg.pose.orientation.z = q["z"] - msg.pose.orientation.w = q["w"] - self.target_pub.publish(msg) - self.get_logger().info(f"Publishing waypoint: {msg.pose.position}") - - # Check distance - dist = math.sqrt(dx**2 + dy**2) - target_yaw = self.get_yaw_from_quaternion(msg.pose.orientation) - current_yaw = self.get_yaw_from_quaternion( - self.current_pose.pose.orientation - ) - yaw_error = abs(self.normalize_angle(target_yaw - current_yaw)) - reach_threshold = ( - dist < self.dist_threshold and yaw_error < self.yaw_threshold - ) - self.get_logger().info( - f"dist to target: {dist}, current yaw: {current_yaw}, target_yaw: {target_yaw} reach_threshold: {reach_threshold}" - ) - - if dist < self.dist_threshold: - self.get_logger().info(f"Waypoint {self.wp_index} reached!") - self.wp_index += 1 - if self.wp_index >= len(self.waypoints): - self.state = "FINISHED" - - elif self.state == "FINISHED": - self.get_logger().info("Mission successful.") - raise SystemExit - - -def main(args=None): - rclpy.init(args=args) - node = BlueROVSquareMission() - try: - rclpy.spin(node) - except SystemExit: - pass - node.destroy_node() - rclpy.shutdown() - - -if __name__ == "__main__": - main()