├── .dockerignore ├── .github ├── dependabot.yml └── workflows │ └── build_test.yml ├── .gitignore ├── LICENSE ├── README.md ├── docker ├── Dockerfile ├── README.md ├── build.sh └── run.sh ├── images ├── gazebo.png └── rviz2.png ├── orca_base ├── CMakeLists.txt ├── README.md ├── include │ └── orca_base │ │ ├── base_context.hpp │ │ └── underwater_motion.hpp ├── package.xml ├── scripts │ └── test_base.py └── src │ ├── base_controller.cpp │ ├── camera_info_publisher.cpp │ ├── manager.cpp │ ├── odom_to_path_node.cpp │ ├── pose_to_path_node.cpp │ └── underwater_motion.cpp ├── orca_bringup ├── CMakeLists.txt ├── README.md ├── behavior_trees │ └── orca4_bt.xml ├── cfg │ ├── sim_launch.rviz │ ├── sim_left.ini │ ├── sim_right.ini │ └── sub.parm ├── launch │ ├── bringup.py │ ├── navigation_launch.py │ └── sim_launch.py ├── missions │ └── missions.txt ├── package.xml ├── params │ ├── nav2_params.yaml │ ├── rosbag2_record_qos.yaml │ ├── sim_mavros_params.yaml │ └── sim_orca_params.yaml └── scripts │ ├── auv.sh │ ├── disarm.sh │ ├── dump_rosout.py │ ├── mission_runner.py │ └── rov.sh ├── orca_description ├── CMakeLists.txt ├── README.md ├── models │ └── orca4 │ │ ├── meshes │ │ ├── bluerov2.dae │ │ ├── t200-ccw-prop.dae │ │ └── t200-cw-prop.dae │ │ ├── model.config │ │ ├── model.sdf │ │ └── model.sdf.in ├── package.xml ├── scripts │ └── generate_model.py └── worlds │ └── sand.world ├── orca_msgs ├── CMakeLists.txt ├── README.md ├── action │ └── TargetMode.action ├── msg │ ├── Effort.msg │ └── Motion.msg └── package.xml ├── orca_nav2 ├── CMakeLists.txt ├── README.md ├── goal_checker_3d_plugin.xml ├── include │ └── orca_nav2 │ │ └── param_macro.hpp ├── package.xml ├── progress_checker_3d_plugin.xml ├── pure_pursuit_controller_3d_plugin.xml ├── src │ ├── goal_checker_3d.cpp │ ├── progress_checker_3d.cpp │ ├── pure_pursuit_controller_3d.cpp │ └── straight_line_planner_3d.cpp └── straight_line_planner_3d_plugin.xml ├── orca_shared ├── CMakeLists.txt ├── README.md ├── include │ └── orca_shared │ │ ├── model.hpp │ │ ├── pwm.hpp │ │ └── util.hpp ├── package.xml └── src │ ├── model.cpp │ ├── pwm.cpp │ └── util.cpp ├── setup.bash └── workspace.repos /.dockerignore: -------------------------------------------------------------------------------- 1 | .idea/ 2 | cmake-build-* 3 | .cmake-build-* 4 | -------------------------------------------------------------------------------- /.github/dependabot.yml: -------------------------------------------------------------------------------- 1 | version: 2 2 | updates: 3 | - package-ecosystem: github-actions 4 | directory: "/" 5 | schedule: 6 | interval: "weekly" 7 | -------------------------------------------------------------------------------- /.github/workflows/build_test.yml: -------------------------------------------------------------------------------- 1 | name: ROS2 CI 2 | 3 | on: 4 | pull_request: 5 | push: 6 | branches: 7 | - 'main' 8 | 9 | jobs: 10 | test_environment: 11 | runs-on: ubuntu-latest 12 | strategy: 13 | fail-fast: false 14 | 15 | matrix: 16 | ros_distribution: [humble] 17 | 18 | include: 19 | - ros_distribution: humble 20 | docker_image: rostooling/setup-ros-docker:ubuntu-jammy-ros-humble-ros-base-latest 21 | 22 | container: 23 | image: ${{ matrix.docker_image }} 24 | 25 | steps: 26 | - run: mkdir tmp_ws 27 | 28 | - uses: actions/checkout@v4 29 | with: 30 | path: tmp_ws 31 | 32 | # Note that action-ros-ci will rm -rf ros_ws 33 | - uses: ros-tooling/action-ros-ci@0.4.3 34 | with: 35 | package-name: | 36 | orca_base 37 | orca_bringup 38 | orca_description 39 | orca_msgs 40 | orca_nav2 41 | orca_shared 42 | target-ros2-distro: ${{ matrix.ros_distribution }} 43 | vcs-repo-file-url: tmp_ws/workspace.repos 44 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | cmake-build-* 2 | .cmake-build-* 3 | .idea 4 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2022 Clyde McQueen 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Orca4 ![ROS2 CI](https://github.com/clydemcqueen/orca4/actions/workflows/build_test.yml/badge.svg?branch=main) 2 | 3 | Orca4 is a set of [ROS2](http://www.ros.org/) packages that provide basic AUV (Autonomous Underwater 4 | Vehicle) functionality for the [BlueRobotics BlueROV2](https://www.bluerobotics.com). 5 | 6 | Orca4 uses [ArduSub](http://www.ardusub.com/) as the flight controller and 7 | [mavros](https://github.com/mavlink/mavros) as the GCS. 8 | 9 | Orca4 runs in [Gazebo Harmonic](https://gazebosim.org/home) using the standard buoyancy, hydrodynamics and thruster 10 | plugins. The connection between ArduSub and Gazebo is provided by [ardupilot_gazebo](https://github.com/ArduPilot/ardupilot_gazebo). 11 | 12 | ## Sensors 13 | 14 | The BlueROV2 provides the following interesting sensors: 15 | * An [external barometer](https://bluerobotics.com/product-category/sensors-sonars-cameras/sensors/) provides depth 16 | * An IMU provides attitude 17 | 18 | Orca4 adds a simulated down-facing stereo camera and [ORB_SLAM2](https://github.com/clydemcqueen/orb_slam_2_ros/tree/orca4_galactic) 19 | to generate a 3D pose as long as the camera has a good view of the seafloor. 20 | The pose is sent to ArduSub and fused with the other sensor information. 21 | 22 | If there is no view of the seafloor a synthetic pose is generated based on the last good pose and a simple motion model. 23 | 24 | See [orca_base](orca_base/README.md) for details. 25 | 26 | ## Navigation 27 | 28 | Orca4 uses the [Navigation2](https://navigation.ros.org/index.html) framework for mission 29 | planning and navigation. Several simple Nav2 plugins are provided to work in a 3D environment: 30 | * straight_line_planner_3d 31 | * pure_pursuit_3d 32 | * progress_checker_3d 33 | * goal_checker_3d 34 | 35 | See [orca_nav2](orca_nav2/README.md) for details. 36 | 37 | ## Installation 38 | 39 | See the [Dockerfile](docker/Dockerfile) for installation details. 40 | 41 | Install these packages: 42 | * [ROS2 Humble](https://docs.ros.org/en/humble/Installation.html) 43 | * [Gazebo Harmonic](https://gazebosim.org/docs/harmonic/install) 44 | * [ros_gz for Humble + Harmonic (ros-humble-gzharmonic)](https://gazebosim.org/docs/latest/ros_installation/) 45 | * [ardupilot_gazebo for Harmonic](https://github.com/ArduPilot/ardupilot_gazebo) 46 | * [ArduSub](https://ardupilot.org/dev/docs/building-setup-linux.html) 47 | 48 | Build ArduSub for SITL: 49 | ~~~ 50 | cd ~/ardupilot 51 | ./waf configure --board sitl 52 | ./waf sub 53 | ~~~ 54 | 55 | Populate the workspace: 56 | ~~~ 57 | mkdir -p ~/colcon_ws/src 58 | cd colcon_ws/src 59 | git clone https://github.com/clydemcqueen/orca4 60 | vcs import < orca4/workspace.repos 61 | ~~~ 62 | 63 | 64 | Get dependencies: 65 | ~~~ 66 | rosdep update 67 | rosdep install -y --from-paths . --ignore-src 68 | ~~~ 69 | 70 | MAVROS depends on GeographicLib, and [GeographicLib needs some datasets](https://ardupilot.org/dev/docs/ros-install.html): 71 | ~~~ 72 | wget https://raw.githubusercontent.com/mavlink/mavros/master/mavros/scripts/install_geographiclib_datasets.sh 73 | chmod a+x install_geographiclib_datasets.sh 74 | sudo ./install_geographiclib_datasets.sh 75 | ~~~ 76 | 77 | Build the workspace: 78 | ~~~ 79 | cd ~/colcon_ws 80 | colcon build 81 | ~~~ 82 | 83 | ## Simulation 84 | 85 | In a terminal run: 86 | ~~~ 87 | source src/orca4/setup.bash 88 | ros2 launch orca_bringup sim_launch.py 89 | ~~~ 90 | 91 | This will bring up all of the components, including the Gazebo UI. 92 | The surface of the water is at Z=0 and the sub will be sitting at the surface. 93 | The world contains a sandy seafloor 10 meters below the surface. 94 | 95 | ![GAZEBO GUI](images/gazebo.png) 96 | 97 | You should see ArduSub establish a connection to the ardupilot_gazebo plugin: 98 | ~~~ 99 | [ardusub-2] JSON received: 100 | [ardusub-2] timestamp 101 | [ardusub-2] imu: gyro 102 | [ardusub-2] imu: accel_body 103 | [ardusub-2] position 104 | [ardusub-2] quaternion 105 | [ardusub-2] velocity 106 | ~~~ 107 | 108 | At this point SLAM is not running because the seafloor is too far away, 109 | but the sub can still move using dead-reckoning. 110 | The [base_controller](orca_base/src/base_controller.cpp) node will send default camera poses to ArduSub 111 | to warm up the EKF and the [manager](orca_base/src/manager.cpp) node will request attitude information at 20Hz. 112 | Initialization completes when there is a good pose from the EKF: 113 | 114 | ~~~ 115 | [mavros_node-8] [INFO] [mavros.imu/handle_attitude_quaternion]: IMU: Attitude quaternion IMU detected! 116 | [manager-9] [INFO] [manager/operator()]: EKF is running 117 | [base_controller-10] [INFO] [base_controller/change_state]: EKF is running, state => RUN_NO_MAP 118 | [base_controller-10] [INFO] [base_controller/UnderwaterMotion]: initialize odometry to {{-2.52304e-05, -3.28182e-05, -0.228547}, {0, 0, -5.00936e-05}} 119 | ~~~ 120 | 121 | Execute a mission in a second terminal: 122 | ~~~ 123 | source src/orca4/setup.bash 124 | ros2 run orca_bringup mission_runner.py 125 | ~~~ 126 | 127 | ![RVIZ2_GUI](images/rviz2.png) 128 | 129 | The default mission will dive to -7m and move in a large rectangle. 130 | At -6m the cameras will pick up a view of the seafloor at and ORB_SLAM2 will start: 131 | ~~~ 132 | [orb_slam2_ros_stereo-13] New map created with 571 points 133 | [base_controller-10] [INFO] [base_controller/change_state]: map created, state => RUN_LOCALIZED 134 | ~~~ 135 | 136 | 137 | You should notice a loop closure sometime during the 2nd run around the rectangle. The adjustment is very small. 138 | 139 | ~~~ 140 | [orb_slam2_ros_stereo-13] Loop detected! 141 | [orb_slam2_ros_stereo-13] Local Mapping STOP 142 | [orb_slam2_ros_stereo-13] Local Mapping RELEASE 143 | [orb_slam2_ros_stereo-13] Starting Global Bundle Adjustment 144 | [orb_slam2_ros_stereo-13] Global Bundle Adjustment finished 145 | [orb_slam2_ros_stereo-13] Updating map ... 146 | [orb_slam2_ros_stereo-13] Local Mapping STOP 147 | [orb_slam2_ros_stereo-13] Local Mapping RELEASE 148 | [orb_slam2_ros_stereo-13] Map updated! 149 | ~~~ 150 | 151 | ## Packages 152 | 153 | * [`orca_base` Base controller, localization, frames](orca_base) 154 | * [`orca_bringup` Launch files](orca_bringup) 155 | * [`orca_description` SDF files](orca_description) 156 | * [`orca_msgs` Custom messages](orca_msgs) 157 | * [`orca_nav2` Nav2 plugins](orca_nav2) 158 | * [`orca_shared` Dynamics model, shared utilities](orca_shared) 159 | -------------------------------------------------------------------------------- /docker/Dockerfile: -------------------------------------------------------------------------------- 1 | FROM osrf/ros:humble-desktop AS base 2 | 3 | ARG USERNAME=orca4 4 | ARG USER_UID=1000 5 | ARG USER_GID=$USER_UID 6 | ARG DEBIAN_FRONTEND=noninteractive 7 | 8 | RUN apt-get update && apt-get upgrade -y 9 | 10 | # Install a few handy tools 11 | RUN apt-get update \ 12 | && apt-get -y --quiet --no-install-recommends install \ 13 | bash-completion \ 14 | build-essential \ 15 | git \ 16 | glmark2 \ 17 | gnupg \ 18 | iputils-ping \ 19 | lsb-release \ 20 | mlocate \ 21 | software-properties-common \ 22 | sudo \ 23 | wget \ 24 | vim \ 25 | && rm -rf /var/lib/apt/lists/* 26 | 27 | # Install Gazebo Harmonic for use with ROS2 Humble 28 | RUN wget https://packages.osrfoundation.org/gazebo.gpg -O /usr/share/keyrings/pkgs-osrf-archive-keyring.gpg 29 | RUN echo "deb [arch=$(dpkg --print-architecture) signed-by=/usr/share/keyrings/pkgs-osrf-archive-keyring.gpg] http://packages.osrfoundation.org/gazebo/ubuntu-stable $(lsb_release -cs) main" | tee /etc/apt/sources.list.d/gazebo-stable.list > /dev/null 30 | RUN apt-get update \ 31 | && apt-get -y --quiet --no-install-recommends install \ 32 | gz-harmonic \ 33 | ros-humble-ros-gzharmonic \ 34 | && rm -rf /var/lib/apt/lists/* 35 | 36 | # Install NVIDIA software 37 | RUN apt-get update \ 38 | && apt-get -y --quiet --no-install-recommends install \ 39 | libglvnd0 \ 40 | libgl1 \ 41 | libglx0 \ 42 | libegl1 \ 43 | libxext6 \ 44 | libx11-6 \ 45 | && rm -rf /var/lib/apt/lists/* \ 46 | ENV NVIDIA_VISIBLE_DEVICES=all 47 | ENV NVIDIA_DRIVER_CAPABILITIES=graphics,utility,compute 48 | ENV QT_X11_NO_MITSHM=1 49 | 50 | # Install some ardupilot and ardupilot_gazebo prereqs (libgz-sim8-dev is for gz harmonic) 51 | RUN apt-get update \ 52 | && apt-get -y --quiet --no-install-recommends install \ 53 | python3-wxgtk4.0 \ 54 | rapidjson-dev \ 55 | xterm \ 56 | libgz-sim8-dev \ 57 | libopencv-dev \ 58 | libgstreamer1.0-dev \ 59 | libgstreamer-plugins-base1.0-dev \ 60 | gstreamer1.0-plugins-bad \ 61 | gstreamer1.0-libav \ 62 | gstreamer1.0-gl \ 63 | && rm -rf /var/lib/apt/lists/* 64 | 65 | # Create a non-root user 66 | # Required for ArduSub install, but generally a good idea 67 | RUN groupadd --gid $USER_GID $USERNAME \ 68 | && useradd -s /bin/bash --uid $USER_UID --gid $USER_GID -m $USERNAME \ 69 | && echo $USERNAME ALL=\(root\) NOPASSWD:ALL > /etc/sudoers.d/$USERNAME \ 70 | && chmod 0440 /etc/sudoers.d/$USERNAME \ 71 | && echo "\n# Added by orca4 Dockerfile:" >> /home/$USERNAME/.bashrc \ 72 | && echo "source /usr/share/bash-completion/completions/git" >> /home/$USERNAME/.bashrc 73 | 74 | # Switch to our new user 75 | USER $USERNAME 76 | ENV USER=$USERNAME 77 | 78 | # Clone ArduSub code 79 | WORKDIR /home/$USERNAME 80 | RUN git clone https://github.com/ArduPilot/ardupilot.git --recurse-submodules 81 | 82 | # Install ArduSub prereqs (this also appends to .bashrc) 83 | WORKDIR /home/$USERNAME/ardupilot 84 | ENV SKIP_AP_EXT_ENV=1 SKIP_AP_GRAPHIC_ENV=1 SKIP_AP_COV_ENV=1 SKIP_AP_GIT_CHECK=1 85 | RUN Tools/environment_install/install-prereqs-ubuntu.sh -y 86 | 87 | # Build ArduSub 88 | # Note: waf will capture all of the environment variables in ardupilot/.lock-waf_linux_build. 89 | # Any change to enviroment variables will cause a re-build. 90 | # To avoid this call sim_vehicle.py with the "--no-rebuild" option. 91 | WORKDIR /home/$USERNAME/ardupilot 92 | RUN modules/waf/waf-light configure --board sitl \ 93 | && modules/waf/waf-light build --target bin/ardusub 94 | 95 | # Clone ardupilot_gazebo code 96 | WORKDIR /home/$USERNAME 97 | RUN git clone https://github.com/ArduPilot/ardupilot_gazebo.git 98 | 99 | # Build for Gazebo Harmonic, define this before building ardupilot_gazebo and ros_gz 100 | ENV GZ_VERSION=harmonic 101 | 102 | # Build ardupilot_gazebo 103 | RUN [ "/bin/bash" , "-c" , " \ 104 | cd ardupilot_gazebo \ 105 | && git checkout 4b30a3d8 \ 106 | && mkdir build \ 107 | && cd build \ 108 | && cmake .. -DCMAKE_BUILD_TYPE=RelWithDebInfo \ 109 | && make -j4" ] 110 | 111 | # Break orca4 build into multiple stages to speed up 'docker build': 112 | # -- work that depends on workspace.repos 113 | # -- work that depends on orca4 package.xml files 114 | # -- everything else 115 | 116 | # Create colcon workspace and copy workspace.repos file 117 | WORKDIR /home/$USERNAME 118 | RUN mkdir -p colcon_ws/src/orca4 119 | COPY --chown=$USER_UID:$USER_GID workspace.repos colcon_ws/src/orca4 120 | 121 | # Get workspace repos 122 | WORKDIR /home/$USERNAME/colcon_ws/src 123 | RUN vcs import < orca4/workspace.repos 124 | 125 | # Run rosdep over workspace repos 126 | WORKDIR /home/$USERNAME/colcon_ws 127 | RUN rosdep update \ 128 | && rosdep install -y --from-paths . --ignore-src 129 | 130 | # Build everything so far 131 | RUN [ "/bin/bash" , "-c" , "\ 132 | source /opt/ros/humble/setup.bash \ 133 | && colcon build" ] 134 | 135 | # Copy orca4 package.xml files 136 | COPY --chown=$USER_UID:$USER_GID orca_base/package.xml src/orca4/orca_base/ 137 | COPY --chown=$USER_UID:$USER_GID orca_bringup/package.xml src/orca4/orca_bringup/ 138 | COPY --chown=$USER_UID:$USER_GID orca_description/package.xml src/orca4/orca_description/ 139 | COPY --chown=$USER_UID:$USER_GID orca_msgs/package.xml src/orca4/orca_msgs/ 140 | COPY --chown=$USER_UID:$USER_GID orca_nav2/package.xml src/orca4/orca_nav2/ 141 | COPY --chown=$USER_UID:$USER_GID orca_shared/package.xml src/orca4/orca_shared/ 142 | 143 | # Run rosdep over orca4 package.xml files 144 | RUN rosdep update \ 145 | && rosdep install -y --from-paths . --ignore-src 146 | 147 | # MAVROS depends on GeographicLib, and GeographicLib needs some datasets 148 | RUN [ "/bin/bash" , "-c" , "\ 149 | wget https://raw.githubusercontent.com/mavlink/mavros/master/mavros/scripts/install_geographiclib_datasets.sh \ 150 | && chmod +x install_geographiclib_datasets.sh \ 151 | && sudo ./install_geographiclib_datasets.sh" ] 152 | 153 | # Copy everything 154 | COPY --chown=$USER_UID:$USER_GID . src/orca4 155 | 156 | # Build everything 157 | RUN [ "/bin/bash" , "-c" , "\ 158 | source /opt/ros/humble/setup.bash \ 159 | && colcon build" ] 160 | 161 | # Set up the environment 162 | WORKDIR /home/$USERNAME/colcon_ws 163 | RUN echo "export PATH=/home/$USERNAME/.local/bin:\$PATH" >> /home/$USERNAME/.bashrc \ 164 | && echo "export PATH=/home/$USERNAME/.local/lib/python3.10/site-packages:\$PATH" >> /home/$USERNAME/.bashrc \ 165 | && echo "source /home/$USERNAME/colcon_ws/src/orca4/setup.bash" >> /home/$USERNAME/.bashrc 166 | 167 | # Required to use the --console option on sim_vehicle.py: 168 | # RUN pip3 install matplotlib 169 | -------------------------------------------------------------------------------- /docker/README.md: -------------------------------------------------------------------------------- 1 | To build the docker image: 2 | ~~~ 3 | ./build.sh 4 | ~~~ 5 | 6 | To launch Gazebo, RViz, all nodes: 7 | ~~~ 8 | ./run.sh 9 | ros2 launch orca_bringup sim_launch.py 10 | ~~~ 11 | 12 | To execute a mission: 13 | ~~~ 14 | docker exec -it orca4 /bin/bash 15 | ros2 run orca_bringup mission_runner.py 16 | ~~~ 17 | -------------------------------------------------------------------------------- /docker/build.sh: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env bash 2 | 3 | DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" >/dev/null 2>&1 && pwd )" 4 | 5 | cd $DIR 6 | 7 | docker build -f $DIR/Dockerfile -t orca4:latest .. 8 | -------------------------------------------------------------------------------- /docker/run.sh: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env bash 2 | 3 | XAUTH=/tmp/.docker.xauth 4 | if [ ! -f $XAUTH ] 5 | then 6 | xauth_list=$(xauth nlist $DISPLAY) 7 | xauth_list=$(sed -e 's/^..../ffff/' <<< "$xauth_list") 8 | if [ ! -z "$xauth_list" ] 9 | then 10 | echo "$xauth_list" | xauth -f $XAUTH nmerge - 11 | else 12 | touch $XAUTH 13 | fi 14 | chmod a+r $XAUTH 15 | fi 16 | 17 | # Specific for NVIDIA drivers, required for OpenGL >= 3.3 18 | docker run -it \ 19 | --rm \ 20 | --name orca4 \ 21 | -e DISPLAY \ 22 | -e QT_X11_NO_MITSHM=1 \ 23 | -e XAUTHORITY=$XAUTH \ 24 | -e NVIDIA_VISIBLE_DEVICES=all \ 25 | -e NVIDIA_DRIVER_CAPABILITIES=all \ 26 | -v "$XAUTH:$XAUTH" \ 27 | -v "/tmp/.X11-unix:/tmp/.X11-unix" \ 28 | -v "/etc/localtime:/etc/localtime:ro" \ 29 | -v "/dev/input:/dev/input" \ 30 | --privileged \ 31 | --security-opt seccomp=unconfined \ 32 | --gpus all \ 33 | orca4:latest 34 | -------------------------------------------------------------------------------- /images/gazebo.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/clydemcqueen/orca4/b4ae89d42f53cd89ab9be88ab2a547e6a74f1bad/images/gazebo.png -------------------------------------------------------------------------------- /images/rviz2.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/clydemcqueen/orca4/b4ae89d42f53cd89ab9be88ab2a547e6a74f1bad/images/rviz2.png -------------------------------------------------------------------------------- /orca_base/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.12.2) 2 | project(orca_base) 3 | 4 | if(NOT CMAKE_CXX_STANDARD) 5 | set(CMAKE_CXX_STANDARD 17) 6 | endif() 7 | 8 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 9 | add_compile_options(-Wall -Wextra -Wpedantic) 10 | endif() 11 | 12 | find_package(ament_cmake_auto REQUIRED) 13 | ament_auto_find_build_dependencies() 14 | 15 | ament_auto_add_executable( 16 | base_controller 17 | src/base_controller.cpp 18 | src/underwater_motion.cpp 19 | ) 20 | 21 | target_include_directories(base_controller PRIVATE include) 22 | 23 | ament_auto_add_executable( 24 | camera_info_publisher 25 | src/camera_info_publisher.cpp 26 | ) 27 | 28 | target_include_directories(camera_info_publisher PRIVATE include) 29 | 30 | ament_auto_add_executable( 31 | manager 32 | src/manager.cpp 33 | ) 34 | 35 | target_include_directories(manager PRIVATE include) 36 | 37 | # Load & run linters listed in package.xml 38 | if(BUILD_TESTING) 39 | find_package(ament_lint_auto REQUIRED) 40 | ament_lint_auto_find_test_dependencies() 41 | endif() 42 | 43 | # Install scripts 44 | install( 45 | PROGRAMS # PROGRAMS sets execute bits, FILES clears them 46 | scripts/test_base.py 47 | DESTINATION lib/${PROJECT_NAME} 48 | ) 49 | 50 | ament_auto_package() -------------------------------------------------------------------------------- /orca_base/README.md: -------------------------------------------------------------------------------- 1 | # Orca4 Design 2 | 3 | ## Sensors 4 | 5 | * BlueRobotics [Bar30 depth sensor](https://bluerobotics.com/store/sensors-sonars-cameras/sensors/bar30-sensor-r1/) 6 | * 2 IMUs on the [Pixhawk](https://ardupilot.org/copter/docs/common-pixhawk-overview.html) 7 | * A down-facing stereo camera 8 | 9 | ## Current Use Cases 10 | 11 | ORB_SLAM2 detects and manages a single continuous map of ORB features. 12 | 13 | Case 1: The sub starts at the surface with good visibility of the seafloor and maintains visibility 14 | during the mission. 15 | 16 | Case 2: The sub starts at the surface, but the seafloor is not visible. The sub dives to 17 | obtain good visibility of the seafloor. Once the seafloor is discovered the mapping begins. 18 | 19 | ## Future Use Cases 20 | 21 | Case 3: After mapping starts the sub loses visibility of the seafloor and enters into recovery 22 | actions to rediscover a previously mapped portion of the seafloor. No mapping occurs during 23 | recovery. 24 | 25 | Case 4: Visibility is lost and the sub discovers an unmapped patch of seafloor and starts building 26 | a separate map. This may repeat. The origins of the subsequent maps are set by dead reckoning. 27 | The map origins may be adjusted over time as exploration continues. Maps may be merged if they 28 | are found to overlap. 29 | 30 | Case 5: A GPS sensor is available to provide good poses at the surface. These poses can be used to 31 | improve the origin poses of the map(s). The sub may ascend and descend while mapping regions. 32 | 33 | ## Coordinate Frames 34 | 35 | Orca4 uses the 3 coordinate frames described in [ROS REP 105](https://www.ros.org/reps/rep-0105.html): 36 | **base_link**, **odom** and **map**. A fourth **slam** frame is used to handle the case when mapping 37 | starts late. 38 | 39 | ### base_link 40 | 41 | The base_link frame is a body frame, centered on the sub itself. 42 | 43 | The ROS body frame convention is FLU: x-forward, y-left, z-up. 44 | 45 | The ArduSub body frame convention is FRD: x-forward, y-right, z-down. 46 | 47 | ### odom 48 | 49 | The odom frame is a world frame used to describe the sub's pose in the local environment. 50 | 51 | The ROS world frame convention is ENU: x-east, y-north, z-up. 52 | 53 | The ArduSub world frame convention is NED: x-north, y-east, z-down. 54 | 55 | The initial pose is {0, 0, 0} facing east, where the z origin is the surface of the water. 56 | 57 | The pose is predicted from the Nav2-generated desired velocity and a simple motion model. 58 | 59 | ArduSub does not know about the odom frame. 60 | 61 | In the simulation you can see the error between the simple Orca4 motion model and the more accurate 62 | Gazebo motion model by observing the TF chain map -> odom -> base_link. 63 | 64 | ### map 65 | 66 | The map frame is a world frame used to describe the sub's pose in the global environment. 67 | 68 | The initial pose is {0, 0, 0} facing east, where the z origin is the surface of the water. 69 | 70 | The map frame is the same as a MAVLink LOCAL frame such as MAV_FRAME_LOCAL_ENU. 71 | The ArduSub EKF is running in the map frame. 72 | 73 | ArduSub also keeps a MAVLink GLOBAL frame that is expressed in altitude, latitude and longitude. 74 | Orca4 does not use this GLOBAL frame. 75 | 76 | ### slam 77 | 78 | The slam frame records the global position of the sub when the ORB_SLAM2 first generates a map. 79 | 80 | In future use cases there will be multiple slam frames, one for each SLAM-generated map. 81 | 82 | ### Helper frames 83 | 84 | By default ORB_SLAM2 assumes that the stereo camera is facing forward. The **down** frame is rotated 85 | to point the camera down towards the seafloor. 86 | 87 | MAVROS handles NED/ENU and FLU/FRD translations, and publishes transforms for helper frames 88 | **map_ned**, **odom_ned** and **base_link_frd**. 89 | 90 | ## Control Summary 91 | 92 | The [Manager](src/manager.cpp) node manages the state of ArduSub, MAVROS and Orca4. 93 | 94 | The [BaseController](src/base_controller.cpp) node sends `VISION_POSITION_ESTIMATE` messages to ArduSub at 5Hz. 95 | The ArduSub EKF fuses these poses with the barometer and IMU readings. 96 | * When the system boots the default pose {0, 0, 0} is sent to warm the ArduSub EKF 97 | * If the sub moves before mapping starts (dead reckoning), the pose generated by the BaseController motion model is sent 98 | * Once mapping starts the pose from ORB_SLAM2 is sent 99 | 100 | Nav2 uses the latest map pose to plan a route at 1Hz. 101 | Nav2 publishes a desired velocity vector on `/cmd_vel` at 20Hz. 102 | 103 | BaseController turns the desired velocity vector into `SET_POSITION_TARGET_GLOBAL_INT` messages to 104 | set the desired depth, and into `OVERRIDE_RC` messages to control the x and yaw RC inputs. 105 | Both messages are sent at 20Hz. 106 | 107 | ArduSub is in ALT_HOLD mode. ArduSub uses a PID controller to hold the target depth. 108 | ArduSub blends the PID outputs with the RC overrides to drive the thrusters. 109 | 110 | ## Life of a Pose 111 | 112 | _This narrative follows the life of a single map -> base_link pose, showing how it moves from 113 | system to system. Mapping has started. Helper transforms are ignored._ 114 | 115 | Gazebo calculates the actual (ground truth) pose of the sub using the Thruster, Buoyancy 116 | and Hydrodynamics plugins. (This is published on `/model/orca/odometry` by ros_ign and is visible 117 | in Rviz2.) 118 | 119 | ArduPilotPlugin sends the pose to ArduSub, along with gyro and accel data from a simulated IMU. 120 | 121 | ArduSub generates noisy measurements for 2 fake IMUs and a fake barometer. 122 | (I'm not entirely sure who generates the IMU noise -- Gazebo or ArduSub or both?) 123 | 124 | Gazebo also generates simulated camera images and ros_ign publishes them on `/stereo_left` and 125 | `/stereo_right`. 126 | 127 | ORB_SLAM2 processes the camera images, detects ORB features, computes a camera pose, and 128 | publishes it on `/orb_slam2_stereo_node/pose`. 129 | 130 | BaseController subscribes to `/orb_slam2_stereo_node/pose`, computes the pose of the sub (vs 131 | the camera), and publishes it on `/mavros/vision_pose/pose`. 132 | BaseController also publishes the map -> odom transform. 133 | 134 | MAVROS subscribes to `/mavros/vision_pose/pose` and sends a `VISION_POSITION_ESTIMATE` 135 | message to ArduSub. 136 | 137 | ArduSub fuses the vision, barometer and IMU measurements into an estimated pose. 138 | ArduSub splits this pose into 2 messages: LOCAL_POSITION (x, y, z) message and ATTITUDE (roll, pitch, yaw). 139 | 140 | MAVROS recombines the LOCAL_POSITION and ATTITUDE information into a pose and publishes it on 141 | `/mavros/local_position/pose`. 142 | 143 | Nav2 subscribe to all transforms, plans a route at 1Hz, and publishes the desired x, z and yaw 144 | velocities on `/cmd_vel` at 20Hz. 145 | 146 | BaseController subscribes to `/mavros/local_position/pose` and `/cmd_vel`, computes the desired z 147 | position, and publishes it on `/mavros/setpoint_position/global`. 148 | 149 | MAVROS subscribes to `/mavros/setpoint_position/global` and sends a SET_POSITION_TARGET_GLOBAL_INT 150 | message to ArduSub. 151 | 152 | ArduSub calculates servo commands to achieve and hold the desired z position. 153 | 154 | BaseController also computes the desired x and yaw values, computes the thrust required, and 155 | publishes an RC override message on `/mavros/rc/override`. 156 | 157 | MAVROS subscribes to `/mavros/rc/override` and sends an OVERRIDE_RC message to ArduSub. 158 | 159 | ArduSub applies the RC overrides to the RC outputs. 160 | 161 | ArduPilotPlugin sends servo commands to the Thruster plugins. 162 | 163 | The ThrusterPlugins apply thrust forces and spin the propellers in Gazebo. 164 | 165 | ## Manager Node 166 | 167 | The Manager node listens on `/set_target_mode` and moves between the 168 | [DISARMED, ROV, and AUV](../orca_msgs/action/TargetMode.action) modes by monitoring and controlling 169 | other systems. Functions include: 170 | 171 | * arming or disarming the thrusters by calling `/mavros/cmd/arming` 172 | * setting the ArduSub mode by calling `/mavros/set_mode` 173 | * requesting MAVLink messages by calling `/mavros/set_message_interval` 174 | * activating the Nav2 system by calling `/lifecycle_manager_navigation/manage_nodes` 175 | * taking or releasing control of the sub by calling `/conn` 176 | 177 | | Parameter | Type | Default | Notes | 178 | |-----------|--------------|----------|---------------------------------------------------------------------------------------------| 179 | | msg_ids | array of int | [31, 32] | Manager will ask ArduSub to send these MAVLink messages. This request is re-sent every 10s. | 180 | | msg_rate | int | 20 | Desired message rate in Hz. | 181 | 182 | ## BaseController Node 183 | 184 | The BaseController node listens on `/conn` and drives the sub in AUV mode. Functions include: 185 | * listen to `/cmd_vel` and estimate motion 186 | * listen to `/mavros/local_position/pose` and `/orb_slam2_stereo_node/pose` and compute and broadcast all dynamic transforms 187 | * manage localization states, e.g., RUN_NO_MAP, RUN_LOCALIZED, RUN_NOT_LOCALIZED 188 | * if BaseController is driving, publish messages on `/mavros/setpoint_position/global` and `/mavros/rc/override` 189 | * publish diagnostics on `/odom` and `/motion` 190 | 191 | ### Motion model parameters 192 | 193 | There's a very simple motion model that considers buoyancy and drag. 194 | It does not consider added mass. 195 | 196 | | Parameter | Type | Default | Notes | 197 | |---|---|---|---| 198 | | mdl_mass | double | 9.75 | Mass, kg | 199 | | mdl_volume | double | 0.01 | Displacement volume, m^3 | 200 | | mdl_fluid_density | double | 997 | Fluid density, kg/m^3, typically 997 for freshwater, 1027 for seawater | 201 | | mdl_thrust_scale | double | 0.7 | Global thrust scale, used to boost small thrust values typical for AUV operation | 202 | | mdl_drag_coef_x | double | 0.8 | Drag coefficient for forward / back motion | 203 | | mdl_drag_coef_y | double | 0.95 | Drag coefficient for strafing motion | 204 | | mdl_drag_coef_z | double | 0.95 | Drag coefficient for vertical motion | 205 | | mdl_drag_partial_const_yaw | double | 0.004 | Drag coefficient for yaw motion | 206 | | mdl_thrust_dz_pwm | int16 | 35 | Thruster deadzone | 207 | 208 | ### Control parameters 209 | 210 | | Parameter | Type | Default | Notes | 211 | |-------------------------|--------|-----------|--------------------------------------------| 212 | | ardu_frame_id | string | map | ArduSub EKF frame id | 213 | | slam_frame_id | string | slam | ORB_SLAM2 frame id | 214 | | down_frame_id | string | down | Rotated ORB_SLAM2 frame id | 215 | | odom_frame_id | string | odom | Odom frame id | 216 | | base_frame_id | string | base_link | Base frame id | 217 | | slam_timeout_ms | int | 1000 | SLAM timeout | 218 | | transform_expiration_ms | int | 0 | Transform expiration, 0 means don't expire | 219 | | timer_rate | int | 20 | Main loop timer in Hz | 220 | | x_vel | double | 0.4 | Max forward / back velocity, m/s | 221 | | y_vel | double | 0.4 | Max strafing velocity, m/s | 222 | | z_vel | double | 0.2 | Max vertical velocity, m/s | 223 | | yaw_vel | double | 0.4 | Max yaw velocity, r/s | 224 | | x_accel | double | 0.4 | Max forward / back acceleration, m/s^2 | 225 | | y_accel | double | 0.4 | Max strafing acceleration, m/s^2 | 226 | | z_accel | double | 0.2 | Max vertical acceleration, m/s^2 | 227 | | yaw_accel | double | 0.4 | Max yaw acceleration, r/s^2 | 228 | | coast | bool | false | Coast mode (TODO) | 229 | -------------------------------------------------------------------------------- /orca_base/include/orca_base/base_context.hpp: -------------------------------------------------------------------------------- 1 | // MIT License 2 | // 3 | // Copyright (c) 2022 Clyde McQueen 4 | // 5 | // Permission is hereby granted, free of charge, to any person obtaining a copy 6 | // of this software and associated documentation files (the "Software"), to deal 7 | // in the Software without restriction, including without limitation the rights 8 | // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | // copies of the Software, and to permit persons to whom the Software is 10 | // furnished to do so, subject to the following conditions: 11 | // 12 | // The above copyright notice and this permission notice shall be included in all 13 | // copies or substantial portions of the Software. 14 | // 15 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | // SOFTWARE. 22 | 23 | #ifndef ORCA_BASE__BASE_CONTEXT_HPP_ 24 | #define ORCA_BASE__BASE_CONTEXT_HPP_ 25 | 26 | #include 27 | 28 | #include "orca_shared/model.hpp" 29 | 30 | namespace orca_base 31 | { 32 | 33 | #define BASE_PARAMS \ 34 | CXT_MACRO_MEMBER(map_frame_id, std::string, "map") \ 35 | CXT_MACRO_MEMBER(slam_frame_id, std::string, "slam") \ 36 | CXT_MACRO_MEMBER(down_frame_id, std::string, "down") \ 37 | CXT_MACRO_MEMBER(odom_frame_id, std::string, "odom") \ 38 | CXT_MACRO_MEMBER(base_frame_id, std::string, "base_link") \ 39 | CXT_MACRO_MEMBER(camera_frame_id, std::string, "left_camera_link") \ 40 | \ 41 | CXT_MACRO_MEMBER(slam_timeout_ms, int, 1000) \ 42 | CXT_MACRO_MEMBER(transform_expiration_ms, int, 0) \ 43 | \ 44 | CXT_MACRO_MEMBER(timer_rate, int, 20) \ 45 | \ 46 | CXT_MACRO_MEMBER(x_vel, double, 0.4) \ 47 | CXT_MACRO_MEMBER(y_vel, double, 0.4) \ 48 | CXT_MACRO_MEMBER(z_vel, double, 0.2) \ 49 | CXT_MACRO_MEMBER(yaw_vel, double, 0.4) \ 50 | \ 51 | CXT_MACRO_MEMBER(x_accel, double, 0.4) \ 52 | CXT_MACRO_MEMBER(y_accel, double, 0.4) \ 53 | CXT_MACRO_MEMBER(z_accel, double, 0.2) \ 54 | CXT_MACRO_MEMBER(yaw_accel, double, 0.4) \ 55 | \ 56 | CXT_MACRO_MEMBER(coast, bool, false) \ 57 | /* Coast to decelerate (vs powered decel), useful for ROV ops */ \ 58 | /* End of list */ 59 | 60 | #undef CXT_MACRO_MEMBER 61 | #define CXT_MACRO_MEMBER(n, t, d) CXT_MACRO_DEFINE_MEMBER(n, t, d) 62 | 63 | struct BaseContext : orca::Model 64 | { 65 | CXT_MACRO_DEFINE_MEMBERS(BASE_PARAMS) 66 | }; 67 | 68 | #define BASE_ALL_PARAMS \ 69 | MODEL_PARAMS \ 70 | BASE_PARAMS \ 71 | /* End of list */ 72 | 73 | } // namespace orca_base 74 | 75 | #endif // ORCA_BASE__BASE_CONTEXT_HPP_ 76 | -------------------------------------------------------------------------------- /orca_base/include/orca_base/underwater_motion.hpp: -------------------------------------------------------------------------------- 1 | // MIT License 2 | // 3 | // Copyright (c) 2022 Clyde McQueen 4 | // 5 | // Permission is hereby granted, free of charge, to any person obtaining a copy 6 | // of this software and associated documentation files (the "Software"), to deal 7 | // in the Software without restriction, including without limitation the rights 8 | // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | // copies of the Software, and to permit persons to whom the Software is 10 | // furnished to do so, subject to the following conditions: 11 | // 12 | // The above copyright notice and this permission notice shall be included in all 13 | // copies or substantial portions of the Software. 14 | // 15 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | // SOFTWARE. 22 | 23 | #ifndef ORCA_BASE__UNDERWATER_MOTION_HPP_ 24 | #define ORCA_BASE__UNDERWATER_MOTION_HPP_ 25 | 26 | #include 27 | #include 28 | 29 | #include "geometry_msgs/msg/accel_stamped.hpp" 30 | #include "geometry_msgs/msg/pose_stamped.hpp" 31 | #include "geometry_msgs/msg/transform_stamped.hpp" 32 | #include "geometry_msgs/msg/twist_stamped.hpp" 33 | #include "geometry_msgs/msg/wrench_stamped.hpp" 34 | #include "nav_msgs/msg/odometry.hpp" 35 | #include "orca_base/base_context.hpp" 36 | #include "orca_msgs/msg/motion.hpp" 37 | #include "orca_shared/model.hpp" 38 | #include "rclcpp/logger.hpp" 39 | 40 | namespace orca_base 41 | { 42 | 43 | class UnderwaterMotion 44 | { 45 | rclcpp::Logger logger_; 46 | const BaseContext & cxt_; 47 | orca_msgs::msg::Motion motion_; 48 | 49 | double report_and_clamp(std::string func, std::string name, double v, double minmax); 50 | 51 | [[nodiscard]] geometry_msgs::msg::Accel calc_accel( 52 | const geometry_msgs::msg::Twist & v0, 53 | const geometry_msgs::msg::Twist & v1) const; 54 | 55 | [[nodiscard]] geometry_msgs::msg::Twist calc_vel( 56 | const geometry_msgs::msg::Twist & v0, 57 | const geometry_msgs::msg::Accel & a) const; 58 | 59 | [[nodiscard]] geometry_msgs::msg::Pose calc_pose( 60 | const geometry_msgs::msg::Pose & p0, 61 | const geometry_msgs::msg::Twist & v) const; 62 | 63 | public: 64 | UnderwaterMotion( 65 | const rclcpp::Time & t, const rclcpp::Logger & logger, const BaseContext & cxt, 66 | const geometry_msgs::msg::Pose & initial_pose); 67 | 68 | [[nodiscard]] const orca_msgs::msg::Motion & motion() const {return motion_;} 69 | 70 | [[nodiscard]] nav_msgs::msg::Odometry odometry() const; 71 | 72 | [[nodiscard]] std::string frame_id() const {return motion_.header.frame_id;} 73 | 74 | [[nodiscard]] builtin_interfaces::msg::Time stamp() const {return motion_.header.stamp;} 75 | 76 | // Update state from time t-(1/timer_rate) to time t 77 | void update(const rclcpp::Time & t, const geometry_msgs::msg::Twist & cmd_vel); 78 | }; 79 | 80 | } // namespace orca_base 81 | 82 | #endif // ORCA_BASE__UNDERWATER_MOTION_HPP_ 83 | -------------------------------------------------------------------------------- /orca_base/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | orca_base 6 | 0.5.0 7 | Orca controller 8 | 9 | Clyde McQueen 10 | MIT 11 | 12 | https://github.com/clydemcqueen/orca4.git 13 | https://github.com/clydemcqueen/orca4/issues 14 | 15 | Clyde McQueen 16 | 17 | ament_cmake 18 | 19 | ament_lint_auto 20 | 21 | 22 | ament_lint_common 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | camera_info_manager 35 | geographic_msgs 36 | geometry_msgs 37 | mavros_msgs 38 | nav_msgs 39 | nav2_util 40 | orca_msgs 41 | orca_shared 42 | rclcpp 43 | ros2_shared 44 | std_srvs 45 | tf2 46 | tf2_geometry_msgs 47 | tf2_ros 48 | 49 | 50 | ament_cmake 51 | 52 | 53 | -------------------------------------------------------------------------------- /orca_base/scripts/test_base.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | # MIT License 4 | # 5 | # Copyright (c) 2022 Clyde McQueen 6 | # 7 | # Permission is hereby granted, free of charge, to any person obtaining a copy 8 | # of this software and associated documentation files (the "Software"), to deal 9 | # in the Software without restriction, including without limitation the rights 10 | # to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 11 | # copies of the Software, and to permit persons to whom the Software is 12 | # furnished to do so, subject to the following conditions: 13 | # 14 | # The above copyright notice and this permission notice shall be included in all 15 | # copies or substantial portions of the Software. 16 | # 17 | # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 18 | # IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 19 | # FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 20 | # AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 21 | # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 22 | # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 23 | # SOFTWARE. 24 | 25 | """ 26 | Generate /cmd_vel messages to move the sub around a bit. 27 | 28 | Usage: 29 | -- ros2 run orca_base test_base.py 30 | """ 31 | 32 | from geometry_msgs.msg import Twist, Vector3 33 | import rclpy 34 | import rclpy.logging 35 | from rclpy.node import Node 36 | 37 | 38 | class TestBase(Node): 39 | 40 | def __init__(self): 41 | super().__init__('test_base') 42 | 43 | self._cmd_vel_pub = self.create_publisher(Twist, '/cmd_vel', 10) 44 | self._timer = self.create_timer(8.0, self.timer_callback) 45 | 46 | self._trajectory = [ 47 | Twist(linear=Vector3(z=-0.1)), 48 | Twist(angular=Vector3(z=0.1)), 49 | Twist(linear=Vector3(x=0.1)), 50 | Twist(linear=Vector3(y=0.1)), 51 | Twist(linear=Vector3(x=-0.1)), 52 | Twist(linear=Vector3(y=-0.1)), 53 | Twist(linear=Vector3(z=0.1)), 54 | Twist(), 55 | ] 56 | self._segment = 0 57 | 58 | def timer_callback(self): 59 | print(self._trajectory[self._segment]) 60 | self._cmd_vel_pub.publish(self._trajectory[self._segment]) 61 | 62 | self._segment += 1 63 | if self._segment >= len(self._trajectory): 64 | print('reset') 65 | self._segment = 0 66 | 67 | 68 | def main(): 69 | rclpy.init() 70 | 71 | node = TestBase() 72 | 73 | try: 74 | rclpy.spin(node) 75 | except KeyboardInterrupt: 76 | node.get_logger().info('ctrl-C detected, shutting down') 77 | finally: 78 | node.destroy_node() 79 | rclpy.shutdown() 80 | 81 | 82 | if __name__ == '__main__': 83 | main() 84 | -------------------------------------------------------------------------------- /orca_base/src/camera_info_publisher.cpp: -------------------------------------------------------------------------------- 1 | // MIT License 2 | // 3 | // Copyright (c) 2022 Clyde McQueen 4 | // 5 | // Permission is hereby granted, free of charge, to any person obtaining a copy 6 | // of this software and associated documentation files (the "Software"), to deal 7 | // in the Software without restriction, including without limitation the rights 8 | // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | // copies of the Software, and to permit persons to whom the Software is 10 | // furnished to do so, subject to the following conditions: 11 | // 12 | // The above copyright notice and this permission notice shall be included in all 13 | // copies or substantial portions of the Software. 14 | // 15 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | // SOFTWARE. 22 | 23 | #include 24 | #include 25 | 26 | #include "camera_info_manager/camera_info_manager.hpp" 27 | #include "rclcpp/rclcpp.hpp" 28 | #include "ros2_shared/context_macros.hpp" 29 | 30 | namespace orca_base 31 | { 32 | 33 | #define CAMERA_INFO_PARAMS \ 34 | CXT_MACRO_MEMBER(timer_period_ms, int, 1000) \ 35 | CXT_MACRO_MEMBER(camera_info_url, std::string, "/path/to/camera/info") \ 36 | CXT_MACRO_MEMBER(camera_name, std::string, "forward_camera") \ 37 | CXT_MACRO_MEMBER(frame_id, std::string, "forward_camera_frame") \ 38 | /* End of list */ 39 | 40 | #undef CXT_MACRO_MEMBER 41 | #define CXT_MACRO_MEMBER(n, t, d) CXT_MACRO_DEFINE_MEMBER(n, t, d) 42 | 43 | struct CameraInfoContext 44 | { 45 | CXT_MACRO_DEFINE_MEMBERS(CAMERA_INFO_PARAMS) 46 | }; 47 | 48 | class CameraInfoPublisher : public rclcpp::Node 49 | { 50 | CameraInfoContext cxt_; 51 | std::unique_ptr camera_info_manager_; 52 | rclcpp::Publisher::SharedPtr camera_info_pub_; 53 | rclcpp::TimerBase::SharedPtr spin_timer_; 54 | 55 | void validate_parameters() 56 | { 57 | camera_info_manager_ = std::make_unique(this); 58 | camera_info_manager_->setCameraName(cxt_.camera_name_); 59 | 60 | if (camera_info_manager_->validateURL(cxt_.camera_info_url_)) { 61 | camera_info_manager_->loadCameraInfo(cxt_.camera_info_url_); 62 | RCLCPP_INFO(get_logger(), "Loaded camera info from %s", cxt_.camera_info_url_.c_str()); 63 | } else { 64 | RCLCPP_ERROR( 65 | get_logger(), "Camera info url '%s' is not valid, missing 'file://' prefix?", 66 | cxt_.camera_info_url_.c_str()); 67 | } 68 | } 69 | 70 | public: 71 | CameraInfoPublisher() 72 | : Node("camera_info_publisher") 73 | { 74 | (void) camera_info_pub_; 75 | (void) spin_timer_; 76 | 77 | // Get parameters, this will immediately call validate_parameters() 78 | #undef CXT_MACRO_MEMBER 79 | #define CXT_MACRO_MEMBER(n, t, d) CXT_MACRO_LOAD_PARAMETER((*this), cxt_, n, t, d) 80 | CXT_MACRO_INIT_PARAMETERS(CAMERA_INFO_PARAMS, validate_parameters) 81 | 82 | // Register parameters 83 | #undef CXT_MACRO_MEMBER 84 | #define CXT_MACRO_MEMBER(n, t, d) CXT_MACRO_PARAMETER_CHANGED(n, t) 85 | CXT_MACRO_REGISTER_PARAMETERS_CHANGED((*this), cxt_, CAMERA_INFO_PARAMS, validate_parameters) 86 | 87 | // Log parameters 88 | #undef CXT_MACRO_MEMBER 89 | #define CXT_MACRO_MEMBER(n, t, d) CXT_MACRO_LOG_PARAMETER(RCLCPP_INFO, get_logger(), cxt_, n, t, d) 90 | CAMERA_INFO_PARAMS 91 | 92 | // Check that all command line parameters are defined 93 | #undef CXT_MACRO_MEMBER 94 | #define CXT_MACRO_MEMBER(n, t, d) CXT_MACRO_CHECK_CMDLINE_PARAMETER(n, t, d) 95 | CXT_MACRO_CHECK_CMDLINE_PARAMETERS((*this), CAMERA_INFO_PARAMS) 96 | 97 | camera_info_pub_ = create_publisher("camera_info", 10); 98 | 99 | spin_timer_ = create_wall_timer( 100 | std::chrono::milliseconds{cxt_.timer_period_ms_}, [this]() 101 | { 102 | auto camera_info_msg = camera_info_manager_->getCameraInfo(); 103 | // camera_info_msg.header.stamp = now(); 104 | // camera_info_msg.header.frame_id = cxt_.frame_id_; 105 | camera_info_pub_->publish(camera_info_msg); 106 | }); 107 | } 108 | }; 109 | 110 | } // namespace orca_base 111 | 112 | int main(int argc, char ** argv) 113 | { 114 | setvbuf(stdout, nullptr, _IONBF, BUFSIZ); 115 | rclcpp::init(argc, argv); 116 | auto node = std::make_shared(); 117 | rclcpp::spin(node); 118 | rclcpp::shutdown(); 119 | return 0; 120 | } 121 | -------------------------------------------------------------------------------- /orca_base/src/odom_to_path_node.cpp: -------------------------------------------------------------------------------- 1 | // MIT License 2 | // 3 | // Copyright (c) 2022 Clyde McQueen 4 | // 5 | // Permission is hereby granted, free of charge, to any person obtaining a copy 6 | // of this software and associated documentation files (the "Software"), to deal 7 | // in the Software without restriction, including without limitation the rights 8 | // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | // copies of the Software, and to permit persons to whom the Software is 10 | // furnished to do so, subject to the following conditions: 11 | // 12 | // The above copyright notice and this permission notice shall be included in all 13 | // copies or substantial portions of the Software. 14 | // 15 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | // SOFTWARE. 22 | 23 | #include 24 | 25 | #include "nav_msgs/msg/odometry.hpp" 26 | #include "nav_msgs/msg/path.hpp" 27 | #include "rclcpp/rclcpp.hpp" 28 | #include "ros2_shared/context_macros.hpp" 29 | 30 | namespace orca_vision 31 | { 32 | 33 | #define PARAMS \ 34 | CXT_MACRO_MEMBER(subscribe_best_effort, bool, true) \ 35 | CXT_MACRO_MEMBER(max_poses, int, 500) \ 36 | /* End of list */ 37 | 38 | struct Parameters 39 | { 40 | #undef CXT_MACRO_MEMBER 41 | #define CXT_MACRO_MEMBER(n, t, d) CXT_MACRO_DEFINE_MEMBER(n, t, d) 42 | CXT_MACRO_DEFINE_MEMBERS(PARAMS) 43 | }; 44 | 45 | class OdomToPathNode : public rclcpp::Node 46 | { 47 | Parameters params_; 48 | nav_msgs::msg::Path path_; 49 | rclcpp::Subscription::SharedPtr odom_sub_; 50 | rclcpp::Publisher::SharedPtr path_pub_; 51 | 52 | void init_parameters() 53 | { 54 | // Get parameters, this will immediately call validate_parameters() 55 | #undef CXT_MACRO_MEMBER 56 | #define CXT_MACRO_MEMBER(n, t, d) CXT_MACRO_LOAD_PARAMETER((*this), params_, n, t, d) 57 | CXT_MACRO_INIT_PARAMETERS(PARAMS, validate_parameters) 58 | 59 | // Register parameters 60 | #undef CXT_MACRO_MEMBER 61 | #define CXT_MACRO_MEMBER(n, t, d) CXT_MACRO_PARAMETER_CHANGED(n, t) 62 | CXT_MACRO_REGISTER_PARAMETERS_CHANGED((*this), params_, PARAMS, validate_parameters) 63 | 64 | // Log parameters 65 | #undef CXT_MACRO_MEMBER 66 | #define CXT_MACRO_MEMBER(n, t, d) CXT_MACRO_LOG_PARAMETER( \ 67 | RCLCPP_INFO, get_logger(), params_, n, t, d) 68 | PARAMS 69 | 70 | // Check that all command line parameters are defined 71 | #undef CXT_MACRO_MEMBER 72 | #define CXT_MACRO_MEMBER(n, t, d) CXT_MACRO_CHECK_CMDLINE_PARAMETER(n, t, d) 73 | CXT_MACRO_CHECK_CMDLINE_PARAMETERS((*this), PARAMS) 74 | } 75 | 76 | void validate_parameters() 77 | { 78 | } 79 | 80 | public: 81 | OdomToPathNode() 82 | : Node{"odom_to_path"} 83 | { 84 | (void) odom_sub_; 85 | 86 | init_parameters(); 87 | 88 | // Gazebo p3d plugin uses best-effort QoS 89 | rclcpp::QoS qos(10); 90 | if (params_.subscribe_best_effort_) { 91 | qos.best_effort(); 92 | } else { 93 | qos.reliable(); 94 | } 95 | 96 | path_pub_ = create_publisher("path", 10); 97 | odom_sub_ = create_subscription( 98 | "odom", qos, [this](const nav_msgs::msg::Odometry::ConstSharedPtr msg) 99 | { 100 | if (path_pub_->get_subscription_count() > 0) { 101 | path_.header = msg->header; 102 | if (path_.poses.size() > (uint64_t)params_.max_poses_) { 103 | path_.poses.clear(); 104 | } 105 | geometry_msgs::msg::PoseStamped pose_stamped; 106 | pose_stamped.header = msg->header; 107 | pose_stamped.pose = msg->pose.pose; 108 | path_.poses.push_back(pose_stamped); 109 | path_pub_->publish(path_); 110 | } 111 | }); 112 | } 113 | }; 114 | 115 | } // namespace orca_vision 116 | 117 | int main(int argc, char ** argv) 118 | { 119 | rclcpp::init(argc, argv); 120 | auto node = std::make_shared(); 121 | rclcpp::spin(node); 122 | rclcpp::shutdown(); 123 | return 0; 124 | } 125 | -------------------------------------------------------------------------------- /orca_base/src/pose_to_path_node.cpp: -------------------------------------------------------------------------------- 1 | // MIT License 2 | // 3 | // Copyright (c) 2022 Clyde McQueen 4 | // 5 | // Permission is hereby granted, free of charge, to any person obtaining a copy 6 | // of this software and associated documentation files (the "Software"), to deal 7 | // in the Software without restriction, including without limitation the rights 8 | // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | // copies of the Software, and to permit persons to whom the Software is 10 | // furnished to do so, subject to the following conditions: 11 | // 12 | // The above copyright notice and this permission notice shall be included in all 13 | // copies or substantial portions of the Software. 14 | // 15 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | // SOFTWARE. 22 | 23 | #include 24 | 25 | #include "nav_msgs/msg/path.hpp" 26 | #include "rclcpp/rclcpp.hpp" 27 | 28 | namespace orca_vision 29 | { 30 | 31 | class PoseToPathNode : public rclcpp::Node 32 | { 33 | nav_msgs::msg::Path path_; 34 | rclcpp::Subscription::SharedPtr pose_sub_; 35 | rclcpp::Publisher::SharedPtr path_pub_; 36 | 37 | public: 38 | PoseToPathNode() 39 | : Node{"pose_to_path"} 40 | { 41 | (void) pose_sub_; 42 | 43 | path_pub_ = create_publisher("path", 10); 44 | pose_sub_ = create_subscription( 45 | "pose", 10, [this](const geometry_msgs::msg::PoseStamped::ConstSharedPtr msg) 46 | { 47 | if (path_pub_->get_subscription_count() > 0) { 48 | path_.header = msg->header; 49 | if (path_.poses.size() > 200) { 50 | path_.poses.clear(); 51 | } 52 | path_.poses.push_back(*msg); 53 | path_pub_->publish(path_); 54 | } 55 | }); 56 | } 57 | }; 58 | 59 | } // namespace orca_vision 60 | 61 | int main(int argc, char ** argv) 62 | { 63 | rclcpp::init(argc, argv); 64 | auto node = std::make_shared(); 65 | rclcpp::spin(node); 66 | rclcpp::shutdown(); 67 | return 0; 68 | } 69 | -------------------------------------------------------------------------------- /orca_base/src/underwater_motion.cpp: -------------------------------------------------------------------------------- 1 | // MIT License 2 | // 3 | // Copyright (c) 2022 Clyde McQueen 4 | // 5 | // Permission is hereby granted, free of charge, to any person obtaining a copy 6 | // of this software and associated documentation files (the "Software"), to deal 7 | // in the Software without restriction, including without limitation the rights 8 | // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | // copies of the Software, and to permit persons to whom the Software is 10 | // furnished to do so, subject to the following conditions: 11 | // 12 | // The above copyright notice and this permission notice shall be included in all 13 | // copies or substantial portions of the Software. 14 | // 15 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | // SOFTWARE. 22 | 23 | #include 24 | #include 25 | 26 | #include "orca_base/underwater_motion.hpp" 27 | #include "orca_shared/util.hpp" 28 | 29 | namespace orca_base 30 | { 31 | 32 | // Purpose: given cmd_vel, calculate motion. 33 | // 34 | // Pose is in the world frame; velocity, acceleration and thrust are in the robot frame. 35 | // 36 | // There are several sources of cmd_vel: 37 | // -- orca_nav2::PurePursuitController3D, the primary path following controller. 38 | // PurePursuitController3D builds a constant acceleration motion (trapezoidal velocity) plan, 39 | // so [in theory] there's no need to clamp acceleration and velocity. 40 | // -- recovery controllers (spin, wait) interrupt PurePursuitController3D and may cause rapid 41 | // acceleration or deceleration. These will be clamped. 42 | 43 | UnderwaterMotion::UnderwaterMotion( 44 | const rclcpp::Time & t, const rclcpp::Logger & logger, 45 | const BaseContext & cxt, const geometry_msgs::msg::Pose & initial_pose) 46 | : logger_{logger}, cxt_{cxt} 47 | { 48 | motion_.header.stamp = t; 49 | motion_.header.frame_id = cxt_.odom_frame_id_; 50 | motion_.pose.position.x = initial_pose.position.x; 51 | motion_.pose.position.y = initial_pose.position.y; 52 | motion_.pose.position.z = initial_pose.position.z; 53 | auto initial_yaw = orca::get_yaw(initial_pose.orientation); 54 | orca::set_yaw(motion_.pose.orientation, initial_yaw); 55 | RCLCPP_INFO( 56 | logger_, "initialize odometry to {{%g, %g, %g}, {0, 0, %g}}", 57 | motion_.pose.position.x, motion_.pose.position.y, motion_.pose.position.z, initial_yaw); 58 | } 59 | 60 | // Loud vs quiet clamp functions 61 | // #define CLAMP(v, minmax) report_and_clamp(__func__, #v, v, minmax) 62 | #define CLAMP(v, minmax) orca::clamp(v, minmax) 63 | #define EPSILON 0.00001 64 | 65 | double 66 | UnderwaterMotion::report_and_clamp(std::string func, std::string name, double v, double minmax) 67 | { 68 | if (v > minmax + EPSILON) { 69 | RCLCPP_INFO( 70 | logger_, "%s: {%s} %g too high, clamp to %g", func.c_str(), name.c_str(), v, 71 | minmax); 72 | return minmax; 73 | } else if (v < -minmax - EPSILON) { 74 | RCLCPP_INFO( 75 | logger_, "%s: {%s} %g too low, clamp to %g", func.c_str(), name.c_str(), v, 76 | -minmax); 77 | return -minmax; 78 | } else { 79 | return v; 80 | } 81 | } 82 | 83 | // a = (v1 - v0) / dt 84 | geometry_msgs::msg::Accel UnderwaterMotion::calc_accel( 85 | const geometry_msgs::msg::Twist & v0, 86 | const geometry_msgs::msg::Twist & v1) const 87 | { 88 | geometry_msgs::msg::Accel result; 89 | result.linear.x = CLAMP((v1.linear.x - v0.linear.x) / motion_.dt, cxt_.x_accel_); 90 | result.linear.y = CLAMP((v1.linear.y - v0.linear.y) / motion_.dt, cxt_.y_accel_); 91 | result.linear.z = CLAMP((v1.linear.z - v0.linear.z) / motion_.dt, cxt_.z_accel_); 92 | result.angular.z = CLAMP((v1.angular.z - v0.angular.z) / motion_.dt, cxt_.yaw_accel_); 93 | return result; 94 | } 95 | 96 | // v = v0 + a * dt 97 | geometry_msgs::msg::Twist UnderwaterMotion::calc_vel( 98 | const geometry_msgs::msg::Twist & v0, 99 | const geometry_msgs::msg::Accel & a) const 100 | { 101 | geometry_msgs::msg::Twist result; 102 | result.linear.x = CLAMP(v0.linear.x + a.linear.x * motion_.dt, cxt_.x_vel_); 103 | result.linear.y = CLAMP(v0.linear.y + a.linear.y * motion_.dt, cxt_.y_vel_); 104 | result.linear.z = CLAMP(v0.linear.z + a.linear.z * motion_.dt, cxt_.z_vel_); 105 | result.angular.z = CLAMP(v0.angular.z + a.angular.z * motion_.dt, cxt_.yaw_vel_); 106 | return result; 107 | } 108 | 109 | // p = p0 + v * dt 110 | geometry_msgs::msg::Pose UnderwaterMotion::calc_pose( 111 | const geometry_msgs::msg::Pose & p0, 112 | const geometry_msgs::msg::Twist & v) const 113 | { 114 | geometry_msgs::msg::Pose result; 115 | auto yaw = orca::get_yaw(p0.orientation); 116 | result.position.x = p0.position.x + (v.linear.x * cos(yaw) + v.linear.y * sin(-yaw)) * motion_.dt; 117 | result.position.y = p0.position.y + (v.linear.x * sin(yaw) + v.linear.y * cos(-yaw)) * motion_.dt; 118 | result.position.z = p0.position.z + v.linear.z * motion_.dt; 119 | yaw += v.angular.z * motion_.dt; 120 | orca::set_yaw(result.orientation, yaw); 121 | 122 | if (result.position.z > 0) { 123 | // Don't go above the surface 124 | result.position.z = 0; 125 | } 126 | 127 | return result; 128 | } 129 | 130 | nav_msgs::msg::Odometry UnderwaterMotion::odometry() const 131 | { 132 | static std::array covariance{ 133 | 1, 0, 0, 0, 0, 0, 134 | 0, 1, 0, 0, 0, 0, 135 | 0, 0, 1, 0, 0, 0, 136 | 0, 0, 0, 1, 0, 0, 137 | 0, 0, 0, 0, 1, 0, 138 | 0, 0, 0, 0, 0, 1 139 | }; 140 | 141 | nav_msgs::msg::Odometry result; 142 | result.header.stamp = motion_.header.stamp; 143 | result.header.frame_id = cxt_.odom_frame_id_; 144 | result.child_frame_id = cxt_.base_frame_id_; 145 | result.pose.pose = motion_.pose; 146 | result.pose.covariance = covariance; 147 | result.twist.twist = 148 | orca::robot_to_world_frame(motion_.vel, orca::get_yaw(motion_.pose.orientation)); 149 | result.twist.covariance = covariance; 150 | return result; 151 | } 152 | 153 | // Coast: if cmd_vel is 0, then force acceleration to 0. Only applies to x, y and yaw. 154 | // Helpful for some ROV operations. TODO(clyde) 155 | void coast(const geometry_msgs::msg::Twist & cmd_vel, geometry_msgs::msg::Accel & model_plus_drag) 156 | { 157 | static double epsilon = 0.01; 158 | if (std::abs(cmd_vel.linear.x) < epsilon) { 159 | model_plus_drag.linear.x = 0; 160 | } 161 | if (std::abs(cmd_vel.linear.y) < epsilon) { 162 | model_plus_drag.linear.y = 0; 163 | } 164 | if (std::abs(cmd_vel.angular.z) < epsilon) { 165 | model_plus_drag.angular.z = 0; 166 | } 167 | } 168 | 169 | void UnderwaterMotion::update(const rclcpp::Time & t, const geometry_msgs::msg::Twist & cmd_vel) 170 | { 171 | motion_.header.stamp = t; 172 | motion_.dt = 1.0 / cxt_.timer_rate_; 173 | motion_.cmd_vel = cmd_vel; 174 | 175 | // Pose and vel don't honor coast TODO(clyde) 176 | motion_.pose = calc_pose(motion_.pose, motion_.vel); 177 | motion_.vel = calc_vel(motion_.vel, motion_.accel_model); 178 | 179 | // Accelerate to cmd_vel 180 | motion_.accel_model = calc_accel(motion_.vel, cmd_vel); 181 | 182 | // Counteract drag 183 | motion_.accel_drag = -cxt_.drag_accel(motion_.vel); 184 | 185 | // Combine model and drag 186 | auto accel_total = motion_.accel_model + motion_.accel_drag; 187 | 188 | // Experiment for ROV operations 189 | if (cxt_.coast_) { 190 | coast(cmd_vel, accel_total); 191 | } 192 | 193 | motion_.accel_total = accel_total; 194 | motion_.force = cxt_.accel_to_wrench(accel_total); 195 | motion_.effort = cxt_.wrench_to_effort(motion_.force); 196 | } 197 | 198 | } // namespace orca_base 199 | -------------------------------------------------------------------------------- /orca_bringup/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.12.2) 2 | project(orca_bringup) 3 | 4 | find_package(ament_cmake_auto REQUIRED) 5 | ament_auto_find_build_dependencies() 6 | 7 | # Load & run linters listed in package.xml 8 | if(BUILD_TESTING) 9 | find_package(ament_lint_auto REQUIRED) 10 | ament_lint_auto_find_test_dependencies() 11 | endif() 12 | 13 | # Install scripts 14 | install( 15 | PROGRAMS 16 | scripts/dump_rosout.py 17 | scripts/mission_runner.py 18 | DESTINATION lib/${PROJECT_NAME} 19 | ) 20 | 21 | ament_auto_package(INSTALL_TO_SHARE behavior_trees cfg launch params) -------------------------------------------------------------------------------- /orca_bringup/README.md: -------------------------------------------------------------------------------- 1 | ## Launch files 2 | 3 | ### [sim_launch.py](launch/sim_launch.py) 4 | 5 | Launch ROV or AUV simulation in Gazebo. 6 | Calls [bringup.py](launch/bringup.py). 7 | 8 | To see parameters: `ros2 launch --show-args orca_bringup sim_launch.py` 9 | 10 | ### [bringup.py](launch/bringup.py) 11 | 12 | Bring up all core ROV and AUV nodes, including ORB_SLAM2 and Nav2. 13 | Calls [navigation_launch.py](launch/navigation_launch.py). 14 | 15 | ### [navigation_launch.py](launch/navigation_launch.py) 16 | 17 | Nav2 navigation launch file, modified to avoid launch the velocity smoother. 18 | 19 | ## Scenarios 20 | 21 | ### Full automation 22 | 23 | In a terminal run: 24 | ~~~ 25 | source src/orca4/setup.bash 26 | ros2 launch orca_bringup sim_launch.py 27 | ~~~ 28 | 29 | Execute a mission in a 2nd terminal: 30 | ~~~ 31 | source src/orca4/setup.bash 32 | ros2 run orca_bringup mission_runner.py 33 | ~~~ 34 | 35 | ### Using MAVProxy 36 | 37 | It is possible to launch Gazebo and ArduSub and control the sub using MAVProxy. 38 | 39 | Launch a minimal system: 40 | ~~~ 41 | ros2 launch orca_bringup sim_launch.py base:=false mavros:=false nav:=false rviz:=false slam:=false 42 | ~~~ 43 | 44 | Launch MAVProxy in a 2nd terminal: 45 | ~~~ 46 | mavproxy.py --master tcp:127.0.0.1:5760 --sitl 127.0.0.1:5501 --out 127.0.0.1:14550 --out 127.0.0.1:14551 --out udp:0.0.0.0:14550 --console 47 | ~~~ 48 | 49 | You can use MAVProxy to send commands directly to ArduSub: 50 | ~~~ 51 | arm throttle 52 | rc 3 1450 53 | rc 3 1500 54 | mode alt_hold 55 | disarm 56 | ~~~ 57 | 58 | RC channels: 59 | * RC 3 -- vertical 60 | * RC 4 -- yaw 61 | * RC 5 -- forward 62 | * RC 6 -- strafe 63 | 64 | ### MAVProxy + SLAM 65 | 66 | This will bring up a minimal system with SLAM and RViz: 67 | ~~~ 68 | ros2 launch orca_bringup sim_launch.py base:=false mavros:=false nav:=false 69 | ~~~ 70 | 71 | You can use MAVProxy to drive the sub around the seafloor and build a map. 72 | 73 | ## Video pipeline 74 | 75 | The simulation uses Gazebo camera sensors to generate uncompressed 800x600 images in an 76 | ideal stereo configuration. The frame rate is throttled to 5Hz to reduce CPU load in ORB_SLAM2, but 77 | it can easily go higher. 78 | -------------------------------------------------------------------------------- /orca_bringup/behavior_trees/orca4_bt.xml: -------------------------------------------------------------------------------- 1 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | -------------------------------------------------------------------------------- /orca_bringup/cfg/sim_launch.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz_common/Displays 3 | Help Height: 85 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /TF1/Frames1 8 | - /TF1/Tree1 9 | - /PointCloud21/Autocompute Value Bounds1 10 | - /Pose1/Topic1 11 | - /Odometry1/Shape1 12 | Splitter Ratio: 0.5 13 | Tree Height: 1233 14 | - Class: rviz_common/Selection 15 | Name: Selection 16 | - Class: rviz_common/Tool Properties 17 | Expanded: 18 | - /2D Goal Pose1 19 | - /Publish Point1 20 | Name: Tool Properties 21 | Splitter Ratio: 0.5886790156364441 22 | - Class: rviz_common/Views 23 | Expanded: 24 | - /Current View1 25 | Name: Views 26 | Splitter Ratio: 0.5 27 | Visualization Manager: 28 | Class: "" 29 | Displays: 30 | - Alpha: 0.5 31 | Cell Size: 1 32 | Class: rviz_default_plugins/Grid 33 | Color: 160; 160; 164 34 | Enabled: true 35 | Line Style: 36 | Line Width: 0.029999999329447746 37 | Value: Lines 38 | Name: Grid 39 | Normal Cell Count: 0 40 | Offset: 41 | X: 0 42 | Y: 0 43 | Z: 0 44 | Plane: XY 45 | Plane Cell Count: 10 46 | Reference Frame: 47 | Value: true 48 | - Class: rviz_default_plugins/Axes 49 | Enabled: false 50 | Length: 1 51 | Name: Axes 52 | Radius: 0.10000000149011612 53 | Reference Frame: 54 | Value: false 55 | - Class: rviz_default_plugins/TF 56 | Enabled: true 57 | Frame Timeout: 15 58 | Frames: 59 | All Enabled: false 60 | base_link: 61 | Value: true 62 | base_link_frd: 63 | Value: false 64 | down: 65 | Value: true 66 | left_camera_link: 67 | Value: true 68 | map: 69 | Value: true 70 | map_ned: 71 | Value: false 72 | odom: 73 | Value: true 74 | odom_ned: 75 | Value: false 76 | slam: 77 | Value: true 78 | Marker Scale: 1 79 | Name: TF 80 | Show Arrows: true 81 | Show Axes: true 82 | Show Names: false 83 | Tree: 84 | map: 85 | map_ned: 86 | {} 87 | odom: 88 | base_link: 89 | base_link_frd: 90 | {} 91 | left_camera_link: 92 | {} 93 | odom_ned: 94 | {} 95 | slam: 96 | down: 97 | {} 98 | Update Interval: 0 99 | Value: true 100 | - Alpha: 1 101 | Buffer Length: 1 102 | Class: rviz_default_plugins/Path 103 | Color: 25; 255; 0 104 | Enabled: true 105 | Head Diameter: 0.30000001192092896 106 | Head Length: 0.20000000298023224 107 | Length: 0.30000001192092896 108 | Line Style: Lines 109 | Line Width: 0.029999999329447746 110 | Name: Path 111 | Offset: 112 | X: 0 113 | Y: 0 114 | Z: 0 115 | Pose Color: 255; 85; 255 116 | Pose Style: None 117 | Radius: 0.029999999329447746 118 | Shaft Diameter: 0.10000000149011612 119 | Shaft Length: 0.10000000149011612 120 | Topic: 121 | Depth: 5 122 | Durability Policy: Volatile 123 | Filter size: 10 124 | History Policy: Keep Last 125 | Reliability Policy: Reliable 126 | Value: /plan 127 | Value: true 128 | - Alpha: 1 129 | Autocompute Intensity Bounds: true 130 | Autocompute Value Bounds: 131 | Max Value: -9999 132 | Min Value: 9999 133 | Value: true 134 | Axis: Z 135 | Channel Name: intensity 136 | Class: rviz_default_plugins/PointCloud2 137 | Color: 255; 255; 255 138 | Color Transformer: AxisColor 139 | Decay Time: 0 140 | Enabled: true 141 | Invert Rainbow: false 142 | Max Color: 255; 255; 255 143 | Max Intensity: 4096 144 | Min Color: 0; 0; 0 145 | Min Intensity: 0 146 | Name: PointCloud2 147 | Position Transformer: XYZ 148 | Selectable: true 149 | Size (Pixels): 3 150 | Size (m): 0.05000000074505806 151 | Style: Flat Squares 152 | Topic: 153 | Depth: 5 154 | Durability Policy: Volatile 155 | Filter size: 10 156 | History Policy: Keep Last 157 | Reliability Policy: Reliable 158 | Value: /orb_slam2_stereo_node/map_points 159 | Use Fixed Frame: true 160 | Use rainbow: true 161 | Value: true 162 | - Class: rviz_default_plugins/Image 163 | Enabled: true 164 | Max Value: 1 165 | Median window: 5 166 | Min Value: 0 167 | Name: Image 168 | Normalize Range: true 169 | Topic: 170 | Depth: 5 171 | Durability Policy: Volatile 172 | History Policy: Keep Last 173 | Reliability Policy: Reliable 174 | Value: /orb_slam2_stereo_node/debug_image 175 | Value: true 176 | - Alpha: 1 177 | Axes Length: 0.10000000149011612 178 | Axes Radius: 0.009999999776482582 179 | Class: rviz_default_plugins/Pose 180 | Color: 255; 25; 0 181 | Enabled: true 182 | Head Length: 0.30000001192092896 183 | Head Radius: 0.10000000149011612 184 | Name: Pose 185 | Shaft Length: 1 186 | Shaft Radius: 0.05000000074505806 187 | Shape: Arrow 188 | Topic: 189 | Depth: 5 190 | Durability Policy: Volatile 191 | Filter size: 10 192 | History Policy: Keep Last 193 | Reliability Policy: Best Effort 194 | Value: /mavros/local_position/pose 195 | Value: true 196 | - Angle Tolerance: 0.10000000149011612 197 | Class: rviz_default_plugins/Odometry 198 | Covariance: 199 | Orientation: 200 | Alpha: 0.5 201 | Color: 255; 255; 127 202 | Color Style: Unique 203 | Frame: Local 204 | Offset: 1 205 | Scale: 1 206 | Value: true 207 | Position: 208 | Alpha: 0.30000001192092896 209 | Color: 204; 51; 204 210 | Scale: 1 211 | Value: true 212 | Value: false 213 | Enabled: true 214 | Keep: 1 215 | Name: Odometry 216 | Position Tolerance: 0.009999999776482582 217 | Shape: 218 | Alpha: 1 219 | Axes Length: 0.10000000149011612 220 | Axes Radius: 0.009999999776482582 221 | Color: 87; 227; 137 222 | Head Length: 0.30000001192092896 223 | Head Radius: 0.10000000149011612 224 | Shaft Length: 1 225 | Shaft Radius: 0.05000000074505806 226 | Value: Arrow 227 | Topic: 228 | Depth: 5 229 | Durability Policy: Volatile 230 | Filter size: 10 231 | History Policy: Keep Last 232 | Reliability Policy: Reliable 233 | Value: /model/orca4/odometry 234 | Value: true 235 | - Alpha: 1 236 | Axes Length: 1 237 | Axes Radius: 0.10000000149011612 238 | Class: rviz_default_plugins/Pose 239 | Color: 252; 233; 79 240 | Enabled: true 241 | Head Length: 0.30000001192092896 242 | Head Radius: 0.10000000149011612 243 | Name: Pose 244 | Shaft Length: 1 245 | Shaft Radius: 0.05000000074505806 246 | Shape: Arrow 247 | Topic: 248 | Depth: 5 249 | Durability Policy: Volatile 250 | Filter size: 10 251 | History Policy: Keep Last 252 | Reliability Policy: Reliable 253 | Value: /orb_slam2_stereo_node/pose 254 | Value: true 255 | Enabled: true 256 | Global Options: 257 | Background Color: 48; 48; 48 258 | Fixed Frame: map 259 | Frame Rate: 30 260 | Name: root 261 | Tools: 262 | - Class: rviz_default_plugins/Interact 263 | Hide Inactive Objects: true 264 | - Class: rviz_default_plugins/MoveCamera 265 | - Class: rviz_default_plugins/Select 266 | - Class: rviz_default_plugins/FocusCamera 267 | - Class: rviz_default_plugins/Measure 268 | Line color: 128; 128; 0 269 | - Class: rviz_default_plugins/SetInitialPose 270 | Covariance x: 0.25 271 | Covariance y: 0.25 272 | Covariance yaw: 0.06853891909122467 273 | Topic: 274 | Depth: 5 275 | Durability Policy: Volatile 276 | History Policy: Keep Last 277 | Reliability Policy: Reliable 278 | Value: /initialpose 279 | - Class: rviz_default_plugins/SetGoal 280 | Topic: 281 | Depth: 5 282 | Durability Policy: Volatile 283 | History Policy: Keep Last 284 | Reliability Policy: Reliable 285 | Value: /goal_pose 286 | - Class: rviz_default_plugins/PublishPoint 287 | Single click: true 288 | Topic: 289 | Depth: 5 290 | Durability Policy: Volatile 291 | History Policy: Keep Last 292 | Reliability Policy: Reliable 293 | Value: /clicked_point 294 | Transformation: 295 | Current: 296 | Class: rviz_default_plugins/TF 297 | Value: true 298 | Views: 299 | Current: 300 | Class: rviz_default_plugins/Orbit 301 | Distance: 26.763681411743164 302 | Enable Stereo Rendering: 303 | Stereo Eye Separation: 0.05999999865889549 304 | Stereo Focal Distance: 1 305 | Swap Stereo Eyes: false 306 | Value: false 307 | Focal Point: 308 | X: -1.6178656816482544 309 | Y: -2.1531503200531006 310 | Z: -2.8248047828674316 311 | Focal Shape Fixed Size: true 312 | Focal Shape Size: 0.05000000074505806 313 | Invert Z Axis: false 314 | Name: Current View 315 | Near Clip Distance: 0.009999999776482582 316 | Pitch: 0.5947968363761902 317 | Target Frame: 318 | Value: Orbit (rviz_default_plugins) 319 | Yaw: 3.972385883331299 320 | Saved: ~ 321 | Window Geometry: 322 | Displays: 323 | collapsed: false 324 | Height: 1980 325 | Hide Left Dock: false 326 | Hide Right Dock: false 327 | Image: 328 | collapsed: false 329 | QMainWindow State: 000000ff00000000fd00000004000000000000021500000754fc020000000afb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006e00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afc00000047000005910000011501000023fa000000010100000002fb0000000a005600690065007700730100000000ffffffff0000010200fffffffb000000100044006900730070006c0061007900730100000000000002e50000016100fffffffb0000000a0049006d00610067006501000005df000001bc0000002f00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d0061006700650000000406000002080000000000000000000000010000010f000007e3fc0200000002fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d006501000000000000045000000000000000000000051e0000075400000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 330 | Selection: 331 | collapsed: false 332 | Tool Properties: 333 | collapsed: false 334 | Views: 335 | collapsed: false 336 | Width: 1850 337 | X: 1952 338 | Y: 93 339 | -------------------------------------------------------------------------------- /orca_bringup/cfg/sim_left.ini: -------------------------------------------------------------------------------- 1 | # oST version 5.0 parameters 2 | 3 | 4 | [image] 5 | 6 | width 7 | 800 8 | 9 | height 10 | 600 11 | 12 | [stereo_left] 13 | 14 | camera matrix 15 | 474.89673285067175 0.000000 400.5 16 | 0.000000 474.89673285067175 300.5 17 | 0.000000 0.000000 1.000000 18 | 19 | distortion 20 | 0.0 0.0 0.0 0.0 0.0 21 | 22 | rectification 23 | 1.0 0.0 0.0 24 | 0.0 1.0 0.0 25 | 0.0 0.0 1.0 26 | 27 | projection 28 | 474.89673285067175 0.000000 400.5 0.000000 29 | 0.000000 474.89673285067175 300.5 0.000000 30 | 0.000000 0.000000 1.000000 0.000000 31 | -------------------------------------------------------------------------------- /orca_bringup/cfg/sim_right.ini: -------------------------------------------------------------------------------- 1 | # oST version 5.0 parameters 2 | 3 | 4 | [image] 5 | 6 | width 7 | 800 8 | 9 | height 10 | 600 11 | 12 | [stereo_right] 13 | 14 | camera matrix 15 | 474.89673285067175 0.000000 400.5 16 | 0.000000 474.89673285067175 300.5 17 | 0.000000 0.000000 1.000000 18 | 19 | distortion 20 | 0.0 0.0 0.0 0.0 0.0 21 | 22 | rectification 23 | 1.0 0.0 0.0 24 | 0.0 1.0 0.0 25 | 0.0 0.0 1.0 26 | 27 | projection 28 | 474.89673285067175 0.000000 400.5 -170.96282382624182 29 | 0.000000 474.89673285067175 300.5 0.000000 30 | 0.000000 0.000000 1.000000 0.000000 31 | -------------------------------------------------------------------------------- /orca_bringup/cfg/sub.parm: -------------------------------------------------------------------------------- 1 | # These settings were copied from ardupilot/Tools/autotest/default_params/sub.parm as of 25fd69b 2 | BATT_MONITOR 4 3 | BTN0_FUNCTION 0 4 | BTN10_SFUNCTION 0 5 | BTN10_FUNCTION 22 6 | BTN10_SFUNCTION 0 7 | BTN11_FUNCTION 42 8 | BTN11_SFUNCTION 47 9 | BTN12_FUNCTION 43 10 | BTN12_SFUNCTION 46 11 | BTN13_FUNCTION 33 12 | BTN13_SFUNCTION 45 13 | BTN14_FUNCTION 32 14 | BTN14_SFUNCTION 44 15 | BTN15_FUNCTION 0 16 | BTN15_SFUNCTION 0 17 | BTN1_FUNCTION 6 18 | BTN1_SFUNCTION 0 19 | BTN2_FUNCTION 8 20 | BTN2_SFUNCTION 0 21 | BTN3_FUNCTION 7 22 | BTN3_SFUNCTION 0 23 | BTN4_FUNCTION 4 24 | BTN4_SFUNCTION 0 25 | BTN5_FUNCTION 1 26 | BTN5_SFUNCTION 0 27 | BTN6_FUNCTION 3 28 | BTN6_SFUNCTION 0 29 | BTN7_FUNCTION 21 30 | BTN7_SFUNCTION 0 31 | BTN8_FUNCTION 48 32 | BTN8_SFUNCTION 0 33 | BTN9_FUNCTION 23 34 | BTN9_SFUNCTION 0 35 | COMPASS_OFS_X 5 36 | COMPASS_OFS_Y 13 37 | COMPASS_OFS_Z -18 38 | COMPASS_OFS2_X 5 39 | COMPASS_OFS2_Y 13 40 | COMPASS_OFS2_Z -18 41 | COMPASS_OFS3_X 5 42 | COMPASS_OFS3_Y 13 43 | COMPASS_OFS3_Z -18 44 | INS_ACCOFFS_X 0.001 45 | INS_ACCOFFS_Y 0.001 46 | INS_ACCOFFS_Z 0.001 47 | INS_ACCSCAL_X 1.001 48 | INS_ACCSCAL_Y 1.001 49 | INS_ACCSCAL_Z 1.001 50 | INS_ACC2OFFS_X 0.001 51 | INS_ACC2OFFS_Y 0.001 52 | INS_ACC2OFFS_Z 0.001 53 | INS_ACC2SCAL_X 1.001 54 | INS_ACC2SCAL_Y 1.001 55 | INS_ACC2SCAL_Z 1.001 56 | INS_ACC3OFFS_X 0.000 57 | INS_ACC3OFFS_Y 0.000 58 | INS_ACC3OFFS_Z 0.000 59 | INS_ACC3SCAL_X 1.000 60 | INS_ACC3SCAL_Y 1.000 61 | INS_ACC3SCAL_Z 1.000 62 | MNT_ANGMAX_TIL 4500 63 | MNT_ANGMIN_TIL -4500 64 | MNT_DEFLT_MODE 3 65 | MNT_JSTICK_SPD 100 66 | MNT_RC_IN_TILT 8 67 | MNT_STAB_TILT 0 68 | MNT_TYPE 1 69 | PSC_ACCZ_P 2 70 | PSC_ACCZ_I 4 71 | PSC_ACCZ_FF 0.75 72 | PSC_VELZ_P 8 73 | PSC_POSZ_P 3 74 | PSC_POSXY_P 2.5 75 | PSC_VELXY_D 0.8 76 | PSC_VELXY_I 0.5 77 | PSC_VELXY_P 5.0 78 | RNGFND1_MAX_CM 3000 79 | RNGFND1_PIN 0 80 | RNGFND1_SCALING 12.12 81 | RNGFND1_TYPE 1 82 | SERVO8_FUNCTION 7 83 | SERVO8_MAX 1900 84 | SERVO8_MIN 1100 85 | BARO_EXT_BUS 1 86 | 87 | # Enable external nav 88 | # From https://ardupilot.org/copter/docs/common-vio-tracking-camera.html 89 | GPS_TYPE 0 # Disable GPS 90 | VISO_TYPE 1 # External vision 91 | EK3_SRC1_POSXY 6 # External nav 92 | EK3_SRC1_VELXY 6 # External nav 93 | EK3_SRC1_POSZ 1 # Baro is the primary z source 94 | EK3_SRC1_VELZ 0 # None 95 | COMPASS_USE 0 # Disable compass 1 96 | COMPASS_USE2 0 # Disable compass 2 97 | COMPASS_USE3 0 # Disable compass 3 98 | EK3_SRC1_YAW 6 # External nav 99 | 100 | # Reduce simulated barometer noise 101 | SIM_BARO_RND 0.02 102 | -------------------------------------------------------------------------------- /orca_bringup/launch/bringup.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | # MIT License 4 | # 5 | # Copyright (c) 2022 Clyde McQueen 6 | # 7 | # Permission is hereby granted, free of charge, to any person obtaining a copy 8 | # of this software and associated documentation files (the "Software"), to deal 9 | # in the Software without restriction, including without limitation the rights 10 | # to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 11 | # copies of the Software, and to permit persons to whom the Software is 12 | # furnished to do so, subject to the following conditions: 13 | # 14 | # The above copyright notice and this permission notice shall be included in all 15 | # copies or substantial portions of the Software. 16 | # 17 | # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 18 | # IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 19 | # FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 20 | # AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 21 | # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 22 | # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 23 | # SOFTWARE. 24 | 25 | """ 26 | Bring up all nodes 27 | 28 | Use a modified navigation_launch.py that doesn't launch velocity_smoother. 29 | """ 30 | 31 | import math 32 | import os 33 | 34 | from ament_index_python.packages import get_package_share_directory 35 | from launch import LaunchDescription 36 | from launch.actions import DeclareLaunchArgument, ExecuteProcess, IncludeLaunchDescription, SetEnvironmentVariable 37 | from launch.conditions import IfCondition, UnlessCondition 38 | from launch.launch_description_sources import PythonLaunchDescriptionSource 39 | from launch.substitutions import LaunchConfiguration 40 | from launch_ros.actions import Node 41 | from nav2_common.launch import RewrittenYaml 42 | 43 | 44 | def generate_launch_description(): 45 | orca_bringup_dir = get_package_share_directory('orca_bringup') 46 | 47 | mavros_params_file = LaunchConfiguration('mavros_params_file') 48 | nav2_bt_file = os.path.join(orca_bringup_dir, 'behavior_trees', 'orca4_bt.xml') 49 | nav2_params_file = os.path.join(orca_bringup_dir, 'params', 'nav2_params.yaml') 50 | orca_params_file = LaunchConfiguration('orca_params_file') 51 | 52 | # get_package_share_directory('orb_slam2_ros') will fail if orb_slam2_ros isn't installed 53 | orb_voc_file = os.path.join('install', 'orb_slam2_ros', 'share', 'orb_slam2_ros', 54 | 'orb_slam2', 'Vocabulary', 'ORBvoc.txt') 55 | 56 | # Rewrite to add the full path 57 | # The rewriter will only rewrite existing keys 58 | configured_nav2_params = RewrittenYaml( 59 | source_file=nav2_params_file, 60 | param_rewrites={ 61 | 'default_nav_to_pose_bt_xml': nav2_bt_file, 62 | }, 63 | convert_types=True) 64 | 65 | return LaunchDescription([ 66 | SetEnvironmentVariable('RCUTILS_LOGGING_BUFFERED_STREAM', '1'), 67 | 68 | DeclareLaunchArgument( 69 | 'base', 70 | default_value='True', 71 | description='Launch base controller?', 72 | ), 73 | 74 | DeclareLaunchArgument( 75 | 'mavros', 76 | default_value='True', 77 | description='Launch mavros?', 78 | ), 79 | 80 | DeclareLaunchArgument( 81 | 'mavros_params_file', 82 | default_value=os.path.join(orca_bringup_dir, 'params', 'sim_mavros_params.yaml'), 83 | description='Full path to the ROS2 parameters file to use for mavros nodes', 84 | ), 85 | 86 | DeclareLaunchArgument( 87 | 'nav', 88 | default_value='True', 89 | description='Launch navigation?', 90 | ), 91 | 92 | DeclareLaunchArgument( 93 | 'orca_params_file', 94 | default_value=os.path.join(orca_bringup_dir, 'params', 'orca_params.yaml'), 95 | description='Full path to the ROS2 parameters file to use for Orca nodes', 96 | ), 97 | 98 | DeclareLaunchArgument( 99 | 'slam', 100 | default_value='True', 101 | description='Launch SLAM?', 102 | ), 103 | 104 | # Translate messages MAV <-> ROS 105 | Node( 106 | package='mavros', 107 | executable='mavros_node', 108 | output='screen', 109 | # mavros_node is actually many nodes, so we can't override the name 110 | # name='mavros_node', 111 | parameters=[mavros_params_file], 112 | condition=IfCondition(LaunchConfiguration('mavros')), 113 | ), 114 | 115 | # Manage overall system (start, stop, etc.) 116 | Node( 117 | package='orca_base', 118 | executable='manager', 119 | output='screen', 120 | name='manager', 121 | parameters=[orca_params_file], 122 | remappings=[ 123 | # Topic is hard coded in orb_slam2_ros to /orb_slam2_stereo_node/pose 124 | ('/camera_pose', '/orb_slam2_stereo_node/pose'), 125 | ], 126 | condition=IfCondition(LaunchConfiguration('base')), 127 | ), 128 | 129 | # Base controller and localizer; manage external nav input, publish tf2 transforms, etc. 130 | Node( 131 | package='orca_base', 132 | executable='base_controller', 133 | output='screen', 134 | name='base_controller', 135 | parameters=[orca_params_file], 136 | remappings=[ 137 | # Topic is hard coded in orb_slam2_ros to /orb_slam2_stereo_node/pose 138 | ('/camera_pose', '/orb_slam2_stereo_node/pose'), 139 | ], 140 | condition=IfCondition(LaunchConfiguration('base')), 141 | ), 142 | 143 | # Replacement for base_controller: complete the tf tree 144 | ExecuteProcess( 145 | cmd=['/opt/ros/humble/lib/tf2_ros/static_transform_publisher', 146 | '--frame-id', 'map', 147 | '--child-frame-id', 'slam'], 148 | output='screen', 149 | condition=UnlessCondition(LaunchConfiguration('base')), 150 | ), 151 | 152 | ExecuteProcess( 153 | cmd=['/opt/ros/humble/lib/tf2_ros/static_transform_publisher', 154 | '--frame-id', 'map', 155 | '--child-frame-id', 'odom'], 156 | output='screen', 157 | condition=UnlessCondition(LaunchConfiguration('base')), 158 | ), 159 | 160 | ExecuteProcess( 161 | cmd=['/opt/ros/humble/lib/tf2_ros/static_transform_publisher', 162 | '--frame-id', 'odom', 163 | '--child-frame-id', 'base_link'], 164 | output='screen', 165 | condition=UnlessCondition(LaunchConfiguration('base')), 166 | ), 167 | 168 | # Replacement for an URDF file: base_link->left_camera_link is static 169 | ExecuteProcess( 170 | cmd=['/opt/ros/humble/lib/tf2_ros/static_transform_publisher', 171 | '--x', '-0.15', 172 | '--y', '0.18', 173 | '--z', '-0.0675', 174 | '--pitch', str(math.pi/2), 175 | '--frame-id', 'base_link', 176 | '--child-frame-id', 'left_camera_link'], 177 | output='screen', 178 | ), 179 | 180 | # Provide down frame to accommodate down-facing cameras 181 | ExecuteProcess( 182 | cmd=['/opt/ros/humble/lib/tf2_ros/static_transform_publisher', 183 | '--pitch', str(math.pi/2), 184 | '--frame-id', 'slam', 185 | '--child-frame-id', 'down'], 186 | output='screen', 187 | ), 188 | 189 | # orb_slam2: build a map of 3d points, localize against the map, and publish the camera pose 190 | Node( 191 | package='orb_slam2_ros', 192 | executable='orb_slam2_ros_stereo', 193 | output='screen', 194 | name='orb_slam2_stereo', 195 | parameters=[orca_params_file, { 196 | 'voc_file': orb_voc_file, 197 | }], 198 | remappings=[ 199 | ('/image_left/image_color_rect', '/stereo_left'), 200 | ('/image_right/image_color_rect', '/stereo_right'), 201 | ('/camera/camera_info', '/stereo_right/camera_info'), 202 | ], 203 | condition=IfCondition(LaunchConfiguration('slam')), 204 | ), 205 | 206 | # Include the rest of Nav2 207 | IncludeLaunchDescription( 208 | PythonLaunchDescriptionSource(os.path.join(orca_bringup_dir, 'launch', 'navigation_launch.py')), 209 | launch_arguments={ 210 | 'namespace': '', 211 | 'use_sim_time': 'False', 212 | 'autostart': 'False', 213 | 'params_file': configured_nav2_params, 214 | 'use_composition': 'False', 215 | 'use_respawn': 'False', 216 | 'container_name': 'nav2_container', 217 | }.items(), 218 | condition=IfCondition(LaunchConfiguration('nav')), 219 | ), 220 | ]) 221 | -------------------------------------------------------------------------------- /orca_bringup/launch/navigation_launch.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2018 Intel Corporation 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | """ 16 | Copied from https://github.com/ros-planning/navigation2 and modified for Orca4 17 | 18 | velocity_smoother doesn't work in 3D, so don't use it. 19 | """ 20 | 21 | import os 22 | 23 | from ament_index_python.packages import get_package_share_directory 24 | 25 | from launch import LaunchDescription 26 | from launch.actions import DeclareLaunchArgument, GroupAction, SetEnvironmentVariable 27 | from launch.conditions import IfCondition 28 | from launch.substitutions import LaunchConfiguration, PythonExpression 29 | from launch_ros.actions import LoadComposableNodes 30 | from launch_ros.actions import Node 31 | from launch_ros.descriptions import ComposableNode 32 | from nav2_common.launch import RewrittenYaml 33 | 34 | 35 | def generate_launch_description(): 36 | # Get the launch directory 37 | bringup_dir = get_package_share_directory('nav2_bringup') 38 | 39 | namespace = LaunchConfiguration('namespace') 40 | use_sim_time = LaunchConfiguration('use_sim_time') 41 | autostart = LaunchConfiguration('autostart') 42 | params_file = LaunchConfiguration('params_file') 43 | use_composition = LaunchConfiguration('use_composition') 44 | container_name = LaunchConfiguration('container_name') 45 | use_respawn = LaunchConfiguration('use_respawn') 46 | log_level = LaunchConfiguration('log_level') 47 | 48 | # Don't launch velocity_smoother 49 | lifecycle_nodes = ['controller_server', 50 | 'smoother_server', 51 | 'planner_server', 52 | 'behavior_server', 53 | 'bt_navigator', 54 | 'waypoint_follower'] 55 | 56 | # Map fully qualified names to relative ones so the node's namespace can be prepended. 57 | # In case of the transforms (tf), currently, there doesn't seem to be a better alternative 58 | # https://github.com/ros/geometry2/issues/32 59 | # https://github.com/ros/robot_state_publisher/pull/30 60 | # TODO(orduno) Substitute with `PushNodeRemapping` 61 | # https://github.com/ros2/launch_ros/issues/56 62 | remappings = [('/tf', 'tf'), 63 | ('/tf_static', 'tf_static')] 64 | 65 | # Create our own temporary YAML files that include substitutions 66 | param_substitutions = { 67 | 'use_sim_time': use_sim_time, 68 | 'autostart': autostart} 69 | 70 | configured_params = RewrittenYaml( 71 | source_file=params_file, 72 | root_key=namespace, 73 | param_rewrites=param_substitutions, 74 | convert_types=True) 75 | 76 | stdout_linebuf_envvar = SetEnvironmentVariable( 77 | 'RCUTILS_LOGGING_BUFFERED_STREAM', '1') 78 | 79 | declare_namespace_cmd = DeclareLaunchArgument( 80 | 'namespace', 81 | default_value='', 82 | description='Top-level namespace') 83 | 84 | declare_use_sim_time_cmd = DeclareLaunchArgument( 85 | 'use_sim_time', 86 | default_value='false', 87 | description='Use simulation (Gazebo) clock if true') 88 | 89 | declare_params_file_cmd = DeclareLaunchArgument( 90 | 'params_file', 91 | default_value=os.path.join(bringup_dir, 'params', 'nav2_params.yaml'), 92 | description='Full path to the ROS2 parameters file to use for all launched nodes') 93 | 94 | declare_autostart_cmd = DeclareLaunchArgument( 95 | 'autostart', default_value='true', 96 | description='Automatically startup the nav2 stack') 97 | 98 | declare_use_composition_cmd = DeclareLaunchArgument( 99 | 'use_composition', default_value='False', 100 | description='Use composed bringup if True') 101 | 102 | declare_container_name_cmd = DeclareLaunchArgument( 103 | 'container_name', default_value='nav2_container', 104 | description='the name of conatiner that nodes will load in if use composition') 105 | 106 | declare_use_respawn_cmd = DeclareLaunchArgument( 107 | 'use_respawn', default_value='False', 108 | description='Whether to respawn if a node crashes. Applied when composition is disabled.') 109 | 110 | declare_log_level_cmd = DeclareLaunchArgument( 111 | 'log_level', default_value='info', 112 | description='log level') 113 | 114 | load_nodes = GroupAction( 115 | condition=IfCondition(PythonExpression(['not ', use_composition])), 116 | actions=[ 117 | Node( 118 | package='nav2_controller', 119 | executable='controller_server', 120 | output='screen', 121 | respawn=use_respawn, 122 | respawn_delay=2.0, 123 | parameters=[configured_params], 124 | arguments=['--ros-args', '--log-level', log_level], 125 | remappings=remappings), 126 | Node( 127 | package='nav2_smoother', 128 | executable='smoother_server', 129 | name='smoother_server', 130 | output='screen', 131 | respawn=use_respawn, 132 | respawn_delay=2.0, 133 | parameters=[configured_params], 134 | arguments=['--ros-args', '--log-level', log_level], 135 | remappings=remappings), 136 | Node( 137 | package='nav2_planner', 138 | executable='planner_server', 139 | name='planner_server', 140 | output='screen', 141 | respawn=use_respawn, 142 | respawn_delay=2.0, 143 | parameters=[configured_params], 144 | arguments=['--ros-args', '--log-level', log_level], 145 | remappings=remappings), 146 | Node( 147 | package='nav2_behaviors', 148 | executable='behavior_server', 149 | name='behavior_server', 150 | output='screen', 151 | respawn=use_respawn, 152 | respawn_delay=2.0, 153 | parameters=[configured_params], 154 | arguments=['--ros-args', '--log-level', log_level], 155 | remappings=remappings), 156 | Node( 157 | package='nav2_bt_navigator', 158 | executable='bt_navigator', 159 | name='bt_navigator', 160 | output='screen', 161 | respawn=use_respawn, 162 | respawn_delay=2.0, 163 | parameters=[configured_params], 164 | arguments=['--ros-args', '--log-level', log_level], 165 | remappings=remappings), 166 | Node( 167 | package='nav2_waypoint_follower', 168 | executable='waypoint_follower', 169 | name='waypoint_follower', 170 | output='screen', 171 | respawn=use_respawn, 172 | respawn_delay=2.0, 173 | parameters=[configured_params], 174 | arguments=['--ros-args', '--log-level', log_level], 175 | remappings=remappings), 176 | Node( 177 | package='nav2_lifecycle_manager', 178 | executable='lifecycle_manager', 179 | name='lifecycle_manager_navigation', 180 | output='screen', 181 | arguments=['--ros-args', '--log-level', log_level], 182 | parameters=[{'use_sim_time': use_sim_time}, 183 | {'autostart': autostart}, 184 | {'node_names': lifecycle_nodes}]), 185 | ] 186 | ) 187 | 188 | load_composable_nodes = LoadComposableNodes( 189 | condition=IfCondition(use_composition), 190 | target_container=container_name, 191 | composable_node_descriptions=[ 192 | ComposableNode( 193 | package='nav2_controller', 194 | plugin='nav2_controller::ControllerServer', 195 | name='controller_server', 196 | parameters=[configured_params], 197 | remappings=remappings), 198 | ComposableNode( 199 | package='nav2_smoother', 200 | plugin='nav2_smoother::SmootherServer', 201 | name='smoother_server', 202 | parameters=[configured_params], 203 | remappings=remappings), 204 | ComposableNode( 205 | package='nav2_planner', 206 | plugin='nav2_planner::PlannerServer', 207 | name='planner_server', 208 | parameters=[configured_params], 209 | remappings=remappings), 210 | ComposableNode( 211 | package='nav2_behaviors', 212 | plugin='behavior_server::BehaviorServer', 213 | name='behavior_server', 214 | parameters=[configured_params], 215 | remappings=remappings), 216 | ComposableNode( 217 | package='nav2_bt_navigator', 218 | plugin='nav2_bt_navigator::BtNavigator', 219 | name='bt_navigator', 220 | parameters=[configured_params], 221 | remappings=remappings), 222 | ComposableNode( 223 | package='nav2_waypoint_follower', 224 | plugin='nav2_waypoint_follower::WaypointFollower', 225 | name='waypoint_follower', 226 | parameters=[configured_params], 227 | remappings=remappings), 228 | ComposableNode( 229 | package='nav2_lifecycle_manager', 230 | plugin='nav2_lifecycle_manager::LifecycleManager', 231 | name='lifecycle_manager_navigation', 232 | parameters=[{'use_sim_time': use_sim_time, 233 | 'autostart': autostart, 234 | 'node_names': lifecycle_nodes}]), 235 | ], 236 | ) 237 | 238 | # Create the launch description and populate 239 | ld = LaunchDescription() 240 | 241 | # Set environment variables 242 | ld.add_action(stdout_linebuf_envvar) 243 | 244 | # Declare the launch options 245 | ld.add_action(declare_namespace_cmd) 246 | ld.add_action(declare_use_sim_time_cmd) 247 | ld.add_action(declare_params_file_cmd) 248 | ld.add_action(declare_autostart_cmd) 249 | ld.add_action(declare_use_composition_cmd) 250 | ld.add_action(declare_container_name_cmd) 251 | ld.add_action(declare_use_respawn_cmd) 252 | ld.add_action(declare_log_level_cmd) 253 | # Add the actions to launch all of the navigation nodes 254 | ld.add_action(load_nodes) 255 | ld.add_action(load_composable_nodes) 256 | 257 | return ld 258 | -------------------------------------------------------------------------------- /orca_bringup/launch/sim_launch.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | # MIT License 4 | # 5 | # Copyright (c) 2022 Clyde McQueen 6 | # 7 | # Permission is hereby granted, free of charge, to any person obtaining a copy 8 | # of this software and associated documentation files (the "Software"), to deal 9 | # in the Software without restriction, including without limitation the rights 10 | # to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 11 | # copies of the Software, and to permit persons to whom the Software is 12 | # furnished to do so, subject to the following conditions: 13 | # 14 | # The above copyright notice and this permission notice shall be included in all 15 | # copies or substantial portions of the Software. 16 | # 17 | # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 18 | # IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 19 | # FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 20 | # AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 21 | # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 22 | # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 23 | # SOFTWARE. 24 | 25 | """ 26 | Launch a simulation. 27 | 28 | Includes Gazebo, ArduSub, RViz, mavros, all ROS nodes. 29 | """ 30 | 31 | import os 32 | 33 | from ament_index_python.packages import get_package_share_directory 34 | from launch import LaunchDescription 35 | from launch.actions import DeclareLaunchArgument, ExecuteProcess, IncludeLaunchDescription 36 | from launch.conditions import IfCondition, UnlessCondition 37 | from launch.launch_description_sources import PythonLaunchDescriptionSource 38 | from launch.substitutions import LaunchConfiguration 39 | from launch_ros.actions import Node 40 | 41 | 42 | def generate_launch_description(): 43 | orca_bringup_dir = get_package_share_directory('orca_bringup') 44 | orca_description_dir = get_package_share_directory('orca_description') 45 | 46 | ardusub_params_file = os.path.join(orca_bringup_dir, 'cfg', 'sub.parm') 47 | mavros_params_file = os.path.join(orca_bringup_dir, 'params', 'sim_mavros_params.yaml') 48 | orca_params_file = os.path.join(orca_bringup_dir, 'params', 'sim_orca_params.yaml') 49 | rosbag2_record_qos_file = os.path.join(orca_bringup_dir, 'params', 'rosbag2_record_qos.yaml') 50 | rviz_file = os.path.join(orca_bringup_dir, 'cfg', 'sim_launch.rviz') 51 | world_file = os.path.join(orca_description_dir, 'worlds', 'sand.world') 52 | 53 | sim_left_ini = os.path.join(orca_bringup_dir, 'cfg', 'sim_left.ini') 54 | sim_right_ini = os.path.join(orca_bringup_dir, 'cfg', 'sim_right.ini') 55 | return LaunchDescription([ 56 | DeclareLaunchArgument( 57 | 'ardusub', 58 | default_value='True', 59 | description='Launch ArduSUB with SIM_JSON?' 60 | ), 61 | 62 | DeclareLaunchArgument( 63 | 'bag', 64 | default_value='False', 65 | description='Bag interesting topics?', 66 | ), 67 | 68 | DeclareLaunchArgument( 69 | 'base', 70 | default_value='True', 71 | description='Launch base controller?', 72 | ), 73 | 74 | DeclareLaunchArgument( 75 | 'gzclient', 76 | default_value='True', 77 | description='Launch Gazebo UI?' 78 | ), 79 | 80 | DeclareLaunchArgument( 81 | 'mavros', 82 | default_value='True', 83 | description='Launch mavros?', 84 | ), 85 | 86 | DeclareLaunchArgument( 87 | 'nav', 88 | default_value='True', 89 | description='Launch navigation?', 90 | ), 91 | 92 | DeclareLaunchArgument( 93 | 'rviz', 94 | default_value='True', 95 | description='Launch rviz?', 96 | ), 97 | 98 | DeclareLaunchArgument( 99 | 'slam', 100 | default_value='True', 101 | description='Launch SLAM?', 102 | ), 103 | 104 | # Bag useful topics 105 | ExecuteProcess( 106 | cmd=[ 107 | 'ros2', 'bag', 'record', 108 | '--qos-profile-overrides-path', rosbag2_record_qos_file, 109 | '--include-hidden-topics', 110 | '/cmd_vel', 111 | '/mavros/local_position/pose', 112 | '/mavros/rc/override', 113 | '/mavros/setpoint_position/global', 114 | '/mavros/state', 115 | '/mavros/vision_pose/pose', 116 | '/model/orca4/odometry', 117 | '/motion', 118 | '/odom', 119 | '/orb_slam2_stereo_node/pose', 120 | '/orb_slam2_stereo_node/status', 121 | '/pid_z', 122 | '/rosout', 123 | '/tf', 124 | '/tf_static', 125 | ], 126 | output='screen', 127 | condition=IfCondition(LaunchConfiguration('bag')), 128 | ), 129 | 130 | # Launch rviz 131 | ExecuteProcess( 132 | cmd=['rviz2', '-d', rviz_file], 133 | output='screen', 134 | condition=IfCondition(LaunchConfiguration('rviz')), 135 | ), 136 | 137 | # Launch ArduSub w/ SIM_JSON 138 | # -w: wipe eeprom 139 | # --home: start location (lat,lon,alt,yaw). Yaw is provided by Gazebo, so the start yaw value is ignored. 140 | # ardusub must be on the $PATH, see src/orca4/setup.bash 141 | ExecuteProcess( 142 | cmd=['ardusub', '-S', '-w', '-M', 'JSON', '--defaults', ardusub_params_file, 143 | '-I0', '--home', '33.810313,-118.39386700000001,0.0,0'], 144 | output='screen', 145 | condition=IfCondition(LaunchConfiguration('ardusub')), 146 | ), 147 | 148 | # Launch Gazebo Sim 149 | # gz must be on the $PATH 150 | # libArduPilotPlugin.so must be on the GZ_SIM_SYSTEM_PLUGIN_PATH 151 | ExecuteProcess( 152 | cmd=['gz', 'sim', '-v', '3', '-r', world_file], 153 | output='screen', 154 | condition=IfCondition(LaunchConfiguration('gzclient')), 155 | ), 156 | 157 | # Launch Gazebo Sim server-only 158 | ExecuteProcess( 159 | cmd=['gz', 'sim', '-v', '3', '-r', '-s', world_file], 160 | output='screen', 161 | condition=UnlessCondition(LaunchConfiguration('gzclient')), 162 | ), 163 | 164 | # Get images from Gazebo Sim to ROS 165 | Node( 166 | package='ros_gz_image', 167 | executable='image_bridge', 168 | arguments=['stereo_left', 'stereo_right'], 169 | output='screen', 170 | ), 171 | 172 | # Gazebo Sim doesn't publish camera info, so do that here 173 | Node( 174 | package='orca_base', 175 | executable='camera_info_publisher', 176 | name='left_info_publisher', 177 | output='screen', 178 | parameters=[{ 179 | 'camera_info_url': 'file://' + sim_left_ini, 180 | 'camera_name': 'stereo_left', 181 | 'frame_id': 'stereo_left_frame', 182 | 'timer_period_ms': 50, 183 | }], 184 | remappings=[ 185 | ('/camera_info', '/stereo_left/camera_info'), 186 | ], 187 | ), 188 | 189 | Node( 190 | package='orca_base', 191 | executable='camera_info_publisher', 192 | name='right_info_publisher', 193 | output='screen', 194 | parameters=[{ 195 | 'camera_info_url': 'file://' + sim_right_ini, 196 | 'camera_name': 'stereo_right', 197 | 'frame_id': 'stereo_right_frame', 198 | 'timer_period_ms': 50, 199 | }], 200 | remappings=[ 201 | ('/camera_info', '/stereo_right/camera_info'), 202 | ], 203 | ), 204 | 205 | # Publish ground truth pose from Ignition Gazebo 206 | Node( 207 | package='ros_gz_bridge', 208 | executable='parameter_bridge', 209 | arguments=[ 210 | '/model/orca4/odometry@nav_msgs/msg/Odometry[gz.msgs.Odometry', 211 | ], 212 | output='screen' 213 | ), 214 | 215 | # Bring up Orca and Nav2 nodes 216 | IncludeLaunchDescription( 217 | PythonLaunchDescriptionSource(os.path.join(orca_bringup_dir, 'launch', 'bringup.py')), 218 | launch_arguments={ 219 | 'base': LaunchConfiguration('base'), 220 | 'mavros': LaunchConfiguration('mavros'), 221 | 'mavros_params_file': mavros_params_file, 222 | 'nav': LaunchConfiguration('nav'), 223 | 'orca_params_file': orca_params_file, 224 | 'slam': LaunchConfiguration('slam'), 225 | }.items(), 226 | ), 227 | ]) 228 | 229 | -------------------------------------------------------------------------------- /orca_bringup/missions/missions.txt: -------------------------------------------------------------------------------- 1 | # Example missions 2 | 3 | # The poses here work well if 2 things are true: 4 | # 1. the world file includes the sand_heightmap at {xyz: {0, 7, -10}, rpy: {0, 0, 0}} 5 | # 2. the first pose drops to xyz: {0, 0, -7} to get a good view of the sand_heightmap 6 | 7 | # Simple launch 8 | ros2 launch orca_bringup sim_launch.py gzclient:=False 9 | 10 | # Disarm 11 | ros2 action send_goal /set_target_mode orca_msgs/action/TargetMode "{target_mode: 1}" 12 | 13 | # ROV mode 14 | ros2 action send_goal /set_target_mode orca_msgs/action/TargetMode "{target_mode: 2}" 15 | 16 | # AUV mode 17 | ros2 action send_goal /set_target_mode orca_msgs/action/TargetMode "{target_mode: 3}" 18 | 19 | # Drop to the 3m above the seafloor and loop around 20 | ros2 action send_goal /follow_waypoints nav2_msgs/action/FollowWaypoints "{poses: [\ 21 | {header: {frame_id: 'map'}, pose: {position: {x: 0, y: 0, z: -7}}},\ 22 | {header: {frame_id: 'map'}, pose: {position: {x: 20, y: -13, z: -7}}},\ 23 | {header: {frame_id: 'map'}, pose: {position: {x: 10, y: -23, z: -7}}},\ 24 | {header: {frame_id: 'map'}, pose: {position: {x: -10, y: -8, z: -7}}},\ 25 | {header: {frame_id: 'map'}, pose: {position: {x: 0, y: 0, z: -7}}},\ 26 | ]}" 27 | 28 | # Same as above, but 2 times around, this always causes a loop closure 29 | ros2 action send_goal /follow_waypoints nav2_msgs/action/FollowWaypoints "{poses: [\ 30 | {header: {frame_id: 'map'}, pose: {position: {x: 0, y: 0, z: -7}}},\ 31 | {header: {frame_id: 'map'}, pose: {position: {x: 20, y: -13, z: -7}}},\ 32 | {header: {frame_id: 'map'}, pose: {position: {x: 10, y: -23, z: -7}}},\ 33 | {header: {frame_id: 'map'}, pose: {position: {x: -10, y: -8, z: -7}}},\ 34 | {header: {frame_id: 'map'}, pose: {position: {x: 0, y: 0, z: -7}}},\ 35 | {header: {frame_id: 'map'}, pose: {position: {x: 20, y: -13, z: -7}}},\ 36 | {header: {frame_id: 'map'}, pose: {position: {x: 10, y: -23, z: -7}}},\ 37 | {header: {frame_id: 'map'}, pose: {position: {x: -10, y: -8, z: -7}}},\ 38 | {header: {frame_id: 'map'}, pose: {position: {x: 0, y: 0, z: -7}}},\ 39 | ]}" 40 | 41 | # Mow the lawn, this causes loop closure in the last step 42 | ros2 action send_goal /follow_waypoints nav2_msgs/action/FollowWaypoints "{poses: [\ 43 | {header: {frame_id: 'map'}, pose: {position: {x: 0, y: 0, z: -7}}},\ 44 | {header: {frame_id: 'map'}, pose: {position: {x: 20, y: -13, z: -7}}},\ 45 | {header: {frame_id: 'map'}, pose: {position: {x: 17.5, y: -15.5, z: -7}}},\ 46 | {header: {frame_id: 'map'}, pose: {position: {x: -2.5, y: -0.5, z: -7}}},\ 47 | {header: {frame_id: 'map'}, pose: {position: {x: -5, y: -3, z: -7}}},\ 48 | {header: {frame_id: 'map'}, pose: {position: {x: 15, y: -18, z: -7}}},\ 49 | {header: {frame_id: 'map'}, pose: {position: {x: 12.5, y: -20.5, z: -7}}},\ 50 | {header: {frame_id: 'map'}, pose: {position: {x: -7.5, y: -5.5, z: -7}}},\ 51 | {header: {frame_id: 'map'}, pose: {position: {x: -10, y: -8, z: -7}}},\ 52 | {header: {frame_id: 'map'}, pose: {position: {x: 10, y: -23, z: -7}}},\ 53 | {header: {frame_id: 'map'}, pose: {position: {x: 0, y: 0, z: -7}}},\ 54 | ]}" 55 | -------------------------------------------------------------------------------- /orca_bringup/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | orca_bringup 6 | 0.5.0 7 | Launch scripts 8 | 9 | Clyde McQueen 10 | MIT 11 | 12 | https://github.com/clydemcqueen/orca4.git 13 | https://github.com/clydemcqueen/orca4/issues 14 | 15 | Clyde McQueen 16 | 17 | ament_cmake 18 | 19 | nav2_bringup 20 | mavros 21 | mavros_extras 22 | 23 | 24 | 25 | 26 | 27 | 28 | ament_lint_auto 29 | ament_cmake_copyright 30 | ament_cmake_lint_cmake 31 | ament_cmake_xmllint 32 | 33 | 34 | ament_cmake 35 | 36 | 37 | 38 | -------------------------------------------------------------------------------- /orca_bringup/params/nav2_params.yaml: -------------------------------------------------------------------------------- 1 | # Params for Nav2 nodes 2 | 3 | bt_navigator: 4 | ros__parameters: 5 | use_sim_time: False 6 | global_frame: "map" 7 | robot_base_frame: "base_link" 8 | odom_topic: "/odom" 9 | bt_loop_duration: 10 10 | default_server_timeout: 20 11 | default_nav_to_pose_bt_xml: "bringup.py should replace me" 12 | 13 | # Nav2 gets mad if we don't load most (all?) of these plugins, even though they are not used 14 | plugin_lib_names: 15 | - nav2_compute_path_to_pose_action_bt_node 16 | - nav2_compute_path_through_poses_action_bt_node 17 | - nav2_follow_path_action_bt_node 18 | - nav2_back_up_action_bt_node 19 | - nav2_spin_action_bt_node 20 | - nav2_wait_action_bt_node 21 | - nav2_clear_costmap_service_bt_node 22 | - nav2_is_stuck_condition_bt_node 23 | - nav2_goal_reached_condition_bt_node 24 | - nav2_goal_updated_condition_bt_node 25 | - nav2_initial_pose_received_condition_bt_node 26 | - nav2_reinitialize_global_localization_service_bt_node 27 | - nav2_rate_controller_bt_node 28 | - nav2_distance_controller_bt_node 29 | - nav2_speed_controller_bt_node 30 | - nav2_truncate_path_action_bt_node 31 | - nav2_goal_updater_node_bt_node 32 | - nav2_recovery_node_bt_node 33 | - nav2_pipeline_sequence_bt_node 34 | - nav2_round_robin_node_bt_node 35 | - nav2_transform_available_condition_bt_node 36 | - nav2_time_expired_condition_bt_node 37 | - nav2_distance_traveled_condition_bt_node 38 | - nav2_single_trigger_bt_node 39 | - nav2_is_battery_low_condition_bt_node 40 | - nav2_navigate_through_poses_action_bt_node 41 | - nav2_navigate_to_pose_action_bt_node 42 | - nav2_remove_passed_goals_action_bt_node 43 | - nav2_planner_selector_bt_node 44 | - nav2_controller_selector_bt_node 45 | - nav2_goal_checker_selector_bt_node 46 | 47 | bt_navigator_rclcpp_node: 48 | ros__parameters: 49 | use_sim_time: False 50 | 51 | controller_server: 52 | ros__parameters: 53 | use_sim_time: False 54 | controller_frequency: 20.0 55 | min_x_velocity_threshold: 0.001 56 | min_y_velocity_threshold: 0.5 57 | min_theta_velocity_threshold: 0.001 58 | progress_checker_plugin: "progress_checker" 59 | goal_checker_plugins: ["goal_checker"] 60 | controller_plugins: ["FollowPath"] 61 | 62 | # Progress checker parameters 63 | progress_checker: 64 | plugin: "orca_nav2::ProgressChecker3D" 65 | radius: 0.5 66 | time_allowance: 10.0 67 | 68 | # Goal checker parameters 69 | goal_checker: 70 | plugin: "orca_nav2::GoalChecker3D" 71 | xy_goal_tolerance: 0.8 72 | z_goal_tolerance: 0.5 73 | 74 | # Controller parameters 75 | FollowPath: 76 | plugin: "orca_nav2::PurePursuitController3D" 77 | lookahead_dist: 1.0 78 | transform_tolerance: 1.0 79 | goal_tolerance: 0.2 80 | tick_rate: 20.0 # Loop rate, must match controller_frequency 81 | 82 | # Desired velocities and accelerations 83 | x_vel: 0.4 84 | x_accel: 0.1 85 | z_vel: 0.3 86 | z_accel: 0.1 87 | yaw_vel: 0.25 88 | yaw_accel: 0.5 89 | 90 | # The motion model may be bogus, so actual velocity may be lower or higher than desired 91 | # velocity. Specify a range using the error terms, e.g., 92 | # [(1 - yaw_error_) * yaw_vel_, (1 + yaw_error_) * yaw_vel_] 93 | x_error: 0.5 94 | yaw_error: 0.6 95 | 96 | controller_server_rclcpp_node: 97 | ros__parameters: 98 | use_sim_time: False 99 | 100 | # The local costmap is always empty 101 | # This configuration can probably be simplified, e.g., there's no laser scanner 102 | local_costmap: 103 | local_costmap: 104 | ros__parameters: 105 | update_frequency: 5.0 106 | publish_frequency: 2.0 107 | global_frame: "odom" 108 | robot_base_frame: "base_link" 109 | use_sim_time: False 110 | rolling_window: True 111 | width: 3 112 | height: 3 113 | resolution: 0.05 114 | robot_radius: 0.22 115 | plugins: ["voxel_layer", "inflation_layer"] 116 | inflation_layer: 117 | plugin: "nav2_costmap_2d::InflationLayer" 118 | cost_scaling_factor: 3.0 119 | voxel_layer: 120 | plugin: "nav2_costmap_2d::VoxelLayer" 121 | enabled: True 122 | publish_voxel_map: True 123 | origin_z: 0.0 124 | z_resolution: 0.05 125 | z_voxels: 16 126 | max_obstacle_height: 2.0 127 | mark_threshold: 0 128 | observation_sources: "scan" 129 | scan: 130 | topic: "/scan" 131 | max_obstacle_height: 2.0 132 | clearing: True 133 | marking: True 134 | data_type: "LaserScan" 135 | always_send_full_costmap: True 136 | local_costmap_client: 137 | ros__parameters: 138 | use_sim_time: False 139 | local_costmap_rclcpp_node: 140 | ros__parameters: 141 | use_sim_time: False 142 | 143 | # The global costmap is always empty 144 | global_costmap: 145 | global_costmap: 146 | ros__parameters: 147 | update_frequency: 1.0 148 | publish_frequency: 1.0 149 | global_frame: "map" 150 | robot_base_frame: "base_link" 151 | use_sim_time: False 152 | width: 100 153 | height: 100 154 | origin_x: -50.0 155 | origin_y: -50.0 156 | resolution: 1.0 157 | robot_radius: 0.22 158 | plugins: ["obstacle_layer", "inflation_layer"] 159 | obstacle_layer: 160 | plugin: "nav2_costmap_2d::ObstacleLayer" 161 | enabled: True 162 | observation_sources: "scan" 163 | scan: 164 | topic: "/scan" 165 | max_obstacle_height: 2.0 166 | clearing: True 167 | marking: True 168 | data_type: "LaserScan" 169 | inflation_layer: 170 | plugin: "nav2_costmap_2d::InflationLayer" 171 | always_send_full_costmap: True 172 | global_costmap_client: 173 | ros__parameters: 174 | use_sim_time: False 175 | global_costmap_rclcpp_node: 176 | ros__parameters: 177 | use_sim_time: False 178 | 179 | planner_server: 180 | ros__parameters: 181 | use_sim_time: False 182 | planner_plugins: ["GridBased"] 183 | 184 | # OrcaPlanner parameters 185 | GridBased: 186 | plugin: "orca_nav2/StraightLinePlanner3D" 187 | planning_dist: 0.1 # Plan a pose every planning_dist meters 188 | 189 | planner_server_rclcpp_node: 190 | ros__parameters: 191 | use_sim_time: False 192 | 193 | # TODO is this used? Should be behaviors? 194 | recoveries_server: 195 | ros__parameters: 196 | costmap_topic: "local_costmap/costmap_raw" 197 | footprint_topic: "local_costmap/published_footprint" 198 | cycle_frequency: 10.0 199 | recovery_plugins: ["spin", "backup", "wait"] 200 | spin: 201 | plugin: "nav2_recoveries/Spin" 202 | backup: 203 | plugin: "nav2_recoveries/BackUp" 204 | wait: 205 | plugin: "nav2_recoveries/Wait" 206 | global_frame: "odom" 207 | robot_base_frame: "base_link" 208 | transform_timeout: 0.1 209 | use_sim_time: False 210 | simulate_ahead_time: 2.0 211 | max_rotational_vel: 1.0 212 | min_rotational_vel: 0.4 213 | rotational_acc_lim: 3.2 214 | 215 | robot_state_publisher: 216 | ros__parameters: 217 | use_sim_time: False 218 | 219 | waypoint_follower: 220 | ros__parameters: 221 | loop_rate: 20 222 | stop_on_failure: False 223 | waypoint_task_executor_plugin: "wait_at_waypoint" 224 | wait_at_waypoint: 225 | plugin: "nav2_waypoint_follower::WaitAtWaypoint" 226 | enabled: True 227 | waypoint_pause_duration: 200 # Sleep for 200ms -------------------------------------------------------------------------------- /orca_bringup/params/rosbag2_record_qos.yaml: -------------------------------------------------------------------------------- 1 | # TODO(clyde): can I force mavros to use RELIABLE? This would simplify things 2 | 3 | /mavros/battery: 4 | reliability: best_effort 5 | history: keep_all 6 | 7 | /mavros/local_position/accel: 8 | reliability: best_effort 9 | history: keep_all 10 | 11 | /mavros/local_position/odom: 12 | reliability: best_effort 13 | history: keep_all 14 | 15 | /mavros/local_position/pose: 16 | reliability: best_effort 17 | history: keep_all 18 | 19 | /mavros/local_position/velocity_body: 20 | reliability: best_effort 21 | history: keep_all 22 | 23 | /mavros/setpoint_position/global: 24 | reliability: best_effort 25 | history: keep_all 26 | 27 | /mavros/setpoint_position/global_to_local: 28 | reliability: best_effort 29 | history: keep_all 30 | 31 | /mavros/setpoint_position/local: 32 | reliability: best_effort 33 | history: keep_all -------------------------------------------------------------------------------- /orca_bringup/params/sim_mavros_params.yaml: -------------------------------------------------------------------------------- 1 | # Connect to the FCU (ArduSub) and pass messages to/from the GCS (QGroundControl, mavproxy) 2 | # 3 | # See https://github.com/mavlink/mavros/tree/ros2/mavros for connection schemas and defaults 4 | # 5 | # udp://[bind_host][:port]@[remote_host[:port]][/?ids=sysid,compid] 6 | # udp default ports: 14555@14550 7 | # 8 | # tcp://[server_host][:port][/?ids=sysid,compid] 9 | # tcp default port: 5760 10 | 11 | mavros: 12 | ros__parameters: 13 | system_id: 255 14 | 15 | # /mavros/local_position/pose requires 2 plugins: 16 | # imu handles the ATTITUDE_QUATERNION msg (MAV id 31) 17 | # local_position handles the LOCAL_POSITION_NED msg (MAV id 32) 18 | # Also be sure to set the message interval to ~50ms for these 2 messages 19 | 20 | # The vision_pose plugin is provided by mavros_extras. 21 | # MAVROS will not complain if this package is missing. 22 | 23 | plugin_allowlist: 24 | - sys_status 25 | - command 26 | - imu 27 | - local_position 28 | - rc_io 29 | - setpoint_position 30 | - vision_pose 31 | 32 | mavros_node: 33 | ros__parameters: 34 | fcu_url: "tcp://localhost" 35 | gcs_url: "udp://@localhost:14550" 36 | 37 | mavros/local_position: 38 | ros__parameters: 39 | frame_id: "map" # frame_id of /mavros/local_position/pose, etc. 40 | tf: 41 | send: false # Do not publish tf 42 | 43 | mavros/vision_pose: 44 | ros__parameters: 45 | tf: 46 | listen: false # Do not listen to tf 47 | -------------------------------------------------------------------------------- /orca_bringup/params/sim_orca_params.yaml: -------------------------------------------------------------------------------- 1 | # Params for Orca nodes: 2 | 3 | manager: 4 | ros__parameters: 5 | mav_msg_ids: [31, 32] 6 | mav_msg_rate: 20 7 | 8 | base_controller: 9 | ros__parameters: 10 | use_sim_time: False 11 | timer_rate: 20 # Motion model period 12 | 13 | # List frames for clarity: 14 | map_frame_id: "map" # ArduSub EKF frame; mavros expects this to be "map" 15 | slam_frame_id: "slam" # Origin of map generated by SLAM 16 | down_frame_id: "down" # Stereo camera points down 17 | odom_frame_id: "odom" 18 | base_frame_id: "base_link" 19 | camera_frame_id: "left_camera_link" # Note: the link, not the frame 20 | 21 | # Motion model 22 | mdl_mass: 9.9 23 | mdl_volume: 0.01 24 | mdl_fluid_density: 997.0 25 | mdl_drag_partial_const_yaw: 0.07 # Hand-tuned drag value for yaw_vel == 0.25, see nav2 params 26 | mdl_thrust_dz_pwm: 0 # No input deadzone for rc override 27 | 28 | # These are maximums, so they should be higher than the nav2 params 29 | x_vel: 0.8 30 | y_vel: 0.2 31 | z_vel: 0.5 32 | yaw_vel: 0.5 33 | x_accel: 1.0 34 | y_accel: 0.2 35 | z_accel: 0.5 36 | yaw_accel: 1.0 37 | 38 | # Params for orb_slam2 nodes: 39 | 40 | orb_slam2_stereo: 41 | ros__parameters: 42 | use_sim_time: False 43 | 44 | # ros_ign_image publishes images RELIABLE 45 | subscribe_best_effort: False 46 | 47 | publish_pointcloud: True 48 | publish_pose: True 49 | publish_tf: False 50 | localize_only: False 51 | 52 | pointcloud_frame_id: "down" # Map frame to use down-facing cameras 53 | camera_frame_id: "left_camera_link" 54 | 55 | map_file: "map.bin" 56 | load_map: False 57 | 58 | ORBextractor/nFeatures: 1200 59 | ORBextractor/scaleFactor: 1.2 60 | ORBextractor/nLevels: 8 61 | ORBextractor/iniThFAST: 20 62 | ORBextractor/minThFAST: 7 63 | 64 | camera_fps: 30 65 | # Color order of the images (0: BGR, 1: RGB. It is ignored if images are grayscale) 66 | camera_rgb_encoding: True 67 | 68 | # See https://github.com/raulmur/ORB_SLAM2/issues/89 for the author's explanation of these: 69 | ThDepth: 40.0 70 | depth_map_factor: 1.0 71 | camera_baseline: 171.0 # Right camera info p[3], flip sign 72 | -------------------------------------------------------------------------------- /orca_bringup/scripts/auv.sh: -------------------------------------------------------------------------------- 1 | # Set orca4 to AUV mode; base_controller will respond to /cmd_vel messages 2 | ros2 action send_goal /set_target_mode orca_msgs/action/TargetMode "{target_mode: 3}" 3 | -------------------------------------------------------------------------------- /orca_bringup/scripts/disarm.sh: -------------------------------------------------------------------------------- 1 | # Disarm orca4; abort active mission 2 | ros2 action send_goal /set_target_mode orca_msgs/action/TargetMode "{target_mode: 1}" 3 | -------------------------------------------------------------------------------- /orca_bringup/scripts/dump_rosout.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | # MIT License 4 | # 5 | # Copyright (c) 2022 Clyde McQueen 6 | # 7 | # Permission is hereby granted, free of charge, to any person obtaining a copy 8 | # of this software and associated documentation files (the "Software"), to deal 9 | # in the Software without restriction, including without limitation the rights 10 | # to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 11 | # copies of the Software, and to permit persons to whom the Software is 12 | # furnished to do so, subject to the following conditions: 13 | # 14 | # The above copyright notice and this permission notice shall be included in all 15 | # copies or substantial portions of the Software. 16 | # 17 | # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 18 | # IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 19 | # FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 20 | # AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 21 | # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 22 | # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 23 | # SOFTWARE. 24 | 25 | """ 26 | Read a ROS2 bag file and dump the /rosout topic. 27 | """ 28 | 29 | import sqlite3 30 | import sys 31 | import time 32 | from rcl_interfaces.msg import Log 33 | from rclpy.serialization import deserialize_message 34 | from rosidl_runtime_py.utilities import get_message 35 | from typing import List 36 | 37 | 38 | class BagParser: 39 | def __init__(self, bag_file=''): 40 | self.conn = sqlite3.connect(bag_file) 41 | self.cursor = self.conn.cursor() 42 | 43 | topics_data = self.cursor.execute('SELECT id, name, type FROM topics').fetchall() 44 | self.topic_type = {name_of: type_of for id_of, name_of, type_of in topics_data} 45 | self.topic_id = {name_of: id_of for id_of, name_of, type_of in topics_data} 46 | self.topic_msg_message = {name_of: get_message(type_of) for id_of, name_of, type_of in 47 | topics_data} 48 | 49 | def __del__(self): 50 | self.conn.close() 51 | 52 | def get_messages(self, topic_name): 53 | topic_id = self.topic_id[topic_name] 54 | rows = self.cursor.execute( 55 | 'SELECT timestamp, data FROM messages WHERE topic_id = {}'.format(topic_id)).fetchall() 56 | return [(timestamp, deserialize_message(data, self.topic_msg_message[topic_name])) for 57 | timestamp, data in rows] 58 | 59 | 60 | def print_usage(): 61 | print('Print all messages:') 62 | print(' ros2 run orca_bringup dump_rosout.py /path/bag.db3') 63 | 64 | print('Print messages from only1, only2, ...:') 65 | print(' ros2 run orca_bringup dump_rosout.py /path/bag.db3 [only only1 [only2 ...]]') 66 | 67 | print('Ignore messages from ignore1, ignore2, ...:') 68 | print(' ros2 run orca_bringup dump_rosout.py /path/bag.db3 [ignore ignore1 [ignore2 ...]]') 69 | 70 | 71 | def main(args: List): 72 | if len(args) < 2 or len(args) == 3: 73 | print_usage() 74 | return 75 | 76 | only = [] 77 | ignore = [] 78 | 79 | if len(args) > 2: 80 | modifier = args[2] 81 | if modifier != 'only' and modifier != 'ignore': 82 | print_usage() 83 | return 84 | 85 | for arg in args[3:]: 86 | if modifier == 'only': 87 | print('Only', arg) 88 | only.append(arg) 89 | else: 90 | print('Ignore', arg) 91 | ignore.append(arg) 92 | 93 | print('Open', args[1]) 94 | print('----------------------------------------------------') 95 | parser = BagParser(args[1]) 96 | rosout = parser.get_messages('/rosout') 97 | for msg in rosout: 98 | log: Log = msg[1] 99 | if (len(only) and log.name in only) or (not len(only) and log.name not in ignore): 100 | stamp = time.strftime('%Y-%m-%d %H:%M:%S', time.localtime(msg[0] / 1e9)) 101 | print(stamp, ':', log.name, ':', log.msg) 102 | 103 | 104 | if __name__ == '__main__': 105 | main(sys.argv) 106 | -------------------------------------------------------------------------------- /orca_bringup/scripts/mission_runner.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | # MIT License 4 | # 5 | # Copyright (c) 2022 Clyde McQueen 6 | # 7 | # Permission is hereby granted, free of charge, to any person obtaining a copy 8 | # of this software and associated documentation files (the "Software"), to deal 9 | # in the Software without restriction, including without limitation the rights 10 | # to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 11 | # copies of the Software, and to permit persons to whom the Software is 12 | # furnished to do so, subject to the following conditions: 13 | # 14 | # The above copyright notice and this permission notice shall be included in all 15 | # copies or substantial portions of the Software. 16 | # 17 | # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 18 | # IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 19 | # FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 20 | # AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 21 | # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 22 | # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 23 | # SOFTWARE. 24 | 25 | """ 26 | Run the "huge loop" mission 27 | 28 | Code inspired by https://github.com/ros2/ros2cli/blob/rolling/ros2action/ros2action/verb/send_goal.py 29 | 30 | Usage: 31 | -- ros2 run orca_bringup mission_runner.py 32 | """ 33 | 34 | from enum import Enum 35 | 36 | import rclpy 37 | import rclpy.logging 38 | from action_msgs.msg import GoalStatus 39 | from geometry_msgs.msg import Point, Pose, PoseStamped 40 | from nav2_msgs.action import FollowWaypoints 41 | from orca_msgs.action import TargetMode 42 | from rclpy.action import ActionClient 43 | from std_msgs.msg import Header 44 | 45 | 46 | class SendGoalResult(Enum): 47 | SUCCESS = 0 # Goal succeeded 48 | FAILURE = 1 # Goal failed 49 | CANCELED = 2 # Goal canceled (KeyboardInterrupt exception) 50 | 51 | 52 | def make_pose(x: float, y: float, z: float): 53 | return PoseStamped(header=Header(frame_id='map'), pose=Pose(position=Point(x=x, y=y, z=z))) 54 | 55 | 56 | # Go to AUV mode 57 | go_auv = TargetMode.Goal() 58 | go_auv.target_mode = TargetMode.Goal.ORCA_MODE_AUV 59 | 60 | # Go to ROV mode 61 | go_rov = TargetMode.Goal() 62 | go_rov.target_mode = TargetMode.Goal.ORCA_MODE_ROV 63 | 64 | # Go home (1m deep) 65 | go_home = FollowWaypoints.Goal() 66 | go_home.poses.append(make_pose(x=0.0, y=0.0, z=-1.0)) 67 | 68 | # Dive to 8m 69 | dive = FollowWaypoints.Goal() 70 | dive.poses.append(make_pose(x=0.0, y=0.0, z=-8.0)) 71 | 72 | # Big loop, will eventually result in a loop closure 73 | delay_loop = FollowWaypoints.Goal() 74 | delay_loop.poses.append(make_pose(x=0.0, y=0.0, z=-7.0)) 75 | for _ in range(2): 76 | delay_loop.poses.append(make_pose(x=20.0, y=-13.0, z=-7.0)) 77 | delay_loop.poses.append(make_pose(x=10.0, y=-23.0, z=-7.0)) 78 | delay_loop.poses.append(make_pose(x=-10.0, y=-8.0, z=-7.0)) 79 | delay_loop.poses.append(make_pose(x=0.0, y=0.0, z=-7.0)) 80 | 81 | 82 | # Send a goal to an action server and wait for the result. 83 | # Cancel the goal if the user hits ^C (KeyboardInterrupt). 84 | def send_goal(node, action_client, send_goal_msg) -> SendGoalResult: 85 | goal_handle = None 86 | 87 | try: 88 | action_client.wait_for_server() 89 | 90 | print('Sending goal...') 91 | goal_future = action_client.send_goal_async(send_goal_msg) 92 | rclpy.spin_until_future_complete(node, goal_future) 93 | goal_handle = goal_future.result() 94 | 95 | if goal_handle is None: 96 | raise RuntimeError('Exception while sending goal: {!r}'.format(goal_future.exception())) 97 | 98 | if not goal_handle.accepted: 99 | print('Goal rejected') 100 | return SendGoalResult.FAILURE 101 | 102 | print('Goal accepted with ID: {}'.format(bytes(goal_handle.goal_id.uuid).hex())) 103 | result_future = goal_handle.get_result_async() 104 | rclpy.spin_until_future_complete(node, result_future) 105 | 106 | result = result_future.result() 107 | 108 | if result is None: 109 | raise RuntimeError('Exception while getting result: {!r}'.format(result_future.exception())) 110 | 111 | print('Goal completed') 112 | return SendGoalResult.SUCCESS 113 | 114 | except KeyboardInterrupt: 115 | # Cancel the goal if it's still active 116 | # TODO(clyde): this seems to work, but a second exception is generated -- why? 117 | if (goal_handle is not None and 118 | (GoalStatus.STATUS_ACCEPTED == goal_handle.status or 119 | GoalStatus.STATUS_EXECUTING == goal_handle.status)): 120 | print('Canceling goal...') 121 | cancel_future = goal_handle.cancel_goal_async() 122 | rclpy.spin_until_future_complete(node, cancel_future) 123 | cancel_response = cancel_future.result() 124 | 125 | if cancel_response is None: 126 | raise RuntimeError('Exception while canceling goal: {!r}'.format(cancel_future.exception())) 127 | 128 | if len(cancel_response.goals_canceling) == 0: 129 | raise RuntimeError('Failed to cancel goal') 130 | if len(cancel_response.goals_canceling) > 1: 131 | raise RuntimeError('More than one goal canceled') 132 | if cancel_response.goals_canceling[0].goal_id != goal_handle.goal_id: 133 | raise RuntimeError('Canceled goal with incorrect goal ID') 134 | 135 | print('Goal canceled') 136 | return SendGoalResult.CANCELED 137 | 138 | 139 | def main(): 140 | node = None 141 | set_target_mode = None 142 | follow_waypoints = None 143 | 144 | rclpy.init() 145 | 146 | try: 147 | node = rclpy.create_node("mission_runner") 148 | 149 | set_target_mode = ActionClient(node, TargetMode, '/set_target_mode') 150 | follow_waypoints = ActionClient(node, FollowWaypoints, '/follow_waypoints') 151 | 152 | print('>>> Setting mode to AUV <<<') 153 | if send_goal(node, set_target_mode, go_auv) == SendGoalResult.SUCCESS: 154 | print('>>> Executing mission <<<') 155 | send_goal(node, follow_waypoints, delay_loop) 156 | 157 | print('>>> Setting mode to ROV <<<') 158 | send_goal(node, set_target_mode, go_rov) 159 | 160 | print('>>> Mission complete <<<') 161 | else: 162 | print('>>> Failed to set mode to AUV, quit <<<') 163 | 164 | finally: 165 | if set_target_mode is not None: 166 | set_target_mode.destroy() 167 | if follow_waypoints is not None: 168 | follow_waypoints.destroy() 169 | if node is not None: 170 | node.destroy_node() 171 | 172 | rclpy.shutdown() 173 | 174 | 175 | if __name__ == '__main__': 176 | main() 177 | -------------------------------------------------------------------------------- /orca_bringup/scripts/rov.sh: -------------------------------------------------------------------------------- 1 | # Set orca4 to ROV mode; abort active mission 2 | ros2 action send_goal /set_target_mode orca_msgs/action/TargetMode "{target_mode: 2}" 3 | -------------------------------------------------------------------------------- /orca_description/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.12.2) 2 | project(orca_description) 3 | 4 | find_package(ament_cmake_auto REQUIRED) 5 | ament_auto_find_build_dependencies() 6 | 7 | # Load & run linters listed in package.xml 8 | if(BUILD_TESTING) 9 | find_package(ament_lint_auto REQUIRED) 10 | ament_lint_auto_find_test_dependencies() 11 | endif() 12 | 13 | ament_auto_package(INSTALL_TO_SHARE models scripts worlds) -------------------------------------------------------------------------------- /orca_description/README.md: -------------------------------------------------------------------------------- 1 | # Orca4 SDF Files 2 | 3 | Orca4 uses the worlds and models from [bluerov2_gz](https://github.com/clydemcqueen/bluerov2_gz). 4 | 5 | The [bluerov2](https://github.com/clydemcqueen/bluerov2_gz/tree/main/models/bluerov2) model 6 | was copied and modified to create the [orca4](models/orca4) model. 7 | 8 | The [underwater](https://github.com/clydemcqueen/bluerov2_gz/tree/main/worlds/underwater) world 9 | was copied and modified to create the [sand](models/orca4) world. 10 | 11 | The [generate_model.py](scripts/generate_model.py) script takes the [orca/model.sdf.in](models/orca4/model.sdf.in) 12 | file and replaces strings of the form `@foo` with calculated values to generate 13 | [orca/model.sdf](models/orca4/model.sdf). 14 | The script must be run manually when the model is changed. 15 | 16 | An URDF file is not provided. In practice this means 2 things: 17 | 1. static transforms are published by instances `tf2_ros/static_transform_publisher` 18 | 2. Rviz2 will only show transforms (Rviz2 can't read SDF) -------------------------------------------------------------------------------- /orca_description/models/orca4/model.config: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | orca4 9 | 4.0 10 | model.sdf 11 | 12 | 13 | Clyde McQueen 14 | clyde@mcqueen.net 15 | 16 | 17 | Clyde McQueen 18 | 19 | 20 | Orca model 21 | 22 | 23 | -------------------------------------------------------------------------------- /orca_description/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | orca_description 6 | 0.5.0 7 | Orca description files 8 | 9 | Clyde McQueen 10 | MIT 11 | 12 | https://github.com/clydemcqueen/orca4.git 13 | https://github.com/clydemcqueen/orca4/issues 14 | 15 | Clyde McQueen 16 | 17 | ament_cmake 18 | 19 | ament_lint_auto 20 | 21 | ament_cmake_copyright 22 | ament_cmake_lint_cmake 23 | ament_cmake_xmllint 24 | 25 | 26 | ament_cmake 27 | 28 | 29 | -------------------------------------------------------------------------------- /orca_description/scripts/generate_model.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | # MIT License 4 | # 5 | # Copyright (c) 2022 Clyde McQueen 6 | # 7 | # Permission is hereby granted, free of charge, to any person obtaining a copy 8 | # of this software and associated documentation files (the "Software"), to deal 9 | # in the Software without restriction, including without limitation the rights 10 | # to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 11 | # copies of the Software, and to permit persons to whom the Software is 12 | # furnished to do so, subject to the following conditions: 13 | # 14 | # The above copyright notice and this permission notice shall be included in all 15 | # copies or substantial portions of the Software. 16 | # 17 | # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 18 | # IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 19 | # FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 20 | # AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 21 | # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 22 | # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 23 | # SOFTWARE. 24 | 25 | """ 26 | Generate the model.sdf file by substituting strings of the form "@foo" with calculated values 27 | 28 | The SDF file uses the ArduPilotPlugin COMMAND control method; this sends commands to a specified 29 | ign-transport topic rather than directly controlling a joint. 30 | 31 | We use the COMMAND method to send commands to the Gazebo Sim ThrusterPlugin. The ThrusterPlugin 32 | supports 2 control methods: 33 | control thrust via /cmd_thrust 34 | control angular velocity via /cmd_vel 35 | 36 | The ThrusterPlugin uses the Fossen equation to relate thrust to angular velocity, and will apply 37 | thrust force to the joint and spin the propeller. Propellers have bounding boxes and inertia, so 38 | spinning the propeller does affect the simulation. 39 | """ 40 | 41 | import math 42 | import re 43 | import sys 44 | 45 | # SDF 1.9 supports degrees="true"; provide some nice vars for earlier versions 46 | d180 = math.pi 47 | d90 = d180 / 2 48 | d45 = d90 / 2 49 | d135 = d90 + d45 50 | 51 | mass = 10 52 | visual_x = 0.457 53 | visual_y = 0.338 54 | visual_z = 0.25 55 | fluid_density = 1000 56 | 57 | # The ROV should be positively buoyant 58 | buoyancy_adjustment = 0.05 59 | displaced_mass = mass + buoyancy_adjustment 60 | 61 | # The collision box is used by the BuoyancyPlugin 62 | # collision_x * collision_y * collision_z * density == displaced_mass 63 | collision_x = visual_x 64 | collision_y = visual_y 65 | collision_z = displaced_mass / (visual_x * visual_y * fluid_density) 66 | 67 | # The center of mass is just above the origin 68 | mass_z = 0.011 69 | 70 | # The center of volume is directly above the center of mass, resulting in a restoring force 71 | volume_z = 0.06 72 | 73 | ixx = mass / 12 * (collision_y * collision_y + collision_z * collision_z) 74 | iyy = mass / 12 * (collision_x * collision_x + collision_z * collision_z) 75 | izz = mass / 12 * (collision_x * collision_x + collision_y * collision_y) 76 | 77 | # 2nd order stability for the HydrodynamicsPlugin 78 | xUabsU = -0.5 * visual_y * visual_z * 0.8 * fluid_density 79 | yVabsV = -0.5 * visual_x * visual_z * 0.95 * fluid_density 80 | zWabsW = -0.5 * visual_x * visual_y * 0.95 * fluid_density 81 | kPabsP = -0.5 * 0.008 * fluid_density 82 | mQabsQ = -0.5 * 0.008 * fluid_density 83 | nRabsR = -0.5 * 0.008 * fluid_density 84 | 85 | # Thruster placement 86 | thruster_x = 0.14 87 | thruster_y = 0.092 88 | thruster_z = -0.009 89 | vert_thruster_y = 0.109 90 | vert_thruster_z = 0.077 91 | 92 | # Propeller link parameters 93 | propeller_size = "0.1 0.02 0.01" 94 | propeller_mass = 0.002 95 | propeller_ixx = 0.001 96 | propeller_iyy = 0.001 97 | propeller_izz = 0.001 98 | 99 | # ThrusterPlugin parameters 100 | propeller_diameter = 0.1 101 | thrust_coefficient = 0.02 102 | 103 | # Max thrust force, N 104 | # Both forward and reverse thrust must be the same 105 | max_thrust = 50 106 | 107 | # ArduPilotPlugin control parameters 108 | servo_min = 1100 109 | servo_max = 1900 110 | control_offset = -0.5 111 | 112 | # From the command line 113 | use_angvel_cmd = False 114 | 115 | # Set by update_globals() 116 | cw_control_multiplier = 0 # Thrusters 3, 4 and 6 117 | ccw_control_multiplier = 0 # Thrusters 1, 2 and 5 118 | thruster1_topic = "/model/orca4/joint/thruster1_joint/cmd_" 119 | thruster2_topic = "/model/orca4/joint/thruster2_joint/cmd_" 120 | thruster3_topic = "/model/orca4/joint/thruster3_joint/cmd_" 121 | thruster4_topic = "/model/orca4/joint/thruster4_joint/cmd_" 122 | thruster5_topic = "/model/orca4/joint/thruster5_joint/cmd_" 123 | thruster6_topic = "/model/orca4/joint/thruster6_joint/cmd_" 124 | 125 | # Stereo camera 126 | # TODO(clyde) adjust mass, add collision (buoyancy) volume, etc. 127 | stereo_baseline = 0.36 # Use same baseline as Orca3 for now 128 | camera_radius = 0.0275 129 | camera_height = 0.135 130 | camera_x = -0.18 131 | camera_y = stereo_baseline / 2 132 | camera_z = (camera_height - visual_z) / 2 - 0.01 133 | camera_sensor_z = -camera_height / 2 - 0.02 134 | camera_mass = 0.001 135 | camera_ixx = 0.001 136 | camera_iyy = 0.001 137 | camera_izz = 0.001 138 | camera_far_clip = 4 # Furthest distance the camera can "see" 139 | 140 | # Fossen equation, see "Guidance and Control of Ocean Vehicles" p. 246 141 | def thrust_to_ang_vel(thrust): 142 | assert thrust >= 0 143 | assert thrust_coefficient >= 0 144 | return math.sqrt(thrust / (fluid_density * thrust_coefficient * pow(propeller_diameter, 4))) 145 | 146 | 147 | def update_globals(): 148 | global cw_control_multiplier 149 | global ccw_control_multiplier 150 | global thruster1_topic 151 | global thruster2_topic 152 | global thruster3_topic 153 | global thruster4_topic 154 | global thruster5_topic 155 | global thruster6_topic 156 | 157 | if use_angvel_cmd: 158 | print("control method: angular velocity") 159 | thruster1_topic += "vel" 160 | thruster2_topic += "vel" 161 | thruster3_topic += "vel" 162 | thruster4_topic += "vel" 163 | thruster5_topic += "vel" 164 | thruster6_topic += "vel" 165 | 166 | # Angular velocity range in rad/s 167 | # Thrust ~ sqrt(angular velocity), so the curves are quite different 168 | # Reverse the angular velocity for thrusters 3, 4 and 6 169 | cw_control_multiplier = -thrust_to_ang_vel(max_thrust) * 2 170 | ccw_control_multiplier = thrust_to_ang_vel(max_thrust) * 2 171 | else: 172 | print("control method: thrust force") 173 | thruster1_topic += "thrust" 174 | thruster2_topic += "thrust" 175 | thruster3_topic += "thrust" 176 | thruster4_topic += "thrust" 177 | thruster5_topic += "thrust" 178 | thruster6_topic += "thrust" 179 | 180 | # Force range [-50, 50] in N 181 | cw_control_multiplier = max_thrust * 2 182 | ccw_control_multiplier = max_thrust * 2 183 | 184 | 185 | def generate_model(input_path, output_path): 186 | s = open(input_path, "r").read() 187 | pattern = re.compile(r"@(\w+)") 188 | # globals()['foo'] will return the value of foo 189 | # TODO(clyde) trim floats 190 | s = re.sub(pattern, lambda m: str(globals()[m.group(1)]), s) 191 | open(output_path, "w").write(s) 192 | 193 | 194 | if __name__ == "__main__": 195 | if len(sys.argv) != 4: 196 | print("Usage:") 197 | print("generate_model.py infile outfile 0|1") 198 | print("0: control thrust force") 199 | print("1: control angular velocity") 200 | exit(-100) 201 | 202 | use_angvel_cmd = bool(int(sys.argv[3])) 203 | 204 | update_globals() 205 | 206 | generate_model(sys.argv[1], sys.argv[2]) 207 | -------------------------------------------------------------------------------- /orca_description/worlds/sand.world: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 9 | 10 | 13 | 14 | 17 | 18 | 21 | 22 | 23 | 24 | 25 | 28 | 29 | 30 | 31 | 34 | 35 | 36 | 1000 37 | 38 | 0 39 | 1 40 | 41 | 42 | 43 | orca4 44 | 45 | 46 | 47 | https://fuel.gazebosim.org/1.0/OpenRobotics/models/Sun 48 | 49 | 50 | 51 | model://sand_heightmap 52 | 0 7 -10 0 0 0 53 | 54 | 55 | 56 | model://orca4 57 | 0 0 -0.2 0 0 0 58 | 59 | 60 | 61 | model://axes 62 | 0 0 0.2 0 0 0 63 | 64 | 65 | 66 | 67 | -------------------------------------------------------------------------------- /orca_msgs/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.12.2) 2 | project(orca_msgs) 3 | 4 | if(NOT CMAKE_CXX_STANDARD) 5 | set(CMAKE_CXX_STANDARD 17) 6 | endif() 7 | 8 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 9 | add_compile_options(-Wall -Wextra -Wpedantic) 10 | endif() 11 | 12 | # Debugging: set _dump_all_variables to true 13 | set(_dump_all_variables false) 14 | if(_dump_all_variables) 15 | get_cmake_property(_variable_names VARIABLES) 16 | list(SORT _variable_names) 17 | foreach(_variable_name ${_variable_names}) 18 | message(STATUS "${_variable_name}=${${_variable_name}}") 19 | endforeach() 20 | endif() 21 | 22 | find_package(ament_cmake_auto REQUIRED) 23 | ament_auto_find_build_dependencies() 24 | 25 | # Generate ROS messages 26 | rosidl_generate_interfaces( 27 | orca_msgs 28 | action/TargetMode.action 29 | msg/Effort.msg 30 | msg/Motion.msg 31 | DEPENDENCIES geometry_msgs std_msgs 32 | ) 33 | 34 | ament_auto_package() -------------------------------------------------------------------------------- /orca_msgs/README.md: -------------------------------------------------------------------------------- 1 | # Orca4 Messages and Actions 2 | 3 | ## Diagnostic Messages 4 | 5 | | Message | Description | 6 | |-----|----------------------------------------------------------------------| 7 | | Effort | Force and torque divided by maximum force and torque (range \[-1, 1\]) | 8 | | Motion | Output of motion model | 9 | 10 | ## Actions 11 | 12 | | Action | Description | 13 | |------------|-----------------------------------------| 14 | | TargetMode | Transition to DISARMED, ROV or AUV mode | 15 | -------------------------------------------------------------------------------- /orca_msgs/action/TargetMode.action: -------------------------------------------------------------------------------- 1 | # Goal 2 | uint8 ORCA_MODE_DISARMED = 1 3 | uint8 ORCA_MODE_ROV = 2 4 | uint8 ORCA_MODE_AUV = 3 5 | 6 | uint8 target_mode 7 | --- 8 | # No result 9 | --- 10 | # No feedback -------------------------------------------------------------------------------- /orca_msgs/msg/Effort.msg: -------------------------------------------------------------------------------- 1 | geometry_msgs/Vector3 force 2 | geometry_msgs/Vector3 torque -------------------------------------------------------------------------------- /orca_msgs/msg/Motion.msg: -------------------------------------------------------------------------------- 1 | std_msgs/Header header 2 | 3 | # Inputs 4 | geometry_msgs/Twist cmd_vel 5 | float64 dt 6 | 7 | # Outputs 8 | # Pose is in the world frame; acceleration, velocity, force and effort are in the robot frame 9 | 10 | geometry_msgs/Accel accel_model # Accel to move from v0 to v1 11 | geometry_msgs/Accel accel_drag # Accel to counteract drag 12 | geometry_msgs/Accel accel_total 13 | 14 | geometry_msgs/Twist vel 15 | 16 | geometry_msgs/Pose pose 17 | 18 | geometry_msgs/Wrench force 19 | orca_msgs/Effort effort # Force normalized to [-1, 1] 20 | -------------------------------------------------------------------------------- /orca_msgs/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | orca_msgs 6 | 0.5.0 7 | Orca messages 8 | 9 | Clyde McQueen 10 | MIT 11 | 12 | https://github.com/clydemcqueen/orca4.git 13 | https://github.com/clydemcqueen/orca4/issues 14 | 15 | Clyde McQueen 16 | 17 | ament_cmake 18 | 19 | rosidl_default_generators 20 | 21 | rosidl_default_runtime 22 | 23 | rosidl_interface_packages 24 | 25 | geometry_msgs 26 | std_msgs 27 | 28 | 29 | ament_cmake 30 | 31 | 32 | 33 | -------------------------------------------------------------------------------- /orca_nav2/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.12.2) 2 | project(orca_nav2) 3 | 4 | if(NOT CMAKE_CXX_STANDARD) 5 | set(CMAKE_CXX_STANDARD 17) 6 | endif() 7 | 8 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 9 | add_compile_options(-Wall -Wextra -Wpedantic) 10 | endif() 11 | 12 | find_package(ament_cmake_auto REQUIRED) 13 | ament_auto_find_build_dependencies() 14 | 15 | ament_auto_add_library(pure_pursuit_controller_3d SHARED src/pure_pursuit_controller_3d.cpp) 16 | target_include_directories(pure_pursuit_controller_3d PRIVATE include) 17 | target_compile_definitions(pure_pursuit_controller_3d PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS") 18 | pluginlib_export_plugin_description_file(nav2_core pure_pursuit_controller_3d_plugin.xml) 19 | 20 | ament_auto_add_library(straight_line_planner_3d SHARED src/straight_line_planner_3d.cpp) 21 | target_include_directories(straight_line_planner_3d PRIVATE include) 22 | target_compile_definitions(straight_line_planner_3d PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS") 23 | pluginlib_export_plugin_description_file(nav2_core straight_line_planner_3d_plugin.xml) 24 | 25 | ament_auto_add_library(goal_checker_3d SHARED src/goal_checker_3d.cpp) 26 | target_include_directories(goal_checker_3d PRIVATE include) 27 | target_compile_definitions(goal_checker_3d PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS") 28 | pluginlib_export_plugin_description_file(nav2_core goal_checker_3d_plugin.xml) 29 | 30 | ament_auto_add_library(progress_checker_3d SHARED src/progress_checker_3d.cpp) 31 | target_include_directories(progress_checker_3d PRIVATE include) 32 | target_compile_definitions(progress_checker_3d PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS") 33 | pluginlib_export_plugin_description_file(nav2_core progress_checker_3d_plugin.xml) 34 | 35 | # Load & run linters listed in package.xml 36 | if(BUILD_TESTING) 37 | find_package(ament_lint_auto REQUIRED) 38 | ament_lint_auto_find_test_dependencies() 39 | endif() 40 | 41 | install( 42 | FILES 43 | pure_pursuit_controller_3d_plugin.xml 44 | straight_line_planner_3d_plugin.xml 45 | goal_checker_3d_plugin.xml 46 | progress_checker_3d_plugin.xml 47 | DESTINATION share/${PROJECT_NAME} 48 | ) 49 | 50 | ament_auto_package() 51 | -------------------------------------------------------------------------------- /orca_nav2/README.md: -------------------------------------------------------------------------------- 1 | # Orca4 Nav2 Plugins 2 | 3 | The following Nav2 plugins provide basic 3D planning and control suitable for open water. They all 4 | ignore the global and local costmaps. They all largely ignore yaw, though the 5 | `PurePursuitController3D` will generate a value for yaw (`angular.z`) in `cmd_vel`. 6 | 7 | > Note for Humble: Nav2 now includes a [velocity smoother](https://navigation.ros.org/configuration/packages/configuring-velocity-smoother.html) 8 | node which takes cmd_vel output from the Nav2 controller(s) and does velocity, acceleration, and deadband smoothing. 9 | This node isn't 3D-aware, so it isn't used in Orca4. 10 | 11 | ## StraightLinePlanner3D 12 | 13 | The `StraightLinePlanner3D` plugin is similar to the 14 | [`StraightLinePlanner`](https://github.com/ros-planning/navigation2_tutorials/tree/master/nav2_straightline_planner) plugin. 15 | The planner can plan in two segments (vertical motion first, then horizontal) or one segment 16 | (vertical and horizontal motion combined). 17 | 18 | Parameters: 19 | 20 | | Parameter | Type | Default | Notes | 21 | |---------------|--------|---------|---------------------------------------| 22 | | planning_dist | double | 0.1 | How far apart the plan poses are, m | 23 | | z_before_xy | bool | false | If true plan in 2 segments: z then xy | 24 | 25 | ## PurePursuitController3D 26 | 27 | The `PurePursuitController3D` plugin is similar to the 28 | [`PurePursuitController`](https://github.com/ros-planning/navigation2_tutorials/tree/master/nav2_pure_pursuit_controller) plugin. 29 | It limits horizontal, vertical and yaw acceleration to reduce pitching and drift. 30 | The BlueROV2 frame is holonomic, but forward/backward drag is lower than the left/right drag, so the 31 | controller generates diff-drive motion (`linear.x`, `linear.z` and `angular.z`). 32 | 33 | Parameters: 34 | 35 | | Parameter | Type | Default | Notes | 36 | |---|---|---|---| 37 | | xy_vel | double | 0.4 | Desired horizontal velocity, m/s | 38 | | xy_accel | double | 0.4 | Max horizontal acceleration, m/s^2 | 39 | | z_vel | double | 0.2 | Desired vertical velocity, m/s | 40 | | z_accel | double | 0.2 | Max vertical acceleration, m/s^2 | 41 | | yaw_vel | double | 0.4 | Desired yaw velocity, r/s | 42 | | yaw_accel | double | 0.4 | Max yaw acceleration, r/s^2 | 43 | | lookahead_dist | double | 1.0 | Lookahead distance for pure pursuit algorithm, m | 44 | | transform_tolerance | double | 1.0 | How stale a transform can be and still be used, s | 45 | | goal_tolerance | double | 0.1 | Stop moving when goal is closer than goal_tolerance. Should be less than the values in GoalChecker3D | 46 | | tick_rate | double | 20 | The BT tick rate, used to calculate dt | 47 | 48 | ## ProgressChecker3D 49 | 50 | The `ProgressChecker3D` plugin is similar to the 51 | [`SimpleProgressChecker`](https://github.com/ros-planning/navigation2/tree/main/nav2_controller/plugins) plugin. 52 | 53 | Parameters: 54 | 55 | | Parameter | Type | Default | Notes | 56 | |---|---|---|---| 57 | | radius | double | 0.5 | Minimum distance to move, m | 58 | | time_allowance | double | 10.0 | Time allowed to move the minimum distance, s | 59 | 60 | ## GoalChecker3D 61 | 62 | The `GoalChecker3D` plugin is similar to the 63 | [`SimpleGoalChecker`](https://github.com/ros-planning/navigation2/tree/main/nav2_controller/plugins) plugin. 64 | 65 | Parameters: 66 | 67 | | Parameter | Type | Default | Notes | 68 | |---|---|---|---| 69 | | xy_goal_tolerance | double | 0.25 | Horizontal goal tolerance | 70 | | z_goal_tolerance | double | 0.25 | Vertical goal tolerance | 71 | -------------------------------------------------------------------------------- /orca_nav2/goal_checker_3d_plugin.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 3D goal checker. 4 | 5 | 6 | 7 | -------------------------------------------------------------------------------- /orca_nav2/include/orca_nav2/param_macro.hpp: -------------------------------------------------------------------------------- 1 | // MIT License 2 | // 3 | // Copyright (c) 2022 Clyde McQueen 4 | // 5 | // Permission is hereby granted, free of charge, to any person obtaining a copy 6 | // of this software and associated documentation files (the "Software"), to deal 7 | // in the Software without restriction, including without limitation the rights 8 | // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | // copies of the Software, and to permit persons to whom the Software is 10 | // furnished to do so, subject to the following conditions: 11 | // 12 | // The above copyright notice and this permission notice shall be included in all 13 | // copies or substantial portions of the Software. 14 | // 15 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | // SOFTWARE. 22 | 23 | // Simple ROS2 parameter macro that declares, gets and logs a parameter 24 | // Inspired by https://github.com/ptrmu/ros2_shared 25 | 26 | #ifndef ORCA_NAV2__PARAM_MACRO_HPP_ 27 | #define ORCA_NAV2__PARAM_MACRO_HPP_ 28 | 29 | #include "nav2_util/node_utils.hpp" 30 | 31 | #define PARAMETER(node, prefix, param, default) \ 32 | nav2_util::declare_parameter_if_not_declared( \ 33 | node, prefix + "." + #param, rclcpp::ParameterValue(default)); \ 34 | node->get_parameter(prefix + "." + #param, param ## _); \ 35 | std::cout << prefix << "." << #param << " = " << param ## _ << std::endl; 36 | 37 | #endif // ORCA_NAV2__PARAM_MACRO_HPP_ 38 | -------------------------------------------------------------------------------- /orca_nav2/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | orca_nav2 6 | 0.5.0 7 | Orca Nav2 plugins 8 | 9 | Clyde McQueen 10 | MIT 11 | 12 | https://github.com/clydemcqueen/orca4.git 13 | https://github.com/clydemcqueen/orca4/issues 14 | 15 | Clyde McQueen 16 | 17 | ament_cmake 18 | 19 | ament_lint_auto 20 | 21 | 22 | ament_lint_common 23 | 24 | builtin_interfaces 25 | geometry_msgs 26 | nav_msgs 27 | nav2_core 28 | nav2_costmap_2d 29 | nav2_util 30 | nav2_msgs 31 | orca_shared 32 | rclcpp 33 | rclcpp_action 34 | rclcpp_lifecycle 35 | pluginlib 36 | std_msgs 37 | tf2_ros 38 | visualization_msgs 39 | 40 | 41 | ament_cmake 42 | 43 | 44 | 45 | 46 | 47 | 48 | -------------------------------------------------------------------------------- /orca_nav2/progress_checker_3d_plugin.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 3D progress checker. 4 | 5 | 6 | 7 | -------------------------------------------------------------------------------- /orca_nav2/pure_pursuit_controller_3d_plugin.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 3D pure pursuit controller with acceleration limits. 4 | 5 | 6 | 7 | -------------------------------------------------------------------------------- /orca_nav2/src/goal_checker_3d.cpp: -------------------------------------------------------------------------------- 1 | // MIT License 2 | // 3 | // Copyright (c) 2022 Clyde McQueen 4 | // 5 | // Permission is hereby granted, free of charge, to any person obtaining a copy 6 | // of this software and associated documentation files (the "Software"), to deal 7 | // in the Software without restriction, including without limitation the rights 8 | // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | // copies of the Software, and to permit persons to whom the Software is 10 | // furnished to do so, subject to the following conditions: 11 | // 12 | // The above copyright notice and this permission notice shall be included in all 13 | // copies or substantial portions of the Software. 14 | // 15 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | // SOFTWARE. 22 | 23 | #include 24 | #include 25 | 26 | #include "angles/angles.h" 27 | #include "nav2_core/goal_checker.hpp" 28 | #include "orca_nav2/param_macro.hpp" 29 | 30 | /*** 31 | * Waiting for PurePursuitController3D to finish decelerating makes the interaction between 32 | * progress_checker & goal_checker somewhat fragile and generally slows down navigation. 33 | */ 34 | #undef WAIT_FOR_DECEL 35 | 36 | namespace orca_nav2 37 | { 38 | 39 | class GoalChecker3D : public nav2_core::GoalChecker 40 | { 41 | double xy_goal_tolerance_{}; 42 | double z_goal_tolerance_{}; 43 | 44 | #ifdef WAIT_FOR_DECEL 45 | // Most recent cmd_vel message 46 | geometry_msgs::msg::Twist cmd_vel_; 47 | 48 | rclcpp::Subscription::SharedPtr cmd_vel_sub_; 49 | #endif 50 | 51 | public: 52 | void initialize( 53 | const rclcpp_lifecycle::LifecycleNode::WeakPtr & weak_parent, 54 | const std::string & plugin_name, 55 | const std::shared_ptr) override 56 | { 57 | auto parent = weak_parent.lock(); 58 | 59 | PARAMETER(parent, plugin_name, xy_goal_tolerance, 0.25) 60 | PARAMETER(parent, plugin_name, z_goal_tolerance, 0.25) 61 | 62 | #ifdef WAIT_FOR_DECEL 63 | cmd_vel_sub_ = parent->create_subscription( 64 | "cmd_vel", 1, 65 | [this](geometry_msgs::msg::Twist::ConstSharedPtr msg) // NOLINT 66 | { 67 | cmd_vel_ = *msg; 68 | }); 69 | #endif 70 | 71 | RCLCPP_INFO(parent->get_logger(), "GoalChecker3D configured"); 72 | } 73 | 74 | void reset() override {} 75 | 76 | bool isGoalReached( 77 | const geometry_msgs::msg::Pose & query_pose, 78 | const geometry_msgs::msg::Pose & goal_pose, 79 | const geometry_msgs::msg::Twist &) override 80 | { 81 | #ifdef WAIT_FOR_DECEL 82 | // Wait for PurePursuitController3D to finish decelerating 83 | if (cmd_vel_.linear.x > 0 || cmd_vel_.linear.z > 0 || cmd_vel_.angular.z > 0) { 84 | return false; 85 | } 86 | #endif 87 | 88 | double dx = query_pose.position.x - goal_pose.position.x; 89 | double dy = query_pose.position.y - goal_pose.position.y; 90 | double dz = query_pose.position.z - goal_pose.position.z; 91 | 92 | // Check xy position 93 | if (dx * dx + dy * dy > xy_goal_tolerance_ * xy_goal_tolerance_) { 94 | return false; 95 | } 96 | 97 | // Check z position 98 | return abs(dz) <= z_goal_tolerance_; 99 | } 100 | 101 | // Return tolerances for use by the controller (added in Galactic) 102 | bool getTolerances( 103 | geometry_msgs::msg::Pose & pose_tolerance, 104 | geometry_msgs::msg::Twist & vel_tolerance) override 105 | { 106 | double invalid_field = std::numeric_limits::lowest(); 107 | 108 | pose_tolerance.position.x = xy_goal_tolerance_; 109 | pose_tolerance.position.y = xy_goal_tolerance_; 110 | pose_tolerance.position.z = z_goal_tolerance_; 111 | 112 | pose_tolerance.orientation.w = invalid_field; 113 | pose_tolerance.orientation.x = invalid_field; 114 | pose_tolerance.orientation.y = invalid_field; 115 | pose_tolerance.orientation.z = invalid_field; 116 | 117 | vel_tolerance.linear.x = invalid_field; 118 | vel_tolerance.linear.y = invalid_field; 119 | vel_tolerance.linear.z = invalid_field; 120 | 121 | vel_tolerance.angular.x = invalid_field; 122 | vel_tolerance.angular.y = invalid_field; 123 | vel_tolerance.angular.z = invalid_field; 124 | 125 | return true; 126 | } 127 | }; 128 | 129 | } // namespace orca_nav2 130 | 131 | #include "pluginlib/class_list_macros.hpp" 132 | 133 | PLUGINLIB_EXPORT_CLASS(orca_nav2::GoalChecker3D, nav2_core::GoalChecker) 134 | -------------------------------------------------------------------------------- /orca_nav2/src/progress_checker_3d.cpp: -------------------------------------------------------------------------------- 1 | // MIT License 2 | // 3 | // Copyright (c) 2022 Clyde McQueen 4 | // 5 | // Permission is hereby granted, free of charge, to any person obtaining a copy 6 | // of this software and associated documentation files (the "Software"), to deal 7 | // in the Software without restriction, including without limitation the rights 8 | // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | // copies of the Software, and to permit persons to whom the Software is 10 | // furnished to do so, subject to the following conditions: 11 | // 12 | // The above copyright notice and this permission notice shall be included in all 13 | // copies or substantial portions of the Software. 14 | // 15 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | // SOFTWARE. 22 | 23 | #include 24 | 25 | #include "nav2_core/progress_checker.hpp" 26 | #include "orca_nav2/param_macro.hpp" 27 | #include "orca_shared/util.hpp" 28 | 29 | namespace orca_nav2 30 | { 31 | 32 | class ProgressChecker3D : public nav2_core::ProgressChecker 33 | { 34 | rclcpp::Clock::SharedPtr clock_; 35 | 36 | // Progress is defined as moving by more than radius_ w/in the time_allowance_ 37 | double radius_{}; 38 | double time_allowance_{}; 39 | rclcpp::Duration time_allowance_d_{0, 0}; 40 | 41 | geometry_msgs::msg::Pose baseline_; 42 | rclcpp::Time baseline_time_; 43 | bool baseline_set_{false}; 44 | 45 | void set_baseline(const geometry_msgs::msg::Pose & pose) 46 | { 47 | baseline_ = pose; 48 | baseline_time_ = clock_->now(); 49 | baseline_set_ = true; 50 | } 51 | 52 | public: 53 | void initialize( 54 | const rclcpp_lifecycle::LifecycleNode::WeakPtr & weak_parent, 55 | const std::string & plugin_name) override 56 | { 57 | auto parent = weak_parent.lock(); 58 | 59 | clock_ = parent->get_clock(); 60 | 61 | PARAMETER(parent, plugin_name, radius, 0.5) 62 | PARAMETER(parent, plugin_name, time_allowance, 10.0) 63 | 64 | time_allowance_d_ = rclcpp::Duration::from_seconds(time_allowance_); 65 | 66 | RCLCPP_INFO(parent->get_logger(), "ProgressChecker3D configured"); 67 | } 68 | 69 | bool check(geometry_msgs::msg::PoseStamped & pose) override 70 | { 71 | if (!baseline_set_ || orca::dist(pose.pose.position, baseline_.position) > radius_) { 72 | set_baseline(pose.pose); 73 | return true; 74 | } 75 | 76 | return (clock_->now() - baseline_time_) < time_allowance_d_; 77 | } 78 | 79 | void reset() override 80 | { 81 | baseline_set_ = false; 82 | } 83 | }; 84 | 85 | } // namespace orca_nav2 86 | 87 | #include "pluginlib/class_list_macros.hpp" 88 | 89 | PLUGINLIB_EXPORT_CLASS(orca_nav2::ProgressChecker3D, nav2_core::ProgressChecker) 90 | -------------------------------------------------------------------------------- /orca_nav2/src/pure_pursuit_controller_3d.cpp: -------------------------------------------------------------------------------- 1 | // MIT License 2 | // 3 | // Copyright (c) 2022 Clyde McQueen 4 | // 5 | // Permission is hereby granted, free of charge, to any person obtaining a copy 6 | // of this software and associated documentation files (the "Software"), to deal 7 | // in the Software without restriction, including without limitation the rights 8 | // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | // copies of the Software, and to permit persons to whom the Software is 10 | // furnished to do so, subject to the following conditions: 11 | // 12 | // The above copyright notice and this permission notice shall be included in all 13 | // copies or substantial portions of the Software. 14 | // 15 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | // SOFTWARE. 22 | 23 | // Inspired by 24 | // https://navigation.ros.org/plugin_tutorials/docs/writing_new_nav2controller_plugin.html 25 | 26 | #include 27 | #include 28 | #include 29 | #include 30 | #include 31 | 32 | #include "orca_nav2/param_macro.hpp" 33 | #include "orca_shared/util.hpp" 34 | #include "nav2_core/controller.hpp" 35 | #include "nav2_core/exceptions.hpp" 36 | #include "rclcpp/rclcpp.hpp" 37 | #include "pluginlib/class_loader.hpp" 38 | 39 | namespace orca_nav2 40 | { 41 | 42 | constexpr bool sign(const double & v) {return v > 0;} 43 | 44 | class Limiter 45 | { 46 | double max_a_{}; 47 | double max_dv_{}; 48 | 49 | public: 50 | Limiter() = default; 51 | 52 | Limiter(const double & max_a, const double & dt) 53 | : max_a_{max_a}, max_dv_{max_a * dt} 54 | { 55 | assert(max_a > 0); 56 | assert(dt > 0); 57 | } 58 | 59 | // Ramp down velocity as the sub approaches the goal 60 | // Particularly important for linear.x, as momentum will carry the sub a good distance 61 | // Less important for linear.z, as drag is higher and buoyancy tends to dominate 62 | void decelerate(double & v, const double & goal_dist) const 63 | { 64 | assert(sign(v) == sign(goal_dist)); 65 | auto decel_v = std::sqrt(2 * std::abs(goal_dist) * max_a_); 66 | auto result_v = std::min(std::abs(v), decel_v); 67 | v = (sign(v) ? result_v : -result_v); 68 | } 69 | 70 | // Limit acceleration 71 | void limit(double & v, const double & prev_v) const 72 | { 73 | auto dv = v - prev_v; 74 | if (dv > max_dv_) { 75 | v = prev_v + max_dv_; 76 | } else if (dv < -max_dv_) { 77 | v = prev_v - max_dv_; 78 | } 79 | } 80 | }; 81 | 82 | class PurePursuitController3D : public nav2_core::Controller 83 | { 84 | rclcpp::Logger logger_{rclcpp::get_logger("placeholder_will_be_set_in_configure")}; 85 | std::shared_ptr tf_; 86 | std::string base_frame_id_; 87 | 88 | // Parameters 89 | double x_vel_{}; 90 | double x_accel_{}; 91 | double z_vel_{}; 92 | double z_accel_{}; 93 | double yaw_vel_{}; 94 | double yaw_accel_{}; 95 | double lookahead_dist_{}; 96 | double transform_tolerance_{}; 97 | double goal_tolerance_{}; // Stop motion when we're very close to the goal 98 | double tick_rate_{}; // Tick rate, used to compute dt 99 | 100 | Limiter x_limiter_; 101 | Limiter z_limiter_; 102 | Limiter yaw_limiter_; 103 | 104 | rclcpp::Duration transform_tolerance_d_{0, 0}; 105 | 106 | // Plan from StraightLinePlanner3D 107 | nav_msgs::msg::Path plan_; 108 | 109 | // Keep track of the previous cmd_vel to limit acceleration 110 | geometry_msgs::msg::Twist prev_vel_{}; 111 | 112 | // Updated 12-Apr-22: the Orca motion model might be wrong for various 113 | // reasons, so actual accel/vel might be higher or lower than desired 114 | // accel/vel. Error factors are introduced so that actual yaw vel might be 115 | // somewhere in the range: 116 | // [(1 - yaw_error_) * yaw_vel_, (1 + yaw_error_) * yaw_vel_] 117 | 118 | double x_error_{}; 119 | double yaw_error_{}; 120 | 121 | static constexpr double lower(double v, double e) {return (1.0 - e) * v;} 122 | static constexpr double upper(double v, double e) {return (1.0 + e) * v;} 123 | 124 | constexpr double x_vel_upper() const {return upper(x_vel_, x_error_);} 125 | 126 | constexpr double yaw_vel_lower() const {return lower(yaw_vel_, yaw_error_);} 127 | 128 | // Return the first pose in the plan > lookahead distance away, or the last pose in the plan 129 | geometry_msgs::msg::PoseStamped 130 | find_goal(const geometry_msgs::msg::PoseStamped & pose_f_map) const 131 | { 132 | // Walk the plan calculating distance. The plan may be stale, so distances may be 133 | // decreasing for a while. When they start to increase we've found the closest pose. Then look 134 | // for the first pose > lookahead_dist_. Return the last pose if we run out of poses. 135 | auto min_dist = std::numeric_limits::max(); 136 | bool dist_decreasing = true; 137 | 138 | for (const auto & item : plan_.poses) { 139 | auto item_dist = orca::dist( 140 | item.pose.position.x - pose_f_map.pose.position.x, 141 | item.pose.position.y - pose_f_map.pose.position.y, 142 | item.pose.position.z - pose_f_map.pose.position.z); 143 | 144 | if (dist_decreasing) { 145 | if (item_dist < min_dist) { 146 | min_dist = item_dist; 147 | } else { 148 | dist_decreasing = false; 149 | } 150 | } 151 | 152 | if (!dist_decreasing) { 153 | if (item_dist > lookahead_dist_) { 154 | return item; 155 | } 156 | } 157 | } 158 | 159 | return plan_.poses[plan_.poses.size() - 1]; 160 | } 161 | 162 | // Modified pure pursuit path tracking algorithm: works in 3D and supports deceleration 163 | // Reference "Implementation of the Pure Pursuit Path Tracking Algorithm" by R. Craig Coulter 164 | geometry_msgs::msg::Twist 165 | pure_pursuit_3d(const geometry_msgs::msg::PoseStamped & pose_f_odom) const 166 | { 167 | // Transform pose odom -> map 168 | geometry_msgs::msg::PoseStamped pose_f_map; 169 | if (!orca::transform_with_tolerance( 170 | logger_, tf_, plan_.header.frame_id, 171 | pose_f_odom, pose_f_map, 172 | transform_tolerance_d_)) 173 | { 174 | return geometry_msgs::msg::Twist{}; 175 | } 176 | 177 | // Find goal 178 | auto goal_f_map = find_goal(pose_f_map); 179 | 180 | // Plan poses are stale, update the timestamp to get recent map -> odom -> base transforms 181 | goal_f_map.header.stamp = pose_f_map.header.stamp; 182 | 183 | // Transform goal map -> base 184 | geometry_msgs::msg::PoseStamped goal_f_base; 185 | if (!orca::transform_with_tolerance( 186 | logger_, tf_, base_frame_id_, 187 | goal_f_map, goal_f_base, 188 | transform_tolerance_d_)) 189 | { 190 | return geometry_msgs::msg::Twist{}; 191 | } 192 | 193 | auto xy_dist_sq = orca::dist_sq(goal_f_base.pose.position.x, goal_f_base.pose.position.y); 194 | auto xy_dist = std::sqrt(xy_dist_sq); 195 | auto z_dist = std::abs(goal_f_base.pose.position.z); 196 | 197 | #if 0 198 | // Useful for debugging, but happens frequently as the sub decelerates 199 | if (z_dist < goal_tolerance_ && xy_dist < goal_tolerance_) { 200 | std::cout << "Decelerating / coasting" << std::endl; 201 | std::cout << "pose_f_odom: " << orca::to_str(pose_f_odom) << std::endl; 202 | std::cout << "pose_f_map: " << orca::to_str(pose_f_map) << std::endl; 203 | std::cout << "goal_f_map: " << orca::to_str(goal_f_map) << std::endl; 204 | std::cout << "goal_f_base: " << orca::to_str(goal_f_base) << std::endl; 205 | std::cout << "prev_vel: " << orca::to_str(prev_vel_) << std::endl; 206 | } 207 | #endif 208 | 209 | geometry_msgs::msg::Twist cmd_vel; 210 | 211 | // Calc linear.z 212 | if (z_dist > goal_tolerance_) { 213 | cmd_vel.linear.z = goal_f_base.pose.position.z > 0 ? z_vel_ : -z_vel_; 214 | 215 | // Decelerate 216 | z_limiter_.decelerate(cmd_vel.linear.z, goal_f_base.pose.position.z); 217 | } 218 | 219 | // Calc linear.x and angular.z using pure pursuit algorithm 220 | if (xy_dist > goal_tolerance_) { 221 | if (goal_f_base.pose.position.x > 0) { 222 | // Goal is ahead of the sub: move forward along the shortest curve 223 | auto curvature = 2.0 * goal_f_base.pose.position.y / xy_dist_sq; 224 | if (std::abs(curvature) * x_vel_upper() <= yaw_vel_lower()) { 225 | // Move at constant velocity 226 | cmd_vel.linear.x = x_vel_; 227 | cmd_vel.angular.z = curvature * x_vel_; 228 | } else { 229 | // Tight curve... don't exceed angular velocity limit 230 | cmd_vel.linear.x = yaw_vel_ / std::abs(curvature); 231 | cmd_vel.angular.z = curvature > 0 ? yaw_vel_ : -yaw_vel_; 232 | } 233 | 234 | // Decelerate 235 | x_limiter_.decelerate(cmd_vel.linear.x, xy_dist); 236 | } else { 237 | // Goal is behind the sub: rotate to face it 238 | cmd_vel.angular.z = goal_f_base.pose.position.y > 0 ? yaw_vel_ : -yaw_vel_; 239 | } 240 | } 241 | 242 | return cmd_vel; 243 | } 244 | 245 | public: 246 | PurePursuitController3D() = default; 247 | ~PurePursuitController3D() override = default; 248 | 249 | void configure( 250 | const rclcpp_lifecycle::LifecycleNode::WeakPtr & weak_parent, 251 | std::string name, 252 | const std::shared_ptr tf, 253 | const std::shared_ptr costmap_ros) override 254 | { 255 | auto parent = weak_parent.lock(); 256 | 257 | logger_ = parent->get_logger(); 258 | tf_ = tf; 259 | base_frame_id_ = costmap_ros->getBaseFrameID(); 260 | 261 | PARAMETER(parent, name, x_vel, 0.4) 262 | PARAMETER(parent, name, x_accel, 0.4) 263 | PARAMETER(parent, name, z_vel, 0.2) 264 | PARAMETER(parent, name, z_accel, 0.2) 265 | PARAMETER(parent, name, yaw_vel, 0.4) 266 | PARAMETER(parent, name, yaw_accel, 0.4) 267 | PARAMETER(parent, name, lookahead_dist, 1.0) 268 | PARAMETER(parent, name, transform_tolerance, 1.0) 269 | PARAMETER(parent, name, goal_tolerance, 0.1) 270 | PARAMETER(parent, name, tick_rate, 20.0) 271 | PARAMETER(parent, name, x_error, 0.0) 272 | PARAMETER(parent, name, yaw_error, 0.0) 273 | 274 | x_limiter_ = Limiter(x_accel_, 1. / tick_rate_); 275 | z_limiter_ = Limiter(z_accel_, 1. / tick_rate_); 276 | yaw_limiter_ = Limiter(yaw_accel_, 1. / tick_rate_); 277 | 278 | transform_tolerance_d_ = rclcpp::Duration::from_seconds(transform_tolerance_); 279 | 280 | RCLCPP_INFO(logger_, "PurePursuitController3D configured"); 281 | } 282 | 283 | void cleanup() override 284 | { 285 | // std::cout << "cleanup" << std::endl; 286 | } 287 | 288 | void activate() override 289 | { 290 | // std::cout << "activate" << std::endl; 291 | } 292 | 293 | void deactivate() override 294 | { 295 | // std::cout << "deactivate" << std::endl; 296 | } 297 | 298 | // Pose is base_f_odom, it's 3D, and it comes from /tf via: 299 | // nav2_controller::ControllerServer::getRobotPose 300 | // Using the local_costmap (odom frame), not the global_costmap (map frame) 301 | // nav2_costmap_2d::Costmap2DROS::getRobotPose 302 | // nav2_util::getCurrentPose 303 | // 304 | // Twist comes from /odom, but it's stripped to 2D in nav2_controller::ControllerServer, so 305 | // ignore it. 306 | geometry_msgs::msg::TwistStamped computeVelocityCommands( 307 | const geometry_msgs::msg::PoseStamped & pose, 308 | const geometry_msgs::msg::Twist &, 309 | nav2_core::GoalChecker *) override 310 | { 311 | geometry_msgs::msg::TwistStamped cmd_vel; 312 | cmd_vel.header = pose.header; 313 | 314 | // Track the plan 315 | cmd_vel.twist = pure_pursuit_3d(pose); 316 | 317 | // Limit acceleration 318 | x_limiter_.limit(cmd_vel.twist.linear.x, prev_vel_.linear.x); 319 | z_limiter_.limit(cmd_vel.twist.linear.z, prev_vel_.linear.z); 320 | yaw_limiter_.limit(cmd_vel.twist.angular.z, prev_vel_.angular.z); 321 | 322 | // Twist parameter from nav2_controller is generated from a Twist2D, so linear.z is always 0 323 | // Keep a copy of the previous cmd_vel instead 324 | prev_vel_ = cmd_vel.twist; 325 | 326 | #if 0 327 | // I'm getting jumps in velocity from max_v to 0 to max_v, but it is not coming from this 328 | // routine. Somewhere upstream there appears to be a "cmd_vel = {}". 329 | 330 | if (cmd_vel.twist.angular.z < 0.01 && cmd_vel.twist.angular.z > -0.01) { 331 | std::cout << "sending 0 yaw vel" << std::endl; 332 | } 333 | #endif 334 | 335 | return cmd_vel; 336 | } 337 | 338 | void setPlan(const nav_msgs::msg::Path & plan) override 339 | { 340 | if (plan.poses.empty()) { 341 | throw nav2_core::PlannerException("Received plan with zero length"); 342 | } 343 | plan_ = plan; 344 | } 345 | 346 | void setSpeedLimit(const double &, const bool &) override 347 | { 348 | // Not supported 349 | std::cout << "ERROR: speed limit is not supported" << std::endl; 350 | } 351 | }; 352 | 353 | } // namespace orca_nav2 354 | 355 | #include "pluginlib/class_list_macros.hpp" 356 | 357 | // Register this controller as a nav2_core plugin 358 | PLUGINLIB_EXPORT_CLASS(orca_nav2::PurePursuitController3D, nav2_core::Controller) 359 | -------------------------------------------------------------------------------- /orca_nav2/src/straight_line_planner_3d.cpp: -------------------------------------------------------------------------------- 1 | // MIT License 2 | // 3 | // Copyright (c) 2022 Clyde McQueen 4 | // 5 | // Permission is hereby granted, free of charge, to any person obtaining a copy 6 | // of this software and associated documentation files (the "Software"), to deal 7 | // in the Software without restriction, including without limitation the rights 8 | // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | // copies of the Software, and to permit persons to whom the Software is 10 | // furnished to do so, subject to the following conditions: 11 | // 12 | // The above copyright notice and this permission notice shall be included in all 13 | // copies or substantial portions of the Software. 14 | // 15 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | // SOFTWARE. 22 | 23 | // Inspired by 24 | // https://navigation.ros.org/plugin_tutorials/docs/writing_new_nav2planner_plugin.html 25 | 26 | #include 27 | #include 28 | #include 29 | 30 | #include "nav_msgs/msg/path.hpp" 31 | #include "nav2_core/global_planner.hpp" 32 | #include "nav2_costmap_2d/costmap_2d_ros.hpp" 33 | #include "nav2_util/lifecycle_node.hpp" 34 | #include "nav2_util/robot_utils.hpp" 35 | #include "orca_nav2/param_macro.hpp" 36 | #include "rclcpp/rclcpp.hpp" 37 | 38 | namespace orca_nav2 39 | { 40 | 41 | class StraightLinePlanner3D : public nav2_core::GlobalPlanner 42 | { 43 | rclcpp::Clock::SharedPtr clock_; 44 | rclcpp::Logger logger_{rclcpp::get_logger("placeholder_will_be_set_in_configure")}; 45 | std::string global_frame_id_; 46 | 47 | // Parameters 48 | double planning_dist_{}; 49 | bool z_before_xy_{}; 50 | 51 | public: 52 | StraightLinePlanner3D() = default; 53 | ~StraightLinePlanner3D() override = default; 54 | 55 | void configure( 56 | const rclcpp_lifecycle::LifecycleNode::WeakPtr & weak_parent, 57 | std::string name, 58 | std::shared_ptr, 59 | std::shared_ptr costmap_ros) override 60 | { 61 | auto parent = weak_parent.lock(); 62 | 63 | clock_ = parent->get_clock(); 64 | logger_ = parent->get_logger(); 65 | global_frame_id_ = costmap_ros->getGlobalFrameID(); // Map frame 66 | 67 | PARAMETER(parent, name, planning_dist, 0.1) 68 | PARAMETER(parent, name, z_before_xy, false) 69 | 70 | if (planning_dist_ < 0.01) { 71 | RCLCPP_WARN(logger_, "planning_dist too low, setting to default 0.1"); 72 | planning_dist_ = 0.1; 73 | } 74 | 75 | RCLCPP_INFO(logger_, "StraightLinePlanner3D configured"); 76 | } 77 | 78 | void cleanup() override {} 79 | 80 | void activate() override {} 81 | 82 | void deactivate() override {} 83 | 84 | // Move in one 3D segment 85 | void createOneSegmentPlan( 86 | const geometry_msgs::msg::PoseStamped & start, 87 | const geometry_msgs::msg::PoseStamped & goal, 88 | nav_msgs::msg::Path & global_path) 89 | { 90 | int num_poses = static_cast(std::hypot( 91 | goal.pose.position.x - start.pose.position.x, 92 | goal.pose.position.y - start.pose.position.y, 93 | goal.pose.position.z - start.pose.position.z) / 94 | planning_dist_); 95 | 96 | double x_increment = (goal.pose.position.x - start.pose.position.x) / num_poses; 97 | double y_increment = (goal.pose.position.y - start.pose.position.y) / num_poses; 98 | double z_increment = (goal.pose.position.z - start.pose.position.z) / num_poses; 99 | 100 | geometry_msgs::msg::PoseStamped pose = start; 101 | pose.header.stamp = clock_->now(); 102 | pose.header.frame_id = global_frame_id_; 103 | 104 | // Controller should ignore orientation, except for final pose 105 | pose.pose.orientation.x = 0.0; 106 | pose.pose.orientation.y = 0.0; 107 | pose.pose.orientation.z = 0.0; 108 | pose.pose.orientation.w = 1.0; 109 | 110 | for (int i = 0; i < num_poses; ++i) { 111 | pose.pose.position.x = start.pose.position.x + x_increment * i; 112 | pose.pose.position.y = start.pose.position.y + y_increment * i; 113 | pose.pose.position.z = start.pose.position.z + z_increment * i; 114 | global_path.poses.push_back(pose); 115 | } 116 | } 117 | 118 | // Move vertically, then horizontally 119 | void createTwoSegmentPlan( 120 | const geometry_msgs::msg::PoseStamped & start, 121 | const geometry_msgs::msg::PoseStamped & goal, 122 | nav_msgs::msg::Path & global_path) 123 | { 124 | int num_z_poses = static_cast(std::abs(goal.pose.position.z - start.pose.position.z) / 125 | planning_dist_); 126 | 127 | int num_xy_poses = static_cast(std::hypot( 128 | goal.pose.position.x - start.pose.position.x, 129 | goal.pose.position.y - start.pose.position.y) / 130 | planning_dist_); 131 | 132 | double x_increment = (goal.pose.position.x - start.pose.position.x) / num_xy_poses; 133 | double y_increment = (goal.pose.position.y - start.pose.position.y) / num_xy_poses; 134 | double z_increment = (goal.pose.position.z - start.pose.position.z) / num_z_poses; 135 | 136 | geometry_msgs::msg::PoseStamped pose = start; 137 | pose.header.stamp = clock_->now(); 138 | pose.header.frame_id = global_frame_id_; 139 | 140 | // Controller should ignore orientation, except for final pose 141 | pose.pose.orientation.x = 0.0; 142 | pose.pose.orientation.y = 0.0; 143 | pose.pose.orientation.z = 0.0; 144 | pose.pose.orientation.w = 1.0; 145 | 146 | // Move vertically 147 | for (int i = 0; i < num_z_poses; ++i) { 148 | pose.pose.position.z = start.pose.position.z + z_increment * i; 149 | global_path.poses.push_back(pose); 150 | } 151 | 152 | // Goal z 153 | pose.pose.position.z = goal.pose.position.z; 154 | global_path.poses.push_back(pose); 155 | 156 | // Move horizontally 157 | for (int i = 0; i < num_xy_poses; ++i) { 158 | pose.pose.position.x = start.pose.position.x + x_increment * i; 159 | pose.pose.position.y = start.pose.position.y + y_increment * i; 160 | global_path.poses.push_back(pose); 161 | } 162 | } 163 | 164 | nav_msgs::msg::Path createPlan( 165 | const geometry_msgs::msg::PoseStamped & start, 166 | const geometry_msgs::msg::PoseStamped & goal) override 167 | { 168 | nav_msgs::msg::Path global_path; 169 | 170 | if (start.header.frame_id != global_frame_id_) { 171 | RCLCPP_ERROR(logger_, "Start pose must be in the %s frame", global_frame_id_.c_str()); 172 | return global_path; 173 | } 174 | 175 | if (goal.header.frame_id != global_frame_id_) { 176 | RCLCPP_ERROR(logger_, "Goal pose must be in the %s frame", global_frame_id_.c_str()); 177 | return global_path; 178 | } 179 | 180 | global_path.header.stamp = clock_->now(); 181 | global_path.header.frame_id = global_frame_id_; 182 | 183 | if (z_before_xy_) { 184 | createTwoSegmentPlan(start, goal, global_path); 185 | } else { 186 | createOneSegmentPlan(start, goal, global_path); 187 | } 188 | 189 | // Goal x, y, z and yaw 190 | global_path.poses.push_back(goal); 191 | 192 | return global_path; 193 | } 194 | }; 195 | 196 | } // namespace orca_nav2 197 | 198 | #include "pluginlib/class_list_macros.hpp" 199 | 200 | PLUGINLIB_EXPORT_CLASS(orca_nav2::StraightLinePlanner3D, nav2_core::GlobalPlanner) 201 | -------------------------------------------------------------------------------- /orca_nav2/straight_line_planner_3d_plugin.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 3D straight line planner. 4 | 5 | 6 | -------------------------------------------------------------------------------- /orca_shared/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.12.2) 2 | project(orca_shared) 3 | 4 | if(NOT CMAKE_CXX_STANDARD) 5 | set(CMAKE_CXX_STANDARD 17) 6 | endif() 7 | 8 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 9 | add_compile_options(-Wall -Wextra -Wpedantic) 10 | endif() 11 | 12 | find_package(ament_cmake_auto REQUIRED) 13 | ament_auto_find_build_dependencies() 14 | 15 | ament_auto_add_library( 16 | orca_shared SHARED 17 | src/model.cpp 18 | src/pwm.cpp 19 | src/util.cpp 20 | ) 21 | 22 | target_include_directories(orca_shared PUBLIC include) 23 | 24 | # Load & run linters listed in package.xml 25 | if(BUILD_TESTING) 26 | find_package(ament_lint_auto REQUIRED) 27 | ament_lint_auto_find_test_dependencies() 28 | endif() 29 | 30 | ament_auto_package() -------------------------------------------------------------------------------- /orca_shared/README.md: -------------------------------------------------------------------------------- 1 | # Orca4 Shared Utilities 2 | 3 | Common or useful utilities. 4 | 5 | [model.hpp](include/model.hpp) describes the simple motion model 6 | 7 | [pwm.hpp](include/pwm.hpp) includes utilities for working with PWM outputs 8 | 9 | [util.hpp](include/util.hpp) includes utilities for working with tf2, printing ROS2 messages, etc. -------------------------------------------------------------------------------- /orca_shared/include/orca_shared/pwm.hpp: -------------------------------------------------------------------------------- 1 | // MIT License 2 | // 3 | // Copyright (c) 2022 Clyde McQueen 4 | // 5 | // Permission is hereby granted, free of charge, to any person obtaining a copy 6 | // of this software and associated documentation files (the "Software"), to deal 7 | // in the Software without restriction, including without limitation the rights 8 | // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | // copies of the Software, and to permit persons to whom the Software is 10 | // furnished to do so, subject to the following conditions: 11 | // 12 | // The above copyright notice and this permission notice shall be included in all 13 | // copies or substantial portions of the Software. 14 | // 15 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | // SOFTWARE. 22 | 23 | #ifndef ORCA_SHARED__PWM_HPP_ 24 | #define ORCA_SHARED__PWM_HPP_ 25 | 26 | #include 27 | 28 | namespace orca 29 | { 30 | //============================================================================= 31 | // Camera tilt servo 32 | //============================================================================= 33 | 34 | constexpr int TILT_MIN = -45; 35 | constexpr int TILT_MAX = 45; 36 | 37 | constexpr uint16_t PWM_TILT_45_UP = 1100; 38 | constexpr uint16_t PWM_TILT_0 = 1500; 39 | constexpr uint16_t PWM_TILT_45_DOWN = 1900; 40 | 41 | uint16_t tilt_to_pwm(int tilt); 42 | 43 | int pwm_to_tilt(uint16_t pwm); 44 | 45 | //============================================================================= 46 | // BlueRobotics Lumen subsea light 47 | //============================================================================= 48 | 49 | constexpr int BRIGHTNESS_MIN = 0; 50 | constexpr int BRIGHTNESS_MAX = 100; 51 | 52 | constexpr uint16_t PWM_LIGHTS_OFF = 1100; 53 | constexpr uint16_t PWM_LIGHTS_FULL = 1900; 54 | 55 | uint16_t brightness_to_pwm(int brightness); 56 | 57 | int pwm_to_brightness(uint16_t pwm); 58 | 59 | //============================================================================= 60 | // BlueRobotics T200 thruster + ESC 61 | //============================================================================= 62 | 63 | constexpr double THRUST_FULL_REV = -1.0; 64 | constexpr double THRUST_STOP = 0.0; 65 | constexpr double THRUST_FULL_FWD = 1.0; 66 | 67 | constexpr uint16_t PWM_THRUST_FULL_REV = 1100; 68 | constexpr uint16_t PWM_THRUST_STOP = 1500; 69 | constexpr uint16_t PWM_THRUST_FULL_FWD = 1900; 70 | 71 | uint16_t effort_to_pwm(uint16_t thrust_dz_pwm, double effort); 72 | 73 | double pwm_to_effort(uint16_t thrust_dz_pwm, uint16_t pwm); 74 | 75 | } // namespace orca 76 | 77 | #endif // ORCA_SHARED__PWM_HPP_ 78 | -------------------------------------------------------------------------------- /orca_shared/include/orca_shared/util.hpp: -------------------------------------------------------------------------------- 1 | // MIT License 2 | // 3 | // Copyright (c) 2022 Clyde McQueen 4 | // 5 | // Permission is hereby granted, free of charge, to any person obtaining a copy 6 | // of this software and associated documentation files (the "Software"), to deal 7 | // in the Software without restriction, including without limitation the rights 8 | // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | // copies of the Software, and to permit persons to whom the Software is 10 | // furnished to do so, subject to the following conditions: 11 | // 12 | // The above copyright notice and this permission notice shall be included in all 13 | // copies or substantial portions of the Software. 14 | // 15 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | // SOFTWARE. 22 | 23 | #ifndef ORCA_SHARED__UTIL_HPP_ 24 | #define ORCA_SHARED__UTIL_HPP_ 25 | 26 | #include 27 | #include 28 | #include 29 | 30 | #include "geometry_msgs/msg/accel.hpp" 31 | #include "geometry_msgs/msg/point.hpp" 32 | #include "geometry_msgs/msg/pose.hpp" 33 | #include "geometry_msgs/msg/pose_stamped.hpp" 34 | #include "geometry_msgs/msg/quaternion.hpp" 35 | #include "geometry_msgs/msg/transform.hpp" 36 | #include "geometry_msgs/msg/twist.hpp" 37 | #include "geometry_msgs/msg/vector3.hpp" 38 | #include "geometry_msgs/msg/wrench.hpp" 39 | #include "rclcpp/time.hpp" 40 | #include "sensor_msgs/msg/joy.hpp" 41 | #include "std_msgs/msg/header.hpp" 42 | #include "tf2/LinearMath/Transform.h" 43 | #include "tf2_ros/buffer.h" 44 | #include "tf2_ros/transform_listener.h" 45 | 46 | namespace orca 47 | { 48 | 49 | //===================================================================================== 50 | // C++ std stuff, e.g., std::clamp is in C++17, but Foxy is still on C++14 51 | //===================================================================================== 52 | 53 | template 54 | constexpr T clamp(const T v, const T min, const T max) 55 | { 56 | return v > max ? max : (v < min ? min : v); 57 | } 58 | 59 | template 60 | constexpr T clamp(const T v, const T minmax) 61 | { 62 | return clamp(v, -minmax, minmax); 63 | } 64 | 65 | template 66 | constexpr B scale(const A a, const A a_min, const A a_max, const B b_min, const B b_max) 67 | { 68 | return clamp( 69 | static_cast(b_min + static_cast(b_max - b_min) / (a_max - a_min) * (a - a_min)), 70 | b_min, 71 | b_max); 72 | } 73 | 74 | //===================================================================================== 75 | // Geometry 76 | //===================================================================================== 77 | 78 | double dist_sq(double x, double y); 79 | 80 | double dist(double x, double y); 81 | 82 | double dist_sq(double x, double y, double z); 83 | 84 | double dist(double x, double y, double z); 85 | 86 | double dist(const geometry_msgs::msg::Point & p1, const geometry_msgs::msg::Point & p2); 87 | 88 | void get_rpy(const geometry_msgs::msg::Quaternion & q, double & r, double & p, double & y); 89 | 90 | void set_rpy( 91 | geometry_msgs::msg::Quaternion & q, const double & r, const double & p, const double & y); 92 | 93 | double get_yaw(const geometry_msgs::msg::Quaternion & q); 94 | 95 | void set_yaw(geometry_msgs::msg::Quaternion & q, const double & yaw); 96 | 97 | geometry_msgs::msg::Twist robot_to_world_frame( 98 | const geometry_msgs::msg::Twist & vel, const double & yaw_f_world); 99 | 100 | bool is_zero(const geometry_msgs::msg::Twist & v); 101 | 102 | //===================================================================================== 103 | // Time 104 | //===================================================================================== 105 | 106 | // True if time is valid (non-zero) 107 | bool valid(const rclcpp::Time & stamp); 108 | 109 | //===================================================================================== 110 | // Common tf2 functions -- easy to find 111 | //===================================================================================== 112 | 113 | tf2::Transform pose_msg_to_transform(const geometry_msgs::msg::Pose & pose); 114 | 115 | geometry_msgs::msg::Pose transform_to_pose_msg(const tf2::Transform & transform); 116 | 117 | geometry_msgs::msg::Transform transform_to_transform_msg(const tf2::Transform & transform); 118 | 119 | geometry_msgs::msg::Transform pose_msg_to_transform_msg(const geometry_msgs::msg::Pose & pose); 120 | 121 | geometry_msgs::msg::TransformStamped pose_msg_to_transform_msg( 122 | const geometry_msgs::msg::PoseStamped & msg, 123 | const std::string & child_frame_id); 124 | 125 | tf2::Transform transform_msg_to_transform(const geometry_msgs::msg::Transform & msg); 126 | 127 | tf2::Transform transform_msg_to_transform(const geometry_msgs::msg::TransformStamped & msg); 128 | 129 | geometry_msgs::msg::PoseStamped transform_msg_to_pose_msg( 130 | const geometry_msgs::msg::TransformStamped & msg); 131 | 132 | geometry_msgs::msg::Pose invert(const geometry_msgs::msg::Pose & pose); 133 | 134 | geometry_msgs::msg::PoseStamped invert( 135 | const geometry_msgs::msg::PoseStamped & msg, 136 | const std::string & frame_id); 137 | 138 | //===================================================================================== 139 | // tf2_ros::Buffer functions 140 | //===================================================================================== 141 | 142 | bool transform_with_wait( 143 | const rclcpp::Logger & logger, 144 | const std::shared_ptr & tf, 145 | const std::string & frame, 146 | const geometry_msgs::msg::PoseStamped & in_pose, 147 | geometry_msgs::msg::PoseStamped & out_pose, 148 | int wait_ms); 149 | 150 | bool transform_with_tolerance( 151 | const rclcpp::Logger & logger, 152 | const std::shared_ptr & tf, 153 | const std::string & frame, 154 | const geometry_msgs::msg::PoseStamped & in_pose, 155 | geometry_msgs::msg::PoseStamped & out_pose, 156 | const rclcpp::Duration & tolerance); 157 | 158 | bool do_transform( 159 | const std::shared_ptr & tf, 160 | const std::string & frame, 161 | const geometry_msgs::msg::PoseStamped & in_pose, 162 | geometry_msgs::msg::PoseStamped & out_pose); 163 | 164 | //===================================================================================== 165 | // str() 166 | //===================================================================================== 167 | 168 | #define OSTR(v) std::cout << #v << ": " << orca::str(v) << std::endl; 169 | 170 | std::string str(const builtin_interfaces::msg::Time & v); 171 | 172 | std::string str(const geometry_msgs::msg::Accel & v); 173 | 174 | std::string str(const geometry_msgs::msg::Point & v); 175 | 176 | std::string str(const geometry_msgs::msg::Pose & v); 177 | 178 | std::string str(const geometry_msgs::msg::PoseStamped & v); 179 | 180 | std::string str(const geometry_msgs::msg::Quaternion & v); 181 | 182 | std::string str(const geometry_msgs::msg::Twist & v); 183 | 184 | std::string str(const geometry_msgs::msg::Vector3 & v); 185 | 186 | std::string str(const geometry_msgs::msg::Wrench & v); 187 | 188 | std::string str(const rclcpp::Time & v); 189 | 190 | std::string str(const std_msgs::msg::Header & v); 191 | 192 | std::string str(const tf2::Matrix3x3 & r); 193 | 194 | std::string str(const tf2::Transform & t); 195 | 196 | std::string str(const tf2::Vector3 & v); 197 | 198 | } // namespace orca 199 | 200 | //===================================================================================== 201 | // geometry_msgs::msg operators 202 | //===================================================================================== 203 | 204 | namespace geometry_msgs::msg 205 | { 206 | 207 | geometry_msgs::msg::Accel operator+( 208 | const geometry_msgs::msg::Accel & lhs, const geometry_msgs::msg::Accel & rhs); 209 | 210 | geometry_msgs::msg::Accel operator-( 211 | const geometry_msgs::msg::Accel & lhs, const geometry_msgs::msg::Accel & rhs); 212 | 213 | geometry_msgs::msg::Accel operator-(const geometry_msgs::msg::Accel & a); 214 | 215 | geometry_msgs::msg::Twist operator-(const geometry_msgs::msg::Twist & v); 216 | 217 | } // namespace geometry_msgs::msg 218 | 219 | #endif // ORCA_SHARED__UTIL_HPP_ 220 | -------------------------------------------------------------------------------- /orca_shared/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | orca_shared 6 | 0.5.0 7 | Orca shared headers and code 8 | 9 | Clyde McQueen 10 | MIT 11 | 12 | https://github.com/clydemcqueen/orca4.git 13 | https://github.com/clydemcqueen/orca4/issues 14 | 15 | Clyde McQueen 16 | 17 | ament_cmake 18 | 19 | ament_lint_auto 20 | 21 | ament_cmake_copyright 22 | ament_cmake_cpplint 23 | ament_cmake_flake8 24 | ament_cmake_lint_cmake 25 | ament_cmake_pep257 26 | ament_cmake_uncrustify 27 | ament_cmake_xmllint 28 | 29 | 30 | 31 | 32 | geometry_msgs 33 | image_geometry 34 | orca_msgs 35 | rclcpp 36 | ros2_shared 37 | sensor_msgs 38 | tf2 39 | tf2_geometry_msgs 40 | tf2_ros 41 | 42 | 43 | ament_cmake 44 | 45 | 46 | -------------------------------------------------------------------------------- /orca_shared/src/model.cpp: -------------------------------------------------------------------------------- 1 | // MIT License 2 | // 3 | // Copyright (c) 2022 Clyde McQueen 4 | // 5 | // Permission is hereby granted, free of charge, to any person obtaining a copy 6 | // of this software and associated documentation files (the "Software"), to deal 7 | // in the Software without restriction, including without limitation the rights 8 | // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | // copies of the Software, and to permit persons to whom the Software is 10 | // furnished to do so, subject to the following conditions: 11 | // 12 | // The above copyright notice and this permission notice shall be included in all 13 | // copies or substantial portions of the Software. 14 | // 15 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | // SOFTWARE. 22 | 23 | #include "orca_shared/model.hpp" 24 | #include "orca_shared/util.hpp" 25 | #include "orca_shared/pwm.hpp" 26 | #include "rclcpp/logging.hpp" 27 | 28 | namespace orca 29 | { 30 | 31 | geometry_msgs::msg::Accel Model::drag_accel(const geometry_msgs::msg::Twist & vel) const 32 | { 33 | geometry_msgs::msg::Accel result; 34 | result.linear.x = drag_accel_x(vel.linear.x); 35 | result.linear.y = drag_accel_y(vel.linear.y); 36 | result.linear.z = drag_accel_z(vel.linear.z); 37 | result.angular.z = drag_accel_yaw(vel.angular.z); 38 | return result; 39 | } 40 | 41 | geometry_msgs::msg::Wrench Model::accel_to_wrench(const geometry_msgs::msg::Accel & accel) const 42 | { 43 | geometry_msgs::msg::Wrench result; 44 | result.force.x = accel_to_force(accel.linear.x); 45 | result.force.y = accel_to_force(accel.linear.y); 46 | result.force.z = accel_to_force(accel.linear.z); 47 | result.torque.z = accel_to_torque_yaw(accel.angular.z); 48 | return result; 49 | } 50 | 51 | orca_msgs::msg::Effort Model::wrench_to_effort(const geometry_msgs::msg::Wrench & wrench) const 52 | { 53 | orca_msgs::msg::Effort result; 54 | result.force.x = clamp(force_to_effort_xy(wrench.force.x), 1.0); 55 | result.force.y = clamp(force_to_effort_xy(wrench.force.y), 1.0); 56 | result.force.z = clamp(force_to_effort_z(wrench.force.z), 1.0); 57 | result.torque.z = clamp(torque_to_effort_yaw(wrench.torque.z), 1.0); 58 | return result; 59 | } 60 | 61 | orca_msgs::msg::Effort Model::accel_to_effort(const geometry_msgs::msg::Accel & accel) const 62 | { 63 | return wrench_to_effort(accel_to_wrench(accel)); 64 | } 65 | 66 | void Model::log_info(const rclcpp::Logger & logger) const 67 | { 68 | // Describe hover force, effort and pwm 69 | auto hover_accel = hover_accel_z(); 70 | auto hover_force = accel_to_force(hover_accel); 71 | auto hover_effort = force_to_effort_z(hover_force); 72 | auto hover_pwm = orca::effort_to_pwm(mdl_thrust_dz_pwm_, hover_effort); 73 | RCLCPP_INFO( 74 | logger, "hover accel: %g, force: %g, effort: %g, pwm: %d", 75 | hover_accel, hover_force, hover_effort, hover_pwm); 76 | 77 | // Describe force, effort and pwm for a representative forward velocity 78 | double fwd_velo = 0.4; 79 | auto fwd_accel = -drag_accel_x(fwd_velo); 80 | auto fwd_force = accel_to_force(fwd_accel); 81 | auto fwd_effort = force_to_effort_xy(fwd_force); 82 | auto fwd_pwm = orca::effort_to_pwm(mdl_thrust_dz_pwm_, fwd_effort); 83 | RCLCPP_INFO( 84 | logger, "fwd velo: %g, accel: %g, force: %g, effort: %g, pwm: %d", 85 | fwd_velo, fwd_accel, fwd_force, fwd_effort, fwd_pwm); 86 | } 87 | 88 | } // namespace orca 89 | -------------------------------------------------------------------------------- /orca_shared/src/pwm.cpp: -------------------------------------------------------------------------------- 1 | // MIT License 2 | // 3 | // Copyright (c) 2022 Clyde McQueen 4 | // 5 | // Permission is hereby granted, free of charge, to any person obtaining a copy 6 | // of this software and associated documentation files (the "Software"), to deal 7 | // in the Software without restriction, including without limitation the rights 8 | // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | // copies of the Software, and to permit persons to whom the Software is 10 | // furnished to do so, subject to the following conditions: 11 | // 12 | // The above copyright notice and this permission notice shall be included in all 13 | // copies or substantial portions of the Software. 14 | // 15 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | // SOFTWARE. 22 | 23 | #include "orca_shared/pwm.hpp" 24 | 25 | #include "orca_shared/util.hpp" 26 | 27 | namespace orca 28 | { 29 | 30 | uint16_t tilt_to_pwm(const int tilt) 31 | { 32 | return orca::scale( 33 | tilt, TILT_MIN, TILT_MAX, 34 | PWM_TILT_45_UP, PWM_TILT_45_DOWN); 35 | } 36 | 37 | int pwm_to_tilt(const uint16_t pwm) 38 | { 39 | return orca::scale( 40 | pwm, PWM_TILT_45_UP, 41 | PWM_TILT_45_DOWN, 42 | TILT_MIN, TILT_MAX); 43 | } 44 | 45 | uint16_t brightness_to_pwm(const int brightness) 46 | { 47 | return orca::scale( 48 | brightness, BRIGHTNESS_MIN, BRIGHTNESS_MAX, 49 | PWM_LIGHTS_OFF, PWM_LIGHTS_FULL); 50 | } 51 | 52 | int pwm_to_brightness(const uint16_t pwm) 53 | { 54 | return orca::scale( 55 | pwm, PWM_LIGHTS_OFF, PWM_LIGHTS_FULL, 56 | BRIGHTNESS_MIN, BRIGHTNESS_MAX); 57 | } 58 | 59 | uint16_t effort_to_pwm(const uint16_t thrust_dz_pwm, const double effort) 60 | { 61 | uint16_t thrust_range_pwm = 400 - thrust_dz_pwm; 62 | 63 | return clamp( 64 | static_cast(PWM_THRUST_STOP + 65 | (effort > THRUST_STOP ? thrust_dz_pwm : (effort < THRUST_STOP ? 66 | -thrust_dz_pwm : 0)) + 67 | std::round(effort * thrust_range_pwm)), 68 | PWM_THRUST_FULL_REV, 69 | PWM_THRUST_FULL_FWD); 70 | } 71 | 72 | double pwm_to_effort(const uint16_t thrust_dz_pwm, const uint16_t pwm) 73 | { 74 | uint16_t thrust_range_pwm = 400 - thrust_dz_pwm; 75 | 76 | return static_cast( 77 | pwm - PWM_THRUST_STOP + 78 | (pwm > PWM_THRUST_STOP ? -thrust_dz_pwm : 79 | (pwm < PWM_THRUST_STOP ? thrust_dz_pwm : 0))) / 80 | thrust_range_pwm; 81 | } 82 | 83 | } // namespace orca 84 | -------------------------------------------------------------------------------- /setup.bash: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env bash 2 | 3 | # Modify this for your environment 4 | 5 | if [[ -z "${ARDUPILOT_GAZEBO}" ]]; then 6 | export ARDUPILOT_GAZEBO="$HOME/ardupilot_gazebo" 7 | fi 8 | 9 | if [[ -z "${ARDUPILOT_HOME}" ]]; then 10 | export ARDUPILOT_HOME="$HOME/ardupilot" 11 | fi 12 | 13 | if [[ -z "${COLCON_WS}" ]]; then 14 | export COLCON_WS="$HOME/colcon_ws" 15 | fi 16 | 17 | 18 | # Add results of ArduSub build 19 | export PATH=${ARDUPILOT_HOME}/build/sitl/bin:$PATH 20 | 21 | # Add results of colcon build 22 | source ${COLCON_WS}/install/setup.bash 23 | 24 | # Add ardupilot_gazebo plugin 25 | export GZ_SIM_SYSTEM_PLUGIN_PATH=${ARDUPILOT_GAZEBO}/build:$GZ_SIM_SYSTEM_PLUGIN_PATH 26 | 27 | # Add bluerov2_gz models and worlds 28 | export GZ_SIM_RESOURCE_PATH=${COLCON_WS}/src/bluerov2_gz/models:${COLCON_WS}/src/bluerov2_gz/worlds:$GZ_SIM_RESOURCE_PATH 29 | 30 | # Add orca4 models and worlds 31 | export GZ_SIM_RESOURCE_PATH=${COLCON_WS}/src/orca4/orca_description/models:${COLCON_WS}/src/orca4/orca_description/worlds:$GZ_SIM_RESOURCE_PATH 32 | 33 | # Build ros_gz on the humble branch for Gazebo Harmonic 34 | export GZ_VERSION=harmonic 35 | -------------------------------------------------------------------------------- /workspace.repos: -------------------------------------------------------------------------------- 1 | repositories: 2 | 3 | bluerov2_gz: 4 | type: git 5 | url: https://github.com/clydemcqueen/bluerov2_gz 6 | version: main 7 | 8 | orb_slam_2_ros: 9 | type: git 10 | url: https://github.com/clydemcqueen/orb_slam_2_ros 11 | version: orca4_humble 12 | 13 | ros2_shared: 14 | type: git 15 | url: https://github.com/ptrmu/ros2_shared 16 | version: master 17 | --------------------------------------------------------------------------------