From 42beef1df8b17da13a8c2caf1a7c52433fb778ba Mon Sep 17 00:00:00 2001 From: Cruiz102 Date: Tue, 25 Aug 2026 23:09:22 -0700 Subject: [PATCH 1/7] Slim sim build: colcon Stonefish, drop ORB-SLAM/Kalibr/VectorNav deps. Use ros-base-style deps, build Stonefish via colcon into install/, remove orb_slam3_ros2 and kalibr, make joy/usb_cam/vectornav optional, fix mission_executor Rust/CMake paths for fast incremental builds, and add a colcon benchmark script. Co-authored-by: Cursor --- .dockerignore | 8 + .gitignore | 8 + .gitmodules | 4 - Dockerfile | 70 +-- README.md | 247 +++------ imu-workbagsimucam_ros1.yaml | 14 - scripts/bench_build.sh | 86 ++++ src/bringup/launch/hardware_proteus.launch.py | 95 ---- src/bringup/launch/stonefish.launch.py | 14 +- src/bringup/package.xml | 2 +- src/mission_executor/CMakeLists.txt | 22 +- src/orb_slam3_ros2/CMakeLists.txt | 161 ------ src/orb_slam3_ros2/config/imucam_bag.yaml | 93 ---- .../config/orb_slam_full_template.yaml | 273 ---------- .../config/stonefish_hydrus.yaml | 273 ---------- src/orb_slam3_ros2/config/webcamera.yaml | 93 ---- .../launch/orb_slam_sim_launch.py | 275 ---------- src/orb_slam3_ros2/package.xml | 31 -- src/orb_slam3_ros2/src/orb_slam_node.cpp | 485 ------------------ src/zed_custom_wrapper/COLCON_IGNORE | 0 tools/kalibr/README.md | 182 ------- tools/kalibr/config/aprilgrid_6x6_80x30.yaml | 6 - tools/kalibr/config/camchain_seed.yaml | 7 - tools/kalibr/config/imu.yaml | 8 - tools/kalibr/extract_orbslam_tbc.py | 42 -- tools/kalibr/run_kalibr.sh | 30 -- vendor/stonefish | 2 +- vendor/stonefish_ros2 | 2 +- 28 files changed, 228 insertions(+), 2305 deletions(-) delete mode 100644 imu-workbagsimucam_ros1.yaml create mode 100755 scripts/bench_build.sh delete mode 100644 src/orb_slam3_ros2/CMakeLists.txt delete mode 100644 src/orb_slam3_ros2/config/imucam_bag.yaml delete mode 100644 src/orb_slam3_ros2/config/orb_slam_full_template.yaml delete mode 100644 src/orb_slam3_ros2/config/stonefish_hydrus.yaml delete mode 100644 src/orb_slam3_ros2/config/webcamera.yaml delete mode 100644 src/orb_slam3_ros2/launch/orb_slam_sim_launch.py delete mode 100644 src/orb_slam3_ros2/package.xml delete mode 100644 src/orb_slam3_ros2/src/orb_slam_node.cpp create mode 100644 src/zed_custom_wrapper/COLCON_IGNORE delete mode 100644 tools/kalibr/README.md delete mode 100644 tools/kalibr/config/aprilgrid_6x6_80x30.yaml delete mode 100644 tools/kalibr/config/camchain_seed.yaml delete mode 100644 tools/kalibr/config/imu.yaml delete mode 100644 tools/kalibr/extract_orbslam_tbc.py delete mode 100644 tools/kalibr/run_kalibr.sh diff --git a/.dockerignore b/.dockerignore index b5d44fd..5ea0f1b 100644 --- a/.dockerignore +++ b/.dockerignore @@ -41,6 +41,14 @@ src/neural_depth/ src/neural_depth_ros2/ src/rviz_plugin_zed_od/ src/zed*/ +src/orb_slam3_ros2/ +tools/kalibr/ +vendor/ORB_SLAM3/ +vendor/zed-ros-interfaces/ +vendor/zed-ros2-examples/ +vendor/depth_anything_v2/ +vendor/joystick_drivers/ +external/ # Compiled files compile_commands.json diff --git a/.gitignore b/.gitignore index d96ecea..51d012b 100644 --- a/.gitignore +++ b/.gitignore @@ -7,6 +7,9 @@ log debug target +# Local benchmark output +build_benchmark.log + # These are backup files generated by rustfmt **/*.rs.bk @@ -26,4 +29,9 @@ src/bringup/launch/depth_with_simulator.launch.py __pycache__/ *.pyc +# Removed / local-only vision tooling +vendor/ORB_SLAM3/ +vendor/stonefish/build/ +vendor/stonefish/3rdparty/SDL2/ + diff --git a/.gitmodules b/.gitmodules index 2fa1635..64742db 100644 --- a/.gitmodules +++ b/.gitmodules @@ -10,10 +10,6 @@ [submodule "vendor/depth_anything_v2"] path = vendor/depth_anything_v2 url = https://github.com/DepthAnything/Depth-Anything-V2.git -[submodule "vendor/vectornav"] - path = vendor/vectornav - url = https://github.com/dawonn/vectornav.git - branch = ros2 [submodule "vendor/joystick_drivers"] path = vendor/joystick_drivers url = https://github.com/ros-drivers/joystick_drivers.git diff --git a/Dockerfile b/Dockerfile index 18154c2..f6fe614 100644 --- a/Dockerfile +++ b/Dockerfile @@ -1,106 +1,78 @@ +# syntax=docker/dockerfile:1 FROM ros:jazzy-ros-base -# Install dependencies matching README Ubuntu instructions RUN apt-get update && apt-get install -y \ python3 \ python3-pip \ - python3-venv \ build-essential \ curl \ cmake \ git \ - # Clang/LLVM (required for Rust ROS 2 bindings) + pkg-config \ libclang-dev \ llvm-dev \ clang \ - # ROS 2 build tools python3-colcon-common-extensions \ python3-rosdep \ - # ROS 2 packages needed by stonefish_ros2 ros-jazzy-vision-msgs \ ros-jazzy-image-transport \ ros-jazzy-pcl-conversions \ - ros-jazzy-pcl-ros \ - # Stonefish dependencies (from README) + ros-jazzy-visualization-msgs \ libfreetype6-dev \ libsdl2-dev \ libglm-dev \ libeigen3-dev \ - libogre-1.9-dev \ - libopencv-dev \ - libssl-dev \ - libboost-all-dev \ - libepoxy-dev \ + libtinyxml2-dev \ + libgl1-mesa-dev \ && rm -rf /var/lib/apt/lists/* -# Install Rust RUN curl --proto '=https' --tlsv1.2 -sSf https://sh.rustup.rs | sh -s -- -y ENV PATH="/root/.cargo/bin:${PATH}" ENV CARGO_TARGET_DIR=/ros2_ws/target -# Create workspace WORKDIR /ros2_ws -COPY vendor/stonefish ./vendor/stonefish - -# Build Stonefish library -RUN cd vendor/stonefish && \ - mkdir -p build && cd build && \ - cmake .. && \ - make -j$(nproc) && \ - make install && \ - ldconfig -# Copy only package.xml files for dependency caching +# Stonefish + package manifests (layer cache) +COPY vendor/stonefish ./vendor/stonefish COPY src/interfaces/package.xml ./src/interfaces/package.xml COPY src/bringup/package.xml ./src/bringup/package.xml COPY src/bridge_stonefish/package.xml ./src/bridge_stonefish/package.xml COPY src/mission_executor/package.xml ./src/mission_executor/package.xml +COPY src/detection_mocker/package.xml ./src/detection_mocker/package.xml COPY vendor/stonefish_ros2/package.xml ./src/stonefish_ros2/package.xml -RUN rosdep init || true && \ - rosdep update +RUN rosdep init || true && rosdep update RUN bash -c "source /opt/ros/jazzy/setup.bash && \ - rosdep install --from-paths src --ignore-src -r -y || true" + rosdep install --from-paths src vendor/stonefish --ignore-src -r -y || true" -# build interfaces first COPY src/interfaces ./src/interfaces RUN bash -lc "source /opt/ros/jazzy/setup.bash && \ - colcon build --packages-select interfaces --cmake-args -DCMAKE_BUILD_TYPE=Release" + colcon build --packages-select Stonefish interfaces \ + --cmake-args -DCMAKE_BUILD_TYPE=Release" -# copy mission_executor files except actual src/mission_executor/src/* COPY Cargo.toml Cargo.lock ./ COPY src/mission_executor/Cargo.toml ./src/mission_executor/Cargo.toml COPY src/mission_executor/CMakeLists.txt ./src/mission_executor/CMakeLists.txt +RUN mkdir -p src/mission_executor/src && printf "fn main() {}" > src/mission_executor/src/main.rs -# add dummy main.rs to src/mission_executor/src/ so cargo allows building mission_executor -RUN mkdir -p src/mission_executor/src && \ - printf "fn main() {}" > src/mission_executor/src/main.rs - -# fetch and build dependencies for mission_executor -RUN bash -lc "source /opt/ros/jazzy/setup.bash && \ - source install/setup.bash && \ - colcon build --packages-select mission_executor --cmake-args -DCMAKE_BUILD_TYPE=Release" +RUN bash -lc "source /opt/ros/jazzy/setup.bash && source install/setup.bash && \ + colcon build --packages-select mission_executor \ + --cmake-args -DCMAKE_BUILD_TYPE=Release" COPY src/bringup ./src/bringup COPY src/bridge_stonefish ./src/bridge_stonefish +COPY src/detection_mocker ./src/detection_mocker COPY vendor/stonefish_ros2 ./src/stonefish_ros2 -# build ROS 2 workspace except interfaces and mission_executor -RUN bash -c "source /opt/ros/jazzy/setup.bash && \ - colcon build \ - --packages-select stonefish_ros2 bridge_stonefish bringup \ +RUN bash -c "source /opt/ros/jazzy/setup.bash && source install/setup.bash && \ + colcon build --packages-select stonefish_ros2 bridge_stonefish bringup detection_mocker \ --cmake-args -DCMAKE_BUILD_TYPE=Release" -# build mission_executor with actual source code COPY src/mission_executor/src ./src/mission_executor/src RUN ln -sf /ros2_ws/src/mission_executor/target /ros2_ws/target && \ - bash -c "source /opt/ros/jazzy/setup.bash && \ - source install/setup.bash && \ - colcon build \ - --packages-select mission_executor \ + bash -c "source /opt/ros/jazzy/setup.bash && source install/setup.bash && \ + colcon build --packages-select mission_executor \ --cmake-args -DCMAKE_BUILD_TYPE=Release" -# Set environment variables ENV ROS_DOMAIN_ID=0 - diff --git a/README.md b/README.md index d527e85..29016ea 100644 --- a/README.md +++ b/README.md @@ -1,4 +1,3 @@ - ## Simulation ### Clone and go in repo @@ -31,210 +30,108 @@ docker run --rm \ ``` ## Local Development Setup - System Dependencies +System Dependencies ### Required Tools - Python 3 -- C++ compiler (GCC) -- Rust +- C++ compiler (GCC) + CMake +- Rust ([rustup](https://rustup.rs)) +- Clang/LLVM (for Rust ROS 2 bindings / r2r) + +Install a **minimal** ROS 2 Jazzy (`ros-base`), not `desktop`. Optional tools (joy, usb_cam, rviz) are listed under the profiles that need them. ### Fedora: ```sh -# Install essential build tools -sudo dnf install python3 python3-pip gcc gcc-c++ rust cargo +# Build tools + Rust +sudo dnf install python3 python3-pip gcc gcc-c++ cmake pkgconf-pkg-config rust cargo -# Install ROS 2 +# ROS 2 (minimal) + sim msgs sudo dnf copr enable tavie/ros2 -sudo dnf install ros-jazzy-desktop -sudo dnf install ros-jazzy-vision-msgs -sudo dnf install freetype-devel -sudo dnf install SDL2-devel -sudo dnf install glm-devel -sudo dnf install eigen3-devel -sudo dnf install ogre-devel -sudo dnf install opencv-devel -sudo dnf install openssl-devel -sudo dnf install boost-devel -sudo dnf install libepoxy-devel +sudo dnf install ros-jazzy-ros-base ros-jazzy-vision-msgs \ + ros-jazzy-image-transport ros-jazzy-pcl-conversions ros-jazzy-visualization-msgs \ + python3-colcon-common-extensions + +# Stonefish + detection_mocker system libs (include SDL2 for Stonefish) +sudo dnf install freetype-devel glm-devel eigen3-devel tinyxml2-devel \ + mesa-libGL-devel libclang-devel clang SDL2-devel + python3 -m pip install wheel ``` + ### Ubuntu: ```bash -# Install essential build tools sudo apt update -sudo apt install -y python3 python3-pip python3-venv build-essential curl - -# Install Clang/LLVM (required for Rust ROS 2 bindings) -sudo apt install -y libclang-dev llvm-dev clang +sudo apt install -y python3 python3-pip build-essential cmake pkg-config curl \ + libclang-dev llvm-dev clang \ + python3-colcon-common-extensions # Add ROS 2 repository (if not already added) sudo apt install -y software-properties-common sudo add-apt-repository universe sudo apt update sudo apt install -y curl gnupg lsb-release -sudo curl -sSL https://raw.githubusercontent.com/ros/rosdistro/master/ros.key -o /usr/share/keyrings/ros-archive-keyring.gpg +sudo curl -sSL https://raw.githubusercontent.com/ros/rosdistro/master/ros.key \ + -o /usr/share/keyrings/ros-archive-keyring.gpg -# Install ROS 2 and dependencies +# System dependencies sudo apt update -sudo apt install -y ros-jazzy-desktop \ - ros-jazzy-vision-msgs \ - ros-jazzy-sensor-msgs \ - ros-jazzy-geometry-msgs \ - ros-jazzy-rviz2 \ - ros-jazzy-usb-cam \ - libfreetype6-dev \ - libsdl2-dev \ - libglm-dev \ - libeigen3-dev \ - libogre-1.12-dev \ - libopencv-dev \ - libssl-dev \ - libboost-all-dev \ - libepoxy-dev \ - libtinyxml2-dev \ - ros-jazzy-pangolin \ - socat \ - pkg-config -``` - -### Install Stonefish Simulator -```sh -cd ./vendor/stonefish -mkdir build -cd build -cmake .. -make -j16 # (where X is the number of threads) -sudo make install -cd ../../../../../ -``` - -## Computer Vision - -### ZED Custom Wrapper - -### Dependencies - - [ZED-SDK 5.1](https://www.stereolabs.com/developers/release) - - [Cuda 12.8](https://developer.nvidia.com/cuda-12-8-0-download-archive) - -```sh -colcon build --packages-select zed_msg zed_custom_wrapper && source ./install/setup.bash && ros2 launch zed_custom_wrapper zed_custom.launch.py onnx_model_path:=./src/zed_custom_wrapper/yolov8n.onnx +sudo apt install -y ros-jazzy-ros-base \ + ros-jazzy-vision-msgs \ + ros-jazzy-image-transport \ + ros-jazzy-pcl-conversions \ + ros-jazzy-visualization-msgs \ + libfreetype6-dev \ + libglm-dev \ + libeigen3-dev \ + libtinyxml2-dev \ + libgl1-mesa-dev + +# Optional: system SDL2 (Stonefish uses libsdl2-dev on Linux) +sudo apt install -y libsdl2-dev + +# Install Rust +curl --proto '=https' --tlsv1.2 -sSf https://sh.rustup.rs | sh -s -- -y ``` -### Jetson run using prebuilt Orb-Slam3 libraries +### Optional profiles (install only if you need them) -This is the recommended path on Jetson devices when ORB-SLAM3 cannot be compiled locally. - -#### 1. Download the prebuilt bundle ```bash -cd /home/cesar/autonomy-stack -curl -L -o /tmp/orbslam-artifacts.zip \ - https://github.com/Rumarino-Team/autonomy-stack/releases/download/orb_slam_libs/artifacts.zip -``` - -#### 2. Install the bundle into the workspace (Jetson Only) -```bash -rm -rf /tmp/orbslam-artifacts-extract -mkdir -p /tmp/orbslam-artifacts-extract -unzip -o /tmp/orbslam-artifacts.zip -d /tmp/orbslam-artifacts-extract -ART=/tmp/orbslam-artifacts-extract/artifacts/orbslam-arm64/ -sudo install -m 644 $ART/lib/* /usr/lib/ && sudo ldconfig -``` - -#### 3. Run the launch file -```bash -ros2 launch orb_slam3_ros2 orb_slam_sim_launch.py -``` - - -Example using all optional arguments: - -```bash -ros2 launch orb_slam3_ros2 orb_slam_sim_launch.py \ - use_viewer:=true \ - use_imu:=true \ - use_depth:=false \ - image_topic:=/camera/color/image_raw \ - depth_topic:=/camera/depth/image_raw \ - imu_topic:=/vectornav/imu \ - settings_file:=/home/cesar/autonomy-stack/src/orb_slam3_ros2/config/webcamera.yaml \ - use_usb_cam:=true \ - use_vectornav:=true -``` - -#### Notes -* The artifact bundle is built for Jetson-compatible Ubuntu 22.04 / ROS Humble ABI levels. +# Teleop joystick (stonefish.launch.py use_joy:=true) +sudo apt install -y ros-jazzy-joy # Fedora: ros-jazzy-joy -## Kalibr (Camera + IMU calibration) +# Hardware Proteus camera / mock serial +sudo apt install -y ros-jazzy-usb-cam socat -Use the repository Kalibr wrapper in `tools/kalibr` to calibrate camera intrinsics and camera-IMU extrinsics. - -### 1) Prepare tools -```sh -docker pull stereolabs/kalibr:latest -chmod +x tools/kalibr/run_kalibr.sh -chmod +x tools/kalibr/extract_orbslam_tbc.py -``` - -### 2) Record calibration bags -```sh -# Camera intrinsics bag -ros2 bag record /usb_cam/image_raw -o bags/cam_intrinsics -# Camera + IMU bag for extrinsics -ros2 bag record /usb_cam/image_raw /vectornav/imu -o bags/imucam +# GUI visualization (not required for sim/CI) +sudo apt install -y ros-jazzy-rviz2 ``` -### 3) Calibrate camera intrinsics -```sh -bash tools/kalibr/run_kalibr.sh kalibr_calibrate_cameras \ - --bag /work/bags/cam_intrinsics.bag \ - --topics /usb_cam/image_raw \ - --models pinhole-radtan \ - --target /work/tools/kalibr/config/aprilgrid_6x6_80x30.yaml -``` - -Output: `camchain-cam_intrinsics.yaml` - -### 4) Prepare IMU noise config -Edit `tools/kalibr/config/imu.yaml` and fill real Allan-variance noise values. - -### 5) Calibrate camera-IMU extrinsics +### Build ```sh -bash tools/kalibr/run_kalibr.sh kalibr_calibrate_imu_camera \ - --bag /work/bags/imucam.bag \ - --cam /work/camchain-cam_intrinsics.yaml \ - --imu /work/tools/kalibr/config/imu.yaml \ - --target /work/tools/kalibr/config/aprilgrid_6x6_80x30.yaml +source /opt/ros/jazzy/setup.bash +colcon build --cmake-args -DCMAKE_BUILD_TYPE=Release +source install/setup.bash ``` -Output: `camchain-imucam.yaml` +Stonefish (`vendor/stonefish`) is a colcon cmake package and installs into `install/` with everything else. `stonefish_ros2` declares a build dependency on it, so colcon builds them in order. -### 6) Get ORB-SLAM calibration matrix -```sh -python3 tools/kalibr/extract_orbslam_tbc.py camchain-imucam.yaml -``` +Optional stacks (ZED vision) are ignored via `COLCON_IGNORE` under `src/zed_custom_wrapper/`. -Copy the printed matrix into both keys in `src/orb_slam3_ros2/config/webcamera.yaml`: -- `IMU.T_b_c1` -- `Tbc` - -### Notes -- If Kalibr cannot read your ROS 2 bag directly, convert it to ROS1 bag format first. -- Ensure the topic names passed to Kalibr exactly match your recorded bag topics. +## Computer Vision +### ZED Custom Wrapper +`zed_custom_wrapper` is ignored by default (`COLCON_IGNORE`) so a normal sim build stays lean. Remove that file before building vision. +### Dependencies + - [ZED-SDK 5.1](https://www.stereolabs.com/developers/release) + - [Cuda 12.8](https://developer.nvidia.com/cuda-12-8-0-download-archive) -## Build using bridge_stonefish ```sh -# Source ROS 2 environment -source /opt/ros/jazzy/setup.bash - -# Build packages -colcon build \ - --packages-select interfaces bringup mission_executor bridge_stonefish Stonefish stonefish_ros2 detection_mocker joy sdl2_vendor - -# Source the workspace -source install/setup.bash +rm -f src/zed_custom_wrapper/COLCON_IGNORE +# also init vendor/zed-ros-interfaces if you need zed_msgs +colcon build --packages-select zed_msgs zed_custom_wrapper && source ./install/setup.bash && \ + ros2 launch zed_custom_wrapper zed_custom.launch.py onnx_model_path:=./src/zed_custom_wrapper/yolov8n.onnx ``` ## Simulate Missions using bridge_stonefish @@ -258,7 +155,7 @@ ros2 launch bringup stonefish.launch.py \ env_file_name:=hydrus_env.scn \ headless:=false -# proteus, teleop mission +# proteus, teleop mission (needs ros-jazzy-joy) # if you don't have xterm, set TERMINAL to your terminal or install xterm. # sudo apt install xterm # sudo dnf install xterm @@ -266,14 +163,16 @@ ros2 launch bringup stonefish.launch.py \ mission_name:=teleop \ auv_name:=proteus \ env_file_name:=proteus_env.scn \ - headless:=false + headless:=false \ + use_joy:=true # bluerov2, teleop mission ros2 launch bringup stonefish.launch.py \ mission_name:=teleop \ auv_name:=bluerov2 \ env_file_name:=pool_env.scn \ - headless:=false + headless:=false \ + use_joy:=true # optional manual override (advanced) ros2 launch bringup stonefish.launch.py \ @@ -281,7 +180,8 @@ ros2 launch bringup stonefish.launch.py \ auv_name:=bluerov2 \ env_file_name:=pool_env.scn \ auv_file_name:=bluerov2.scn \ - headless:=false + headless:=false \ + use_joy:=true # bluerov2 direct actuator sanity test # Use stonefish_only so mission_executor does not overwrite the direct command. @@ -306,6 +206,11 @@ ros2 topic echo /bridge/thruster_state ## Build & Run proteus using bridge_hardware +### Extra deps +```sh +sudo apt install -y ros-jazzy-usb-cam socat # usb_cam optional; socat for mock Arduino +``` + ### Build ```sh # Source ROS 2 environment @@ -313,7 +218,7 @@ source /opt/ros/jazzy/setup.bash # Build packages colcon build \ - --packages-select interfaces bringup mission_executor bridge_hardware vectornav vectornav_msgs \ + --packages-select interfaces bringup mission_executor bridge_hardware \ --cmake-args -DCMAKE_EXPORT_COMPILE_COMMANDS=ON ``` diff --git a/imu-workbagsimucam_ros1.yaml b/imu-workbagsimucam_ros1.yaml deleted file mode 100644 index 67dd532..0000000 --- a/imu-workbagsimucam_ros1.yaml +++ /dev/null @@ -1,14 +0,0 @@ -imu0: - T_i_b: - - [1.0, 0.0, 0.0, 0.0] - - [0.0, 1.0, 0.0, 0.0] - - [0.0, 0.0, 1.0, 0.0] - - [0.0, 0.0, 0.0, 1.0] - accelerometer_noise_density: 0.002 - accelerometer_random_walk: 0.003 - gyroscope_noise_density: 0.00017 - gyroscope_random_walk: 1.9e-05 - model: calibrated - rostopic: /vectornav/imu - time_offset: 0.0 - update_rate: 20.0 diff --git a/scripts/bench_build.sh b/scripts/bench_build.sh new file mode 100755 index 0000000..a0f9c2d --- /dev/null +++ b/scripts/bench_build.sh @@ -0,0 +1,86 @@ +#!/usr/bin/env bash +# Measure clean vs incremental colcon build times for the sim workspace. +set -eo pipefail + +ROOT="$(cd "$(dirname "${BASH_SOURCE[0]}")/.." && pwd)" +cd "$ROOT" + +set +u +source /opt/ros/jazzy/setup.bash +set -u + +JOBS="$(nproc)" +CMAKE_ARGS=(--cmake-args -DCMAKE_BUILD_TYPE=Release) + +fmt_secs() { + awk -v s="$1" 'BEGIN { printf "%.1fs (%.1fm)", s, s/60 }' +} + +run_timed() { + local label="$1" + shift + local start end elapsed + start=$(date +%s.%N) + "$@" + end=$(date +%s.%N) + elapsed=$(awk -v a="$start" -v b="$end" 'BEGIN { printf "%.2f", b - a }') + echo "$label|$elapsed" +} + +verify_stonefish_prefix() { + local cache="$ROOT/build/stonefish_ros2/CMakeCache.txt" + if [[ ! -f "$cache" ]]; then + echo "warning: $cache missing; cannot verify Stonefish prefix" >&2 + return 0 + fi + local found + found=$(grep -m1 '^Stonefish_DIR:' "$cache" | awk '{print $2}') + echo "stonefish_ros2 Stonefish_DIR=$found" + case "$found" in + "$ROOT/install"/*) return 0 ;; + *) echo "error: stonefish_ros2 did not use workspace install/" >&2; return 1 ;; + esac +} + +echo "=== autonomy-stack colcon benchmark ===" +echo "date: $(date -Is)" +echo "root: $ROOT" +echo "jobs: $JOBS" +echo "cpu: $(lscpu | awk -F: '/Model name/{print $2}' | xargs)" +echo + +# --- 1) Colcon, fully clean (no cargo cache) --- +rm -rf build install log target vendor/stonefish/build vendor/stonefish/install +COLCON_CLEAN_NO_CACHE=$(run_timed colcon_clean_no_cache colcon build "${CMAKE_ARGS[@]}") +verify_stonefish_prefix +echo "colcon build (clean, no cargo cache): $(fmt_secs "${COLCON_CLEAN_NO_CACHE#*|}")" + +# --- 2) Colcon, clean workspace but warm cargo cache --- +rm -rf build install log +COLCON_CLEAN_WARM_CACHE=$(run_timed colcon_clean_warm_cache colcon build "${CMAKE_ARGS[@]}") +verify_stonefish_prefix +echo "colcon build (clean ws, warm cargo): $(fmt_secs "${COLCON_CLEAN_WARM_CACHE#*|}")" + +# --- 3) Incremental: no source changes --- +COLCON_NOOP=$(run_timed colcon_noop colcon build "${CMAKE_ARGS[@]}") +echo "colcon build (rebuild, no changes): $(fmt_secs "${COLCON_NOOP#*|}")" + +# --- 4) Incremental: touch one file per package type --- +touch src/bringup/launch/stonefish.launch.py +touch src/detection_mocker/src/main.cpp +touch src/mission_executor/src/main.rs +COLCON_TOUCH=$(run_timed colcon_touch colcon build "${CMAKE_ARGS[@]}") +echo "colcon build (touch launch+cpp+rust): $(fmt_secs "${COLCON_TOUCH#*|}")" + +echo +echo "=== artifact sizes ===" +du -sh build install target 2>/dev/null | sed 's/^/ /' + +echo +echo "=== summary (seconds) ===" +printf '%s\n' \ + "$COLCON_CLEAN_NO_CACHE" \ + "$COLCON_CLEAN_WARM_CACHE" \ + "$COLCON_NOOP" \ + "$COLCON_TOUCH" \ + | column -t -s'|' diff --git a/src/bringup/launch/hardware_proteus.launch.py b/src/bringup/launch/hardware_proteus.launch.py index 8a43223..f20ebcd 100644 --- a/src/bringup/launch/hardware_proteus.launch.py +++ b/src/bringup/launch/hardware_proteus.launch.py @@ -1,35 +1,19 @@ import os from launch.actions import DeclareLaunchArgument -from launch.actions import IncludeLaunchDescription from launch_ros.actions import Node from launch import LaunchDescription from launch.conditions import IfCondition -from launch.launch_description_sources import PythonLaunchDescriptionSource from launch.substitutions import LaunchConfiguration -from launch.substitutions import PathJoinSubstitution -from launch_ros.substitutions import FindPackageShare def generate_launch_description(): mission_name_arg = DeclareLaunchArgument('mission_name') arduino_port_arg = DeclareLaunchArgument('arduino_port') arduino_baud_rate_arg = DeclareLaunchArgument('arduino_baud_rate') - use_vectornav_arg = DeclareLaunchArgument('use_vectornav', default_value='true') use_usb_cam_arg = DeclareLaunchArgument('use_usb_cam', default_value='false') - use_orb_slam_arg = DeclareLaunchArgument('use_orb_slam', default_value='false') - orb_use_viewer_arg = DeclareLaunchArgument('orb_use_viewer', default_value='false') - world_frame_arg = DeclareLaunchArgument('world_frame', default_value='world') base_frame_arg = DeclareLaunchArgument('base_frame', default_value='base_link') - imu_frame_arg = DeclareLaunchArgument('imu_frame', default_value='vectornav') camera_frame_arg = DeclareLaunchArgument('camera_frame', default_value='camera_link') - imu_x_arg = DeclareLaunchArgument('imu_x', default_value='0.0') - imu_y_arg = DeclareLaunchArgument('imu_y', default_value='0.0') - imu_z_arg = DeclareLaunchArgument('imu_z', default_value='0.0') - imu_roll_arg = DeclareLaunchArgument('imu_roll', default_value='0.0') - imu_pitch_arg = DeclareLaunchArgument('imu_pitch', default_value='0.0') - imu_yaw_arg = DeclareLaunchArgument('imu_yaw', default_value='0.0') - camera_x_arg = DeclareLaunchArgument('camera_x', default_value='0.0') camera_y_arg = DeclareLaunchArgument('camera_y', default_value='0.0') camera_z_arg = DeclareLaunchArgument('camera_z', default_value='0.0') @@ -41,20 +25,9 @@ def generate_launch_description(): arduino_port = LaunchConfiguration(arduino_port_arg.name) arduino_baud_rate = LaunchConfiguration(arduino_baud_rate_arg.name) - use_vectornav = LaunchConfiguration(use_vectornav_arg.name) use_usb_cam = LaunchConfiguration(use_usb_cam_arg.name) - use_orb_slam = LaunchConfiguration(use_orb_slam_arg.name) - orb_use_viewer = LaunchConfiguration(orb_use_viewer_arg.name) - world_frame = LaunchConfiguration(world_frame_arg.name) base_frame = LaunchConfiguration(base_frame_arg.name) - imu_frame = LaunchConfiguration(imu_frame_arg.name) camera_frame = LaunchConfiguration(camera_frame_arg.name) - imu_x = LaunchConfiguration(imu_x_arg.name) - imu_y = LaunchConfiguration(imu_y_arg.name) - imu_z = LaunchConfiguration(imu_z_arg.name) - imu_roll = LaunchConfiguration(imu_roll_arg.name) - imu_pitch = LaunchConfiguration(imu_pitch_arg.name) - imu_yaw = LaunchConfiguration(imu_yaw_arg.name) camera_x = LaunchConfiguration(camera_x_arg.name) camera_y = LaunchConfiguration(camera_y_arg.name) camera_z = LaunchConfiguration(camera_z_arg.name) @@ -62,61 +35,14 @@ def generate_launch_description(): camera_pitch = LaunchConfiguration(camera_pitch_arg.name) camera_yaw = LaunchConfiguration(camera_yaw_arg.name) - vectornav_launch = IncludeLaunchDescription( - PythonLaunchDescriptionSource( - PathJoinSubstitution([ - FindPackageShare('vectornav'), - 'launch', - 'vectornav.launch.py', - ]) - ), - condition=IfCondition(use_vectornav), - ) - - orb_slam_launch = IncludeLaunchDescription( - PythonLaunchDescriptionSource( - PathJoinSubstitution([ - FindPackageShare('orb_slam3_ros2'), - 'launch', - 'orb_slam_sim_launch.py', - ]) - ), - launch_arguments={ - 'use_viewer': orb_use_viewer, - 'use_imu': 'true', - 'use_depth': 'false', - 'settings_file': PathJoinSubstitution([ - FindPackageShare('orb_slam3_ros2'), - 'config', - 'webcamera.yaml', - ]), - 'image_topic': '/usb_cam/image_raw', - 'imu_topic': '/vectornav/imu', - 'world_frame_id': world_frame, - 'camera_frame_id': camera_frame, - }.items(), - condition=IfCondition(use_orb_slam), - ) - return LaunchDescription([ mission_name_arg, arduino_port_arg, arduino_baud_rate_arg, - use_vectornav_arg, use_usb_cam_arg, - use_orb_slam_arg, - orb_use_viewer_arg, - world_frame_arg, base_frame_arg, - imu_frame_arg, camera_frame_arg, - imu_x_arg, - imu_y_arg, - imu_z_arg, - imu_roll_arg, - imu_pitch_arg, - imu_yaw_arg, camera_x_arg, camera_y_arg, camera_z_arg, @@ -131,13 +57,11 @@ def generate_launch_description(): 'mission_name': mission_name, 'bridge_name': 'hardware', 'auv_name': 'proteus', - # 'live_config_path': PathJoinSubstitution([FindPackageShare('bringup'), 'config', 'mission_executor.toml']), 'live_config_path': os.path.join( os.getcwd(), 'src', 'bringup', 'config', 'mission_executor.toml' ), }], ), - # TODO: some sort vision stuff Node( package='bridge_hardware', executable='bridge_proteus_node', @@ -156,22 +80,6 @@ def generate_launch_description(): condition=IfCondition(use_usb_cam), ), - Node( - package='tf2_ros', - executable='static_transform_publisher', - name='base_to_imu_tf', - output='screen', - arguments=['--frame-id', base_frame, - '--child-frame-id', imu_frame, - '--x', imu_x, - '--y', imu_y, - '--z', imu_z, - '--roll', imu_roll, - '--pitch', imu_pitch, - '--yaw', imu_yaw], - condition=IfCondition(use_vectornav), - ), - Node( package='tf2_ros', executable='static_transform_publisher', @@ -187,7 +95,4 @@ def generate_launch_description(): '--yaw', camera_yaw], condition=IfCondition(use_usb_cam), ), - - vectornav_launch, - orb_slam_launch, ]) diff --git a/src/bringup/launch/stonefish.launch.py b/src/bringup/launch/stonefish.launch.py index a30bc1c..0961a6c 100644 --- a/src/bringup/launch/stonefish.launch.py +++ b/src/bringup/launch/stonefish.launch.py @@ -33,6 +33,7 @@ def _launch_setup(context, *args, **kwargs): explicit_auv_file_name = LaunchConfiguration('auv_file_name').perform(context) headless = LaunchConfiguration('headless').perform(context).lower() in ('true', '1', 'yes') stonefish_only = LaunchConfiguration('stonefish_only', default="no").perform(context).lower() in ('true', '1', 'yes') + use_joy = LaunchConfiguration('use_joy').perform(context).lower() in ('true', '1', 'yes') cwd = os.getcwd() bridge_share = get_package_share_directory('bridge_stonefish') @@ -101,11 +102,12 @@ def _launch_setup(context, *args, **kwargs): 'publish_all_objects': True, }], ), - Node( + ] + if use_joy: + ret.append(Node( package='joy', executable='joy_node', - ), - ] + )) if not stonefish_only: ret += [ Node( @@ -133,6 +135,11 @@ def generate_launch_description(): auv_file_name_arg = DeclareLaunchArgument('auv_file_name', default_value='') headless_arg = DeclareLaunchArgument('headless', default_value='false') stonefish_only_arg = DeclareLaunchArgument('stonefish_only', default_value='false') + use_joy_arg = DeclareLaunchArgument( + 'use_joy', + default_value='false', + description='Start joy_node (requires ros-jazzy-joy). Use for teleop.', + ) return LaunchDescription([ mission_name_arg, @@ -141,5 +148,6 @@ def generate_launch_description(): auv_file_name_arg, headless_arg, stonefish_only_arg, + use_joy_arg, OpaqueFunction(function=_launch_setup), ]) diff --git a/src/bringup/package.xml b/src/bringup/package.xml index 8c327e9..538e0ad 100644 --- a/src/bringup/package.xml +++ b/src/bringup/package.xml @@ -10,7 +10,7 @@ rclpy launch_ros tf2_ros - vectornav + ament_copyright ament_flake8 diff --git a/src/mission_executor/CMakeLists.txt b/src/mission_executor/CMakeLists.txt index 826b491..c2f00f2 100644 --- a/src/mission_executor/CMakeLists.txt +++ b/src/mission_executor/CMakeLists.txt @@ -3,13 +3,21 @@ project(mission_executor) find_package(ament_cmake REQUIRED) -set(RUST_TARGET ${CMAKE_CURRENT_SOURCE_DIR}/target/release/${PROJECT_NAME}) +# Cargo workspace root is the repo root (see /Cargo.toml). +set(CARGO_TARGET_DIR "${CMAKE_CURRENT_SOURCE_DIR}/../../target") +set(RUST_BINARY "${CARGO_TARGET_DIR}/release/${PROJECT_NAME}") + +file(GLOB_RECURSE RUST_SOURCES CONFIGURE_DEPENDS + "${CMAKE_CURRENT_SOURCE_DIR}/src/*.rs" +) add_custom_command( - OUTPUT ${RUST_TARGET} - COMMAND cargo build --release --quiet - WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR} - DEPENDS ${CMAKE_CURRENT_SOURCE_DIR}/src/main.rs ${CMAKE_CURRENT_SOURCE_DIR}/Cargo.toml + OUTPUT ${RUST_BINARY} + COMMAND ${CMAKE_COMMAND} -E env + CARGO_TARGET_DIR=${CARGO_TARGET_DIR} + cargo build --release --quiet --manifest-path ${CMAKE_CURRENT_SOURCE_DIR}/Cargo.toml + WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}/../.. + DEPENDS ${RUST_SOURCES} ${CMAKE_CURRENT_SOURCE_DIR}/Cargo.toml ${CMAKE_CURRENT_SOURCE_DIR}/../../Cargo.toml ${CMAKE_CURRENT_SOURCE_DIR}/../../Cargo.lock COMMENT "Building Rust executable" VERBATIM ) @@ -19,10 +27,10 @@ if(WIN32) else() set(SUFFIX "") endif() -add_custom_target(build_rust ALL DEPENDS ${RUST_TARGET}) +add_custom_target(build_rust ALL DEPENDS ${RUST_BINARY}) install( - PROGRAMS ${CMAKE_SOURCE_DIR}/../../target/release/${PROJECT_NAME}${SUFFIX} + PROGRAMS ${RUST_BINARY}${SUFFIX} DESTINATION lib/${PROJECT_NAME} ) diff --git a/src/orb_slam3_ros2/CMakeLists.txt b/src/orb_slam3_ros2/CMakeLists.txt deleted file mode 100644 index 63ae888..0000000 --- a/src/orb_slam3_ros2/CMakeLists.txt +++ /dev/null @@ -1,161 +0,0 @@ -cmake_minimum_required(VERSION 3.8) -project(orb_slam3_ros2) - -# ============================== -# Compiler and build settings -# ============================== -if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 14) -endif() - -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic) -endif() - -# ============================== -# ROS 2 and external dependencies -# ============================== -find_package(ament_cmake REQUIRED) -find_package(rclcpp REQUIRED) -find_package(std_msgs REQUIRED) -find_package(sensor_msgs REQUIRED) -find_package(geometry_msgs REQUIRED) -find_package(nav_msgs REQUIRED) -find_package(tf2 REQUIRED) -find_package(tf2_ros REQUIRED) -find_package(tf2_geometry_msgs REQUIRED) -find_package(cv_bridge REQUIRED) -find_package(image_transport REQUIRED) -find_package(message_filters REQUIRED) - -# Resolve OpenCV for headers. We intentionally avoid linking OpenCV directly -# in this package to prevent mixing OpenCV ABIs with prebuilt ORB_SLAM3/cv_bridge. -set(CV_BRIDGE_OPENCV_VERSION "") -if(DEFINED OpenCV_VERSION) - set(CV_BRIDGE_OPENCV_VERSION "${OpenCV_VERSION}") -endif() - -find_package(OpenCV 4 REQUIRED) -message(STATUS "Resolved OpenCV version: ${OpenCV_VERSION}") -if(CV_BRIDGE_OPENCV_VERSION AND NOT OpenCV_VERSION VERSION_EQUAL CV_BRIDGE_OPENCV_VERSION) - message(WARNING - "OpenCV mismatch detected (cv_bridge: ${CV_BRIDGE_OPENCV_VERSION}, this package: ${OpenCV_VERSION}). " - "Direct OpenCV linkage is disabled in orb_slam3_ros2 to reduce ABI conflicts." - ) -endif() - -find_package(Eigen3 3.1.0 REQUIRED) -find_package(Pangolin REQUIRED) - -# ORB_SLAM3 - resolve the checkout from common workspace locations or a user override -set(ORB_SLAM3_ROOT_DIR "" CACHE PATH "Path to the ORB_SLAM3 checkout") -if(NOT ORB_SLAM3_ROOT_DIR) - set(_orb_slam3_candidates - ${CMAKE_CURRENT_SOURCE_DIR}/../../vendor/ORB_SLAM3 - ${CMAKE_CURRENT_SOURCE_DIR}/../../cv/third_party/ORB_SLAM3 - ) - - foreach(_candidate IN LISTS _orb_slam3_candidates) - if(EXISTS "${_candidate}/include/System.h") - set(ORB_SLAM3_ROOT_DIR "${_candidate}") - break() - endif() - endforeach() -endif() - -if(NOT ORB_SLAM3_ROOT_DIR OR NOT EXISTS "${ORB_SLAM3_ROOT_DIR}/include/System.h") - message(FATAL_ERROR - "Could not locate ORB_SLAM3. Set ORB_SLAM3_ROOT_DIR to a checkout containing include/System.h." - ) -endif() - -message(STATUS "Using ORB_SLAM3 from: ${ORB_SLAM3_ROOT_DIR}") -set(ORB_SLAM3_INCLUDE_DIRS - ${ORB_SLAM3_ROOT_DIR} - ${ORB_SLAM3_ROOT_DIR}/include - ${ORB_SLAM3_ROOT_DIR}/include/CameraModels - ${ORB_SLAM3_ROOT_DIR}/Thirdparty/Sophus - ${ORB_SLAM3_ROOT_DIR}/Thirdparty/DBoW2/include - ${ORB_SLAM3_ROOT_DIR}/Thirdparty/g2o/include -) -set(ORB_SLAM3_LIBRARY ${ORB_SLAM3_ROOT_DIR}/lib/libORB_SLAM3.so) -set(DBOW2_LIBRARY ${ORB_SLAM3_ROOT_DIR}/Thirdparty/DBoW2/lib/libDBoW2.so) -set(G2O_LIBRARY ${ORB_SLAM3_ROOT_DIR}/Thirdparty/g2o/lib/libg2o.so) - -# ============================== -# Dependency list for ament -# ============================== -set(dependencies - rclcpp - std_msgs - sensor_msgs - geometry_msgs - nav_msgs - tf2 - tf2_ros - tf2_geometry_msgs - cv_bridge - image_transport - message_filters -) - -# ============================== -# Build executable node -# ============================== -add_executable(orb_slam_node src/orb_slam_node.cpp) - -ament_target_dependencies(orb_slam_node ${dependencies}) - -# Link with ORB_SLAM3 and external libs -target_link_libraries(orb_slam_node - ${ORB_SLAM3_LIBRARY} - ${DBOW2_LIBRARY} - ${G2O_LIBRARY} - ${EIGEN3_LIBS} - ${Pangolin_LIBRARIES} - cv_bridge::cv_bridge - -lboost_serialization - -lcrypto -) - -# ============================== -# Include directories (using direct paths) -# ============================== -target_include_directories(orb_slam_node PRIVATE - ${CMAKE_CURRENT_SOURCE_DIR}/include - ${ORB_SLAM3_INCLUDE_DIRS} - ${OpenCV_INCLUDE_DIRS} - ${EIGEN3_INCLUDE_DIR} - ${Pangolin_INCLUDE_DIRS} -) - -install(TARGETS - orb_slam_node - DESTINATION lib/${PROJECT_NAME} -) - -if(EXISTS "${CMAKE_CURRENT_SOURCE_DIR}/launch") - install(DIRECTORY - launch - DESTINATION share/${PROJECT_NAME} - ) -endif() - -if(EXISTS "${CMAKE_CURRENT_SOURCE_DIR}/config") - install(DIRECTORY - config - DESTINATION share/${PROJECT_NAME} - ) -endif() - -if(EXISTS "${CMAKE_CURRENT_SOURCE_DIR}/../../vendor/ORBvoc.txt") - install(FILES - ${CMAKE_CURRENT_SOURCE_DIR}/../../vendor/ORBvoc.txt - DESTINATION share/${PROJECT_NAME}/vendor - ) -endif() - -# ============================== -# Package export for ament -# ============================== -ament_package() diff --git a/src/orb_slam3_ros2/config/imucam_bag.yaml b/src/orb_slam3_ros2/config/imucam_bag.yaml deleted file mode 100644 index 25306cb..0000000 --- a/src/orb_slam3_ros2/config/imucam_bag.yaml +++ /dev/null @@ -1,93 +0,0 @@ -%YAML:1.0 - -#-------------------------------------------------------------------------------------------- -# Camera Parameters -#-------------------------------------------------------------------------------------------- -Camera.type: "PinHole" - -# Camera calibration from camchain-imucam-workbagsimucam_ros1.yaml -Camera.fx: 1000.0 -Camera.fy: 1000.0 -Camera.cx: 640.0 -Camera.cy: 360.0 - -# Distortion coefficients: (k1, k2, p1, p2, k3) -Camera.k1: 0.0 -Camera.k2: 0.0 -Camera.p1: 0.0 -Camera.p2: 0.0 -Camera.k3: 0.0 - -Camera.width: 1280 -Camera.height: 720 -Camera.fps: 16.62174 -Camera.RGB: 0 - -#-------------------------------------------------------------------------------------------- -# RGB-D Parameters (required when use_depth=true) -#-------------------------------------------------------------------------------------------- -# Camera baseline times fx (for stereo/RGB-D). Set to 0 for monocular -Camera.bf: 40.0 - -# Close/Far threshold. Baseline times -ThDepth: 40.0 - -# Depth map scale factor (convert depth values to meters) -# For DepthAnything output (normalized 0-1), use 1.0 -# For typical depth cameras: 1000.0 (if depth is in mm), 5000.0 (TUM/KITTI format) -DepthMapFactor: 1.0 - -#-------------------------------------------------------------------------------------------- -# IMU Parameters (required for monocular-inertial mode) -#-------------------------------------------------------------------------------------------- -IMU.NoiseGyro: 1.7e-4 -IMU.NoiseAcc: 2.0e-3 -IMU.GyroWalk: 1.9e-5 -IMU.AccWalk: 3.0e-3 -IMU.Frequency: 20 - -# Left camera to IMU transform (4x4) -Tbc: !!opencv-matrix - rows: 4 - cols: 4 - dt: f - data: [0.0757822598, -0.952608595, 0.294608068, 0.00524912371, - 0.553797745, -0.205488663, -0.806896813, 0.00758131847, - 0.829195457, 0.224301748, 0.511980097, -3.27803068e-05, - 0, 0, 0, 1] - -# Keep both keys for compatibility across ORB-SLAM3 forks -IMU.T_b_c1: !!opencv-matrix - rows: 4 - cols: 4 - dt: f - data: [0.0757822598, -0.952608595, 0.294608068, 0.00524912371, - 0.553797745, -0.205488663, -0.806896813, 0.00758131847, - 0.829195457, 0.224301748, 0.511980097, -3.27803068e-05, - 0, 0, 0, 1] - -InsertKFsWhenLost: 1 -IMU.InsertKFsWhenLost: 1 - -#-------------------------------------------------------------------------------------------- -# ORB Parameters -#-------------------------------------------------------------------------------------------- -ORBextractor.nFeatures: 1000 -ORBextractor.scaleFactor: 1.2 -ORBextractor.nLevels: 8 -ORBextractor.iniThFAST: 20 -ORBextractor.minThFAST: 7 - -#-------------------------------------------------------------------------------------------- -# Viewer Parameters -#-------------------------------------------------------------------------------------------- -Viewer.KeyFrameSize: 0.05 -Viewer.KeyFrameLineWidth: 1.0 -Viewer.GraphLineWidth: 0.9 -Viewer.PointSize: 2.0 -Viewer.CameraSize: 0.08 -Viewer.CameraLineWidth: 3.0 -Viewer.ViewpointX: 0.0 -Viewer.ViewpointY: -0.7 -Viewer.ViewpointZ: -1.8 -Viewer.ViewpointF: 500.0 diff --git a/src/orb_slam3_ros2/config/orb_slam_full_template.yaml b/src/orb_slam3_ros2/config/orb_slam_full_template.yaml deleted file mode 100644 index 736cd45..0000000 --- a/src/orb_slam3_ros2/config/orb_slam_full_template.yaml +++ /dev/null @@ -1,273 +0,0 @@ -%YAML:1.0 - -#-------------------------------------------------------------------------------------------- -# ORB-SLAM3 Complete Configuration Template -# All available parameters with descriptions and typical values -#-------------------------------------------------------------------------------------------- - -#-------------------------------------------------------------------------------------------- -# Camera Parameters -#-------------------------------------------------------------------------------------------- - -# Camera model type -# Options: "PinHole" or "KannalaBrandt8" (fisheye) -Camera.type: "PinHole" - -# Camera resolution -Camera.width: 640 -Camera.height: 480 - -# Camera intrinsics (focal length and principal point) -# For PinHole camera model -Camera.fx: 500.0 # Focal length in x direction (pixels) -Camera.fy: 500.0 # Focal length in y direction (pixels) -Camera.cx: 320.0 # Principal point x coordinate (pixels) -Camera.cy: 240.0 # Principal point y coordinate (pixels) - -# Distortion coefficients for PinHole model -# [k1, k2, p1, p2, k3] - radial (k) and tangential (p) distortion -Camera.k1: 0.0 # Radial distortion coefficient k1 -Camera.k2: 0.0 # Radial distortion coefficient k2 -Camera.p1: 0.0 # Tangential distortion coefficient p1 -Camera.p2: 0.0 # Tangential distortion coefficient p2 -Camera.k3: 0.0 # Radial distortion coefficient k3 (optional) - -# For KannalaBrandt8 (fisheye) model -# Camera.k1: 0.0 # Fisheye distortion k1 -# Camera.k2: 0.0 # Fisheye distortion k2 -# Camera.k3: 0.0 # Fisheye distortion k3 -# Camera.k4: 0.0 # Fisheye distortion k4 - -# Overlapping area for fisheye cameras (optional) -# Camera1.overlappingBegin: 0 -# Camera1.overlappingEnd: 640 - -# Camera frame rate (Hz) -Camera.fps: 30.0 - -# Color order: 1 = RGB, 0 = BGR -Camera.RGB: 1 - -# Image resize (optional - for processing at different resolution) -# Camera.newHeight: 480 -# Camera.newWidth: 640 - -#-------------------------------------------------------------------------------------------- -# Stereo Camera Parameters (for stereo/multi-camera systems) -#-------------------------------------------------------------------------------------------- - -# Second camera intrinsics (if using stereo) -# Camera2.fx: 500.0 -# Camera2.fy: 500.0 -# Camera2.cx: 320.0 -# Camera2.cy: 240.0 -# Camera2.k1: 0.0 -# Camera2.k2: 0.0 -# Camera2.p1: 0.0 -# Camera2.p2: 0.0 -# Camera2.k3: 0.0 - -# Stereo baseline * fx (for stereo systems) -# Camera.bf: 40.0 -# Or use baseline directly -# Stereo.b: 0.08 # Baseline in meters - -# Transformation from camera 1 to camera 2 (T_c1_c2) -# 4x4 transformation matrix -# Stereo.T_c1_c2: !!opencv-matrix -# rows: 4 -# cols: 4 -# dt: f -# data: [1.0, 0.0, 0.0, 0.08, -# 0.0, 1.0, 0.0, 0.0, -# 0.0, 0.0, 1.0, 0.0, -# 0.0, 0.0, 0.0, 1.0] - -# Depth threshold for stereo (baseline * threshold) -# Stereo.ThDepth: 40.0 - -#-------------------------------------------------------------------------------------------- -# RGB-D Parameters (for depth cameras) -#-------------------------------------------------------------------------------------------- - -# Depth map factor (converts depth values to meters) -# For TUM datasets: 5000.0 -# For real depth cameras: usually 1000.0 (if depth is in mm) -# For DepthAnything normalized output: 1.0 -Camera.bf: 0.0 # Set to 0 for monocular (required even if not used) -ThDepth: 40.0 # Depth threshold (required even if not used) -DepthMapFactor: 1.0 # Scale factor for depth values -RGBD.DepthMapFactor: 1.0 # Alternative depth map factor parameter - -#-------------------------------------------------------------------------------------------- -# IMU Parameters (for visual-inertial systems) -#-------------------------------------------------------------------------------------------- - -# IMU noise characteristics (from Allan variance analysis) -# These values are critical for accurate IMU integration - -# Gyroscope noise density (rad/s/sqrt(Hz)) -# Typical values: 1e-4 to 1e-3 for consumer IMUs, 1e-5 for high-end -IMU.NoiseGyro: 1.7e-4 - -# Accelerometer noise density (m/s^2/sqrt(Hz)) -# Typical values: 1e-3 to 1e-2 for consumer IMUs, 1e-4 for high-end -IMU.NoiseAcc: 2.0e-3 - -# Gyroscope random walk (rad/s^2/sqrt(Hz)) -# Describes bias drift over time -# Typical values: 1e-6 to 1e-4 -IMU.GyroWalk: 1.9e-5 - -# Accelerometer random walk (m/s^3/sqrt(Hz)) -# Describes bias drift over time -# Typical values: 1e-4 to 1e-2 -IMU.AccWalk: 3.0e-3 - -# IMU sampling frequency (Hz) -# Must match your IMU data rate -IMU.Frequency: 200 - -# Left camera to IMU transform (4x4) -Tbc: !!opencv-matrix - rows: 4 - cols: 4 - dt: f - data: [0.0757822598, -0.952608595, 0.294608068, 0.00524912371, - 0.553797745, -0.205488663, -0.806896813, 0.00758131847, - 0.829195457, 0.224301748, 0.511980097, -3.27803068e-05, - 0, 0, 0, 1] - -# Transformation from body (IMU) frame to camera frame (T_b_c1) -# This is the extrinsic calibration between IMU and camera -# Should be calibrated using tools like Kalibr -# Default: Identity matrix (assumes IMU and camera are aligned) -IMU.T_b_c1: !!opencv-matrix - rows: 4 - cols: 4 - dt: f - data: [0.0757822598, -0.952608595, 0.294608068, 0.00524912371, - 0.553797745, -0.205488663, -0.806896813, 0.00758131847, - 0.829195457, 0.224301748, 0.511980097, -3.27803068e-05, - 0, 0, 0, 1] - -# Insert keyframes when tracking is lost (IMU-inertial mode) -# 1 = true, 0 = false -IMU.InsertKFsWhenLost: 1 - -#-------------------------------------------------------------------------------------------- -# ORB Feature Extractor Parameters -#-------------------------------------------------------------------------------------------- - -# Number of ORB features per frame -# More features = better tracking but slower -# Typical values: 1000-2000 for monocular, 1000-3000 for stereo/RGB-D -ORBextractor.nFeatures: 1000 - -# Scale factor between pyramid levels -# Typical value: 1.2 -# Higher values = fewer levels needed but larger jumps in scale -ORBextractor.scaleFactor: 1.2 - -# Number of pyramid levels -# More levels = better scale invariance but slower -# Typical values: 8 -ORBextractor.nLevels: 8 - -# FAST detector threshold -# Initial threshold for FAST corner detection -# If not enough features found, threshold is reduced to minThFAST -ORBextractor.iniThFAST: 20 - -# Minimum FAST threshold -# Fallback threshold if iniThFAST doesn't find enough corners -ORBextractor.minThFAST: 7 - -#-------------------------------------------------------------------------------------------- -# Viewer Parameters (for ORB-SLAM3 visualization window) -#-------------------------------------------------------------------------------------------- - -# Keyframe visualization size -Viewer.KeyFrameSize: 0.05 - -# Keyframe line width (in visualization) -Viewer.KeyFrameLineWidth: 1.0 - -# Graph line width (covisibility graph) -Viewer.GraphLineWidth: 0.9 - -# Point size (map points) -Viewer.PointSize: 2.0 - -# Camera size (in visualization) -Viewer.CameraSize: 0.08 - -# Camera line width -Viewer.CameraLineWidth: 3.0 - -# Initial viewpoint position -Viewer.ViewpointX: 0.0 -Viewer.ViewpointY: -0.7 -Viewer.ViewpointZ: -1.8 - -# Viewpoint focal length (controls zoom) -Viewer.ViewpointF: 500.0 - -# Image viewer scale (optional, for resizing displayed images) -# Viewer.imageViewScale: 1.0 - -#-------------------------------------------------------------------------------------------- -# System Parameters -#-------------------------------------------------------------------------------------------- - -# Load existing map from file (optional) -# Provide path to .osa file to continue from previous session -# Leave as 0 to start fresh -System.LoadAtlasFromFile: 0 - -# Save map to file on shutdown (optional) -# Provide filename to save map, or 0 to not save -System.SaveAtlasToFile: 0 - -# Threshold for far points (optional) -# Points beyond this distance are considered outliers -# System.thFarPoints: 100.0 - -#-------------------------------------------------------------------------------------------- -# Advanced Parameters (usually not needed) -#-------------------------------------------------------------------------------------------- - -# Sequence name (for multi-session mapping) -# Seq.Name: "sequence_01" - -# Use loop closure (typically always enabled) -# System.loopClosureEnabled: 1 - -# Use local mapping (typically always enabled) -# System.localMappingEnabled: 1 - -#-------------------------------------------------------------------------------------------- -# Notes and Calibration Tips -#-------------------------------------------------------------------------------------------- - -# Camera Calibration: -# - Use tools like: ROS camera_calibration, OpenCV calibration, or MATLAB -# - Record checkerboard video, extract intrinsics and distortion -# - Accurate calibration is CRITICAL for good SLAM performance -# -# IMU Calibration: -# - Use Allan variance analysis for noise parameters -# - Use Kalibr for IMU-camera extrinsic calibration (T_b_c1) -# - Record static IMU data for several hours for Allan variance -# -# Parameter Tuning: -# - Start with these defaults -# - Adjust ORBextractor.nFeatures based on scene texture -# - Adjust IMU noise if you see drift or instability -# - Adjust FAST thresholds if tracking is lost frequently -# -# Debugging: -# - Enable viewer to see tracking status -# - Check that camera frame rate matches Camera.fps -# - Verify Camera.RGB matches your image encoding -# - Ensure depth factor is correct for your depth camera diff --git a/src/orb_slam3_ros2/config/stonefish_hydrus.yaml b/src/orb_slam3_ros2/config/stonefish_hydrus.yaml deleted file mode 100644 index 736cd45..0000000 --- a/src/orb_slam3_ros2/config/stonefish_hydrus.yaml +++ /dev/null @@ -1,273 +0,0 @@ -%YAML:1.0 - -#-------------------------------------------------------------------------------------------- -# ORB-SLAM3 Complete Configuration Template -# All available parameters with descriptions and typical values -#-------------------------------------------------------------------------------------------- - -#-------------------------------------------------------------------------------------------- -# Camera Parameters -#-------------------------------------------------------------------------------------------- - -# Camera model type -# Options: "PinHole" or "KannalaBrandt8" (fisheye) -Camera.type: "PinHole" - -# Camera resolution -Camera.width: 640 -Camera.height: 480 - -# Camera intrinsics (focal length and principal point) -# For PinHole camera model -Camera.fx: 500.0 # Focal length in x direction (pixels) -Camera.fy: 500.0 # Focal length in y direction (pixels) -Camera.cx: 320.0 # Principal point x coordinate (pixels) -Camera.cy: 240.0 # Principal point y coordinate (pixels) - -# Distortion coefficients for PinHole model -# [k1, k2, p1, p2, k3] - radial (k) and tangential (p) distortion -Camera.k1: 0.0 # Radial distortion coefficient k1 -Camera.k2: 0.0 # Radial distortion coefficient k2 -Camera.p1: 0.0 # Tangential distortion coefficient p1 -Camera.p2: 0.0 # Tangential distortion coefficient p2 -Camera.k3: 0.0 # Radial distortion coefficient k3 (optional) - -# For KannalaBrandt8 (fisheye) model -# Camera.k1: 0.0 # Fisheye distortion k1 -# Camera.k2: 0.0 # Fisheye distortion k2 -# Camera.k3: 0.0 # Fisheye distortion k3 -# Camera.k4: 0.0 # Fisheye distortion k4 - -# Overlapping area for fisheye cameras (optional) -# Camera1.overlappingBegin: 0 -# Camera1.overlappingEnd: 640 - -# Camera frame rate (Hz) -Camera.fps: 30.0 - -# Color order: 1 = RGB, 0 = BGR -Camera.RGB: 1 - -# Image resize (optional - for processing at different resolution) -# Camera.newHeight: 480 -# Camera.newWidth: 640 - -#-------------------------------------------------------------------------------------------- -# Stereo Camera Parameters (for stereo/multi-camera systems) -#-------------------------------------------------------------------------------------------- - -# Second camera intrinsics (if using stereo) -# Camera2.fx: 500.0 -# Camera2.fy: 500.0 -# Camera2.cx: 320.0 -# Camera2.cy: 240.0 -# Camera2.k1: 0.0 -# Camera2.k2: 0.0 -# Camera2.p1: 0.0 -# Camera2.p2: 0.0 -# Camera2.k3: 0.0 - -# Stereo baseline * fx (for stereo systems) -# Camera.bf: 40.0 -# Or use baseline directly -# Stereo.b: 0.08 # Baseline in meters - -# Transformation from camera 1 to camera 2 (T_c1_c2) -# 4x4 transformation matrix -# Stereo.T_c1_c2: !!opencv-matrix -# rows: 4 -# cols: 4 -# dt: f -# data: [1.0, 0.0, 0.0, 0.08, -# 0.0, 1.0, 0.0, 0.0, -# 0.0, 0.0, 1.0, 0.0, -# 0.0, 0.0, 0.0, 1.0] - -# Depth threshold for stereo (baseline * threshold) -# Stereo.ThDepth: 40.0 - -#-------------------------------------------------------------------------------------------- -# RGB-D Parameters (for depth cameras) -#-------------------------------------------------------------------------------------------- - -# Depth map factor (converts depth values to meters) -# For TUM datasets: 5000.0 -# For real depth cameras: usually 1000.0 (if depth is in mm) -# For DepthAnything normalized output: 1.0 -Camera.bf: 0.0 # Set to 0 for monocular (required even if not used) -ThDepth: 40.0 # Depth threshold (required even if not used) -DepthMapFactor: 1.0 # Scale factor for depth values -RGBD.DepthMapFactor: 1.0 # Alternative depth map factor parameter - -#-------------------------------------------------------------------------------------------- -# IMU Parameters (for visual-inertial systems) -#-------------------------------------------------------------------------------------------- - -# IMU noise characteristics (from Allan variance analysis) -# These values are critical for accurate IMU integration - -# Gyroscope noise density (rad/s/sqrt(Hz)) -# Typical values: 1e-4 to 1e-3 for consumer IMUs, 1e-5 for high-end -IMU.NoiseGyro: 1.7e-4 - -# Accelerometer noise density (m/s^2/sqrt(Hz)) -# Typical values: 1e-3 to 1e-2 for consumer IMUs, 1e-4 for high-end -IMU.NoiseAcc: 2.0e-3 - -# Gyroscope random walk (rad/s^2/sqrt(Hz)) -# Describes bias drift over time -# Typical values: 1e-6 to 1e-4 -IMU.GyroWalk: 1.9e-5 - -# Accelerometer random walk (m/s^3/sqrt(Hz)) -# Describes bias drift over time -# Typical values: 1e-4 to 1e-2 -IMU.AccWalk: 3.0e-3 - -# IMU sampling frequency (Hz) -# Must match your IMU data rate -IMU.Frequency: 200 - -# Left camera to IMU transform (4x4) -Tbc: !!opencv-matrix - rows: 4 - cols: 4 - dt: f - data: [0.0757822598, -0.952608595, 0.294608068, 0.00524912371, - 0.553797745, -0.205488663, -0.806896813, 0.00758131847, - 0.829195457, 0.224301748, 0.511980097, -3.27803068e-05, - 0, 0, 0, 1] - -# Transformation from body (IMU) frame to camera frame (T_b_c1) -# This is the extrinsic calibration between IMU and camera -# Should be calibrated using tools like Kalibr -# Default: Identity matrix (assumes IMU and camera are aligned) -IMU.T_b_c1: !!opencv-matrix - rows: 4 - cols: 4 - dt: f - data: [0.0757822598, -0.952608595, 0.294608068, 0.00524912371, - 0.553797745, -0.205488663, -0.806896813, 0.00758131847, - 0.829195457, 0.224301748, 0.511980097, -3.27803068e-05, - 0, 0, 0, 1] - -# Insert keyframes when tracking is lost (IMU-inertial mode) -# 1 = true, 0 = false -IMU.InsertKFsWhenLost: 1 - -#-------------------------------------------------------------------------------------------- -# ORB Feature Extractor Parameters -#-------------------------------------------------------------------------------------------- - -# Number of ORB features per frame -# More features = better tracking but slower -# Typical values: 1000-2000 for monocular, 1000-3000 for stereo/RGB-D -ORBextractor.nFeatures: 1000 - -# Scale factor between pyramid levels -# Typical value: 1.2 -# Higher values = fewer levels needed but larger jumps in scale -ORBextractor.scaleFactor: 1.2 - -# Number of pyramid levels -# More levels = better scale invariance but slower -# Typical values: 8 -ORBextractor.nLevels: 8 - -# FAST detector threshold -# Initial threshold for FAST corner detection -# If not enough features found, threshold is reduced to minThFAST -ORBextractor.iniThFAST: 20 - -# Minimum FAST threshold -# Fallback threshold if iniThFAST doesn't find enough corners -ORBextractor.minThFAST: 7 - -#-------------------------------------------------------------------------------------------- -# Viewer Parameters (for ORB-SLAM3 visualization window) -#-------------------------------------------------------------------------------------------- - -# Keyframe visualization size -Viewer.KeyFrameSize: 0.05 - -# Keyframe line width (in visualization) -Viewer.KeyFrameLineWidth: 1.0 - -# Graph line width (covisibility graph) -Viewer.GraphLineWidth: 0.9 - -# Point size (map points) -Viewer.PointSize: 2.0 - -# Camera size (in visualization) -Viewer.CameraSize: 0.08 - -# Camera line width -Viewer.CameraLineWidth: 3.0 - -# Initial viewpoint position -Viewer.ViewpointX: 0.0 -Viewer.ViewpointY: -0.7 -Viewer.ViewpointZ: -1.8 - -# Viewpoint focal length (controls zoom) -Viewer.ViewpointF: 500.0 - -# Image viewer scale (optional, for resizing displayed images) -# Viewer.imageViewScale: 1.0 - -#-------------------------------------------------------------------------------------------- -# System Parameters -#-------------------------------------------------------------------------------------------- - -# Load existing map from file (optional) -# Provide path to .osa file to continue from previous session -# Leave as 0 to start fresh -System.LoadAtlasFromFile: 0 - -# Save map to file on shutdown (optional) -# Provide filename to save map, or 0 to not save -System.SaveAtlasToFile: 0 - -# Threshold for far points (optional) -# Points beyond this distance are considered outliers -# System.thFarPoints: 100.0 - -#-------------------------------------------------------------------------------------------- -# Advanced Parameters (usually not needed) -#-------------------------------------------------------------------------------------------- - -# Sequence name (for multi-session mapping) -# Seq.Name: "sequence_01" - -# Use loop closure (typically always enabled) -# System.loopClosureEnabled: 1 - -# Use local mapping (typically always enabled) -# System.localMappingEnabled: 1 - -#-------------------------------------------------------------------------------------------- -# Notes and Calibration Tips -#-------------------------------------------------------------------------------------------- - -# Camera Calibration: -# - Use tools like: ROS camera_calibration, OpenCV calibration, or MATLAB -# - Record checkerboard video, extract intrinsics and distortion -# - Accurate calibration is CRITICAL for good SLAM performance -# -# IMU Calibration: -# - Use Allan variance analysis for noise parameters -# - Use Kalibr for IMU-camera extrinsic calibration (T_b_c1) -# - Record static IMU data for several hours for Allan variance -# -# Parameter Tuning: -# - Start with these defaults -# - Adjust ORBextractor.nFeatures based on scene texture -# - Adjust IMU noise if you see drift or instability -# - Adjust FAST thresholds if tracking is lost frequently -# -# Debugging: -# - Enable viewer to see tracking status -# - Check that camera frame rate matches Camera.fps -# - Verify Camera.RGB matches your image encoding -# - Ensure depth factor is correct for your depth camera diff --git a/src/orb_slam3_ros2/config/webcamera.yaml b/src/orb_slam3_ros2/config/webcamera.yaml deleted file mode 100644 index e86c828..0000000 --- a/src/orb_slam3_ros2/config/webcamera.yaml +++ /dev/null @@ -1,93 +0,0 @@ -%YAML:1.0 - -#-------------------------------------------------------------------------------------------- -# Camera Parameters -#-------------------------------------------------------------------------------------------- -Camera.type: "PinHole" - -# Camera calibration (EXAMPLE - calibrate your camera!) -Camera.fx: 500.0 -Camera.fy: 500.0 -Camera.cx: 320.0 -Camera.cy: 240.0 - -# Distortion coefficients: (k1, k2, p1, p2, k3) -Camera.k1: 0.0 -Camera.k2: 0.0 -Camera.p1: 0.0 -Camera.p2: 0.0 -Camera.k3: 0.0 - -Camera.width: 640 -Camera.height: 480 -Camera.fps: 30.0 -Camera.RGB: 0 - -#-------------------------------------------------------------------------------------------- -# RGB-D Parameters (required when use_depth=true) -#-------------------------------------------------------------------------------------------- -# Camera baseline times fx (for stereo/RGB-D). Set to 0 for monocular -Camera.bf: 40.0 - -# Close/Far threshold. Baseline times -ThDepth: 40.0 - -# Depth map scale factor (convert depth values to meters) -# For DepthAnything output (normalized 0-1), use 1.0 -# For typical depth cameras: 1000.0 (if depth is in mm), 5000.0 (TUM/KITTI format) -DepthMapFactor: 1.0 - -#-------------------------------------------------------------------------------------------- -# IMU Parameters (required for monocular-inertial mode) -#-------------------------------------------------------------------------------------------- -IMU.NoiseGyro: 1.7e-4 -IMU.NoiseAcc: 2.0e-3 -IMU.GyroWalk: 1.9e-5 -IMU.AccWalk: 3.0e-3 -IMU.Frequency: 20 - -# Left camera to IMU transform (4x4) -Tbc: !!opencv-matrix - rows: 4 - cols: 4 - dt: f - data: [0.0757822598, -0.952608595, 0.294608068, 0.00524912371, - 0.553797745, -0.205488663, -0.806896813, 0.00758131847, - 0.829195457, 0.224301748, 0.511980097, -3.27803068e-05, - 0, 0, 0, 1] - -# Keep both keys for compatibility across ORB-SLAM3 forks -IMU.T_b_c1: !!opencv-matrix - rows: 4 - cols: 4 - dt: f - data: [0.0757822598, -0.952608595, 0.294608068, 0.00524912371, - 0.553797745, -0.205488663, -0.806896813, 0.00758131847, - 0.829195457, 0.224301748, 0.511980097, -3.27803068e-05, - 0, 0, 0, 1] - -InsertKFsWhenLost: 1 -IMU.InsertKFsWhenLost: 1 - -#-------------------------------------------------------------------------------------------- -# ORB Parameters -#-------------------------------------------------------------------------------------------- -ORBextractor.nFeatures: 1000 -ORBextractor.scaleFactor: 1.2 -ORBextractor.nLevels: 8 -ORBextractor.iniThFAST: 20 -ORBextractor.minThFAST: 7 - -#-------------------------------------------------------------------------------------------- -# Viewer Parameters -#-------------------------------------------------------------------------------------------- -Viewer.KeyFrameSize: 0.05 -Viewer.KeyFrameLineWidth: 1.0 -Viewer.GraphLineWidth: 0.9 -Viewer.PointSize: 2.0 -Viewer.CameraSize: 0.08 -Viewer.CameraLineWidth: 3.0 -Viewer.ViewpointX: 0.0 -Viewer.ViewpointY: -0.7 -Viewer.ViewpointZ: -1.8 -Viewer.ViewpointF: 500.0 diff --git a/src/orb_slam3_ros2/launch/orb_slam_sim_launch.py b/src/orb_slam3_ros2/launch/orb_slam_sim_launch.py deleted file mode 100644 index 1a21eb9..0000000 --- a/src/orb_slam3_ros2/launch/orb_slam_sim_launch.py +++ /dev/null @@ -1,275 +0,0 @@ -from launch_ros.actions import Node -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.substitutions import FindPackageShare -import os -from ament_index_python.packages import get_package_share_directory - - -def generate_launch_description(): - # Get package directory - orb_slam_share = get_package_share_directory("orb_slam3_ros2") - - # Declare launch arguments - use_viewer = LaunchConfiguration("use_viewer") - use_imu = LaunchConfiguration("use_imu") - use_depth = LaunchConfiguration("use_depth") - image_topic = LaunchConfiguration("image_topic") - depth_topic = LaunchConfiguration("depth_topic") - imu_topic = LaunchConfiguration("imu_topic") - world_frame_id = LaunchConfiguration("world_frame_id") - camera_frame_id = LaunchConfiguration("camera_frame_id") - settings_file = LaunchConfiguration("settings_file") - use_usb_cam = LaunchConfiguration("use_usb_cam") - use_vectornav = LaunchConfiguration("use_vectornav") - usb_cam_video_device = LaunchConfiguration("usb_cam_video_device") - usb_cam_framerate = LaunchConfiguration("usb_cam_framerate") - usb_cam_io_method = LaunchConfiguration("usb_cam_io_method") - usb_cam_pixel_format = LaunchConfiguration("usb_cam_pixel_format") - usb_cam_image_width = LaunchConfiguration("usb_cam_image_width") - usb_cam_image_height = LaunchConfiguration("usb_cam_image_height") - usb_cam_camera_info_url = LaunchConfiguration("usb_cam_camera_info_url") - usb_cam_camera_name = LaunchConfiguration("usb_cam_camera_name") - usb_cam_brightness = LaunchConfiguration("usb_cam_brightness") - usb_cam_contrast = LaunchConfiguration("usb_cam_contrast") - usb_cam_saturation = LaunchConfiguration("usb_cam_saturation") - usb_cam_sharpness = LaunchConfiguration("usb_cam_sharpness") - usb_cam_gain = LaunchConfiguration("usb_cam_gain") - usb_cam_auto_white_balance = LaunchConfiguration("usb_cam_auto_white_balance") - usb_cam_white_balance = LaunchConfiguration("usb_cam_white_balance") - usb_cam_autoexposure = LaunchConfiguration("usb_cam_autoexposure") - usb_cam_exposure = LaunchConfiguration("usb_cam_exposure") - usb_cam_autofocus = LaunchConfiguration("usb_cam_autofocus") - usb_cam_focus = LaunchConfiguration("usb_cam_focus") - - # Load ORB-SLAM3 settings - default_settings_file = os.path.join(orb_slam_share, "config", "stonefish_hydrus.yaml") - vocabulary_file = os.path.join(orb_slam_share, "vendor", "ORBvoc.txt") - - return LaunchDescription([ - # Launch arguments - DeclareLaunchArgument( - "use_viewer", - default_value="false", - description="Enable ORB-SLAM3 viewer" - ), - DeclareLaunchArgument( - "use_imu", - default_value="true", - description="Enable IMU-inertial SLAM mode" - ), - DeclareLaunchArgument( - "use_depth", - default_value="true", - description="Enable RGB-D SLAM mode" - ), - DeclareLaunchArgument( - "image_topic", - default_value="/hydrus/rgb_camera/image_raw", - description="Image topic consumed by ORB-SLAM3" - ), - DeclareLaunchArgument( - "depth_topic", - default_value="/hydrus/depth_camera/image_raw", - description="Depth topic consumed by ORB-SLAM3" - ), - DeclareLaunchArgument( - "imu_topic", - default_value="/hydrus/imu", - description="IMU topic consumed by ORB-SLAM3" - ), - DeclareLaunchArgument( - "world_frame_id", - default_value="world", - description="World frame used by ORB-SLAM3" - ), - DeclareLaunchArgument( - "camera_frame_id", - default_value="hydrus_camera", - description="Camera frame used by ORB-SLAM3" - ), - DeclareLaunchArgument( - "settings_file", - default_value=default_settings_file, - description="ORB-SLAM3 settings YAML path" - ), - DeclareLaunchArgument( - "use_usb_cam", - default_value="false", - description="Start usb_cam node in this launch" - ), - DeclareLaunchArgument( - "use_vectornav", - default_value="false", - description="Start vectornav nodes in this launch" - ), - DeclareLaunchArgument( - "usb_cam_video_device", - default_value="/dev/video0", - description="V4L2 device path for usb_cam" - ), - DeclareLaunchArgument( - "usb_cam_framerate", - default_value="30.0", - description="Camera output frame rate (Hz)" - ), - DeclareLaunchArgument( - "usb_cam_io_method", - default_value="mmap", - description="usb_cam io method (mmap/read/userptr)" - ), - DeclareLaunchArgument( - "usb_cam_pixel_format", - default_value="mjpeg2rgb", - description="usb_cam pixel format" - ), - DeclareLaunchArgument( - "usb_cam_image_width", - default_value="640", - description="usb_cam image width" - ), - DeclareLaunchArgument( - "usb_cam_image_height", - default_value="480", - description="usb_cam image height" - ), - DeclareLaunchArgument( - "usb_cam_camera_info_url", - default_value="", - description="CameraInfo URL (e.g. file:///tmp/camera_info.yaml)" - ), - DeclareLaunchArgument( - "usb_cam_camera_name", - default_value="laptop_webcam", - description="Camera name published in CameraInfo" - ), - DeclareLaunchArgument( - "usb_cam_brightness", - default_value="-1", - description="usb_cam brightness (-1 leaves driver default)" - ), - DeclareLaunchArgument( - "usb_cam_contrast", - default_value="-1", - description="usb_cam contrast (-1 leaves driver default)" - ), - DeclareLaunchArgument( - "usb_cam_saturation", - default_value="-1", - description="usb_cam saturation (-1 leaves driver default)" - ), - DeclareLaunchArgument( - "usb_cam_sharpness", - default_value="-1", - description="usb_cam sharpness (-1 leaves driver default)" - ), - DeclareLaunchArgument( - "usb_cam_gain", - default_value="-1", - description="usb_cam gain (-1 leaves driver default)" - ), - DeclareLaunchArgument( - "usb_cam_auto_white_balance", - default_value="true", - description="Enable automatic white balance" - ), - DeclareLaunchArgument( - "usb_cam_white_balance", - default_value="4000", - description="Manual white balance value" - ), - DeclareLaunchArgument( - "usb_cam_autoexposure", - default_value="true", - description="Enable automatic exposure" - ), - DeclareLaunchArgument( - "usb_cam_exposure", - default_value="100", - description="Manual exposure value" - ), - DeclareLaunchArgument( - "usb_cam_autofocus", - default_value="false", - description="Enable autofocus" - ), - DeclareLaunchArgument( - "usb_cam_focus", - default_value="-1", - description="Manual focus value" - ), - - IncludeLaunchDescription( - PythonLaunchDescriptionSource( - PathJoinSubstitution([ - FindPackageShare("vectornav"), - "launch", - "vectornav.launch.py", - ]) - ), - condition=IfCondition(use_vectornav), - ), - - Node( - package="usb_cam", - executable="usb_cam_node_exe", - name="usb_cam", - namespace="usb_cam", - output="screen", - parameters=[{ - "video_device": usb_cam_video_device, - "framerate": usb_cam_framerate, - "io_method": usb_cam_io_method, - "frame_id": camera_frame_id, - "pixel_format": usb_cam_pixel_format, - "camera_name": usb_cam_camera_name, - "image_width": usb_cam_image_width, - "image_height": usb_cam_image_height, - "camera_info_url": usb_cam_camera_info_url, - "brightness": usb_cam_brightness, - "contrast": usb_cam_contrast, - "saturation": usb_cam_saturation, - "sharpness": usb_cam_sharpness, - "gain": usb_cam_gain, - "auto_white_balance": usb_cam_auto_white_balance, - "white_balance": usb_cam_white_balance, - "autoexposure": usb_cam_autoexposure, - "exposure": usb_cam_exposure, - "autofocus": usb_cam_autofocus, - "focus": usb_cam_focus, - }], - condition=IfCondition(use_usb_cam), - ), - - # ORB-SLAM3 Node - Node( - package="orb_slam3_ros2", - executable="orb_slam_node", - name="orb_slam3_node", - output="screen", - parameters=[ - { - "vocabulary_path": vocabulary_file, - "settings_path": settings_file, - "use_viewer": use_viewer, - "use_imu": use_imu, - "use_depth": use_depth, - "image_topic": image_topic, - "depth_topic": depth_topic, - "imu_topic": imu_topic, - # Output topics - "pose_topic": "orb_slam3/camera_pose", - "odom_topic": "orb_slam3/camera_odom", - "path_topic": "orb_slam3/camera_path", - # Frame IDs - "world_frame_id": world_frame_id, - "camera_frame_id": camera_frame_id, - "queue_size": 10, - "publish_tf": True, - } - ] - ), - ]) diff --git a/src/orb_slam3_ros2/package.xml b/src/orb_slam3_ros2/package.xml deleted file mode 100644 index 072fedf..0000000 --- a/src/orb_slam3_ros2/package.xml +++ /dev/null @@ -1,31 +0,0 @@ - - - - orb_slam3_ros2 - 1.0.0 - ROS2 monocular wrapper for ORB-SLAM3 - - Your Name - GPLv3 - - ament_cmake - - rclcpp - std_msgs - sensor_msgs - geometry_msgs - nav_msgs - tf2 - tf2_ros - tf2_geometry_msgs - cv_bridge - image_transport - message_filters - - ament_lint_auto - ament_lint_common - - - ament_cmake - - \ No newline at end of file diff --git a/src/orb_slam3_ros2/src/orb_slam_node.cpp b/src/orb_slam3_ros2/src/orb_slam_node.cpp deleted file mode 100644 index 4e69406..0000000 --- a/src/orb_slam3_ros2/src/orb_slam_node.cpp +++ /dev/null @@ -1,485 +0,0 @@ -/** - * ROS2 SLAM Node for ORB-SLAM3 - * - * Configurable sensor modes: - * - MONOCULAR mode (default) - * - RGB-D mode (if use_depth parameter is true) - * - * Publishes: - * - Camera pose (geometry_msgs/PoseStamped) - * - Camera odometry (nav_msgs/Odometry) - * - Camera trajectory path (nav_msgs/Path) - * - TF transforms (camera pose in world frame) - */ - -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include - -#include -#include - -#include -#include - -#include - -#include "System.h" -#include "ImuTypes.h" - -using namespace std; - -class SlamNode : public rclcpp::Node -{ -public: - SlamNode() - : Node("orb_slam3_node") - { - // Declare and get ROS parameters - this->declare_parameter("vocabulary_path", ""); - this->declare_parameter("settings_path", ""); - this->declare_parameter("use_viewer", true); - this->declare_parameter("use_depth", false); // Enable RGB-D mode - this->declare_parameter("use_imu", false); // Enable IMU-Inertial mode - - this->declare_parameter("image_topic", "/camera/image_raw"); - this->declare_parameter("depth_topic", "/camera/depth/image_raw"); - this->declare_parameter("imu_topic", "/imu"); - this->declare_parameter("pose_topic", "orb_slam3/camera_pose"); - this->declare_parameter("odom_topic", "orb_slam3/camera_odom"); - this->declare_parameter("path_topic", "orb_slam3/camera_path"); - this->declare_parameter("world_frame_id", "world"); - this->declare_parameter("camera_frame_id", "camera"); - this->declare_parameter("queue_size", 10); - this->declare_parameter("publish_tf", true); - - // Get parameters - std::string vocabulary_path = this->get_parameter("vocabulary_path").as_string(); - std::string settings_path = this->get_parameter("settings_path").as_string(); - bool use_viewer = this->get_parameter("use_viewer").as_bool(); - use_depth_ = this->get_parameter("use_depth").as_bool(); - use_imu_ = this->get_parameter("use_imu").as_bool(); - - std::string image_topic = this->get_parameter("image_topic").as_string(); - std::string depth_topic = this->get_parameter("depth_topic").as_string(); - std::string imu_topic = this->get_parameter("imu_topic").as_string(); - std::string pose_topic = this->get_parameter("pose_topic").as_string(); - std::string odom_topic = this->get_parameter("odom_topic").as_string(); - std::string path_topic = this->get_parameter("path_topic").as_string(); - world_frame_id_ = this->get_parameter("world_frame_id").as_string(); - camera_frame_id_ = this->get_parameter("camera_frame_id").as_string(); - int queue_size = this->get_parameter("queue_size").as_int(); - publish_tf_ = this->get_parameter("publish_tf").as_bool(); - - // Validate required parameters - if (vocabulary_path.empty()) { - RCLCPP_ERROR(this->get_logger(), "Parameter 'vocabulary_path' is required!"); - throw std::runtime_error("Missing vocabulary_path parameter"); - } - if (settings_path.empty()) { - RCLCPP_ERROR(this->get_logger(), "Parameter 'settings_path' is required!"); - throw std::runtime_error("Missing settings_path parameter"); - } - - // Determine sensor mode - ORB_SLAM3::System::eSensor sensor_mode; - if (use_depth_) { - sensor_mode = ORB_SLAM3::System::RGBD; - RCLCPP_INFO(this->get_logger(), "=== RGB-D SLAM MODE ==="); - } else if (use_imu_) { - sensor_mode = ORB_SLAM3::System::IMU_MONOCULAR; - RCLCPP_INFO(this->get_logger(), "=== MONOCULAR-INERTIAL SLAM MODE ==="); - } else { - sensor_mode = ORB_SLAM3::System::MONOCULAR; - RCLCPP_INFO(this->get_logger(), "=== MONOCULAR SLAM MODE ==="); - } - - RCLCPP_INFO(this->get_logger(), "Vocabulary: %s", vocabulary_path.c_str()); - RCLCPP_INFO(this->get_logger(), "Settings: %s", settings_path.c_str()); - RCLCPP_INFO(this->get_logger(), "Use viewer: %s", use_viewer ? "true" : "false"); - RCLCPP_INFO(this->get_logger(), "Image topic: %s", image_topic.c_str()); - if (use_depth_) { - RCLCPP_INFO(this->get_logger(), "Depth topic: %s", depth_topic.c_str()); - } - if (use_imu_) { - RCLCPP_INFO(this->get_logger(), "IMU topic: %s", imu_topic.c_str()); - } - RCLCPP_INFO(this->get_logger(), "Pose topic: %s", pose_topic.c_str()); - RCLCPP_INFO(this->get_logger(), "Odom topic: %s", odom_topic.c_str()); - RCLCPP_INFO(this->get_logger(), "Path topic: %s", path_topic.c_str()); - - // Initialize ORB-SLAM3 - RCLCPP_INFO(this->get_logger(), "Initializing ORB-SLAM3..."); - slam_system_ = std::make_shared( - vocabulary_path, - settings_path, - sensor_mode, - use_viewer - ); - RCLCPP_INFO(this->get_logger(), "ORB-SLAM3 initialized!"); - - // Create subscribers - image_sub_ = this->create_subscription( - image_topic, - queue_size, - std::bind(&SlamNode::imageCallback, this, std::placeholders::_1) - ); - - if (use_depth_) { - depth_sub_ = this->create_subscription( - depth_topic, - queue_size, - std::bind(&SlamNode::depthCallback, this, std::placeholders::_1) - ); - } - - if (use_imu_) { - imu_sub_ = this->create_subscription( - imu_topic, - queue_size, - std::bind(&SlamNode::imuCallback, this, std::placeholders::_1) - ); - } - - // Create publishers - pose_pub_ = this->create_publisher(pose_topic, queue_size); - path_pub_ = this->create_publisher(path_topic, queue_size); - odom_pub_ = this->create_publisher(odom_topic, queue_size); - - // TF broadcaster - if (publish_tf_) { - tf_broadcaster_ = std::make_shared(this); - } - - // Initialize path message - path_msg_.header.frame_id = world_frame_id_; - - RCLCPP_INFO(this->get_logger(), "SLAM node ready!"); - } - - ~SlamNode() - { - if (slam_system_) { - RCLCPP_INFO(this->get_logger(), "Shutting down ORB-SLAM3..."); - slam_system_->Shutdown(); - - if (has_valid_pose_.load()) { - slam_system_->SaveKeyFrameTrajectoryTUM("KeyFrameTrajectory.txt"); - slam_system_->SaveTrajectoryTUM("CameraTrajectory.txt"); - RCLCPP_INFO(this->get_logger(), "Trajectories saved!"); - } else { - RCLCPP_WARN(this->get_logger(), "Skipping trajectory export: no valid poses were tracked."); - } - } - } - -private: - void imuCallback(const sensor_msgs::msg::Imu::SharedPtr imu_msg) - { - std::lock_guard lock(imu_mutex_); - - // Buffer IMU messages for preintegration between frames - imu_buffer_.push_back(imu_msg); - } - - void depthCallback(const sensor_msgs::msg::Image::SharedPtr depth_msg) - { - std::lock_guard lock(depth_mutex_); - - // Store latest depth image - try { - latest_depth_ = cv_bridge::toCvCopy(depth_msg, sensor_msgs::image_encodings::TYPE_32FC1); - } catch (cv_bridge::Exception& e) { - RCLCPP_WARN(this->get_logger(), "Depth cv_bridge exception: %s", e.what()); - } - } - - void imageCallback(const sensor_msgs::msg::Image::SharedPtr msg) - { - // Convert ROS image to OpenCV - cv_bridge::CvImagePtr cv_ptr; - try { - cv_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::MONO8); - } catch (cv_bridge::Exception& e) { - RCLCPP_ERROR(this->get_logger(), "cv_bridge exception: %s", e.what()); - return; - } - - // Get timestamp - double timestamp = msg->header.stamp.sec + msg->header.stamp.nanosec * 1e-9; - if (last_image_timestamp_ > 0.0 && timestamp <= last_image_timestamp_) { - RCLCPP_WARN_THROTTLE( - this->get_logger(), - *this->get_clock(), - 2000, - "Dropping non-increasing image timestamp: %.9f (last %.9f)", - timestamp, - last_image_timestamp_ - ); - return; - } - - Sophus::SE3f Tcw_se3; - - try { - // Use appropriate tracking mode - if (use_depth_) { - std::lock_guard lock(depth_mutex_); - - if (latest_depth_) { - // RGB-D tracking - Tcw_se3 = slam_system_->TrackRGBD(cv_ptr->image, latest_depth_->image, timestamp); - } else { - RCLCPP_WARN_THROTTLE(this->get_logger(), *this->get_clock(), 1000, - "RGB-D mode enabled but no depth data received yet"); - return; - } - } else if (use_imu_) { - // IMU-Monocular tracking with timestamp-aligned preintegration - std::vector vImuMeas; - - { - std::lock_guard lock(imu_mutex_); - - while (!imu_buffer_.empty()) { - const auto& imu_msg = imu_buffer_.front(); - double imu_timestamp = imu_msg->header.stamp.sec + imu_msg->header.stamp.nanosec * 1e-9; - - if (imu_timestamp > timestamp) { - break; - } - - ORB_SLAM3::IMU::Point imu_point( - imu_msg->linear_acceleration.x, - imu_msg->linear_acceleration.y, - imu_msg->linear_acceleration.z, - imu_msg->angular_velocity.x, - imu_msg->angular_velocity.y, - imu_msg->angular_velocity.z, - imu_timestamp - ); - vImuMeas.push_back(imu_point); - imu_buffer_.pop_front(); - } - } - - if (vImuMeas.empty()) { - RCLCPP_WARN_THROTTLE(this->get_logger(), *this->get_clock(), 1000, - "Waiting for IMU samples at or before image timestamp"); - return; - } - - Tcw_se3 = slam_system_->TrackMonocular(cv_ptr->image, timestamp, vImuMeas); - - RCLCPP_INFO_THROTTLE(this->get_logger(), *this->get_clock(), 1000, - "Using %zu IMU measurements for tracking", vImuMeas.size()); - } else { - // Monocular tracking (no IMU) - Tcw_se3 = slam_system_->TrackMonocular(cv_ptr->image, timestamp); - } - } catch (const cv::Exception& e) { - RCLCPP_ERROR(this->get_logger(), "ORB-SLAM OpenCV exception: %s", e.what()); - return; - } catch (const std::exception& e) { - RCLCPP_ERROR(this->get_logger(), "ORB-SLAM exception: %s", e.what()); - return; - } - - last_image_timestamp_ = timestamp; - - // Check if tracking succeeded - const float eps = 1e-6f; - if (Tcw_se3.translation().norm() < eps && - Tcw_se3.rotationMatrix().isApprox(Eigen::Matrix3f::Identity(), 1e-6f)) { - return; // Tracking failed - } - - // Convert Sophus::SE3f to cv::Mat - cv::Mat Tcw = cv::Mat::eye(4, 4, CV_32F); - Eigen::Matrix3f R = Tcw_se3.rotationMatrix(); - Eigen::Vector3f t = Tcw_se3.translation(); - for (int r = 0; r < 3; ++r) - for (int c = 0; c < 3; ++c) - Tcw.at(r, c) = R(r, c); - Tcw.at(0, 3) = t(0); - Tcw.at(1, 3) = t(1); - Tcw.at(2, 3) = t(2); - - // Publish pose - has_valid_pose_.store(true); - publishPose(Tcw, msg->header.stamp); - } - - void publishPose(const cv::Mat& Tcw, const builtin_interfaces::msg::Time& timestamp) - { - // Convert camera pose to world frame - cv::Mat Rwc = Tcw.rowRange(0,3).colRange(0,3).t(); - cv::Mat twc = -Rwc * Tcw.rowRange(0,3).col(3); - - // Create pose message - geometry_msgs::msg::PoseStamped pose_msg; - pose_msg.header.stamp = timestamp; - pose_msg.header.frame_id = world_frame_id_; - - pose_msg.pose.position.x = twc.at(0); - pose_msg.pose.position.y = twc.at(1); - pose_msg.pose.position.z = twc.at(2); - - // Convert rotation to quaternion - Eigen::Matrix3f rotation_matrix; - rotation_matrix << Rwc.at(0,0), Rwc.at(0,1), Rwc.at(0,2), - Rwc.at(1,0), Rwc.at(1,1), Rwc.at(1,2), - Rwc.at(2,0), Rwc.at(2,1), Rwc.at(2,2); - - Eigen::Quaternionf q(rotation_matrix); - pose_msg.pose.orientation.x = q.x(); - pose_msg.pose.orientation.y = q.y(); - pose_msg.pose.orientation.z = q.z(); - pose_msg.pose.orientation.w = q.w(); - - // Publish - pose_pub_->publish(pose_msg); - - // Add to path - path_msg_.header.stamp = timestamp; - path_msg_.poses.push_back(pose_msg); - path_pub_->publish(path_msg_); - - // Publish odometry - nav_msgs::msg::Odometry odom_msg; - odom_msg.header.stamp = timestamp; - odom_msg.header.frame_id = world_frame_id_; - odom_msg.child_frame_id = camera_frame_id_; - - // Set pose - odom_msg.pose.pose = pose_msg.pose; - - // Compute velocity if we have a previous pose - if (last_pose_time_ > 0.0) { - double current_time = timestamp.sec + timestamp.nanosec * 1e-9; - double dt = current_time - last_pose_time_; - - if (dt > 0.0 && dt < 1.0) { // Only compute velocity if dt is reasonable - // Linear velocity - odom_msg.twist.twist.linear.x = (twc.at(0) - last_position_[0]) / dt; - odom_msg.twist.twist.linear.y = (twc.at(1) - last_position_[1]) / dt; - odom_msg.twist.twist.linear.z = (twc.at(2) - last_position_[2]) / dt; - - // Angular velocity (simplified - compute from quaternion difference) - Eigen::Quaternionf q_current(q); - Eigen::Quaternionf q_last(last_orientation_[3], last_orientation_[0], - last_orientation_[1], last_orientation_[2]); - Eigen::Quaternionf q_diff = q_current * q_last.inverse(); - - // Convert quaternion to angular velocity - Eigen::AngleAxisf aa(q_diff); - Eigen::Vector3f angular_vel = aa.axis() * aa.angle() / dt; - - odom_msg.twist.twist.angular.x = angular_vel.x(); - odom_msg.twist.twist.angular.y = angular_vel.y(); - odom_msg.twist.twist.angular.z = angular_vel.z(); - } - } - - // Store current pose for next iteration - last_pose_time_ = timestamp.sec + timestamp.nanosec * 1e-9; - last_position_[0] = twc.at(0); - last_position_[1] = twc.at(1); - last_position_[2] = twc.at(2); - last_orientation_[0] = q.x(); - last_orientation_[1] = q.y(); - last_orientation_[2] = q.z(); - last_orientation_[3] = q.w(); - - odom_pub_->publish(odom_msg); - - // Publish TF - if (publish_tf_) { - geometry_msgs::msg::TransformStamped transform; - transform.header.stamp = timestamp; - transform.header.frame_id = world_frame_id_; - transform.child_frame_id = camera_frame_id_; - transform.transform.translation.x = twc.at(0); - transform.transform.translation.y = twc.at(1); - transform.transform.translation.z = twc.at(2); - transform.transform.rotation = pose_msg.pose.orientation; - - tf_broadcaster_->sendTransform(transform); - } - } - - // ORB-SLAM3 - std::shared_ptr slam_system_; - - // ROS2 subscribers - rclcpp::Subscription::SharedPtr image_sub_; - rclcpp::Subscription::SharedPtr depth_sub_; - rclcpp::Subscription::SharedPtr imu_sub_; - - // ROS2 publishers - rclcpp::Publisher::SharedPtr pose_pub_; - rclcpp::Publisher::SharedPtr path_pub_; - rclcpp::Publisher::SharedPtr odom_pub_; - std::shared_ptr tf_broadcaster_; - - // Path - nav_msgs::msg::Path path_msg_; - - // Parameters - std::string world_frame_id_; - std::string camera_frame_id_; - bool publish_tf_; - bool use_depth_; - bool use_imu_; - - // Odometry tracking for velocity computation - double last_pose_time_ = 0.0; - float last_position_[3] = {0.0f, 0.0f, 0.0f}; - float last_orientation_[4] = {0.0f, 0.0f, 0.0f, 1.0f}; // x, y, z, w - - // Depth data - cv_bridge::CvImagePtr latest_depth_; - std::mutex depth_mutex_; - - // IMU data buffer for preintegration - std::deque imu_buffer_; - std::mutex imu_mutex_; - - double last_image_timestamp_ = 0.0; - std::atomic has_valid_pose_{false}; -}; - -int main(int argc, char** argv) -{ - rclcpp::init(argc, argv); - - try { - auto node = std::make_shared(); - rclcpp::spin(node); - } catch (const std::exception& e) { - RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), "Exception: %s", e.what()); - rclcpp::shutdown(); - return 1; - } - - rclcpp::shutdown(); - return 0; -} diff --git a/src/zed_custom_wrapper/COLCON_IGNORE b/src/zed_custom_wrapper/COLCON_IGNORE new file mode 100644 index 0000000..e69de29 diff --git a/tools/kalibr/README.md b/tools/kalibr/README.md deleted file mode 100644 index 495b5a1..0000000 --- a/tools/kalibr/README.md +++ /dev/null @@ -1,182 +0,0 @@ -# Kalibr Workflow (Docker on Ubuntu 24.04 + ROS 2 Jazzy) - -This workspace uses Kalibr through Docker because native Kalibr is ROS1/catkin-based. - -## Installed - -Kalibr image installed locally: - -- `stereolabs/kalibr:latest` - -Wrapper script: - -- `tools/kalibr/run_kalibr.sh` - -## 0) Make wrapper executable - -```bash -chmod +x tools/kalibr/run_kalibr.sh -chmod +x tools/kalibr/extract_orbslam_tbc.py -``` - -## 1) Record calibration data - -### Camera intrinsics bag - -Record only camera topic while moving AprilGrid through FOV: - -```bash -ros2 bag record /camera/image_raw -o bags/cam_intrinsics -``` - -Convert to ROS1 bag if needed for Kalibr (Kalibr image expects ROS1 bag format). - -### IMU + camera bag for extrinsics - -Record synchronized camera + IMU while doing rich 6-DoF motion: - -```bash -ros2 bag record /camera/image_raw /imu/data -o bags/imucam -``` - -Tips: - -- Use 2-3 minutes of motion with rotations around all axes. -- Keep the calibration target visible often. -- Avoid motion blur. - -## 2) Calibrate camera intrinsics - -Use your AprilGrid config (example provided): - -- `tools/kalibr/config/aprilgrid_6x6_80x30.yaml` - -Run: - -```bash -./tools/kalibr/run_kalibr.sh kalibr_calibrate_cameras \ - --bag /work/bags/cam_intrinsics.bag \ - --topics /camera/image_raw \ - --models pinhole-radtan \ - --target /work/tools/kalibr/config/aprilgrid_6x6_80x30.yaml -``` - -Outputs include a `camchain-*.yaml` file. - -## 3) Estimate IMU noise constants (Allan variance) - -Kalibr needs an `imu.yaml` with real noise constants. - -Use a long static IMU recording (at least 30 minutes, ideally 2+ hours), then compute: - -- `accelerometer_noise_density` -- `accelerometer_random_walk` -- `gyroscope_noise_density` -- `gyroscope_random_walk` - -Template: - -- `tools/kalibr/config/imu.yaml.template` - -Save your filled file as `tools/kalibr/config/imu.yaml`. - -## 4) Calibrate IMU-camera extrinsics (T_b_c1 / Tbc) - -Run Kalibr IMU-camera calibration with: - -```bash -./tools/kalibr/run_kalibr.sh kalibr_calibrate_imu_camera \ - --bag /work/bags/imucam.bag \ - --cam /work/camchain-cam_intrinsics.yaml \ - --imu /work/tools/kalibr/config/imu.yaml \ - --target /work/tools/kalibr/config/aprilgrid_6x6_80x30.yaml -``` - -Main output: `camchain-imucam-*.yaml` - -## 5) Convert to ORB-SLAM matrix block - -Extract `T_cam_imu` and print ORB-SLAM-ready matrix: - -```bash -python3 tools/kalibr/extract_orbslam_tbc.py camchain-imucam.yaml -``` - -Then copy the printed matrix into both keys in your ORB-SLAM config: - -- `IMU.T_b_c1` -- `Tbc` - -In your file: - -- `src/orb_slam3_ros2/config/webcamera.yaml` - -## Notes - -- If your ROS2 recording is not directly readable by Kalibr, convert your ROS2 bag to ROS1 bag format before running Kalibr. -- Ensure topic names in `imu.yaml` and command-line args exactly match your bag. - -## VectorNav + Laptop Webcam Quickstart - -This is a practical recipe for calibrating a VectorNav IMU with a laptop webcam. - -1. Start the sensors - -Typical commands (adjust to your setup): - -```bash -ros2 launch vectornav vectornav.launch.py -ros2 run usb_cam usb_cam_node_exe --ros-args -p video_device:=/dev/video0 -``` - -2. Verify real topic names and rates - -```bash -ros2 topic list | grep -E 'vectornav|imu|usb_cam|image_raw' -ros2 topic hz /vectornav/imu -ros2 topic hz /usb_cam/image_raw -``` - -If your IMU topic is different, use that exact topic in the IMU yaml. - -3. Record calibration bags - -```bash -ros2 bag record /usb_cam/image_raw -o bags/cam_intrinsics -ros2 bag record /usb_cam/image_raw /vectornav/imu -o bags/imucam -``` - -4. Use VectorNav IMU template - -Start from: - -- `tools/kalibr/config/imu_vectornav.yaml.template` - -Copy it to `tools/kalibr/config/imu.yaml` and replace all four noise/random-walk constants with Allan-variance results from your own IMU. - -5. Run calibrations (same commands as above) - -```bash -bash tools/kalibr/run_kalibr.sh kalibr_calibrate_cameras \ - --bag /work/bags/cam_intrinsics.bag \ - --topics /usb_cam/image_raw \ - --models pinhole-radtan \ - --target /work/tools/kalibr/config/aprilgrid_6x6_80x30.yaml - -bash tools/kalibr/run_kalibr.sh kalibr_calibrate_imu_camera \ - --bag /work/bags/imucam.bag \ - --cam /work/camchain-cam_intrinsics.yaml \ - --imu /work/tools/kalibr/config/imu.yaml \ - --target /work/tools/kalibr/config/aprilgrid_6x6_80x30.yaml -``` - -6. Update ORB-SLAM matrix block - -```bash -python3 tools/kalibr/extract_orbslam_tbc.py camchain-imucam.yaml -``` - -Then paste into: - -- `IMU.T_b_c1` in `src/orb_slam3_ros2/config/webcamera.yaml` -- `Tbc` in `src/orb_slam3_ros2/config/webcamera.yaml` diff --git a/tools/kalibr/config/aprilgrid_6x6_80x30.yaml b/tools/kalibr/config/aprilgrid_6x6_80x30.yaml deleted file mode 100644 index 0f8633a..0000000 --- a/tools/kalibr/config/aprilgrid_6x6_80x30.yaml +++ /dev/null @@ -1,6 +0,0 @@ -target_type: aprilgrid - -tagCols: 6 -tagRows: 6 -tagSize: 0.08 -tagSpacing: 0.30 diff --git a/tools/kalibr/config/camchain_seed.yaml b/tools/kalibr/config/camchain_seed.yaml deleted file mode 100644 index 292fa4e..0000000 --- a/tools/kalibr/config/camchain_seed.yaml +++ /dev/null @@ -1,7 +0,0 @@ -cam0: - camera_model: pinhole - distortion_model: radtan - intrinsics: [1000.0, 1000.0, 640.0, 360.0] - distortion_coeffs: [0.0, 0.0, 0.0, 0.0] - resolution: [1280, 720] - rostopic: /image_raw diff --git a/tools/kalibr/config/imu.yaml b/tools/kalibr/config/imu.yaml deleted file mode 100644 index aa6a0b8..0000000 --- a/tools/kalibr/config/imu.yaml +++ /dev/null @@ -1,8 +0,0 @@ -rostopic: /vectornav/imu -update_rate: 20.0 - -# Replace with Allan-variance-derived constants for production use. -accelerometer_noise_density: 0.0020 -accelerometer_random_walk: 0.0030 -gyroscope_noise_density: 0.00017 -gyroscope_random_walk: 0.000019 diff --git a/tools/kalibr/extract_orbslam_tbc.py b/tools/kalibr/extract_orbslam_tbc.py deleted file mode 100644 index 45b3ebc..0000000 --- a/tools/kalibr/extract_orbslam_tbc.py +++ /dev/null @@ -1,42 +0,0 @@ -#!/usr/bin/env python3 -import argparse -import yaml - - -def fmt_row(values): - return ", ".join(f"{float(v):.9g}" for v in values) - - -def main(): - parser = argparse.ArgumentParser(description="Extract T_cam_imu from Kalibr camchain and print ORB-SLAM matrix block.") - parser.add_argument("camchain", help="Path to camchain-imucam.yaml produced by kalibr_calibrate_imu_camera") - parser.add_argument("--camera", default="cam0", help="Camera key in camchain file (default: cam0)") - args = parser.parse_args() - - with open(args.camchain, "r", encoding="utf-8") as f: - data = yaml.safe_load(f) - - if args.camera not in data: - raise KeyError(f"Camera key '{args.camera}' not found. Available keys: {list(data.keys())}") - - t_cam_imu = data[args.camera].get("T_cam_imu") - if t_cam_imu is None: - raise KeyError(f"T_cam_imu not found under '{args.camera}'") - - if len(t_cam_imu) != 4 or any(len(r) != 4 for r in t_cam_imu): - raise ValueError("T_cam_imu must be a 4x4 matrix") - - print("IMU.T_b_c1: !!opencv-matrix") - print(" rows: 4") - print(" cols: 4") - print(" dt: f") - print(" data: [" + fmt_row(t_cam_imu[0]) + ",") - print(" " + fmt_row(t_cam_imu[1]) + ",") - print(" " + fmt_row(t_cam_imu[2]) + ",") - print(" " + fmt_row(t_cam_imu[3]) + "]") - print() - print("# For compatibility with your ORB-SLAM config, copy the same matrix into Tbc as well.") - - -if __name__ == "__main__": - main() diff --git a/tools/kalibr/run_kalibr.sh b/tools/kalibr/run_kalibr.sh deleted file mode 100644 index 083738b..0000000 --- a/tools/kalibr/run_kalibr.sh +++ /dev/null @@ -1,30 +0,0 @@ -#!/usr/bin/env bash -set -euo pipefail - -# Thin wrapper around the installed Kalibr Docker image. -# Usage examples: -# ./tools/kalibr/run_kalibr.sh kalibr_calibrate_cameras --bag /data/my.bag ... -# ./tools/kalibr/run_kalibr.sh kalibr_calibrate_imu_camera --bag /data/my.bag ... - -ROOT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")/../.." && pwd)" -IMAGE="stereolabs/kalibr:latest" - -# X11 forwarding is optional. It is useful for target detection visualization. -XSOCK="/tmp/.X11-unix" -XAUTH="/tmp/.docker.xauth" - -if [[ ! -f "${XAUTH}" ]]; then - touch "${XAUTH}" - xauth nlist "${DISPLAY:-:0}" | sed -e 's/^..../ffff/' | xauth -f "${XAUTH}" nmerge - >/dev/null 2>&1 || true -fi - -docker run --rm -it \ - --net=host \ - -e DISPLAY="${DISPLAY:-:0}" \ - -e QT_X11_NO_MITSHM=1 \ - -e XAUTHORITY="${XAUTH}" \ - -v "${XSOCK}:${XSOCK}:rw" \ - -v "${XAUTH}:${XAUTH}:rw" \ - -v "${ROOT_DIR}:/work:rw" \ - -w /work \ - "${IMAGE}" "$@" diff --git a/vendor/stonefish b/vendor/stonefish index 7d52673..4e6de52 160000 --- a/vendor/stonefish +++ b/vendor/stonefish @@ -1 +1 @@ -Subproject commit 7d52673791834caa743907dde83a1949134ef2f0 +Subproject commit 4e6de528bd8266bc47d4896275dd3a41e6694a9f diff --git a/vendor/stonefish_ros2 b/vendor/stonefish_ros2 index 29e56f6..793b8d0 160000 --- a/vendor/stonefish_ros2 +++ b/vendor/stonefish_ros2 @@ -1 +1 @@ -Subproject commit 29e56f6b62412e3c64b09792616e099363257a59 +Subproject commit 793b8d07c5b9bd16c2213c58b4d6de164a20f8b2 From 77cca91d4c8298995d7790f05264389ce60a4909 Mon Sep 17 00:00:00 2001 From: Cruiz102 Date: Tue, 25 Aug 2026 23:10:20 -0700 Subject: [PATCH 2/7] Point stonefish submodules at Rumarino-Team forks for colcon integration commits. Co-authored-by: Cursor --- .gitmodules | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/.gitmodules b/.gitmodules index 64742db..455a67a 100644 --- a/.gitmodules +++ b/.gitmodules @@ -1,9 +1,9 @@ [submodule "vendor/stonefish"] path = vendor/stonefish - url = https://github.com/patrykcieslak/stonefish.git + url = https://github.com/Rumarino-Team/stonefish.git [submodule "vendor/stonefish_ros2"] path = vendor/stonefish_ros2 - url = https://github.com/JuanDelPueblo/stonefish_ros2.git + url = https://github.com/Rumarino-Team/stonefish_ros2.git [submodule "vendor/zed-ros-interfaces"] path = vendor/zed-ros-interfaces url = https://github.com/stereolabs/zed-ros2-interfaces.git From 11c07a0d189947647fad4ac3b76d29172f0255f9 Mon Sep 17 00:00:00 2001 From: Cruiz102 Date: Tue, 25 Aug 2026 23:18:35 -0700 Subject: [PATCH 3/7] Rewrite README with Ubuntu-only install, build, and sim run steps. MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Remove Fedora instructions and organize the doc into clone → dependencies → build → launch for simulation. Co-authored-by: Cursor --- README.md | 296 +++++++++++++++++++++++++----------------------------- 1 file changed, 139 insertions(+), 157 deletions(-) diff --git a/README.md b/README.md index 29016ea..b831c4b 100644 --- a/README.md +++ b/README.md @@ -1,81 +1,50 @@ -## Simulation +# autonomy-stack -### Clone and go in repo -```sh -git clone --recursive https://github.com/Rumarino-Team/autonomy-stack.git -cd ./autonomy-stack -``` +ROS 2 simulation and control stack for Rumarino AUVs (Stonefish + `mission_executor`). -## Quick Start with Docker (Recommended for CI/CD) - -```bash -# Build the Docker image -docker build -t rumarino-headless:latest . +**Supported platform:** Ubuntu (22.04 on Jetson, 24.04 on desktop — use the ROS 2 release that matches your Ubuntu version). This repo targets **ROS 2 Jazzy** on **Ubuntu 24.04**. -# Run headless simulation test -docker run --rm \ - --name headless-test \ - rumarino-headless:latest \ - bash -c " - source /opt/ros/jazzy/setup.bash && \ - source /ros2_ws/install/setup.bash && \ - ros2 launch bringup test_mission_executor_headless.launch.py \ - mission_name:=prequalify \ - controller_name:=stonefish_hydrus \ - env_file_name:=hydrus_env_headless.scn & - LAUNCH_PID=\$! && \ - sleep 15 && \ - kill \$LAUNCH_PID 2>/dev/null || true - " -``` +--- -## Local Development Setup -System Dependencies +## 1. Clone -### Required Tools -- Python 3 -- C++ compiler (GCC) + CMake -- Rust ([rustup](https://rustup.rs)) -- Clang/LLVM (for Rust ROS 2 bindings / r2r) +```sh +git clone --recursive https://github.com/Rumarino-Team/autonomy-stack.git +cd autonomy-stack +``` -Install a **minimal** ROS 2 Jazzy (`ros-base`), not `desktop`. Optional tools (joy, usb_cam, rviz) are listed under the profiles that need them. +If you already cloned without submodules: -### Fedora: ```sh -# Build tools + Rust -sudo dnf install python3 python3-pip gcc gcc-c++ cmake pkgconf-pkg-config rust cargo +git submodule update --init --recursive +``` -# ROS 2 (minimal) + sim msgs -sudo dnf copr enable tavie/ros2 -sudo dnf install ros-jazzy-ros-base ros-jazzy-vision-msgs \ - ros-jazzy-image-transport ros-jazzy-pcl-conversions ros-jazzy-visualization-msgs \ - python3-colcon-common-extensions +--- -# Stonefish + detection_mocker system libs (include SDL2 for Stonefish) -sudo dnf install freetype-devel glm-devel eigen3-devel tinyxml2-devel \ - mesa-libGL-devel libclang-devel clang SDL2-devel +## 2. Install dependencies (Ubuntu) -python3 -m pip install wheel -``` +Run from any directory. You only need to do this once per machine. + +### 2a. ROS 2 repository (skip if ROS Jazzy is already installed) -### Ubuntu: ```bash sudo apt update -sudo apt install -y python3 python3-pip build-essential cmake pkg-config curl \ - libclang-dev llvm-dev clang \ - python3-colcon-common-extensions - -# Add ROS 2 repository (if not already added) -sudo apt install -y software-properties-common +sudo apt install -y software-properties-common curl gnupg lsb-release sudo add-apt-repository universe -sudo apt update -sudo apt install -y curl gnupg lsb-release sudo curl -sSL https://raw.githubusercontent.com/ros/rosdistro/master/ros.key \ -o /usr/share/keyrings/ros-archive-keyring.gpg +sudo apt update +``` -# System dependencies +### 2b. Build tools, ROS 2, and simulation libraries + +```bash sudo apt update -sudo apt install -y ros-jazzy-ros-base \ +sudo apt install -y \ + python3 python3-pip build-essential cmake pkg-config curl \ + libclang-dev llvm-dev clang \ + python3-colcon-common-extensions \ + ros-jazzy-ros-base \ ros-jazzy-vision-msgs \ ros-jazzy-image-transport \ ros-jazzy-pcl-conversions \ @@ -84,193 +53,194 @@ sudo apt install -y ros-jazzy-ros-base \ libglm-dev \ libeigen3-dev \ libtinyxml2-dev \ - libgl1-mesa-dev + libgl1-mesa-dev \ + libsdl2-dev +``` -# Optional: system SDL2 (Stonefish uses libsdl2-dev on Linux) -sudo apt install -y libsdl2-dev +### 2c. Rust (required for `mission_executor`) -# Install Rust +```bash curl --proto '=https' --tlsv1.2 -sSf https://sh.rustup.rs | sh -s -- -y +source "$HOME/.cargo/env" ``` -### Optional profiles (install only if you need them) +### Optional (only if you need them) ```bash -# Teleop joystick (stonefish.launch.py use_joy:=true) -sudo apt install -y ros-jazzy-joy # Fedora: ros-jazzy-joy +# Teleop with a gamepad (pass use_joy:=true at launch) +sudo apt install -y ros-jazzy-joy xterm -# Hardware Proteus camera / mock serial +# Hardware Proteus USB camera / mock serial sudo apt install -y ros-jazzy-usb-cam socat - -# GUI visualization (not required for sim/CI) -sudo apt install -y ros-jazzy-rviz2 ``` -### Build +--- + +## 3. Build + +From the **repo root** (`autonomy-stack/`): + ```sh source /opt/ros/jazzy/setup.bash colcon build --cmake-args -DCMAKE_BUILD_TYPE=Release source install/setup.bash ``` -Stonefish (`vendor/stonefish`) is a colcon cmake package and installs into `install/` with everything else. `stonefish_ros2` declares a build dependency on it, so colcon builds them in order. +Stonefish is built by colcon as part of this command (via `vendor/stonefish`) and installs into `install/` with the rest of the workspace. -Optional stacks (ZED vision) are ignored via `COLCON_IGNORE` under `src/zed_custom_wrapper/`. +Re-build after code changes: -## Computer Vision +```sh +source /opt/ros/jazzy/setup.bash +colcon build --cmake-args -DCMAKE_BUILD_TYPE=Release +source install/setup.bash +``` -### ZED Custom Wrapper +--- -`zed_custom_wrapper` is ignored by default (`COLCON_IGNORE`) so a normal sim build stays lean. Remove that file before building vision. +## 4. Run simulation -### Dependencies - - [ZED-SDK 5.1](https://www.stereolabs.com/developers/release) - - [Cuda 12.8](https://developer.nvidia.com/cuda-12-8-0-download-archive) +Always launch from the **repo root** so config paths resolve correctly. ```sh -rm -f src/zed_custom_wrapper/COLCON_IGNORE -# also init vendor/zed-ros-interfaces if you need zed_msgs -colcon build --packages-select zed_msgs zed_custom_wrapper && source ./install/setup.bash && \ - ros2 launch zed_custom_wrapper zed_custom.launch.py onnx_model_path:=./src/zed_custom_wrapper/yolov8n.onnx +source /opt/ros/jazzy/setup.bash +source install/setup.bash + +# Hydrus — prequalify mission (GUI) +ros2 launch bringup stonefish.launch.py \ + mission_name:=prequalify \ + auv_name:=hydrus \ + env_file_name:=hydrus_env.scn \ + headless:=false ``` -## Simulate Missions using bridge_stonefish -```sh -# Note: -# - For `env_file_name:=pool_env.scn`, the launch script auto-selects the -# matching pool scenario wrapper for the chosen AUV. -# - `auv_file_name` is optional and only needed as a manual override. +Headless (no GPU window): -# proteus, prequalify mission +```sh ros2 launch bringup stonefish.launch.py \ - mission_name:=prequalify \ - auv_name:=proteus \ - env_file_name:=proteus_env.scn \ - headless:=false + mission_name:=prequalify \ + auv_name:=hydrus \ + env_file_name:=hydrus_env.scn \ + headless:=true +``` -# hydrus, prequalify mission -ros2 launch bringup stonefish.launch.py \ - mission_name:=prequalify \ - auv_name:=hydrus \ - env_file_name:=hydrus_env.scn \ - headless:=false - -# proteus, teleop mission (needs ros-jazzy-joy) -# if you don't have xterm, set TERMINAL to your terminal or install xterm. -# sudo apt install xterm -# sudo dnf install xterm +Proteus prequalify: + +```sh ros2 launch bringup stonefish.launch.py \ - mission_name:=teleop \ - auv_name:=proteus \ - env_file_name:=proteus_env.scn \ - headless:=false \ - use_joy:=true + mission_name:=prequalify \ + auv_name:=proteus \ + env_file_name:=proteus_env.scn \ + headless:=false +``` + +Teleop (requires `ros-jazzy-joy`; install in optional step above): -# bluerov2, teleop mission +```sh ros2 launch bringup stonefish.launch.py \ mission_name:=teleop \ - auv_name:=bluerov2 \ - env_file_name:=pool_env.scn \ + auv_name:=proteus \ + env_file_name:=proteus_env.scn \ headless:=false \ use_joy:=true +``` + +--- + +## More simulation examples -# optional manual override (advanced) +```sh +# pool scenario — env file is auto-selected per AUV ros2 launch bringup stonefish.launch.py \ mission_name:=teleop \ auv_name:=bluerov2 \ env_file_name:=pool_env.scn \ - auv_file_name:=bluerov2.scn \ headless:=false \ use_joy:=true -# bluerov2 direct actuator sanity test -# Use stonefish_only so mission_executor does not overwrite the direct command. +# direct thruster test (mission_executor disabled) ros2 launch bringup stonefish.launch.py \ mission_name:=teleop \ auv_name:=bluerov2 \ env_file_name:=pool_env.scn \ headless:=false \ stonefish_only:=true +``` -# In another terminal, publish 8 normalized thruster values. -source install/setup.bash +In another terminal: -# Equal horizontal commands cancel on this angled layout; this pattern drives body +X. +```sh +source install/setup.bash ros2 topic pub -r 10 /bridge/thrusters std_msgs/msg/Float64MultiArray \ "{data: [-0.6, -0.6, 0.6, 0.6, 0.0, 0.0, 0.0, 0.0]}" - -# Watch Stonefish's actuator feedback. ros2 topic echo /bridge/thruster_state ``` +--- + +## Docker (CI / headless test) + +```bash +docker build -t rumarino-headless:latest . + +docker run --rm --name headless-test rumarino-headless:latest +``` + +--- -## Build & Run proteus using bridge_hardware +## Hardware (Proteus + Arduino) + +Extra dependency: -### Extra deps ```sh -sudo apt install -y ros-jazzy-usb-cam socat # usb_cam optional; socat for mock Arduino +sudo apt install -y socat # mock serial testing ``` -### Build +Build: + ```sh -# Source ROS 2 environment source /opt/ros/jazzy/setup.bash - -# Build packages colcon build \ --packages-select interfaces bringup mission_executor bridge_hardware \ - --cmake-args -DCMAKE_EXPORT_COMPILE_COMMANDS=ON + --cmake-args -DCMAKE_BUILD_TYPE=Release +source install/setup.bash ``` -### Run with real Arduino -```sh -# Source workspace after build -source install/setup.bash +Run: -# Launch full hardware stack +```sh ros2 launch bringup hardware_proteus.launch.py \ mission_name:=prequalify \ arduino_port:=/dev/ttyACM0 \ arduino_baud_rate:=115200 - -# Or run bridge node directly -ros2 run bridge_hardware bridge_proteus_node --ros-args \ - -p arduino_port:=/dev/ttyACM0 \ - -p arduino_baud_rate:=115200 ``` +### Arduino firmware -### Arduino CLI (real board) - -#### 1. Install AVR board support and Servo library dependency ```sh arduino-cli core update-index arduino-cli core install arduino:avr arduino-cli lib install Servo -``` -#### 2. Compile and upload Proteus firmware -```sh arduino-cli compile --fqbn arduino:avr:uno arduino/sketches/Proteus arduino-cli upload -p /dev/ttyACM0 --fqbn arduino:avr:uno arduino/sketches/Proteus ``` -### Mock Arduino test +### Mock Arduino (virtual serial) + +Terminal 1: -#### 1. Create connected virtual serial ports ```sh -rm -f /tmp/ttyMOCK /tmp/ttyBRIDGE /tmp/socat_pair.log -socat -d -d \ - pty,raw,echo=0,link=/tmp/ttyMOCK \ - pty,raw,echo=0,link=/tmp/ttyBRIDGE \ - 2>&1 | tee /tmp/socat_pair.log +socat -d -d pty,raw,echo=0,link=/tmp/ttyMOCK pty,raw,echo=0,link=/tmp/ttyBRIDGE ``` -#### 2. Start mock Arduino (new terminal) +Terminal 2: + ```sh python3 tools/mock_arduino.py /tmp/ttyMOCK 115200 ``` -#### 3. Start bridge_hardware with virtual bridge port (new terminal) +Terminal 3: + ```sh source install/setup.bash ros2 run bridge_hardware bridge_proteus_node --ros-args \ @@ -278,8 +248,20 @@ ros2 run bridge_hardware bridge_proteus_node --ros-args \ -p arduino_baud_rate:=115200 ``` -#### 4. Publish thruster commands (new terminal) +--- + +## Computer vision (ZED, optional) + +`zed_custom_wrapper` is ignored by default (`src/zed_custom_wrapper/COLCON_IGNORE`). + +Requires [ZED SDK 5.1](https://www.stereolabs.com/developers/release) and [CUDA 12.8](https://developer.nvidia.com/cuda-12-8-0-download-archive). + ```sh +rm -f src/zed_custom_wrapper/COLCON_IGNORE +git submodule update --init vendor/zed-ros-interfaces +source /opt/ros/jazzy/setup.bash +colcon build --packages-select zed_msgs zed_custom_wrapper source install/setup.bash -ros2 topic pub -r 10 /bridge/thrusters std_msgs/msg/Float64MultiArray "{data: [0.45, -0.25, 0.15, -0.15, 0.05, -0.05]}" +ros2 launch zed_custom_wrapper zed_custom.launch.py \ + onnx_model_path:=./src/zed_custom_wrapper/yolov8n.onnx ``` From 89ea9cc43344342f7960da9f80e2b80d89d95620 Mon Sep 17 00:00:00 2001 From: Cruiz102 Date: Tue, 25 Aug 2026 23:26:43 -0700 Subject: [PATCH 4/7] Switch to ROS 2 Humble and move setup into install script and launch args. Add scripts/install_deps.sh, slim the README, and expose missions/AUVs/scenarios as choices in stonefish.launch.py for --show-args discovery. Co-authored-by: Cursor --- .github/workflows/headless-simulation.yml | 2 +- Dockerfile | 20 +- README.md | 254 +----------------- run_example.sh | 2 +- scripts/bench_build.sh | 2 +- scripts/install_deps.sh | 92 +++++++ src/bringup/launch/hardware_proteus.launch.py | 15 +- src/bringup/launch/stonefish.launch.py | 115 ++++++-- 8 files changed, 227 insertions(+), 275 deletions(-) create mode 100755 scripts/install_deps.sh diff --git a/.github/workflows/headless-simulation.yml b/.github/workflows/headless-simulation.yml index 4d04e73..d98f9e5 100644 --- a/.github/workflows/headless-simulation.yml +++ b/.github/workflows/headless-simulation.yml @@ -36,7 +36,7 @@ jobs: --name headless-test \ rumarino-headless:latest \ bash -c " - source /opt/ros/jazzy/setup.bash && \ + source /opt/ros/humble/setup.bash && \ source /ros2_ws/install/setup.bash && \ ros2 launch bringup test_mission_executor_headless.launch.py \ mission_name:=prequalify \ diff --git a/Dockerfile b/Dockerfile index f6fe614..ff500db 100644 --- a/Dockerfile +++ b/Dockerfile @@ -1,5 +1,5 @@ # syntax=docker/dockerfile:1 -FROM ros:jazzy-ros-base +FROM ros:humble-ros-base RUN apt-get update && apt-get install -y \ python3 \ @@ -14,10 +14,10 @@ RUN apt-get update && apt-get install -y \ clang \ python3-colcon-common-extensions \ python3-rosdep \ - ros-jazzy-vision-msgs \ - ros-jazzy-image-transport \ - ros-jazzy-pcl-conversions \ - ros-jazzy-visualization-msgs \ + ros-humble-vision-msgs \ + ros-humble-image-transport \ + ros-humble-pcl-conversions \ + ros-humble-visualization-msgs \ libfreetype6-dev \ libsdl2-dev \ libglm-dev \ @@ -43,11 +43,11 @@ COPY vendor/stonefish_ros2/package.xml ./src/stonefish_ros2/package.xml RUN rosdep init || true && rosdep update -RUN bash -c "source /opt/ros/jazzy/setup.bash && \ +RUN bash -c "source /opt/ros/humble/setup.bash && \ rosdep install --from-paths src vendor/stonefish --ignore-src -r -y || true" COPY src/interfaces ./src/interfaces -RUN bash -lc "source /opt/ros/jazzy/setup.bash && \ +RUN bash -lc "source /opt/ros/humble/setup.bash && \ colcon build --packages-select Stonefish interfaces \ --cmake-args -DCMAKE_BUILD_TYPE=Release" @@ -56,7 +56,7 @@ COPY src/mission_executor/Cargo.toml ./src/mission_executor/Cargo.toml COPY src/mission_executor/CMakeLists.txt ./src/mission_executor/CMakeLists.txt RUN mkdir -p src/mission_executor/src && printf "fn main() {}" > src/mission_executor/src/main.rs -RUN bash -lc "source /opt/ros/jazzy/setup.bash && source install/setup.bash && \ +RUN bash -lc "source /opt/ros/humble/setup.bash && source install/setup.bash && \ colcon build --packages-select mission_executor \ --cmake-args -DCMAKE_BUILD_TYPE=Release" @@ -65,13 +65,13 @@ COPY src/bridge_stonefish ./src/bridge_stonefish COPY src/detection_mocker ./src/detection_mocker COPY vendor/stonefish_ros2 ./src/stonefish_ros2 -RUN bash -c "source /opt/ros/jazzy/setup.bash && source install/setup.bash && \ +RUN bash -c "source /opt/ros/humble/setup.bash && source install/setup.bash && \ colcon build --packages-select stonefish_ros2 bridge_stonefish bringup detection_mocker \ --cmake-args -DCMAKE_BUILD_TYPE=Release" COPY src/mission_executor/src ./src/mission_executor/src RUN ln -sf /ros2_ws/src/mission_executor/target /ros2_ws/target && \ - bash -c "source /opt/ros/jazzy/setup.bash && source install/setup.bash && \ + bash -c "source /opt/ros/humble/setup.bash && source install/setup.bash && \ colcon build --packages-select mission_executor \ --cmake-args -DCMAKE_BUILD_TYPE=Release" diff --git a/README.md b/README.md index b831c4b..a6a2e22 100644 --- a/README.md +++ b/README.md @@ -1,267 +1,41 @@ # autonomy-stack -ROS 2 simulation and control stack for Rumarino AUVs (Stonefish + `mission_executor`). +ROS 2 Humble simulation and control stack for Rumarino AUVs (Stonefish + `mission_executor`). -**Supported platform:** Ubuntu (22.04 on Jetson, 24.04 on desktop — use the ROS 2 release that matches your Ubuntu version). This repo targets **ROS 2 Jazzy** on **Ubuntu 24.04**. +**Platform:** Ubuntu 22.04 (desktop and Jetson). ---- - -## 1. Clone +## Setup ```sh git clone --recursive https://github.com/Rumarino-Team/autonomy-stack.git cd autonomy-stack +./scripts/install_deps.sh ``` -If you already cloned without submodules: - -```sh -git submodule update --init --recursive -``` - ---- - -## 2. Install dependencies (Ubuntu) - -Run from any directory. You only need to do this once per machine. - -### 2a. ROS 2 repository (skip if ROS Jazzy is already installed) - -```bash -sudo apt update -sudo apt install -y software-properties-common curl gnupg lsb-release -sudo add-apt-repository universe -sudo curl -sSL https://raw.githubusercontent.com/ros/rosdistro/master/ros.key \ - -o /usr/share/keyrings/ros-archive-keyring.gpg -sudo apt update -``` - -### 2b. Build tools, ROS 2, and simulation libraries - -```bash -sudo apt update -sudo apt install -y \ - python3 python3-pip build-essential cmake pkg-config curl \ - libclang-dev llvm-dev clang \ - python3-colcon-common-extensions \ - ros-jazzy-ros-base \ - ros-jazzy-vision-msgs \ - ros-jazzy-image-transport \ - ros-jazzy-pcl-conversions \ - ros-jazzy-visualization-msgs \ - libfreetype6-dev \ - libglm-dev \ - libeigen3-dev \ - libtinyxml2-dev \ - libgl1-mesa-dev \ - libsdl2-dev -``` - -### 2c. Rust (required for `mission_executor`) - -```bash -curl --proto '=https' --tlsv1.2 -sSf https://sh.rustup.rs | sh -s -- -y -source "$HOME/.cargo/env" -``` - -### Optional (only if you need them) - -```bash -# Teleop with a gamepad (pass use_joy:=true at launch) -sudo apt install -y ros-jazzy-joy xterm - -# Hardware Proteus USB camera / mock serial -sudo apt install -y ros-jazzy-usb-cam socat -``` - ---- - -## 3. Build - -From the **repo root** (`autonomy-stack/`): - -```sh -source /opt/ros/jazzy/setup.bash -colcon build --cmake-args -DCMAKE_BUILD_TYPE=Release -source install/setup.bash -``` +Optional flags: `--with-joy` (teleop), `--with-hardware` (Proteus USB cam / mock serial). -Stonefish is built by colcon as part of this command (via `vendor/stonefish`) and installs into `install/` with the rest of the workspace. +## Build -Re-build after code changes: +From the repo root: ```sh -source /opt/ros/jazzy/setup.bash +source /opt/ros/humble/setup.bash colcon build --cmake-args -DCMAKE_BUILD_TYPE=Release source install/setup.bash ``` ---- - -## 4. Run simulation - -Always launch from the **repo root** so config paths resolve correctly. - -```sh -source /opt/ros/jazzy/setup.bash -source install/setup.bash - -# Hydrus — prequalify mission (GUI) -ros2 launch bringup stonefish.launch.py \ - mission_name:=prequalify \ - auv_name:=hydrus \ - env_file_name:=hydrus_env.scn \ - headless:=false -``` - -Headless (no GPU window): +## Run simulation -```sh -ros2 launch bringup stonefish.launch.py \ - mission_name:=prequalify \ - auv_name:=hydrus \ - env_file_name:=hydrus_env.scn \ - headless:=true -``` - -Proteus prequalify: - -```sh -ros2 launch bringup stonefish.launch.py \ - mission_name:=prequalify \ - auv_name:=proteus \ - env_file_name:=proteus_env.scn \ - headless:=false -``` - -Teleop (requires `ros-jazzy-joy`; install in optional step above): +Launch from the **repo root**. All missions, AUVs, and scenario options are defined in the launch file: ```sh -ros2 launch bringup stonefish.launch.py \ - mission_name:=teleop \ - auv_name:=proteus \ - env_file_name:=proteus_env.scn \ - headless:=false \ - use_joy:=true +ros2 launch bringup stonefish.launch.py --show-args ``` ---- - -## More simulation examples +Example: ```sh -# pool scenario — env file is auto-selected per AUV -ros2 launch bringup stonefish.launch.py \ - mission_name:=teleop \ - auv_name:=bluerov2 \ - env_file_name:=pool_env.scn \ - headless:=false \ - use_joy:=true - -# direct thruster test (mission_executor disabled) -ros2 launch bringup stonefish.launch.py \ - mission_name:=teleop \ - auv_name:=bluerov2 \ - env_file_name:=pool_env.scn \ - headless:=false \ - stonefish_only:=true +ros2 launch bringup stonefish.launch.py mission_name:=prequalify auv_name:=hydrus headless:=false ``` -In another terminal: - -```sh -source install/setup.bash -ros2 topic pub -r 10 /bridge/thrusters std_msgs/msg/Float64MultiArray \ - "{data: [-0.6, -0.6, 0.6, 0.6, 0.0, 0.0, 0.0, 0.0]}" -ros2 topic echo /bridge/thruster_state -``` - ---- - -## Docker (CI / headless test) - -```bash -docker build -t rumarino-headless:latest . - -docker run --rm --name headless-test rumarino-headless:latest -``` - ---- - -## Hardware (Proteus + Arduino) - -Extra dependency: - -```sh -sudo apt install -y socat # mock serial testing -``` - -Build: - -```sh -source /opt/ros/jazzy/setup.bash -colcon build \ - --packages-select interfaces bringup mission_executor bridge_hardware \ - --cmake-args -DCMAKE_BUILD_TYPE=Release -source install/setup.bash -``` - -Run: - -```sh -ros2 launch bringup hardware_proteus.launch.py \ - mission_name:=prequalify \ - arduino_port:=/dev/ttyACM0 \ - arduino_baud_rate:=115200 -``` - -### Arduino firmware - -```sh -arduino-cli core update-index -arduino-cli core install arduino:avr -arduino-cli lib install Servo -arduino-cli compile --fqbn arduino:avr:uno arduino/sketches/Proteus -arduino-cli upload -p /dev/ttyACM0 --fqbn arduino:avr:uno arduino/sketches/Proteus -``` - -### Mock Arduino (virtual serial) - -Terminal 1: - -```sh -socat -d -d pty,raw,echo=0,link=/tmp/ttyMOCK pty,raw,echo=0,link=/tmp/ttyBRIDGE -``` - -Terminal 2: - -```sh -python3 tools/mock_arduino.py /tmp/ttyMOCK 115200 -``` - -Terminal 3: - -```sh -source install/setup.bash -ros2 run bridge_hardware bridge_proteus_node --ros-args \ - -p arduino_port:=/tmp/ttyBRIDGE \ - -p arduino_baud_rate:=115200 -``` - ---- - -## Computer vision (ZED, optional) - -`zed_custom_wrapper` is ignored by default (`src/zed_custom_wrapper/COLCON_IGNORE`). - -Requires [ZED SDK 5.1](https://www.stereolabs.com/developers/release) and [CUDA 12.8](https://developer.nvidia.com/cuda-12-8-0-download-archive). - -```sh -rm -f src/zed_custom_wrapper/COLCON_IGNORE -git submodule update --init vendor/zed-ros-interfaces -source /opt/ros/jazzy/setup.bash -colcon build --packages-select zed_msgs zed_custom_wrapper -source install/setup.bash -ros2 launch zed_custom_wrapper zed_custom.launch.py \ - onnx_model_path:=./src/zed_custom_wrapper/yolov8n.onnx -``` +Direct thruster test (no mission executor): add `stonefish_only:=true`, then publish to `/bridge/thrusters`. diff --git a/run_example.sh b/run_example.sh index 2cc2c38..455e785 100755 --- a/run_example.sh +++ b/run_example.sh @@ -42,6 +42,6 @@ colcon build --packages-select bridge_stonefish --symlink-install echo "" echo "Launching Stonefish simulator..." -source /opt/ros/jazzy/setup.bash +source /opt/ros/humble/setup.bash source "$WORKSPACE_ROOT/install/setup.bash" ros2 launch bringup test_mission_executor.launch.py mission_name:=prequalify env_file_name:="$OUTPUT_SCN" diff --git a/scripts/bench_build.sh b/scripts/bench_build.sh index a0f9c2d..ea8b6fc 100755 --- a/scripts/bench_build.sh +++ b/scripts/bench_build.sh @@ -6,7 +6,7 @@ ROOT="$(cd "$(dirname "${BASH_SOURCE[0]}")/.." && pwd)" cd "$ROOT" set +u -source /opt/ros/jazzy/setup.bash +source /opt/ros/humble/setup.bash set -u JOBS="$(nproc)" diff --git a/scripts/install_deps.sh b/scripts/install_deps.sh new file mode 100755 index 0000000..24ac9d9 --- /dev/null +++ b/scripts/install_deps.sh @@ -0,0 +1,92 @@ +#!/usr/bin/env bash +# Install system dependencies for autonomy-stack (Ubuntu 22.04 + ROS 2 Humble). +set -euo pipefail + +ROS_DISTRO=humble +WITH_JOY=false +WITH_HARDWARE=false + +usage() { + cat <&2; usage >&2; exit 1 ;; + esac + shift +done + +if ! command -v apt-get >/dev/null 2>&1; then + echo "This script supports Ubuntu (apt) only." >&2 + exit 1 +fi + +ros_base_pkg="ros-${ROS_DISTRO}-ros-base" +if ! dpkg -s "$ros_base_pkg" >/dev/null 2>&1; then + echo "Setting up ROS 2 ${ROS_DISTRO} apt repository..." + sudo apt update + sudo apt install -y software-properties-common curl gnupg lsb-release + sudo add-apt-repository -y universe + sudo curl -sSL https://raw.githubusercontent.com/ros/rosdistro/master/ros.key \ + -o /usr/share/keyrings/ros-archive-keyring.gpg + echo "deb [arch=$(dpkg --print-architecture) signed-by=/usr/share/keyrings/ros-archive-keyring.gpg] \ +http://packages.ros.org/ros2/ubuntu $(. /etc/os-release && echo "${UBUNTU_CODENAME}") main" \ + | sudo tee /etc/apt/sources.list.d/ros2.list >/dev/null + sudo apt update +else + echo "ROS 2 ${ROS_DISTRO} already installed; skipping repository setup." +fi + +echo "Installing build tools, ROS 2, and simulation libraries..." +sudo apt update +sudo apt install -y \ + python3 python3-pip build-essential cmake pkg-config curl \ + libclang-dev llvm-dev clang \ + python3-colcon-common-extensions \ + "ros-${ROS_DISTRO}-ros-base" \ + "ros-${ROS_DISTRO}-vision-msgs" \ + "ros-${ROS_DISTRO}-image-transport" \ + "ros-${ROS_DISTRO}-pcl-conversions" \ + "ros-${ROS_DISTRO}-visualization-msgs" \ + libfreetype6-dev \ + libglm-dev \ + libeigen3-dev \ + libtinyxml2-dev \ + libgl1-mesa-dev \ + libsdl2-dev + +if [[ "$WITH_JOY" == true ]]; then + sudo apt install -y "ros-${ROS_DISTRO}-joy" xterm +fi + +if [[ "$WITH_HARDWARE" == true ]]; then + sudo apt install -y "ros-${ROS_DISTRO}-usb-cam" socat +fi + +if ! command -v rustc >/dev/null 2>&1; then + echo "Installing Rust (required for mission_executor)..." + curl --proto '=https' --tlsv1.2 -sSf https://sh.rustup.rs | sh -s -- -y + # shellcheck disable=SC1091 + source "$HOME/.cargo/env" +else + echo "Rust already installed; skipping rustup." +fi + +echo "Done. Source ROS and build from the repo root:" +echo " source /opt/ros/${ROS_DISTRO}/setup.bash" +echo " colcon build --cmake-args -DCMAKE_BUILD_TYPE=Release" +echo " source install/setup.bash" +echo " ros2 launch bringup stonefish.launch.py --show-args" diff --git a/src/bringup/launch/hardware_proteus.launch.py b/src/bringup/launch/hardware_proteus.launch.py index f20ebcd..fb9fbf0 100644 --- a/src/bringup/launch/hardware_proteus.launch.py +++ b/src/bringup/launch/hardware_proteus.launch.py @@ -5,8 +5,21 @@ from launch.conditions import IfCondition from launch.substitutions import LaunchConfiguration +MISSIONS = ( + 'prequalify', + 'teleop', + 'drop_into_box', + 'cardinal_directions', +) + + def generate_launch_description(): - mission_name_arg = DeclareLaunchArgument('mission_name') + mission_name_arg = DeclareLaunchArgument( + 'mission_name', + default_value='prequalify', + description='Mission loaded by mission_executor.', + choices=list(MISSIONS), + ) arduino_port_arg = DeclareLaunchArgument('arduino_port') arduino_baud_rate_arg = DeclareLaunchArgument('arduino_baud_rate') diff --git a/src/bringup/launch/stonefish.launch.py b/src/bringup/launch/stonefish.launch.py index 0961a6c..c9dade2 100644 --- a/src/bringup/launch/stonefish.launch.py +++ b/src/bringup/launch/stonefish.launch.py @@ -6,17 +6,54 @@ from launch.launch_description_sources import PythonLaunchDescriptionSource from launch.substitutions import LaunchConfiguration +# --- discoverable configuration (also validated via --show-args choices) --- + +MISSIONS = ( + 'prequalify', + 'teleop', + 'drop_into_box', + 'cardinal_directions', +) + +AUVS = ( + 'hydrus', + 'proteus', + 'bluerov2', + 'girona500', +) + +ENV_FILES = ( + 'auto', + 'hydrus_env.scn', + 'hydrus_env_headless.scn', + 'proteus_env.scn', + 'pool_env.scn', + 'pool_env_hydrus.scn', + 'pool_env_proteus.scn', + 'pool_env_bluerov2.scn', + 'pool_env_girona500.scn', + 'bluerov2_tank.scn', +) DEFAULT_AUV_FILE_BY_NAME = { 'bluerov2': 'bluerov2.scn', 'proteus': 'proteus_auv.scn', 'hydrus': 'hydrus_auv.scn', + 'girona500': 'girona500_auv.scn', +} + +DEFAULT_ENV_BY_AUV = { + 'hydrus': 'hydrus_env.scn', + 'proteus': 'proteus_env.scn', + 'bluerov2': 'pool_env.scn', + 'girona500': 'pool_env.scn', } POOL_ENV_SCENARIO_BY_AUV = { 'bluerov2': 'pool_env_bluerov2.scn', 'proteus': 'pool_env_proteus.scn', 'hydrus': 'pool_env_hydrus.scn', + 'girona500': 'pool_env_girona500.scn', } @@ -26,10 +63,19 @@ def _resolve_auv_file_name(auv_name, explicit_auv_file_name): return DEFAULT_AUV_FILE_BY_NAME.get(auv_name, f'{auv_name}.scn') +def _resolve_env_file_name(auv_name, env_file_name): + if env_file_name in ('', 'auto'): + return DEFAULT_ENV_BY_AUV.get(auv_name, f'{auv_name}_env.scn') + return env_file_name + + def _launch_setup(context, *args, **kwargs): mission_name = LaunchConfiguration('mission_name').perform(context) auv_name = LaunchConfiguration('auv_name').perform(context) - env_file_name = LaunchConfiguration('env_file_name').perform(context) + env_file_name = _resolve_env_file_name( + auv_name, + LaunchConfiguration('env_file_name').perform(context), + ) explicit_auv_file_name = LaunchConfiguration('auv_file_name').perform(context) headless = LaunchConfiguration('headless').perform(context).lower() in ('true', '1', 'yes') stonefish_only = LaunchConfiguration('stonefish_only', default="no").perform(context).lower() in ('true', '1', 'yes') @@ -128,26 +174,53 @@ def _launch_setup(context, *args, **kwargs): def generate_launch_description(): - mission_name_arg = DeclareLaunchArgument('mission_name') - auv_name_arg = DeclareLaunchArgument('auv_name') - - env_file_name_arg = DeclareLaunchArgument('env_file_name') - auv_file_name_arg = DeclareLaunchArgument('auv_file_name', default_value='') - headless_arg = DeclareLaunchArgument('headless', default_value='false') - stonefish_only_arg = DeclareLaunchArgument('stonefish_only', default_value='false') - use_joy_arg = DeclareLaunchArgument( - 'use_joy', - default_value='false', - description='Start joy_node (requires ros-jazzy-joy). Use for teleop.', - ) - return LaunchDescription([ - mission_name_arg, - auv_name_arg, - env_file_name_arg, - auv_file_name_arg, - headless_arg, - stonefish_only_arg, - use_joy_arg, + DeclareLaunchArgument( + 'mission_name', + default_value='prequalify', + description='Mission loaded by mission_executor.', + choices=list(MISSIONS), + ), + DeclareLaunchArgument( + 'auv_name', + default_value='hydrus', + description='AUV model; selects default scenario and mesh when env/auv files are omitted.', + choices=list(AUVS), + ), + DeclareLaunchArgument( + 'env_file_name', + default_value='auto', + description=( + 'Stonefish environment scenario under bridge_stonefish/data/scenarios/. ' + 'auto picks the default for auv_name. pool_env.scn auto-picks the per-AUV pool file.' + ), + choices=list(ENV_FILES), + ), + DeclareLaunchArgument( + 'auv_file_name', + default_value='', + description=( + 'Override AUV mesh scenario. Empty uses the default for auv_name ' + f'({", ".join(f"{k}→{v}" for k, v in DEFAULT_AUV_FILE_BY_NAME.items())}).' + ), + ), + DeclareLaunchArgument( + 'headless', + default_value='false', + description='Run Stonefish without a GPU window (nogpu launch file).', + choices=['true', 'false'], + ), + DeclareLaunchArgument( + 'stonefish_only', + default_value='false', + description='Skip mission_executor; publish thrusters on /bridge/thrusters for direct testing.', + choices=['true', 'false'], + ), + DeclareLaunchArgument( + 'use_joy', + default_value='false', + description='Start joy_node (install with scripts/install_deps.sh --with-joy). Required for teleop.', + choices=['true', 'false'], + ), OpaqueFunction(function=_launch_setup), ]) From ab105683ba4a74df6e06bb9a8da10d50d17e94b6 Mon Sep 17 00:00:00 2001 From: Cruiz102 Date: Tue, 25 Aug 2026 23:43:35 -0700 Subject: [PATCH 5/7] Fix real-time sim control after main merge sim-time defaults. Disable sim-time odometry stamps and RTF cap unless fast_fixed_step is on, and fall back to 100 ms PID dt when odometry stamps stall or repeat. Co-authored-by: Cursor --- src/bringup/launch/stonefish.launch.py | 29 +++++++++++++++------- src/mission_executor/src/main.rs | 33 +++++++++++++------------- 2 files changed, 37 insertions(+), 25 deletions(-) diff --git a/src/bringup/launch/stonefish.launch.py b/src/bringup/launch/stonefish.launch.py index a890984..436063a 100644 --- a/src/bringup/launch/stonefish.launch.py +++ b/src/bringup/launch/stonefish.launch.py @@ -79,8 +79,19 @@ def _launch_setup(context, *args, **kwargs): explicit_auv_file_name = LaunchConfiguration('auv_file_name').perform(context) headless = LaunchConfiguration('headless').perform(context).lower() in ('true', '1', 'yes') stonefish_only = LaunchConfiguration('stonefish_only', default="no").perform(context).lower() in ('true', '1', 'yes') + fast_fixed_step_s = LaunchConfiguration('fast_fixed_step').perform(context) + fast_fixed_step = fast_fixed_step_s.lower() in ('true', '1', 'yes') use_joy = LaunchConfiguration('use_joy').perform(context).lower() in ('true', '1', 'yes') + # Sim-time stamps and RTF cap are for fast_fixed_step runs. Real-time graphical + # sim uses wall-clock odometry stamps (stonefish_ros2 default when fast=false). + use_sim_time_stamps = LaunchConfiguration('use_sim_time_stamps').perform(context) + if not fast_fixed_step: + use_sim_time_stamps = 'false' + realtime_factor_cap = LaunchConfiguration('realtime_factor_cap').perform(context) + if not fast_fixed_step: + realtime_factor_cap = '0.0' + cwd = os.getcwd() bridge_share = get_package_share_directory('bridge_stonefish') stonefish_share = get_package_share_directory('stonefish_ros2') @@ -100,10 +111,6 @@ def _launch_setup(context, *args, **kwargs): # into IncludeLaunchDescription can resolve against stonefish_ros2's own # defaults (use_sim_time_stamps=false). simulation_rate = LaunchConfiguration('simulation_rate').perform(context) - fast_fixed_step_s = LaunchConfiguration('fast_fixed_step').perform(context) - use_sim_time_stamps = LaunchConfiguration('use_sim_time_stamps').perform(context) - realtime_factor_cap = LaunchConfiguration('realtime_factor_cap').perform(context) - fast_fixed_step = fast_fixed_step_s.lower() in ('true', '1', 'yes') simulator_launch = 'stonefish_simulator_nogpu.launch.py' if headless else 'stonefish_simulator.launch.py' simulator_arguments = { @@ -249,14 +256,20 @@ def generate_launch_description(): ), DeclareLaunchArgument( 'use_sim_time_stamps', - default_value='true', - description='Stamp odometry with simulation time so PID dt stays correct when running faster than realtime.', + default_value='false', + description=( + 'Stamp odometry with simulation time (for fast_fixed_step). ' + 'Automatically forced false for real-time sim.' + ), choices=['true', 'false'], ), DeclareLaunchArgument( 'realtime_factor_cap', - default_value='5.0', - description='Max sim-time/wall-time ratio when fast_fixed_step is on. 0.0 disables the cap.', + default_value='0.0', + description=( + 'Max sim-time/wall-time ratio when fast_fixed_step is on. ' + 'Automatically 0 (disabled) for real-time sim. CI uses 5.0.' + ), ), OpaqueFunction(function=_launch_setup), ]) diff --git a/src/mission_executor/src/main.rs b/src/mission_executor/src/main.rs index 2f46e77..452c09e 100644 --- a/src/mission_executor/src/main.rs +++ b/src/mission_executor/src/main.rs @@ -401,18 +401,21 @@ async fn main() { let tam_x_y_z_roll_pitch_yaw = ¤t_cfg.tam; let timestamp_ns = pose_stamp_ns(&msg.header.stamp); - let dt = previous_timestamp_ns.and_then(|previous| { - let elapsed_ns = timestamp_ns - previous; - if elapsed_ns > 0 { - Some(elapsed_ns as f64 * 1e-9) - } else { - r2r::log_warn!( - "go_to_goal", - "odometry stamp not increasing (prev={previous} ns, now={timestamp_ns} ns); skipping I/D" - ); - None + let dt = match previous_timestamp_ns { + Some(previous) => { + let elapsed_ns = timestamp_ns - previous; + if elapsed_ns > 0 { + elapsed_ns as f64 * 1e-9 + } else { + r2r::log_warn!( + "go_to_goal", + "odometry stamp not increasing (prev={previous} ns, now={timestamp_ns} ns); using 100 ms dt" + ); + 0.1 + } } - }); + None => 0.1, + }; previous_timestamp_ns = Some(timestamp_ns); let goal = **td.goal.load(); @@ -443,12 +446,8 @@ async fn main() { pose_err[5] = yaw_error; - let vel_err = if let Some(dt) = dt { - sum_err += pose_err * dt; - (pose_err - prev_pose_err) / dt - } else { - Vector6::zeros() - }; + let vel_err = (pose_err - prev_pose_err) / dt; + sum_err += pose_err * dt; let wrench = kp.component_mul(&pose_err) + ki.component_mul(&sum_err) From 83ddb9a8e3ac451b3c351e037fe2b50b13e66be7 Mon Sep 17 00:00:00 2001 From: Cruiz102 Date: Tue, 25 Aug 2026 23:56:58 -0700 Subject: [PATCH 6/7] Fix Hydrus sim stability by reverting PID loop and stonefish_ros2. Restore the 10 Hz wall-clock PID with a separate odometry task, and pin stonefish_ros2 before the sim-time merge that broke real-time control. Co-authored-by: Cursor --- .github/workflows/headless-simulation.yml | 5 +- src/mission_executor/src/main.rs | 82 ++++++++++------------- vendor/stonefish_ros2 | 2 +- 3 files changed, 38 insertions(+), 51 deletions(-) diff --git a/.github/workflows/headless-simulation.yml b/.github/workflows/headless-simulation.yml index a16f9a1..e6b71e0 100644 --- a/.github/workflows/headless-simulation.yml +++ b/.github/workflows/headless-simulation.yml @@ -44,10 +44,7 @@ jobs: mission_name:=prequalify \ auv_name:=hydrus \ env_file_name:=hydrus_env_headless.scn \ - headless:=true \ - fast_fixed_step:=true \ - use_sim_time_stamps:=true \ - realtime_factor_cap:=5.0 & + headless:=true & LAUNCH_PID=\$! && \ sleep 15 && \ kill \$LAUNCH_PID 2>/dev/null || true diff --git a/src/mission_executor/src/main.rs b/src/mission_executor/src/main.rs index 452c09e..ec7eb95 100644 --- a/src/mission_executor/src/main.rs +++ b/src/mission_executor/src/main.rs @@ -12,7 +12,7 @@ mod inotify; use std::ops::Bound; use std::sync::Arc; use std::sync::atomic::{AtomicBool, AtomicUsize, Ordering}; -use std::time::Duration; +use std::time::{Duration, Instant}; use arc_swap::ArcSwap; use parry3d_f64::shape::Segment; use tokio::sync::{Mutex, Notify}; @@ -73,10 +73,6 @@ fn wrap_angle(angle: f64) -> f64 { (angle + std::f64::consts::PI).rem_euclid(2.0 * std::f64::consts::PI) - std::f64::consts::PI } -fn pose_stamp_ns(stamp: &r2r::builtin_interfaces::msg::Time) -> i64 { - i64::from(stamp.sec) * 1_000_000_000 + i64::from(stamp.nanosec) -} - impl MissionExecutor { pub fn new(node: r2r::Node) -> Self { // hardcoded so it doesn't freak out while it waits for first odometry @@ -364,6 +360,12 @@ async fn main() { } }; + let consume_odometry_sub = |td: Arc| async move { + while let Some(msg) = odometry_sub.next().await { + td.pose.store(Arc::new(Pose::from(&msg.pose.pose))); + } + }; + let cfg = Arc::new(ArcSwap::from_pointee(load_live_config(&live_config_path, &auv_name).unwrap())); let mut inotify_stream = inotify::InotifyStream::new(); @@ -386,38 +388,19 @@ async fn main() { let go_to_goal = |td: Arc| async move { let mut sum_err = Vector6::zeros(); let mut prev_pose_err = Vector6::zeros(); - let mut previous_timestamp_ns: Option = None; + let mut prev_now = Instant::now(); let mut count = 1.0; //Technically can be an integer but since we are multiplying by float... - while let Some(msg) = odometry_sub.next().await { - if td.stop.load(Ordering::Relaxed) { - break; - } - - let pose = Pose::from(&msg.pose.pose); - td.pose.store(Arc::new(pose)); - + let log_interval = Duration::from_millis(500); + let mut last_log = Instant::now(); + while !td.stop.load(Ordering::Relaxed) { let current_cfg = cfg.load(); let PidConfig { kp, ki, kd } = current_cfg.pid[&bridge_name]; let tam_x_y_z_roll_pitch_yaw = ¤t_cfg.tam; - let timestamp_ns = pose_stamp_ns(&msg.header.stamp); - let dt = match previous_timestamp_ns { - Some(previous) => { - let elapsed_ns = timestamp_ns - previous; - if elapsed_ns > 0 { - elapsed_ns as f64 * 1e-9 - } else { - r2r::log_warn!( - "go_to_goal", - "odometry stamp not increasing (prev={previous} ns, now={timestamp_ns} ns); using 100 ms dt" - ); - 0.1 - } - } - None => 0.1, - }; - previous_timestamp_ns = Some(timestamp_ns); + let now = Instant::now(); + let dt = now.duration_since(prev_now).as_secs_f64(); + let pose = **td.pose.load(); let goal = **td.goal.load(); let current_pose = Vector6::::from(pose); @@ -490,24 +473,30 @@ async fn main() { let mut avg_curr = td.avg_current.lock().await; *avg_curr = (*avg_curr * (count - 1.0) + sum_curr) / count; count += 1.0; - r2r::log_info!( - "thruster_report", - "Average thruster usage in runtime: {:.2}", - *avg_curr - ); - r2r::log_info!( - "thruster_report", - "Current sum of thrusters: {:.2}", - sum_curr - ); - r2r::log_info!( - "thruster_report", - "Estimated battery life remaining: {:.2}", - BATTERY_CAPACITY / *avg_curr - ); + if now.duration_since(last_log) >= log_interval { + r2r::log_info!( + "thruster_report", + "Average thruster usage in runtime: {:.2}", + *avg_curr + ); + r2r::log_info!( + "thruster_report", + "Current sum of thrusters: {:.2}", + sum_curr + ); + r2r::log_info!( + "thruster_report", + "Estimated battery life remaining: {:.2}", + BATTERY_CAPACITY / *avg_curr + ); + last_log = now; + } drop(avg_curr); prev_pose_err = pose_err; + prev_now = now; + + tokio::time::sleep(Duration::from_millis(100)).await; } }; @@ -544,6 +533,7 @@ async fn main() { tokio::spawn(consume_inotify_stream()); tokio::spawn(consume_map_sub(Arc::clone(&td))); + tokio::spawn(consume_odometry_sub(Arc::clone(&td))); tokio::spawn(consume_new_objects(Arc::clone(&td))); tokio::spawn(go_to_goal(Arc::clone(&td))); diff --git a/vendor/stonefish_ros2 b/vendor/stonefish_ros2 index d7366e3..793b8d0 160000 --- a/vendor/stonefish_ros2 +++ b/vendor/stonefish_ros2 @@ -1 +1 @@ -Subproject commit d7366e34ae689d1266b349f7209506c262690850 +Subproject commit 793b8d07c5b9bd16c2213c58b4d6de164a20f8b2 From d1f4d127b974837f98fd32ff999d27d17a3474b6 Mon Sep 17 00:00:00 2001 From: Cruiz102 Date: Wed, 26 Aug 2026 00:25:00 -0700 Subject: [PATCH 7/7] Fix Hydrus instability: class IDs, spawn hold, and attitude gains. Align detection_mocker class enums with mission_executor, fix hydrus_env scenario typos, sync the PID goal to the first odometry pose, and zero roll/pitch gains that were flipping the vehicle under the existing TAM. Co-authored-by: Cursor --- .../data/scenarios/hydrus_auv.scn | 50 +++++++------------ .../data/scenarios/hydrus_auv_headless.scn | 48 ++++++------------ .../data/scenarios/hydrus_env.scn | 4 +- .../data/scenarios/hydrus_env_headless.scn | 8 +-- src/bringup/config/mission_executor.toml | 8 +-- src/bringup/launch/stonefish.launch.py | 12 ++++- .../include/detection_mocker/types.hpp | 13 ++--- src/detection_mocker/src/xml_parser.cpp | 24 ++++----- src/mission_executor/src/main.rs | 24 ++++++++- 9 files changed, 93 insertions(+), 98 deletions(-) diff --git a/src/bridge_stonefish/data/scenarios/hydrus_auv.scn b/src/bridge_stonefish/data/scenarios/hydrus_auv.scn index 9dfa1d0..062b59e 100644 --- a/src/bridge_stonefish/data/scenarios/hydrus_auv.scn +++ b/src/bridge_stonefish/data/scenarios/hydrus_auv.scn @@ -59,10 +59,8 @@ - - - - + + @@ -80,10 +78,8 @@ - - - - + + @@ -101,10 +97,8 @@ - - - - + + @@ -122,10 +116,8 @@ - - - - + + @@ -144,10 +136,8 @@ - - - - + + @@ -165,10 +155,8 @@ - - - - + + @@ -186,10 +174,8 @@ - - - - + + @@ -207,10 +193,8 @@ - - - - + + @@ -220,7 +204,7 @@ - + diff --git a/src/bridge_stonefish/data/scenarios/hydrus_auv_headless.scn b/src/bridge_stonefish/data/scenarios/hydrus_auv_headless.scn index 52dccef..5887afd 100644 --- a/src/bridge_stonefish/data/scenarios/hydrus_auv_headless.scn +++ b/src/bridge_stonefish/data/scenarios/hydrus_auv_headless.scn @@ -60,10 +60,8 @@ - - - - + + @@ -81,10 +79,8 @@ - - - - + + @@ -102,10 +98,8 @@ - - - - + + @@ -123,10 +117,8 @@ - - - - + + @@ -145,10 +137,8 @@ - - - - + + @@ -166,10 +156,8 @@ - - - - + + @@ -187,10 +175,8 @@ - - - - + + @@ -208,10 +194,8 @@ - - - - + + diff --git a/src/bridge_stonefish/data/scenarios/hydrus_env.scn b/src/bridge_stonefish/data/scenarios/hydrus_env.scn index 89db18c..ef925cd 100644 --- a/src/bridge_stonefish/data/scenarios/hydrus_env.scn +++ b/src/bridge_stonefish/data/scenarios/hydrus_env.scn @@ -61,7 +61,7 @@ - + @@ -88,7 +88,7 @@ - + diff --git a/src/bridge_stonefish/data/scenarios/hydrus_env_headless.scn b/src/bridge_stonefish/data/scenarios/hydrus_env_headless.scn index 0a621a6..d4179d7 100644 --- a/src/bridge_stonefish/data/scenarios/hydrus_env_headless.scn +++ b/src/bridge_stonefish/data/scenarios/hydrus_env_headless.scn @@ -59,21 +59,21 @@ - + - + - + @@ -81,7 +81,7 @@ - + diff --git a/src/bringup/config/mission_executor.toml b/src/bringup/config/mission_executor.toml index c776d6b..82e5a58 100644 --- a/src/bringup/config/mission_executor.toml +++ b/src/bringup/config/mission_executor.toml @@ -32,9 +32,11 @@ tam = [ ] [hydrus.pid.stonefish] -kp = [1.0, 1.0, 2.0, -2.0, 2.0, 2.0] -ki = [0.0, 0.0, 0.0, 0.0, 3.3, 2.0] -kd = [0.1, 0.1, 0.0, 4.1, 3.1, 2.0] +# Attitude roll/pitch gains left at 0: the previous non-zero values (esp. kd roll=4.1) +# plus the roll sign flip in mission_executor flipped Hydrus. Tune carefully. +kp = [1.0, 1.0, 2.5, 0.0, 0.0, 1.0] +ki = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] +kd = [0.3, 0.3, 0.5, 0.0, 0.0, 0.5] [proteus] diff --git a/src/bringup/launch/stonefish.launch.py b/src/bringup/launch/stonefish.launch.py index 436063a..3abad13 100644 --- a/src/bringup/launch/stonefish.launch.py +++ b/src/bringup/launch/stonefish.launch.py @@ -130,12 +130,20 @@ def _launch_setup(context, *args, **kwargs): 'rendering_quality': 'low' if fast_fixed_step else 'high', }) - # Setup ROS2 workspace based on current working directory + # Prefer underlay from the active shell, then fall back to common installs. + ros_underlay = os.environ.get('ROS_DISTRO') + if ros_underlay: + ros_underlay_setup = f'/opt/ros/{ros_underlay}/setup.bash' + else: + ros_underlay_setup = '/opt/ros/jazzy/setup.bash' + if not os.path.exists(ros_underlay_setup): + ros_underlay_setup = '/opt/ros/humble/setup.bash' ros_setup = os.path.join(cwd, 'install', 'setup.bash') # Common command common_cmd = ( - f'source {ros_setup} && ros2 run mission_executor mission_executor ' + f'source {ros_underlay_setup} && source {ros_setup} && ' + f'ros2 run mission_executor mission_executor ' f'--ros-args -p mission_name:={mission_name} ' f'-p bridge_name:=stonefish ' f'-p auv_name:={auv_name} ' diff --git a/src/detection_mocker/include/detection_mocker/types.hpp b/src/detection_mocker/include/detection_mocker/types.hpp index 27430e0..431dfba 100644 --- a/src/detection_mocker/include/detection_mocker/types.hpp +++ b/src/detection_mocker/include/detection_mocker/types.hpp @@ -22,14 +22,15 @@ namespace detection_mocker /** * @brief The Classifer Class Types from Vision. */ + // Must match mission_executor ObjectCls (src/mission_executor/src/main.rs). enum class ClassType { - CUBE = 1, - RECTANGLE=2, - GATE=3, - SHARK=3, - OTHER=4, - + CUBE = 0, + RECTANGLE = 1, + GATE = 2, + SHARK = 3, + OTHER = 4, + SWORD_FISH = 5, }; diff --git a/src/detection_mocker/src/xml_parser.cpp b/src/detection_mocker/src/xml_parser.cpp index 5ebfb16..bd4dd11 100644 --- a/src/detection_mocker/src/xml_parser.cpp +++ b/src/detection_mocker/src/xml_parser.cpp @@ -202,21 +202,17 @@ namespace detection_mocker { // TODO: Make This A Function if More Classes Are Added. std::string cls = std::string(cls_str); - if( cls == "gate" ) - { + if (cls == "gate") { obj.cls = ClassType::GATE; - } - else if(cls == "cube"){ - obj.cls = ClassType::CUBE; - } - else if(cls == "shark"){ - obj.cls = ClassType::SHARK; - } - else if(cls == "rectangle"){ - obj.cls = ClassType::RECTANGLE; - } - else - { + } else if (cls == "cube") { + obj.cls = ClassType::CUBE; + } else if (cls == "shark") { + obj.cls = ClassType::SHARK; + } else if (cls == "rectangle") { + obj.cls = ClassType::RECTANGLE; + } else if (cls == "swordfish" || cls == "sword_fish") { + obj.cls = ClassType::SWORD_FISH; + } else { obj.cls = ClassType::OTHER; } } diff --git a/src/mission_executor/src/main.rs b/src/mission_executor/src/main.rs index ec7eb95..3708c58 100644 --- a/src/mission_executor/src/main.rs +++ b/src/mission_executor/src/main.rs @@ -63,6 +63,7 @@ struct MissionExecutor { pub goal: ArcSwap>, pub stop: AtomicBool, pub avg_current: Arc>, + pub odometry_ready: AtomicBool, } const CLOSE_ENOUGH: f64 = 1.0; @@ -90,6 +91,7 @@ impl MissionExecutor { goal: ArcSwap::new(Arc::new(goal)), stop: AtomicBool::new(false), avg_current: Arc::new(Mutex::new(0.0)), + odometry_ready: AtomicBool::new(false), } } @@ -362,7 +364,15 @@ async fn main() { let consume_odometry_sub = |td: Arc| async move { while let Some(msg) = odometry_sub.next().await { - td.pose.store(Arc::new(Pose::from(&msg.pose.pose))); + let pose = Pose::from(&msg.pose.pose); + // Hold station at spawn: set goal before pose so the PID never sees a mismatch. + if !td.odometry_ready.load(Ordering::Relaxed) { + td.goal.store(Arc::new(Vector6::from(pose))); + td.pose.store(Arc::new(pose)); + td.odometry_ready.store(true, Ordering::Release); + } else { + td.pose.store(Arc::new(pose)); + } } }; @@ -393,12 +403,20 @@ async fn main() { let log_interval = Duration::from_millis(500); let mut last_log = Instant::now(); while !td.stop.load(Ordering::Relaxed) { + if !td.odometry_ready.load(Ordering::Acquire) { + tokio::time::sleep(Duration::from_millis(50)).await; + prev_now = Instant::now(); + prev_pose_err = Vector6::zeros(); + sum_err = Vector6::zeros(); + continue; + } let current_cfg = cfg.load(); let PidConfig { kp, ki, kd } = current_cfg.pid[&bridge_name]; let tam_x_y_z_roll_pitch_yaw = ¤t_cfg.tam; let now = Instant::now(); - let dt = now.duration_since(prev_now).as_secs_f64(); + // Clamp dt so a stall or first tick after ready cannot explode I/D terms. + let dt = now.duration_since(prev_now).as_secs_f64().clamp(1e-3, 0.2); let pose = **td.pose.load(); let goal = **td.goal.load(); @@ -408,6 +426,8 @@ async fn main() { // r2r::log_info!("pose", "{current_pose:?}"); let mut pose_err = goal - current_pose; + pose_err[3] = wrap_angle(pose_err[3]); + pose_err[4] = wrap_angle(pose_err[4]); let rot = UnitQuaternion::from_quaternion(pose.rot); let forward = rot * Vector3::y(); // if vehicle’s forward is +Y in body frame