├── .dockerignore ├── .github └── workflows │ └── main_ci.yml ├── .gitignore ├── Dockerfile ├── LICENSE ├── README.md ├── images ├── gazebo.png ├── mission_orb.png ├── mission_vlam.png ├── rviz_orb.png └── rviz_vlam.png ├── orca_base ├── CMakeLists.txt ├── README.md ├── include │ └── orca_base │ │ ├── base_context.hpp │ │ ├── pid.hpp │ │ ├── thrusters.hpp │ │ └── underwater_motion.hpp ├── package.xml ├── scripts │ ├── fake_barometer.py │ └── fake_driver.py └── src │ ├── baro_filter_node.cpp │ ├── base_controller.cpp │ ├── depth_node.cpp │ ├── odom_to_path_node.cpp │ ├── pid.cpp │ ├── pose_to_path_node.cpp │ ├── thrusters.cpp │ └── underwater_motion.cpp ├── orca_bringup ├── CMakeLists.txt ├── README.md ├── behavior_trees │ └── orca3_bt.xml ├── cfg │ ├── forward_camera.ini │ ├── sim_launch_orb.rviz │ ├── sim_launch_vlam.rviz │ ├── slam_test_launch.rviz │ ├── stereo_left.ini │ └── stereo_right.ini ├── launch │ ├── bringup.py │ ├── calibrate_launch.py │ ├── sim_launch.py │ ├── slam_test_launch.py │ ├── sub_launch.py │ ├── test_launch.py │ └── topside_launch.py ├── missions │ └── missions.txt ├── package.xml ├── params │ ├── bench_orca_params.yaml │ ├── nav2_params.yaml │ ├── sim_orca_params.yaml │ ├── slam_test_params.yaml │ └── topside_orca_params.yaml ├── scripts │ └── dump_rosout.py └── worlds │ ├── empty.pgm │ ├── empty_map.yaml │ ├── empty_world.yaml │ └── ping_pong_map.yaml ├── orca_description ├── CMakeLists.txt ├── README.md ├── package.xml ├── scad │ └── diagrams │ │ └── stereo_cam_fov.scad └── xacro │ └── orca.urdf.xacro ├── orca_driver ├── CMakeLists.txt ├── COLCON_IGNORE ├── README.md ├── cfg │ └── maestro.cfg ├── include │ └── orca_driver │ │ ├── driver_context.hpp │ │ └── maestro.hpp ├── package.xml ├── scripts │ ├── dance_node.py │ ├── orca_driver.service │ ├── orca_fcam.service │ ├── start_driver.sh │ ├── start_fcam.sh │ ├── thrust_curve_node.py │ └── topside_video.sh └── src │ ├── barometer_node.cpp │ ├── driver_node.cpp │ ├── maestro.cpp │ └── test_node.cpp ├── orca_gazebo ├── CMakeLists.txt ├── README.md ├── models │ └── seafloor │ │ ├── materials │ │ ├── scripts │ │ │ └── seafloor.material │ │ └── textures │ │ │ └── rocks.png │ │ ├── model.config │ │ └── model.sdf ├── package.xml ├── scripts │ ├── build_world.py │ ├── find_transform.py │ ├── reliable_odom.py │ └── seafloor_marker.py └── src │ ├── barometer_plugin.cpp │ ├── buoyancy_plugin.cpp │ ├── drag_plugin.cpp │ └── thruster_plugin.cpp ├── orca_localize ├── CMakeLists.txt ├── README.md ├── package.xml └── src │ ├── camera_info_publisher.cpp │ ├── fiducial_localizer.cpp │ └── orb_slam2_localizer.cpp ├── orca_msgs ├── CMakeLists.txt ├── README.md ├── msg │ ├── Barometer.msg │ ├── CameraTilt.msg │ ├── Depth.msg │ ├── Effort.msg │ ├── Lights.msg │ ├── Motion.msg │ ├── Pid.msg │ ├── Status.msg │ ├── Teleop.msg │ └── Thrust.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 ├── include │ └── orca_shared │ │ ├── baro.hpp │ │ ├── model.hpp │ │ ├── pwm.hpp │ │ └── util.hpp ├── package.xml └── src │ ├── baro.cpp │ ├── model.cpp │ ├── pwm.cpp │ └── util.cpp ├── orca_topside ├── CMakeLists.txt ├── README.md ├── images │ └── topside.png ├── include │ └── orca_topside │ │ ├── fps_calculator.hpp │ │ ├── gst_util.hpp │ │ ├── gst_widget.hpp │ │ ├── image_publisher.hpp │ │ ├── image_widget.hpp │ │ ├── node_spinner.hpp │ │ ├── teleop_node.hpp │ │ ├── topside_layout.hpp │ │ ├── topside_widget.hpp │ │ ├── video_pipeline.hpp │ │ └── xbox.hpp ├── package.xml └── src │ ├── fps_calculator.cpp │ ├── gst_util.cpp │ ├── gst_widget.cpp │ ├── image_publisher.cpp │ ├── image_widget.cpp │ ├── node_spinner.cpp │ ├── teleop_main.cpp │ ├── teleop_node.cpp │ ├── topside_layout.cpp │ ├── topside_widget.cpp │ └── video_pipeline.cpp ├── setup.bash └── workspace.repos /.dockerignore: -------------------------------------------------------------------------------- 1 | cmake-build-* 2 | .cmake-build-* 3 | .idea 4 | -------------------------------------------------------------------------------- /.github/workflows/main_ci.yml: -------------------------------------------------------------------------------- 1 | name: galactic_ci 2 | 3 | on: 4 | pull_request: 5 | branches: 6 | - 'main' 7 | push: 8 | branches: 9 | - 'main' 10 | 11 | jobs: 12 | test_environment: 13 | runs-on: ubuntu-latest 14 | strategy: 15 | fail-fast: false 16 | matrix: 17 | ros_distribution: 18 | - galactic 19 | include: 20 | - docker_image: rostooling/setup-ros-docker:ubuntu-focal-ros-galactic-ros-base-latest 21 | ros_distribution: galactic 22 | ros_version: 2 23 | container: 24 | image: ${{matrix.docker_image}} 25 | steps: 26 | - name: setup directories 27 | run: mkdir -p ros_ws/src 28 | - name: install transformations.py 29 | run: pip3 install transformations 30 | - name: checkout 31 | uses: actions/checkout@v2 32 | with: 33 | path: ros_ws/src 34 | - name: build and test 35 | uses: ros-tooling/action-ros-ci@master 36 | with: 37 | package-name: | 38 | orca_base 39 | orca_bringup 40 | orca_description 41 | orca_gazebo 42 | orca_localize 43 | orca_msgs 44 | orca_nav2 45 | orca_shared 46 | orca_topside 47 | target-ros2-distro: ${{matrix.ros_distribution}} 48 | vcs-repo-file-url: "https://raw.githubusercontent.com/${{github.repository}}/${{github.sha}}/workspace.repos" 49 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | cmake-build-* 2 | .cmake-build-* 3 | .idea 4 | -------------------------------------------------------------------------------- /Dockerfile: -------------------------------------------------------------------------------- 1 | # This Dockerfile can be configured via --build-arg 2 | 3 | # Example build command: 4 | # docker build --pull --no-cache -t orca3:galactic . 5 | 6 | # Example run command using Rocker (see https://github.com/osrf/rocker): 7 | # rocker --x11 --nvidia orca3:galactic 8 | 9 | FROM osrf/ros:galactic-desktop 10 | 11 | RUN apt-get update && apt-get upgrade -y 12 | 13 | RUN apt-get install -y python3-pip 14 | RUN yes | pip3 install transformations 15 | 16 | # Required for orca_topside 17 | RUN apt-get install -y libgstreamer1.0-0 gstreamer1.0-plugins-base gstreamer1.0-plugins-good \ 18 | gstreamer1.0-plugins-bad gstreamer1.0-plugins-ugly gstreamer1.0-libav gstreamer1.0-doc gstreamer1.0-tools \ 19 | gstreamer1.0-x gstreamer1.0-alsa gstreamer1.0-gl gstreamer1.0-gtk3 gstreamer1.0-qt5 gstreamer1.0-pulseaudio \ 20 | libgstreamer-plugins-base1.0-dev 21 | 22 | WORKDIR /work/orca_ws/src 23 | 24 | COPY . orca3 25 | 26 | ARG FIDUCIAL_VLAM_BRANCH=master 27 | ARG H264_IMAGE_TRANSPORT_BRANCH=master 28 | ARG ORB_SLAM2_ROS_BRANCH=clyde_h264_stereo_galactic 29 | ARG ROS2_SHARED_BRANCH=master 30 | ARG SIM_FIDUCIAL_BRANCH=master 31 | ARG STEREO_DECODER_BRANCH=main 32 | ARG UKF_BRANCH=master 33 | 34 | RUN git clone https://github.com/ptrmu/fiducial_vlam.git -b $FIDUCIAL_VLAM_BRANCH 35 | RUN git clone https://github.com/clydemcqueen/h264_image_transport.git -b $H264_IMAGE_TRANSPORT_BRANCH 36 | RUN git clone https://github.com/clydemcqueen/orb_slam_2_ros.git -b $ORB_SLAM2_ROS_BRANCH 37 | RUN git clone https://github.com/ptrmu/ros2_shared.git -b $ROS2_SHARED_BRANCH 38 | RUN git clone https://github.com/clydemcqueen/sim_fiducial.git -b $SIM_FIDUCIAL_BRANCH 39 | RUN git clone https://github.com/clydemcqueen/stereo_decoder.git -b $STEREO_DECODER_BRANCH 40 | RUN git clone https://github.com/clydemcqueen/ukf.git -b $UKF_BRANCH 41 | 42 | WORKDIR /work/orca_ws 43 | 44 | RUN rosdep install -y --from-paths . --ignore-src 45 | 46 | RUN /bin/bash -c "source /opt/ros/galactic/setup.bash && colcon build" 47 | 48 | # Simulation with fiducial_vlam: 49 | # source src/orca3/setup.bash # Required to set up the Gazebo environment correctly 50 | # ros2 launch orca_bringup sim_launch.py gzclient:=True rviz:=True slam:=vlam world:=ping_pong 51 | 52 | # Simulation with orb_slam2_ros: 53 | # source src/orca3/setup.bash # Required to set up the Gazebo environment correctly 54 | # ros2 launch orca_bringup sim_launch.py gzclient:=True rviz:=True slam:=orb world:=empty 55 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2021 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 | -------------------------------------------------------------------------------- /images/gazebo.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/clydemcqueen/orca3/02bfefe346cf04cce13e227b98a763241fdcf534/images/gazebo.png -------------------------------------------------------------------------------- /images/mission_orb.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/clydemcqueen/orca3/02bfefe346cf04cce13e227b98a763241fdcf534/images/mission_orb.png -------------------------------------------------------------------------------- /images/mission_vlam.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/clydemcqueen/orca3/02bfefe346cf04cce13e227b98a763241fdcf534/images/mission_vlam.png -------------------------------------------------------------------------------- /images/rviz_orb.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/clydemcqueen/orca3/02bfefe346cf04cce13e227b98a763241fdcf534/images/rviz_orb.png -------------------------------------------------------------------------------- /images/rviz_vlam.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/clydemcqueen/orca3/02bfefe346cf04cce13e227b98a763241fdcf534/images/rviz_vlam.png -------------------------------------------------------------------------------- /orca_base/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | project(orca_base) 3 | 4 | if(NOT CMAKE_CXX_STANDARD) 5 | set(CMAKE_CXX_STANDARD 14) 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 | # Emulate colcon in CLion 13 | if($ENV{CLION_IDE}) 14 | find_package(fastrtps_cmake_module REQUIRED) 15 | set(FastRTPS_INCLUDE_DIR "/opt/ros/foxy/include") 16 | set(FastRTPS_LIBRARY_RELEASE "/opt/ros/foxy/lib/libfastrtps.so") 17 | set(orca_description_DIR "${PROJECT_SOURCE_DIR}/../../../install/orca_description/share/orca_description/cmake") 18 | set(orca_msgs_DIR "${PROJECT_SOURCE_DIR}/../../../install/orca_msgs/share/orca_msgs/cmake") 19 | set(orca_shared_DIR "${PROJECT_SOURCE_DIR}/../../../install/orca_shared/share/orca_shared/cmake") 20 | set(ros2_shared_DIR "${PROJECT_SOURCE_DIR}/../../../install/ros2_shared/share/ros2_shared/cmake") 21 | set(ukf_DIR "${PROJECT_SOURCE_DIR}/../../../install/ukf/share/ukf/cmake") 22 | endif() 23 | 24 | find_package(ament_cmake_auto REQUIRED) 25 | ament_auto_find_build_dependencies() 26 | 27 | include_directories(include) 28 | 29 | ament_auto_add_executable(baro_filter_node src/baro_filter_node.cpp) 30 | 31 | ament_auto_add_executable(base_controller src/base_controller.cpp src/underwater_motion.cpp src/pid.cpp src/thrusters.cpp) 32 | 33 | ament_auto_add_executable(depth_node src/depth_node.cpp) 34 | 35 | ament_auto_add_executable(pose_to_path src/pose_to_path_node.cpp) 36 | 37 | ament_auto_add_executable(odom_to_path src/odom_to_path_node.cpp) 38 | 39 | # Load & run linters listed in package.xml 40 | if(BUILD_TESTING) 41 | find_package(ament_lint_auto REQUIRED) 42 | ament_lint_auto_find_test_dependencies() 43 | endif() 44 | 45 | # Install scripts 46 | install( 47 | PROGRAMS # PROGRAMS sets execute bits, FILES clears them 48 | scripts/fake_barometer.py 49 | scripts/fake_driver.py 50 | DESTINATION lib/${PROJECT_NAME} 51 | ) 52 | 53 | ament_auto_package() -------------------------------------------------------------------------------- /orca_base/README.md: -------------------------------------------------------------------------------- 1 | # Orca3 Base Controller 2 | 3 | Orca3 is designed for the [BlueRobotics BlueROV2](https://bluerobotics.com/store/rov/bluerov2/), 4 | a simple but powerful ROV (Remotely Operated Vehicle) with two vertical thrusters and 5 | four vectored horizontal thrusters. See [orca_gazebo](../orca_gazebo) for more information. 6 | 7 | ## BaseController 8 | 9 | The `BaseController` node does the following: 10 | * subscribe to `/cmd_vel` from the Nav2 system 11 | * predict motion using a constant acceleration (trapezoidal velocity) model. Roll and pitch are not modeled. 12 | * publish odometry on `/odom`, and broadcast the odom->base_link transform 13 | * subscribe to `/barometer` and run a PID controller to hold depth at the target value 14 | * calculate required thrust forces and publish `/thrust` messages for the Gazebo simulation 15 | * publish diagnotics on `/depth`, `/motion` and `/pid_z` 16 | 17 | Thrust force includes these components: 18 | * thrust due to acceleration 19 | * thrust to counteract the predicted drag. I've estimated the drag of the BlueROV2 frame, but this is very rough, 20 | and each actual AUV will have different drag properties. 21 | * thrust to hold depth, this includes a static component based on buoyancy and the output of the PID controller 22 | 23 | ### Motion model parameters 24 | 25 | There's a very simple motion model that considers buoyancy and drag. 26 | It does not consider added mass, hydrodynamic damping, etc. 27 | 28 | | Parameter | Type | Default | Notes | 29 | |---|---|---|---| 30 | | mdl_mass | double | 9.75 | Mass, kg | 31 | | mdl_volume | double | 0.01 | Displacement volume, m^3 | 32 | | mdl_fluid_density | double | 997 | Fluid density, kg/m^3, typically 997 for freshwater, 1027 for seawater | 33 | | mdl_thrust_scale | double | 0.7 | Global thrust scale, used to boost small thrust values typical for AUV operation | 34 | | mdl_drag_coef_x | double | 0.8 | Drag coefficient for forward / back motion | 35 | | mdl_drag_coef_y | double | 0.95 | Drag coefficient for strafing motion | 36 | | mdl_drag_coef_z | double | 0.95 | Drag coefficient for vertical motion | 37 | | mdl_drag_partial_const_yaw | double | 0.004 | Drag coefficient for yaw motion | 38 | | mdl_thrust_dz_pwm | int16 | 35 | Thruster deadzone | 39 | 40 | ### Control parameters 41 | 42 | | Parameter | Type | Default | Notes | 43 | |---|---|---|---| 44 | | auto_arm | bool | false | Arm the controller on boot | 45 | | stamp_msgs_with_current_time | bool | false | Use now() vs Barometer.header.stamp | 46 | | x_vel | double | 0.4 | Max forward / back velocity, m/s | 47 | | y_vel | double | 0.4 | Max strafing velocity, m/s | 48 | | z_vel | double | 0.2 | Max vertical velocity, m/s | 49 | | x_accel | double | 0.4 | Max forward / back acceleration, m/s^2 | 50 | | y_accel | double | 0.4 | Max strafing acceleration, m/s^2 | 51 | | z_accel | double | 0.2 | Max vertical acceleration, m/s^2 | 52 | | yaw_vel | double | 0.4 | Max yaw velocity, r/s | 53 | | yaw_accel | double | 0.4 | Max yaw acceleration, r/s^2 | 54 | | map_frame_id | string | map | Map frame id | 55 | | odom_frame_id | string | odom | Odom frame id | 56 | | base_frame_id | string | base_link | Base frame id | 57 | | publish_tf | bool | base_link | Publish odom->base_link transform | 58 | | thruster_xy_limit | double | 0.5 | Limit the x and y effort in the vectored horizontal thrusters to provide enough pwm range for yaw motion | 59 | | thruster_accel_limit | double | 1.0 | Limit rapid changes to thruster values | 60 | | pid_enabled | bool | true | Turn the depth PID controller on or off | 61 | | pid_z_kp | double | 0.5 | Kp | 62 | | pid_z_ki | double | 0 | Ki | 63 | | pid_z_kd | double | 0 | Kd | 64 | | pid_z_i_max | double | 0.1 | Windup prevention: max acceleration from Ki term, m/s^2 | 65 | | hover_thrust | bool | true | Add static hover thrust to counteract buoyancy | 66 | -------------------------------------------------------------------------------- /orca_base/include/orca_base/base_context.hpp: -------------------------------------------------------------------------------- 1 | // MIT License 2 | // 3 | // Copyright (c) 2021 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(stamp_msgs_with_current_time, bool, false) \ 35 | \ 36 | CXT_MACRO_MEMBER(x_vel, double, 0.4) \ 37 | CXT_MACRO_MEMBER(y_vel, double, 0.4) \ 38 | CXT_MACRO_MEMBER(z_vel, double, 0.2) \ 39 | CXT_MACRO_MEMBER(yaw_vel, double, 0.4) \ 40 | \ 41 | CXT_MACRO_MEMBER(x_accel, double, 0.4) \ 42 | CXT_MACRO_MEMBER(y_accel, double, 0.4) \ 43 | CXT_MACRO_MEMBER(z_accel, double, 0.2) \ 44 | CXT_MACRO_MEMBER(yaw_accel, double, 0.4) \ 45 | \ 46 | CXT_MACRO_MEMBER(map_frame_id, std::string, "map") \ 47 | CXT_MACRO_MEMBER(odom_frame_id, std::string, "odom") \ 48 | CXT_MACRO_MEMBER(base_frame_id, std::string, "base_link") \ 49 | /* Frame ids */ \ 50 | CXT_MACRO_MEMBER(publish_tf, bool, true) \ 51 | /* Publish odom->base tf */ \ 52 | CXT_MACRO_MEMBER(thruster_xy_limit, double, 0.5) \ 53 | /* Limit fwd/strafe motion, leave room for yaw */ \ 54 | CXT_MACRO_MEMBER(thruster_accel_limit, double, 1.0) \ 55 | /* Limit thruster acceleration, measured in effort units */ \ 56 | CXT_MACRO_MEMBER(pid_z_kp, double, 0.5) \ 57 | CXT_MACRO_MEMBER(pid_z_ki, double, 0.0) \ 58 | CXT_MACRO_MEMBER(pid_z_kd, double, 0.0) \ 59 | CXT_MACRO_MEMBER(pid_z_i_max, double, 0.1) \ 60 | /* Windup prevention: max acceleration from i term (m/s^2) */ \ 61 | CXT_MACRO_MEMBER(coast, bool, false) \ 62 | /* Coast to decelerate (vs powered decel), useful for ROV ops */ \ 63 | /* End of list */ 64 | 65 | #undef CXT_MACRO_MEMBER 66 | #define CXT_MACRO_MEMBER(n, t, d) CXT_MACRO_DEFINE_MEMBER(n, t, d) 67 | 68 | struct BaseContext : orca::Model 69 | { 70 | CXT_MACRO_DEFINE_MEMBERS(BASE_PARAMS) 71 | 72 | // Not parameters -- set from Teleop.msg 73 | bool hover_thrust_{}; // Add hover thrust 74 | bool pid_enabled_{}; // Enable pid controller(s) 75 | }; 76 | 77 | #define BASE_ALL_PARAMS \ 78 | MODEL_PARAMS \ 79 | BASE_PARAMS \ 80 | /* End of list */ 81 | 82 | } // namespace orca_base 83 | 84 | #endif // ORCA_BASE__BASE_CONTEXT_HPP_ 85 | -------------------------------------------------------------------------------- /orca_base/include/orca_base/pid.hpp: -------------------------------------------------------------------------------- 1 | // MIT License 2 | // 3 | // Copyright (c) 2021 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__PID_HPP_ 24 | #define ORCA_BASE__PID_HPP_ 25 | 26 | #include "orca_msgs/msg/pid.hpp" 27 | 28 | namespace pid 29 | { 30 | 31 | class Controller 32 | { 33 | bool angle_; // True if we're controlling an angle [-pi, pi] 34 | orca_msgs::msg::Pid msg_; 35 | double Kp_; 36 | double Ki_; 37 | double Kd_; 38 | double i_max_; // Windup prevention: max accel contribution of the (Ki * integral) term 39 | 40 | public: 41 | Controller(bool angle, double Kp, double Ki, double Kd, double i_max); 42 | 43 | void set_target(double target); 44 | 45 | // Run one calculation 46 | double calc(builtin_interfaces::msg::Time stamp, double state, double dt); 47 | 48 | const orca_msgs::msg::Pid & msg() const {return msg_;} 49 | }; 50 | 51 | } // namespace pid 52 | 53 | #endif // ORCA_BASE__PID_HPP_ 54 | -------------------------------------------------------------------------------- /orca_base/include/orca_base/thrusters.hpp: -------------------------------------------------------------------------------- 1 | // MIT License 2 | // 3 | // Copyright (c) 2021 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__THRUSTERS_HPP_ 24 | #define ORCA_BASE__THRUSTERS_HPP_ 25 | 26 | #include 27 | #include 28 | #include 29 | 30 | #include "orca_base/base_context.hpp" 31 | #include "orca_msgs/msg/effort.hpp" 32 | #include "orca_msgs/msg/thrust.hpp" 33 | 34 | namespace orca_base 35 | { 36 | 37 | struct Thruster 38 | { 39 | std::string frame_id; // URDF link frame id 40 | bool ccw; // True if counterclockwise 41 | double forward; 42 | double strafe; 43 | double yaw; 44 | double vertical; 45 | double prev_effort; // Most recent effort 46 | 47 | Thruster( 48 | std::string _frame_id, bool _ccw, 49 | double _forward, double _strafe, double _yaw, double _vertical) 50 | : frame_id{std::move(_frame_id)}, 51 | ccw{_ccw}, 52 | forward{_forward}, 53 | strafe{_strafe}, 54 | yaw{_yaw}, 55 | vertical{_vertical}, 56 | prev_effort{} {} 57 | 58 | int effort_to_pwm( 59 | const BaseContext & cxt, const orca_msgs::msg::Effort & effort, 60 | bool & saturated); 61 | }; 62 | 63 | class Thrusters 64 | { 65 | std::vector thrusters_; 66 | 67 | public: 68 | Thrusters(); 69 | 70 | std::vector effort_to_thrust( 71 | const BaseContext & cxt, 72 | const orca_msgs::msg::Effort & effort, bool & saturated); 73 | }; 74 | 75 | } // namespace orca_base 76 | 77 | #endif // ORCA_BASE__THRUSTERS_HPP_ 78 | -------------------------------------------------------------------------------- /orca_base/include/orca_base/underwater_motion.hpp: -------------------------------------------------------------------------------- 1 | // MIT License 2 | // 3 | // Copyright (c) 2021 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_base/pid.hpp" 37 | #include "orca_msgs/msg/pid.hpp" 38 | #include "orca_msgs/msg/motion.hpp" 39 | #include "orca_shared/model.hpp" 40 | #include "rclcpp/logger.hpp" 41 | 42 | namespace orca_base 43 | { 44 | 45 | class UnderwaterMotion 46 | { 47 | rclcpp::Logger logger_; 48 | const BaseContext & cxt_; 49 | rclcpp::Time prev_time_; 50 | orca_msgs::msg::Motion motion_; 51 | std::unique_ptr pid_z_; 52 | 53 | double report_and_clamp(std::string func, std::string name, double v, double minmax); 54 | 55 | geometry_msgs::msg::Accel calc_accel( 56 | const geometry_msgs::msg::Twist & v0, 57 | const geometry_msgs::msg::Twist & v1); 58 | 59 | geometry_msgs::msg::Twist calc_vel( 60 | const geometry_msgs::msg::Twist & v0, 61 | const geometry_msgs::msg::Accel & a); 62 | 63 | geometry_msgs::msg::Pose calc_pose( 64 | const geometry_msgs::msg::Pose & p0, 65 | const geometry_msgs::msg::Twist & v); 66 | 67 | public: 68 | UnderwaterMotion( 69 | const rclcpp::Logger & logger, const BaseContext & cxt, 70 | const rclcpp::Time & t, double z); 71 | 72 | const orca_msgs::msg::Motion & motion() const {return motion_;} 73 | 74 | const orca_msgs::msg::Pid & pid_z() const {return pid_z_->msg();} 75 | 76 | nav_msgs::msg::Odometry odometry() const; 77 | 78 | geometry_msgs::msg::TransformStamped transform_stamped() const; 79 | 80 | // Update state from time t-1 to time t 81 | void update(const rclcpp::Time & t, const geometry_msgs::msg::Twist & cmd_vel, double baro_z); 82 | }; 83 | 84 | } // namespace orca_base 85 | 86 | #endif // ORCA_BASE__UNDERWATER_MOTION_HPP_ 87 | -------------------------------------------------------------------------------- /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/orca3.git 13 | https://github.com/clydemcqueen/orca3/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 | geometry_msgs 35 | nav_msgs 36 | orca_msgs 37 | orca_shared 38 | rclcpp 39 | ros2_shared 40 | tf2 41 | tf2_geometry_msgs 42 | tf2_ros 43 | ukf 44 | 45 | 46 | ament_cmake 47 | 48 | 49 | -------------------------------------------------------------------------------- /orca_base/scripts/fake_barometer.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | # MIT License 4 | # 5 | # Copyright (c) 2021 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 fake /barometer messages, useful for testing ROV topside nodes. 27 | 28 | Usage: 29 | -- ros2 run orca_base fake_barometer.py 30 | """ 31 | 32 | from orca_msgs.msg import Barometer 33 | import rclpy 34 | import rclpy.logging 35 | from rclpy.node import Node 36 | 37 | TIMER_PERIOD = 0.05 38 | PRESSURE_AIR = 100000. 39 | PRESSURE_2 = 105000. 40 | 41 | 42 | class FakeBarometer(Node): 43 | 44 | def __init__(self): 45 | super().__init__('fake_barometer') 46 | 47 | self._barometer_pub = self.create_publisher(Barometer, '/barometer', 10) 48 | 49 | # First message should be "air pressure" to calibrate base_controller 50 | msg = Barometer() 51 | msg.header.stamp = self.get_clock().now().to_msg() 52 | msg.pressure = PRESSURE_AIR 53 | self._barometer_pub.publish(msg) 54 | 55 | self._timer = self.create_timer(TIMER_PERIOD, self.timer_callback) 56 | 57 | def timer_callback(self): 58 | msg = Barometer() 59 | msg.header.stamp = self.get_clock().now().to_msg() 60 | msg.pressure_variance = 1. 61 | 62 | if msg.header.stamp.sec % 10 >= 5: 63 | msg.pressure = PRESSURE_AIR 64 | else: 65 | msg.pressure = PRESSURE_2 66 | self._barometer_pub.publish(msg) 67 | 68 | 69 | def main(): 70 | rclpy.init() 71 | 72 | node = FakeBarometer() 73 | 74 | try: 75 | rclpy.spin(node) 76 | except KeyboardInterrupt: 77 | node.get_logger().info('ctrl-C detected, shutting down') 78 | finally: 79 | node.destroy_node() 80 | rclpy.shutdown() 81 | 82 | 83 | if __name__ == '__main__': 84 | main() 85 | -------------------------------------------------------------------------------- /orca_base/scripts/fake_driver.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | # MIT License 4 | # 5 | # Copyright (c) 2021 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 fake /status messages, useful for testing ROV topside nodes. 27 | 28 | Usage: 29 | -- ros2 run orca_base fake_driver.py 30 | """ 31 | import orca_msgs.msg 32 | from orca_msgs.msg import Status 33 | import rclpy 34 | import rclpy.logging 35 | from rclpy.node import Node 36 | 37 | TIMER_PERIOD = 0.05 38 | 39 | 40 | class FakeDriver(Node): 41 | 42 | def __init__(self): 43 | super().__init__('fake_driver') 44 | 45 | self._status_pub = self.create_publisher(Status, '/status', 10) 46 | self._timer = self.create_timer(TIMER_PERIOD, self.timer_callback) 47 | 48 | def timer_callback(self): 49 | msg = Status() 50 | msg.header.stamp = self.get_clock().now().to_msg() 51 | msg.status = orca_msgs.msg.Status.STATUS_READY 52 | msg.voltage = 16. 53 | self._status_pub.publish(msg) 54 | 55 | 56 | def main(): 57 | rclpy.init() 58 | 59 | node = FakeDriver() 60 | 61 | try: 62 | rclpy.spin(node) 63 | except KeyboardInterrupt: 64 | node.get_logger().info('ctrl-C detected, shutting down') 65 | finally: 66 | node.destroy_node() 67 | rclpy.shutdown() 68 | 69 | 70 | if __name__ == '__main__': 71 | main() 72 | -------------------------------------------------------------------------------- /orca_base/src/odom_to_path_node.cpp: -------------------------------------------------------------------------------- 1 | // MIT License 2 | // 3 | // Copyright (c) 2021 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/pid.cpp: -------------------------------------------------------------------------------- 1 | // MIT License 2 | // 3 | // Copyright (c) 2021 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 "orca_base/pid.hpp" 26 | #include "orca_shared/util.hpp" 27 | 28 | namespace pid 29 | { 30 | 31 | Controller::Controller(bool angle, double Kp, double Ki, double Kd, double i_max) 32 | { 33 | angle_ = angle; 34 | Kp_ = Kp; 35 | Ki_ = Ki; 36 | Kd_ = Kd; 37 | i_max_ = std::abs(i_max); 38 | } 39 | 40 | void Controller::set_target(double target) 41 | { 42 | if (angle_) { 43 | while (target < -M_PI) { 44 | target += 2 * M_PI; 45 | } 46 | while (target > M_PI) { 47 | target -= 2 * M_PI; 48 | } 49 | } 50 | 51 | if (std::abs(target - msg_.target) > 0.001) { 52 | msg_.target = target; 53 | msg_.prev_error = 0; 54 | msg_.integral = 0; 55 | } 56 | } 57 | 58 | // Run one calculation 59 | double Controller::calc(builtin_interfaces::msg::Time stamp, double state, double dt) 60 | { 61 | msg_.header.stamp = stamp; 62 | msg_.dt = dt; 63 | msg_.state = state; 64 | msg_.error = msg_.target - state; 65 | 66 | if (angle_) { 67 | while (msg_.error < -M_PI) { 68 | msg_.error += 2 * M_PI; 69 | 70 | // Derivative and integral are poorly defined at the discontinuity 71 | msg_.prev_error = 0; 72 | msg_.integral = 0; 73 | } 74 | while (msg_.error > M_PI) { 75 | msg_.error -= 2 * M_PI; 76 | 77 | msg_.prev_error = 0; 78 | msg_.integral = 0; 79 | } 80 | } 81 | 82 | if (Ki_ != 0) { 83 | msg_.integral = msg_.integral + (msg_.error * dt); 84 | 85 | if (i_max_ > 0) { 86 | // Limit the maximum i term (Ki * integral) by clamping the integral 87 | msg_.integral = orca::clamp(msg_.integral, i_max_ / Ki_); 88 | } 89 | } 90 | 91 | msg_.p_term = Kp_ * msg_.error; 92 | msg_.i_term = Ki_ * msg_.integral; 93 | msg_.d_term = Kd_ * (msg_.error - msg_.prev_error) / dt; 94 | msg_.result = msg_.p_term + msg_.i_term + msg_.d_term; 95 | 96 | msg_.prev_error = msg_.error; 97 | 98 | return msg_.result; 99 | } 100 | 101 | } // namespace pid 102 | -------------------------------------------------------------------------------- /orca_base/src/pose_to_path_node.cpp: -------------------------------------------------------------------------------- 1 | // MIT License 2 | // 3 | // Copyright (c) 2021 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/thrusters.cpp: -------------------------------------------------------------------------------- 1 | // MIT License 2 | // 3 | // Copyright (c) 2021 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 "orca_base/thrusters.hpp" 26 | #include "orca_shared/pwm.hpp" 27 | #include "orca_shared/util.hpp" 28 | 29 | namespace orca_base 30 | { 31 | 32 | int Thruster::effort_to_pwm( 33 | const BaseContext & cxt, const orca_msgs::msg::Effort & effort, 34 | bool & saturated) 35 | { 36 | double combined_effort = effort.force.x * forward + effort.force.y * strafe; 37 | 38 | // Clamp forward + strafe to xy_limit 39 | if (combined_effort > cxt.thruster_xy_limit_) { 40 | combined_effort = cxt.thruster_xy_limit_; 41 | saturated = true; 42 | } else if (combined_effort < -cxt.thruster_xy_limit_) { 43 | combined_effort = -cxt.thruster_xy_limit_; 44 | saturated = true; 45 | } 46 | 47 | combined_effort += effort.torque.z * yaw; 48 | 49 | // Clamp forward + strafe + yaw to max values 50 | if (combined_effort > orca::THRUST_FULL_FWD) { 51 | combined_effort = orca::THRUST_FULL_FWD; 52 | saturated = true; 53 | } else if (combined_effort < orca::THRUST_FULL_REV) { 54 | combined_effort = orca::THRUST_FULL_REV; 55 | saturated = true; 56 | } 57 | 58 | double vertical_effort = effort.force.z * vertical; 59 | 60 | // Clamp vertical effort to max values 61 | if (vertical_effort > orca::THRUST_FULL_FWD) { 62 | vertical_effort = orca::THRUST_FULL_FWD; 63 | saturated = true; 64 | } else if (vertical_effort < orca::THRUST_FULL_REV) { 65 | vertical_effort = orca::THRUST_FULL_REV; 66 | saturated = true; 67 | } 68 | 69 | // Vertical effort is independent from the rest, no need to clamp 70 | double total_effort = combined_effort + vertical_effort; 71 | 72 | // Protect the thruster: 73 | // -- limit change to +/- max_change 74 | // -- don't reverse thruster, i.e., stop at 0.0 75 | total_effort = orca::clamp( 76 | total_effort, 77 | prev_effort - cxt.thruster_accel_limit_, 78 | prev_effort + cxt.thruster_accel_limit_); 79 | if ((total_effort < 0 && prev_effort > 0) || (total_effort > 0 && prev_effort < 0)) { 80 | total_effort = 0; 81 | } 82 | 83 | prev_effort = total_effort; 84 | 85 | return orca::effort_to_pwm(cxt.mdl_thrust_dz_pwm_, total_effort); 86 | } 87 | 88 | Thrusters::Thrusters() 89 | { 90 | // Off-by-1, thruster 1 is thrusters_[0], etc. 91 | // https://bluerobotics.com/learn/bluerov2-assembly/ 92 | thrusters_.emplace_back("t200_link_front_right", false, 1.0, 1.0, 1.0, 0.0); 93 | thrusters_.emplace_back("t200_link_front_left", false, 1.0, -1.0, -1.0, 0.0); 94 | thrusters_.emplace_back("t200_link_rear_right", true, 1.0, -1.0, 1.0, 0.0); 95 | thrusters_.emplace_back("t200_link_rear_left", true, 1.0, 1.0, -1.0, 0.0); 96 | thrusters_.emplace_back("t200_link_vertical_right", false, 0.0, 0.0, 0.0, 1.0); 97 | thrusters_.emplace_back("t200_link_vertical_left", true, 0.0, 0.0, 0.0, -1.0); 98 | } 99 | 100 | std::vector Thrusters::effort_to_thrust( 101 | const BaseContext & cxt, 102 | const orca_msgs::msg::Effort & effort, 103 | bool & saturated) 104 | { 105 | std::vector result; 106 | saturated = false; 107 | 108 | for (auto & thruster : thrusters_) { 109 | result.emplace_back(thruster.effort_to_pwm(cxt, effort, saturated)); 110 | } 111 | 112 | return result; 113 | } 114 | 115 | } // namespace orca_base 116 | -------------------------------------------------------------------------------- /orca_bringup/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | project(orca_bringup) 3 | 4 | # Add -Wno-dev to CLion CMake options to suppress warning about gazebo_dev 5 | find_package(ament_cmake_auto REQUIRED) 6 | ament_auto_find_build_dependencies() 7 | 8 | # Load & run linters listed in package.xml 9 | if(BUILD_TESTING) 10 | find_package(ament_lint_auto REQUIRED) 11 | ament_lint_auto_find_test_dependencies() 12 | endif() 13 | 14 | # Install scripts 15 | install( 16 | PROGRAMS 17 | scripts/dump_rosout.py 18 | DESTINATION lib/${PROJECT_NAME} 19 | ) 20 | 21 | ament_auto_package(INSTALL_TO_SHARE behavior_trees cfg launch params worlds) -------------------------------------------------------------------------------- /orca_bringup/README.md: -------------------------------------------------------------------------------- 1 | ## Launch files 2 | 3 | ### [bringup.py](launch/bringup.py) 4 | 5 | Bring up all core ROV and AUV nodes, including SLAM and Nav2. 6 | This is the primary launch file, but don't launch this file directly: 7 | instead launch [sim_launch.py](launch/sim_launch.py) 8 | or [topside_launch.py](launch/topside_launch.py). 9 | 10 | ### [sim_launch.py](launch/sim_launch.py) 11 | 12 | Launch ROV or AUV simulation in Gazebo (SITL). Includes bringup.py. 13 | 14 | ### [sub_launch.py](launch/sub_launch.py) 15 | 16 | Launch device nodes on the sub (HITL). 17 | 18 | ### [topside_launch.py](launch/topside_launch.py) 19 | 20 | Launch ROV or AUV topside nodes (HITL). Includes bringup.py. 21 | 22 | ### [slam_test_launch.py](launch/slam_test_launch.py) 23 | 24 | Launch a SLAM simulation in Gazebo (SITL). Does not include bringup.py. 25 | Useful for testing the SLAM software. 26 | 27 | ### [calibrate_launch.py](launch/calibrate_launch.py) 28 | 29 | Launch just the nodes required for stereo camera calibration (HITL). 30 | 31 | ### [test_launch.py](launch/test_launch.py) 32 | 33 | Launch device and test nodes on the sub. Useful for simple hardware tests. 34 | 35 | ## Video pipelines 36 | 37 | There are 2 distinct stereo pipelines right now. Ideally the SITL and HITL pipelines would share 38 | more code. This is an area of active development. 39 | 40 | ### SITL 41 | 42 | The simulations use Gazebo camera sensors to generate uncompressed 800x600 images in an ideal stereo 43 | configuration. The Gazebo camera sensor provides for distortion (used in slam_test_launch) but 44 | not rectification. The cameras generate images at 30fps but the SLAM software is able to process the 45 | images at 15-20fps. 46 | 47 | ### HITL 48 | 49 | The sub generates three H264 streams: one 1920x1080 30fps stream for the forward camera and 50 | two 1640x1232 20fps streams for the left and right down-facing cameras. The 4X higher resolution 51 | for the left and right cameras as well as the required decoding, undistortion and rectification 52 | slows the SLAM pipeline down to ~2.5fps. The SLAM software is currently slowed down to 1fps 53 | to reduce CPU usage during testing. 54 | 55 | ### orb_slam2_ros 56 | 57 | The following changes were made on the `clyde_h264_stereo` branch of 58 | [orb_slam2_ros](https://github.com/clydemcqueen/orb_slam_2_ros): 59 | * Rotate the point cloud to support down-facing cameras 60 | * Decode, undistort and rectify H264 streams in proc 61 | * Some bug fixes (don't undistort twice, use camera_info.p instead of camera_info.k for intrinsics) 62 | -------------------------------------------------------------------------------- /orca_bringup/behavior_trees/orca3_bt.xml: -------------------------------------------------------------------------------- 1 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | -------------------------------------------------------------------------------- /orca_bringup/cfg/forward_camera.ini: -------------------------------------------------------------------------------- 1 | # oST version 5.0 parameters 2 | 3 | 4 | [image] 5 | 6 | width 7 | 1920 8 | 9 | height 10 | 1080 11 | 12 | [forward_camera] 13 | 14 | camera matrix 15 | 1041.482343 0.000000 1028.461742 16 | 0.000000 1040.575457 558.582608 17 | 0.000000 0.000000 1.000000 18 | 19 | distortion 20 | 0.008547 -0.006032 0.000450 0.000662 0.000000 21 | 22 | rectification 23 | 1.000000 0.000000 0.000000 24 | 0.000000 1.000000 0.000000 25 | 0.000000 0.000000 1.000000 26 | 27 | projection 28 | 1044.289917 0.000000 1030.517941 0.000000 29 | 0.000000 1044.619019 559.189248 0.000000 30 | 0.000000 0.000000 1.000000 0.000000 31 | 32 | -------------------------------------------------------------------------------- /orca_bringup/cfg/stereo_left.ini: -------------------------------------------------------------------------------- 1 | # oST version 5.0 parameters 2 | 3 | 4 | [image] 5 | 6 | width 7 | 820 8 | 9 | height 10 | 616 11 | 12 | [stereo_left] 13 | 14 | camera matrix 15 | 646.133713 0.000000 400.893729 16 | 0.000000 646.069062 304.787569 17 | 0.000000 0.000000 1.000000 18 | 19 | distortion 20 | 0.156393 -0.261075 -0.000816 0.000350 0.000000 21 | 22 | rectification 23 | 0.998988 -0.037536 0.024776 24 | 0.038042 0.999070 -0.020283 25 | -0.023991 0.021205 0.999487 26 | 27 | projection 28 | 725.697569 0.000000 377.266621 0.000000 29 | 0.000000 725.697569 306.209999 0.000000 30 | 0.000000 0.000000 1.000000 0.000000 31 | -------------------------------------------------------------------------------- /orca_bringup/cfg/stereo_right.ini: -------------------------------------------------------------------------------- 1 | # oST version 5.0 parameters 2 | 3 | 4 | [image] 5 | 6 | width 7 | 820 8 | 9 | height 10 | 616 11 | 12 | [stereo_right] 13 | 14 | camera matrix 15 | 648.188357 0.000000 424.128217 16 | 0.000000 647.890175 309.990045 17 | 0.000000 0.000000 1.000000 18 | 19 | distortion 20 | 0.135296 -0.213864 -0.000560 0.001179 0.000000 21 | 22 | rectification 23 | 0.998066 -0.029026 0.054962 24 | 0.027879 0.999379 0.021526 25 | -0.055553 -0.019952 0.998256 26 | 27 | projection 28 | 725.697569 0.000000 377.266621 -186.354248 29 | 0.000000 725.697569 306.209999 0.000000 30 | 0.000000 0.000000 1.000000 0.000000 31 | -------------------------------------------------------------------------------- /orca_bringup/launch/calibrate_launch.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | # MIT License 4 | # 5 | # Copyright (c) 2021 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 two gscam2 nodes to calibrate two Raspberry Pi cameras. 27 | 28 | Calibration command must be run by hand in Foxy: 29 | ros2 run camera_calibration cameracalibrator --approximate 0.1 --size 11x8 --square 0.03 \ 30 | right:=/stereo/right/image_raw left:=/stereo/left/image_raw \ 31 | right_camera:=/right left_camera:=/left --no-service-check 32 | 33 | Waiting on this PR to get released into Foxy binaries: 34 | https://github.com/ros-perception/image_pipeline/pull/597 35 | """ 36 | 37 | from launch import LaunchDescription 38 | from launch_ros.actions import Node 39 | 40 | 41 | def generate_launch_description(): 42 | gscam_l = 'udpsrc port=5601 ! queue !' \ 43 | ' application/x-rtp,media=video,clock-rate=90000,encoding-name=H264 !' \ 44 | ' rtpjitterbuffer ! rtph264depay ! h264parse ! avdec_h264 ! videoconvert' 45 | gscam_r = 'udpsrc port=5602 ! queue !' \ 46 | ' application/x-rtp,media=video,clock-rate=90000,encoding-name=H264 !' \ 47 | ' rtpjitterbuffer ! rtph264depay ! h264parse ! avdec_h264 ! videoconvert' 48 | 49 | return LaunchDescription([ 50 | # Left camera 51 | Node( 52 | package='gscam2', 53 | executable='gscam_main', 54 | output='screen', 55 | name='gscam_l', 56 | namespace='stereo', 57 | parameters=[{ 58 | 'gscam_config': gscam_l, 59 | 'use_gst_timestamps': False, 60 | 'image_encoding': 'mono8', 61 | 'preroll': True, # Forces pipeline to negotiate early, catching errors 62 | 'camera_info_url': 'no_info', 63 | 'camera_name': 'stereo_left', 64 | 'frame_id': 'left_frame', 65 | }], 66 | remappings=[ 67 | ('image_raw', 'left/image_raw'), 68 | ('camera_info', 'left/camera_info'), 69 | ], 70 | ), 71 | 72 | # Right camera 73 | Node( 74 | package='gscam2', 75 | executable='gscam_main', 76 | output='screen', 77 | name='gscam_r', 78 | namespace='stereo', 79 | parameters=[{ 80 | 'gscam_config': gscam_r, 81 | 'use_gst_timestamps': False, 82 | 'image_encoding': 'mono8', 83 | 'preroll': True, # Forces pipeline to negotiate early, catching errors 84 | 'camera_info_url': 'no_info', 85 | 'camera_name': 'stereo_right', 86 | 'frame_id': 'right_frame', 87 | }], 88 | remappings=[ 89 | ('image_raw', 'right/image_raw'), 90 | ('camera_info', 'right/camera_info'), 91 | ], 92 | ), 93 | ]) 94 | -------------------------------------------------------------------------------- /orca_bringup/launch/sub_launch.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | # MIT License 4 | # 5 | # Copyright (c) 2021 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 | """Launch sub nodes.""" 26 | 27 | from launch import LaunchDescription 28 | from launch_ros.actions import Node 29 | from launch.actions import ExecuteProcess, SetEnvironmentVariable 30 | 31 | 32 | def generate_launch_description(): 33 | driver_node_params = { 34 | # 'maestro_port': 'fake', 35 | # 'read_battery': False, 36 | 'thruster_4_reverse': True, # Thruster 4 on my BlueROV2 is reversed 37 | 'timer_period_ms': 50, # Publish voltage at 20Hz 38 | } 39 | 40 | return LaunchDescription([ 41 | # To see output of `ros2 bag record` immediately 42 | SetEnvironmentVariable('RCUTILS_LOGGING_BUFFERED_STREAM', '1'), 43 | 44 | # Bag local topics 45 | # Turned off because the up1 is stopped by pulling power, and so the bags are not closed 46 | # correctly. There's a new 'ros2 bag reindex' command available in Rolling and Galactic, 47 | # but this may not make it to Foxy. 48 | # ExecuteProcess( 49 | # cmd=['ros2', 'bag', 'record', '/status', '/barometer'], 50 | # output='screen' 51 | # ), 52 | 53 | Node( 54 | package='orca_driver', 55 | executable='barometer_node', 56 | output='screen', 57 | name='barometer_node', 58 | ), 59 | 60 | Node( 61 | package='orca_driver', 62 | executable='driver_node', 63 | output='screen', 64 | name='driver_node', 65 | parameters=[driver_node_params], 66 | remappings=[ 67 | # Remap the thrust topic on the bench to avoid accidents 68 | # ('thrust', 'no_thrust'), 69 | ], 70 | ), 71 | ]) 72 | -------------------------------------------------------------------------------- /orca_bringup/launch/test_launch.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | # MIT License 4 | # 5 | # Copyright (c) 2021 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 | """Test the hardware.""" 26 | 27 | from launch import LaunchDescription 28 | from launch_ros.actions import Node 29 | 30 | 31 | def generate_launch_description(): 32 | return LaunchDescription([ 33 | Node(package='orca_driver', executable='barometer_node', output='screen', 34 | name='barometer_node'), 35 | 36 | Node(package='orca_driver', executable='driver_node', output='screen', 37 | name='driver_node', 38 | parameters=[{ 39 | 'thruster_4_reverse': True # Thruster 4 ESC is programmed incorrectly 40 | }]), 41 | 42 | Node(package='orca_driver', executable='test_node', output='screen'), 43 | ]) 44 | -------------------------------------------------------------------------------- /orca_bringup/missions/missions.txt: -------------------------------------------------------------------------------- 1 | # Launches that work 2 | 3 | # orb_slam2 test environment (minimal nodes) 4 | ros2 launch orca_bringup slam_test_launch.py gzclient:=False rviz:=True 5 | 6 | # fiducial_localizer 7 | ros2 launch orca_bringup sim_launch.py gzclient:=False rviz:=True slam:=vlam world:=ping_pong 8 | 9 | # Example mission for fiducial_localizer 10 | ros2 action send_goal /FollowWaypoints nav2_msgs/action/FollowWaypoints "{poses: [\ 11 | {header: {frame_id: 'map'}, pose: {position: {x: 4, z: -2}}},\ 12 | {header: {frame_id: 'map'}, pose: {position: {x: -4, z: -2}}},\ 13 | {header: {frame_id: 'map'}, pose: {position: {x: 4, z: -2}}},\ 14 | {header: {frame_id: 'map'}, pose: {position: {x: -4, z: -2}}},\ 15 | ]}" 16 | 17 | # orb_slam2_localizer 18 | ros2 launch orca_bringup sim_launch.py gzclient:=False rviz:=True slam:=orb world:=empty 19 | 20 | # Example mission for orb_slam2_localizer 21 | ros2 action send_goal /FollowWaypoints nav2_msgs/action/FollowWaypoints "{poses: [\ 22 | {header: {frame_id: 'map'}, pose: {position: {x: 3, y: 3, z: -2.5}}},\ 23 | {header: {frame_id: 'map'}, pose: {position: {x: -3, y: 3, z: -2.5}}},\ 24 | {header: {frame_id: 'map'}, pose: {position: {x: -3, y: -3, z: -2.5}}},\ 25 | {header: {frame_id: 'map'}, pose: {position: {x: 3, y: -3, z: -2.5}}},\ 26 | {header: {frame_id: 'map'}, pose: {position: {x: 3, y: 3, z: -2.5}}},\ 27 | {header: {frame_id: 'map'}, pose: {position: {x: -3, y: 3, z: -2.5}}},\ 28 | {header: {frame_id: 'map'}, pose: {position: {x: -3, y: -3, z: -2.5}}},\ 29 | {header: {frame_id: 'map'}, pose: {position: {x: 3, y: -3, z: -2.5}}},\ 30 | {header: {frame_id: 'map'}, pose: {position: {x: 3, y: 3, z: -2.5}}},\ 31 | ]}" 32 | 33 | # More examples 34 | 35 | # 2m box 36 | ros2 action send_goal /FollowWaypoints nav2_msgs/action/FollowWaypoints "{poses: [\ 37 | {header: {frame_id: 'map'}, pose: {position: {x: 1, y: 1}}},\ 38 | {header: {frame_id: 'map'}, pose: {position: {x: -1, y: 1}}},\ 39 | {header: {frame_id: 'map'}, pose: {position: {x: -1, y: -1}}},\ 40 | {header: {frame_id: 'map'}, pose: {position: {x: 1, y: -1}}},\ 41 | ]}" 42 | 43 | # Sideways ping pong 44 | ros2 action send_goal /FollowWaypoints nav2_msgs/action/FollowWaypoints "{poses: [\ 45 | {header: {frame_id: 'map'}, pose: {position: {y: 2, z: -2}}},\ 46 | {header: {frame_id: 'map'}, pose: {position: {y: -2, z: -2}}},\ 47 | {header: {frame_id: 'map'}, pose: {position: {y: 2, z: -2}}},\ 48 | {header: {frame_id: 'map'}, pose: {position: {y: -2, z: -2}}},\ 49 | ]}" 50 | 51 | # Up down ping pong 52 | ros2 action send_goal /FollowWaypoints nav2_msgs/action/FollowWaypoints "{poses: [\ 53 | {header: {frame_id: 'map'}, pose: {position: {y: 2, z: -4}}},\ 54 | {header: {frame_id: 'map'}, pose: {position: {y: -2, z: -2}}},\ 55 | {header: {frame_id: 'map'}, pose: {position: {y: 2, z: -4}}},\ 56 | {header: {frame_id: 'map'}, pose: {position: {y: -2, z: -2}}},\ 57 | ]}" 58 | 59 | # Descend to -2m 60 | ros2 action send_goal /FollowWaypoints nav2_msgs/action/FollowWaypoints "{poses: [\ 61 | {header: {frame_id: 'map'}, pose: {position: {z: -2}}},\ 62 | ]}" 63 | 64 | # Descend / ascend 65 | ros2 action send_goal /FollowWaypoints nav2_msgs/action/FollowWaypoints "{poses: [\ 66 | {header: {frame_id: 'map'}, pose: {position: {z: -2.5}}},\ 67 | {header: {frame_id: 'map'}, pose: {position: {z: -1.5}}},\ 68 | {header: {frame_id: 'map'}, pose: {position: {z: -2.5}}},\ 69 | {header: {frame_id: 'map'}, pose: {position: {z: -1.5}}},\ 70 | {header: {frame_id: 'map'}, pose: {position: {z: -2.5}}},\ 71 | {header: {frame_id: 'map'}, pose: {position: {z: -1.5}}},\ 72 | {header: {frame_id: 'map'}, pose: {position: {z: -2.5}}},\ 73 | {header: {frame_id: 'map'}, pose: {position: {z: -1.5}}},\ 74 | {header: {frame_id: 'map'}, pose: {position: {z: -2.5}}},\ 75 | {header: {frame_id: 'map'}, pose: {position: {z: -1.5}}},\ 76 | {header: {frame_id: 'map'}, pose: {position: {z: -2.5}}},\ 77 | {header: {frame_id: 'map'}, pose: {position: {z: -1.5}}},\ 78 | ]}" 79 | -------------------------------------------------------------------------------- /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/orca3.git 13 | https://github.com/clydemcqueen/orca3/issues 14 | 15 | Clyde McQueen 16 | 17 | ament_cmake 18 | 19 | joy_linux 20 | nav2_bringup 21 | 22 | ament_lint_auto 23 | ament_cmake_copyright 24 | ament_cmake_lint_cmake 25 | ament_cmake_xmllint 26 | 27 | 28 | ament_cmake 29 | 30 | 31 | 32 | -------------------------------------------------------------------------------- /orca_bringup/params/slam_test_params.yaml: -------------------------------------------------------------------------------- 1 | orb_slam2_stereo: 2 | ros__parameters: 3 | use_sim_time: True 4 | 5 | # Gazebo 11 camera plugin publishes both camera info and images best-effort 6 | # May need to be modified for real cameras 7 | subscribe_best_effort: True 8 | 9 | publish_pointcloud: false 10 | publish_pose: true 11 | publish_tf: false 12 | localize_only: false 13 | reset_map: false 14 | 15 | load_map: false 16 | map_file: map.bin 17 | 18 | pointcloud_frame_id: map 19 | camera_frame_id: camera_frame # TODO 20 | min_num_kf_in_map: 5 21 | 22 | ORBextractor/nFeatures: 1200 23 | ORBextractor/scaleFactor: 1.2 24 | ORBextractor/nLevels: 8 25 | ORBextractor/iniThFAST: 20 26 | ORBextractor/minThFAST: 7 27 | 28 | camera_fps: 30 29 | # Color order of the images (0: BGR, 1: RGB. It is ignored if images are grayscale) 30 | camera_rgb_encoding: true 31 | 32 | # See https://github.com/raulmur/ORB_SLAM2/issues/89 for the author's explanation of these: 33 | ThDepth: 40.0 34 | depth_map_factor: 1.0 35 | camera_baseline: 171.0 # Right camera info p[3], flip sign 36 | 37 | orb_slam2_localizer: 38 | ros__parameters: 39 | use_sim_time: True 40 | camera_frame_id: "left_camera_link" # Note: the link, not the frame 41 | publish_rate: 20 42 | -------------------------------------------------------------------------------- /orca_bringup/scripts/dump_rosout.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | # MIT License 4 | # 5 | # Copyright (c) 2021 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/worlds/empty.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/clydemcqueen/orca3/02bfefe346cf04cce13e227b98a763241fdcf534/orca_bringup/worlds/empty.pgm -------------------------------------------------------------------------------- /orca_bringup/worlds/empty_map.yaml: -------------------------------------------------------------------------------- 1 | # All marker locations are fixed (f: 1) 2 | 3 | marker_length: 0.1778 4 | markers: 5 | -------------------------------------------------------------------------------- /orca_bringup/worlds/empty_world.yaml: -------------------------------------------------------------------------------- 1 | image: empty.pgm # 400x400 2 | resolution: 0.05 3 | origin: [-10.0, -10.0, 0.0] 4 | negate: 0 5 | occupied_thresh: 0.65 6 | free_thresh: 0.196 7 | -------------------------------------------------------------------------------- /orca_bringup/worlds/ping_pong_map.yaml: -------------------------------------------------------------------------------- 1 | # Used for field tests... note that the marker_length doesn't match the simulation default 2 | # Usage: ros2 launch orca_bringup topside_launch.py slam:=vlam world:=ping_pong 3 | 4 | marker_length: 0.16 5 | markers: 6 | - id: 1 7 | u: 1 8 | f: 1 9 | xyz: [4.85, 0, -0.5] 10 | rpy: [1.5707963267948966, 2.2204460492503126e-16, -1.5707963267948966] 11 | - id: 2 12 | u: 1 13 | f: 1 14 | xyz: [-4.85, 0, -0.5] 15 | rpy: [1.5707963267948968, 2.2204460492503126e-16, 1.5707963267948968] 16 | -------------------------------------------------------------------------------- /orca_description/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | project(orca_description) 3 | 4 | find_package(ament_cmake_auto REQUIRED) 5 | ament_auto_find_build_dependencies() 6 | 7 | #============= 8 | # Run Xacro to turn orca.urdf.xacro into several urdf files based on simulation mode 9 | # Source file is in ${CMAKE_CURRENT_SOURCE_DIR}/xacro/orca.urdf.xacro 10 | # Generated files are in ${CMAKE_CURRENT_BINARY_DIR}/urdf 11 | #============= 12 | 13 | file(MAKE_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}/urdf") 14 | 15 | foreach(SIM_MODE IN ITEMS hw6 slam_test hw7) 16 | set(URDF_FILE "${CMAKE_CURRENT_BINARY_DIR}/urdf/${SIM_MODE}.urdf") 17 | add_custom_command( 18 | OUTPUT ${URDF_FILE} 19 | COMMAND ${PYTHON_EXECUTABLE} "/opt/ros/$ENV{ROS_DISTRO}/bin/xacro" 20 | "${CMAKE_CURRENT_SOURCE_DIR}/xacro/orca.urdf.xacro" "SIM_MODE:=${SIM_MODE}" ">" "${URDF_FILE}" 21 | DEPENDS xacro/orca.urdf.xacro 22 | COMMENT "Generate ${URDF_FILE}" 23 | VERBATIM 24 | ) 25 | add_custom_target(generate_${SIM_MODE}_urdf ALL DEPENDS ${URDF_FILE}) 26 | endforeach() 27 | 28 | # Load & run linters listed in package.xml 29 | if(BUILD_TESTING) 30 | find_package(ament_lint_auto REQUIRED) 31 | ament_lint_auto_find_test_dependencies() 32 | endif() 33 | 34 | ament_auto_package(INSTALL_TO_SHARE "${CMAKE_CURRENT_BINARY_DIR}/urdf") -------------------------------------------------------------------------------- /orca_description/README.md: -------------------------------------------------------------------------------- 1 | # Orca3 Description 2 | 3 | Robot description file for a modified BlueRobotics BlueROV2. 4 | 5 | Colcon will run [Xacro](https://index.ros.org/r/xacro/github-ros-xacro/) to create `*.urdf` from `orca.urdf.xacro`. 6 | To convert manually: 7 | 8 | ~~~ 9 | export ROS_DISTRO= 10 | 11 | # Install Xacro 12 | sudo apt install ros-$ROS_DISTRO-xacro 13 | 14 | # Source setup.bash 15 | source /opt/ros/$ROS_DISTRO/setup.bash 16 | 17 | # Run in orca_description/urdf directory 18 | cd src/orca3/orca_description/urdf 19 | python3 /opt/ros/$ROS_DISTRO/bin/xacro orca.urdf.xacro SIM_MODE:=hw6 > hw6.urdf 20 | python3 /opt/ros/$ROS_DISTRO/bin/xacro orca.urdf.xacro SIM_MODE:=slam_test > slam_test.urdf 21 | python3 /opt/ros/$ROS_DISTRO/bin/xacro orca.urdf.xacro SIM_MODE:=hw7 > hw7.urdf 22 | ~~~ -------------------------------------------------------------------------------- /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/orca3.git 13 | https://github.com/clydemcqueen/orca3/issues 14 | 15 | Clyde McQueen 16 | 17 | ament_cmake 18 | xacro 19 | 20 | ament_lint_auto 21 | 22 | ament_cmake_copyright 23 | ament_cmake_lint_cmake 24 | ament_cmake_xmllint 25 | 26 | 27 | ament_cmake 28 | 29 | 30 | -------------------------------------------------------------------------------- /orca_description/scad/diagrams/stereo_cam_fov.scad: -------------------------------------------------------------------------------- 1 | // Show how various cameras and lenses affect field of view 2 | 3 | // Size of AUV 4 | auv_size = [338, 457, 250]; 5 | 6 | // Down-facing camera tube radius 7 | cam_r = 55; 8 | 9 | // Height of bracket holding the camera 10 | bracket_standoff = 5; 11 | 12 | // Cameras are mounted on the outside of the AUV 13 | half_baseline = auv_size.x / 2 + cam_r / 2 + bracket_standoff; 14 | l_cam_pose = [- half_baseline, 0, 0]; 15 | r_cam_pose = [half_baseline, 0, 0]; 16 | 17 | // Draw the AUV 18 | module auv() { 19 | color("#25cbf5", 0.4) translate([- auv_size.x / 2, - auv_size.y / 2, 0]) cube(auv_size); 20 | } 21 | 22 | // Draw a camera 23 | module cam() { 24 | color("#f200ff", 0.4) cylinder(h = 150, r = cam_r / 2); 25 | } 26 | 27 | // Draw the field of view 28 | module fov(horiz_fov, aspect_ratio, landscape, z) { 29 | x2 = tan(horiz_fov / 2) * z; 30 | y2 = tan(horiz_fov / 2) * z * (aspect_ratio == "4:3" ? 3 / 4 : 9 / 16); 31 | rotation = (landscape ? 0 : 90); 32 | points = [[0, 0, 0], [- x2, - y2, - z], [- x2, y2, - z], [x2, y2, - z], [x2, - y2, - z]]; 33 | faces = [[0, 1, 2], [0, 2, 3], [0, 3, 4], [0, 4, 1], [1, 2, 3, 4]]; 34 | color("grey", 0.3) rotate([0, 0, rotation]) polyhedron(points = points, faces = faces); 35 | } 36 | 37 | // Draw the AUV with the cameras and the overlapping fields of view 38 | // The default projection distance is 1.5m 39 | module assembly(horiz_fov = 62.2, aspect_ratio = "16:9", landscape = true, z = 1500) { 40 | auv(); 41 | translate(l_cam_pose) cam(); 42 | translate(r_cam_pose) cam(); 43 | translate(l_cam_pose) fov(horiz_fov, aspect_ratio, landscape, z); 44 | translate(r_cam_pose) fov(horiz_fov, aspect_ratio, landscape, z); 45 | } 46 | 47 | // These are all Raspicam v2's: 48 | 49 | // Standard lens 50 | assembly(horiz_fov = 62); 51 | 52 | translate([0, 3000, 0]) assembly(horiz_fov = 80); 53 | 54 | translate([0, 6000, 0]) assembly(horiz_fov = 95); 55 | 56 | // BlueRobotics wide angle lens 57 | translate([0, 9000, 0]) assembly(horiz_fov = 110); 58 | -------------------------------------------------------------------------------- /orca_driver/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | project(orca_driver) 3 | 4 | if(NOT CMAKE_CXX_STANDARD) 5 | set(CMAKE_CXX_STANDARD 14) 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 | # Emulate colcon by providing paths to other projects in the workspace 13 | if($ENV{CLION_IDE}) 14 | message("Running inside CLion") 15 | set(br_ms5837_DIR "${PROJECT_SOURCE_DIR}/../../../install/br_ms5837/share/br_ms5837/cmake") 16 | set(orca_msgs_DIR "${PROJECT_SOURCE_DIR}/../../../install/orca_msgs/share/orca_msgs/cmake") 17 | set(ros2_shared_DIR "${PROJECT_SOURCE_DIR}/../../../install/ros2_shared/share/ros2_shared/cmake") 18 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DRUN_INSIDE_CLION") 19 | endif() 20 | 21 | find_package(ament_cmake_auto REQUIRED) 22 | ament_auto_find_build_dependencies() 23 | 24 | # Hack for MRAA 25 | set(mraa_INCLUDE_DIRS "/usr/include") 26 | set(mraa_FOUND true) 27 | 28 | # x86 vs ARM -- this works on Linux 29 | if(CMAKE_HOST_SYSTEM_PROCESSOR MATCHES "aarch64") 30 | message(STATUS "Detected aarch64") 31 | set(mraa_LIBRARIES "/usr/lib/aarch64-linux-gnu/libmraa.so") 32 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DPROCESSOR_AARCH64") 33 | else() 34 | message(STATUS "Default to x86_64") 35 | set(mraa_LIBRARIES "/usr/lib/x86_64-linux-gnu/libmraa.so") 36 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DPROCESSOR_X86_64") 37 | endif() 38 | 39 | include_directories(include) 40 | 41 | ament_auto_add_executable(barometer_node src/barometer_node.cpp) 42 | target_link_libraries(barometer_node mraa) 43 | 44 | ament_auto_add_executable(driver_node src/driver_node.cpp src/maestro.cpp) 45 | target_link_libraries(driver_node mraa) 46 | 47 | ament_auto_add_executable(test_node src/test_node.cpp) 48 | 49 | # This will load & run linters listed in package.xml 50 | if(BUILD_TESTING) 51 | find_package(ament_lint_auto REQUIRED) 52 | ament_lint_auto_find_test_dependencies() 53 | endif() 54 | 55 | # Install scripts 56 | install( 57 | PROGRAMS # PROGRAMS sets execute bits, FILES clears them 58 | scripts/dance_node.py 59 | scripts/start_driver.sh 60 | scripts/start_fcam.sh 61 | scripts/thrust_curve_node.py 62 | DESTINATION lib/${PROJECT_NAME} 63 | ) 64 | 65 | ament_auto_package(INSTALL_TO_SHARE cfg) 66 | -------------------------------------------------------------------------------- /orca_driver/COLCON_IGNORE: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/clydemcqueen/orca3/02bfefe346cf04cce13e227b98a763241fdcf534/orca_driver/COLCON_IGNORE -------------------------------------------------------------------------------- /orca_driver/README.md: -------------------------------------------------------------------------------- 1 | # Orca3 Hardware Drivers 2 | 3 | This package provides an interface between the BlueROV2 hardware and the Orca3 software. 4 | 5 | > Status: ROV operation has been lightly tested with a modified BlueROV2 running Foxy. 6 | > It has not been tested on Galactic. YMMV. 7 | 8 | ## Hardware 9 | 10 | I made the following modifications to my stock 2017 BlueROV2: 11 | * Replaced the Raspberry Pi camera with the BlueRobotics USB low-light camera 12 | * Upgraded to the BlueRobotics R3 ESCs 13 | * Upgraded to the slim tether 14 | * Replaced the Raspberry Pi and the Pixhawk with an x86 15 | [UP Board](https://up-board.org/up/specifications/) and a 16 | [Pololu Maestro 18](https://www.pololu.com/product/1354) 17 | * A simple voltage divider provides a voltage signal from the battery (0-17V) 18 | * There is no current sensor 19 | * There is no IMU 20 | 21 | ## Software Installation 22 | 23 | Below I've outlined rough instructions to install the required software on the UP board. 24 | 25 | TODO(clyde): replace these instructions with a Dockerfile 26 | 27 | ### Install Ubuntu 20.04 LTS Server and the UP kernel 28 | 29 | Install Ubuntu 20.04 LTS Server, then the UP kernel, per these instructions: 30 | https://github.com/up-board/up-community/wiki/Ubuntu_20.04 31 | 32 | Install UP extras and i2c tools: 33 | ~~~ 34 | sudo apt install upboard-extras i2c-tools 35 | ~~~ 36 | 37 | Add `$USER` to several groups to provide access to the hardware: 38 | ~~~ 39 | sudo usermod -a -G gpio ${USER} 40 | sudo usermod -a -G leds ${USER} 41 | sudo usermod -a -G i2c ${USER} 42 | sudo usermod -a -G spi ${USER} 43 | sudo usermod -a -G dialout ${USER} 44 | sudo usermod -a -G video ${USER} 45 | ~~~ 46 | 47 | ### Install MRAA 48 | 49 | MRAA provides an abstraction layer for the hardware: 50 | 51 | ~~~ 52 | sudo add-apt-repository ppa:mraa/mraa 53 | sudo apt-get update 54 | sudo apt-get install libmraa2 libmraa-dev libmraa-java python3-mraa mraa-tools 55 | ~~~ 56 | 57 | ### Install Chrony 58 | 59 | The ROS2 nodes are split across the UP board and a topside computer. 60 | I recommend keeping the clocks in sync. 61 | I've had good luck with [Chrony](https://chrony.tuxfamily.org/doc/3.5/installation.html). 62 | Have the UP board use the topside computer as a reference. 63 | 64 | ### Install Gstreamer 65 | 66 | This is the minimal Gstreamer install: 67 | 68 | ~~~ 69 | sudo apt install gstreamer1.0-tools gstreamer1.0-plugins-good gstreamer1.0-plugins-bad 70 | ~~~ 71 | 72 | ### Install ROS2 Foxy 73 | 74 | Install ROS2 Foxy 75 | [using these instructions](https://index.ros.org/doc/ros2/Installation/Foxy/Linux-Install-Debians/). 76 | Use the `ros-foxy-ros-base` option to avoid installing the GUI tools. 77 | 78 | Install ROS2 development tools 79 | [using these instructions](https://index.ros.org/doc/ros2/Installation/Foxy/Linux-Development-Setup/). 80 | Stop after installing the development tools (before "Get ROS 2 code"). 81 | 82 | Initialize and update rosdep: 83 | ~~~ 84 | sudo rosdep init 85 | rosdep update 86 | ~~~ 87 | 88 | ### Install and build Orca3 89 | 90 | ~~~ 91 | mkdir -p ~/ros2/orca3_ws/src 92 | cd ~/ros2/orca3_ws/src 93 | git clone https://github.com/clydemcqueen/BlueRobotics_MS5837_Library.git -b mraa_ros2 94 | git clone https://github.com/ptrmu/ros2_shared.git 95 | git clone https://github.com/clydemcqueen/orca3.git 96 | # Build orca_bringup, orca_driver and orca_msgs 97 | rm orca3/orca_driver/COLCON_IGNORE 98 | touch orca3/orca_base/COLCON_IGNORE 99 | touch orca3/orca_description/COLCON_IGNORE 100 | touch orca3/orca_gazebo/COLCON_IGNORE 101 | touch orca3/orca_localize/COLCON_IGNORE 102 | touch orca3/orca_nav2/COLCON_IGNORE 103 | touch orca3/orca_shared/COLCON_IGNORE 104 | touch orca3/orca_topside/COLCON_IGNORE 105 | cd ~/ros2/orca3_ws 106 | source /opt/ros/foxy/setup.bash 107 | rosdep install --from-paths . --ignore-src 108 | colcon build 109 | source install/local_setup.bash 110 | ~~~ 111 | 112 | ## Launch on boot 113 | 114 | ~~~ 115 | # Launch ROS nodes: 116 | sudo cp ~/ros2/orca3_ws/src/orca3/orca_driver/scripts/orca_driver.service /lib/systemd/system 117 | sudo systemctl enable orca_driver.service 118 | 119 | # Launch gstreamer pipeline: 120 | sudo cp ~/ros2/orca3_ws/src/orca3/orca_driver/scripts/orca_fcam.service /lib/systemd/system 121 | sudo systemctl enable orca_fcam.service 122 | ~~~ -------------------------------------------------------------------------------- /orca_driver/cfg/maestro.cfg: -------------------------------------------------------------------------------- 1 | 2 | 3 | false 4 | USB_DUAL_PORT 5 | 9600 6 | 0 7 | false 8 | 12 9 | 0 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 36 | 37 | 38 | 39 | 40 | 41 | -------------------------------------------------------------------------------- /orca_gazebo/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | orca_gazebo 6 | 0.5.0 7 | Orca simulation environment 8 | 9 | Clyde McQueen 10 | MIT 11 | 12 | https://github.com/clydemcqueen/orca3.git 13 | https://github.com/clydemcqueen/orca3/issues 14 | 15 | Clyde McQueen 16 | 17 | ament_cmake 18 | 19 | ament_lint_auto 20 | 21 | 22 | ament_lint_common 23 | 24 | gazebo_dev 25 | gazebo_ros 26 | geometry_msgs 27 | orca_description 28 | orca_msgs 29 | orca_shared 30 | rclcpp 31 | ros2_shared 32 | rclpy 33 | 34 | 35 | gazebo_ros_pkgs 36 | sim_fiducial 37 | 38 | 39 | ament_cmake 40 | 41 | 42 | 43 | -------------------------------------------------------------------------------- /orca_gazebo/scripts/find_transform.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | # MIT License 4 | # 5 | # Copyright (c) 2021 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 | """Find cube transforms that satisfy xform @ input = output --or-- input @ xform = output.""" 26 | 27 | import math 28 | 29 | import numpy as np 30 | import transformations as xf 31 | 32 | pi = math.pi 33 | pi2 = math.pi / 2 34 | 35 | 36 | # Pretty-print Euler [r, p, y] 37 | def e_to_str(e): 38 | return '[r={: 06.5f}, p={: 06.5f}, y={: 06.5f}]'.format(e[0], e[1], e[2]) 39 | 40 | 41 | # Pretty-print 3x3 matrix 42 | def m_to_str(m: np.ndarray): 43 | r = m[:3, :3] 44 | return np.array2string(r, suppress_small=True) 45 | 46 | 47 | # Generate all possible cube rotations using Euler angles, there will be duplicates 48 | def gen_all_e_rotations(): 49 | for roll in [0, pi2, pi, -pi2]: 50 | for pitch in [0, pi2, pi, -pi2]: 51 | for yaw in [0, pi2, pi, -pi2]: 52 | yield roll, pitch, yaw 53 | 54 | 55 | # Generate all 24 unique cube rotations 56 | def gen_unique_m_rotations(): 57 | unique = [] 58 | for r in list(gen_all_e_rotations()): 59 | m = xf.euler_matrix(r[0], r[1], r[2]) 60 | found = False 61 | for candidate in unique: 62 | if np.allclose(m, candidate): 63 | found = True 64 | break 65 | if not found: 66 | unique.append(m) 67 | 68 | assert len(unique) == 24 69 | return unique 70 | 71 | 72 | # Try to turn inputs into outputs 73 | def solve(m_xforms, e_inputs, e_outputs): 74 | print('looking for solution...') 75 | for m_xform in m_xforms: 76 | pre_count = 0 77 | pose_count = 0 78 | 79 | for e_input, e_output in zip(e_inputs, e_outputs): 80 | m_input = xf.euler_matrix(e_input[0], e_input[1], e_input[2]) 81 | m_output = xf.euler_matrix(e_output[0], e_output[1], e_output[2]) 82 | m_left = m_xform @ m_input 83 | m_right = m_input @ m_xform 84 | 85 | if np.allclose(m_left, m_output): 86 | pre_count += 1 87 | if np.allclose(m_right, m_output): 88 | pose_count += 1 89 | 90 | if pre_count == len(e_inputs): 91 | print('found pre multiply solution:') 92 | print(m_to_str(m_xform)) 93 | if pose_count == len(e_inputs): 94 | print('found post multiply solution:') 95 | print(m_to_str(m_xform)) 96 | 97 | 98 | def main(): 99 | m_xforms = gen_unique_m_rotations() 100 | e_inputs = [ 101 | [0, -1.5707963267948966, 0], 102 | [0, -1.5707963267948966, -1.5707963267948966], 103 | [0, -1.5707963267948966, 1.5707963267948966], 104 | [0, -1.5707963267948966, 3.141592653589793] 105 | ] 106 | e_outputs = [ 107 | [1.5707963267948966, 0, -1.5707963267948966], 108 | [-1.5707963267948966, 3.141592653589793, 0], 109 | [1.5707963267948966, 0, 0], 110 | [1.5707963267948966, 0, 1.5707963267948966] 111 | ] 112 | solve(m_xforms, e_inputs, e_outputs) 113 | 114 | 115 | main() 116 | -------------------------------------------------------------------------------- /orca_gazebo/scripts/reliable_odom.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | # MIT License 4 | # 5 | # Copyright (c) 2021 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 | """Subscribe to /best_effort and republish /reliable.""" 26 | 27 | # This is a quick hack around https://github.com/ros-visualization/rqt/issues/187 28 | 29 | import nav_msgs.msg 30 | import rclpy 31 | import rclpy.node 32 | import rclpy.qos 33 | 34 | 35 | # TODO Move to C++ as templated Node, params for QoS, param for overwrite stamp 36 | class ReliableOdomNode(rclpy.node.Node): 37 | 38 | def __init__(self): 39 | super().__init__('reliable_odom') 40 | 41 | self._sub = self.create_subscription(nav_msgs.msg.Odometry, 42 | 'best_effort', 43 | self.callback, 44 | rclpy.qos.qos_profile_sensor_data) 45 | 46 | self._pub = self.create_publisher(nav_msgs.msg.Odometry, 47 | 'reliable', 48 | rclpy.qos.qos_profile_services_default) 49 | 50 | def callback(self, msg: nav_msgs.msg.Odometry) -> None: 51 | # Overwrite stamp, allows node to be used with wall clock even during simulations 52 | msg.header.stamp = self.get_clock().now().to_msg() 53 | self._pub.publish(msg) 54 | 55 | 56 | def main(args=None): 57 | rclpy.init(args=args) 58 | node = ReliableOdomNode() 59 | 60 | try: 61 | rclpy.spin(node) 62 | except KeyboardInterrupt: 63 | node.get_logger().info('ctrl-C detected, shutting down') 64 | finally: 65 | node.destroy_node() 66 | rclpy.shutdown() 67 | 68 | 69 | if __name__ == '__main__': 70 | main() 71 | -------------------------------------------------------------------------------- /orca_gazebo/scripts/seafloor_marker.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | # MIT License 4 | # 5 | # Copyright (c) 2021 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 | """Publish a marker representing the seafloor for rviz2.""" 26 | 27 | import rclpy 28 | import rclpy.node 29 | import rclpy.qos 30 | from visualization_msgs.msg import Marker 31 | 32 | 33 | class SeafloorMarkerNode(rclpy.node.Node): 34 | 35 | def __init__(self): 36 | super().__init__('seafloor_marker') 37 | self._pub = self.create_publisher(Marker, 38 | 'seafloor_marker', 39 | rclpy.qos.qos_profile_services_default) 40 | self._timer = self.create_timer(1.0, self.timer_callback) 41 | print('seafloor_marker ready') 42 | 43 | def timer_callback(self): 44 | msg = Marker() 45 | msg.header.stamp = self.get_clock().now().to_msg() 46 | msg.header.frame_id = 'map' 47 | msg.ns = 'seafloor' 48 | msg.id = 0 49 | msg.type = Marker.CUBE 50 | msg.action = Marker.ADD 51 | msg.pose.position.x = 0. 52 | msg.pose.position.y = 0. 53 | msg.pose.position.z = -4. # Must match build_world.py 54 | msg.scale.x = 10. 55 | msg.scale.y = 10. 56 | msg.scale.z = 0.001 57 | msg.color.a = 1.0 58 | msg.color.r = 0.0 59 | msg.color.g = 0.0 60 | msg.color.b = 0.3 61 | self._pub.publish(msg) 62 | 63 | 64 | def main(args=None): 65 | rclpy.init(args=args) 66 | node = SeafloorMarkerNode() 67 | 68 | try: 69 | rclpy.spin(node) 70 | except KeyboardInterrupt: 71 | node.get_logger().info('ctrl-C detected, shutting down') 72 | finally: 73 | node.destroy_node() 74 | rclpy.shutdown() 75 | 76 | 77 | if __name__ == '__main__': 78 | main() 79 | -------------------------------------------------------------------------------- /orca_localize/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | project(orca_localize) 3 | 4 | if(NOT CMAKE_CXX_STANDARD) 5 | set(CMAKE_CXX_STANDARD 14) 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 | # Emulate colcon in CLion 13 | if($ENV{CLION_IDE}) 14 | find_package(fastrtps_cmake_module REQUIRED) 15 | set(FastRTPS_INCLUDE_DIR "/opt/ros/foxy/include") 16 | set(FastRTPS_LIBRARY_RELEASE "/opt/ros/foxy/lib/libfastrtps.so") 17 | set(fiducial_vlam_msgs_DIR "${PROJECT_SOURCE_DIR}/../../../install/fiducial_vlam_msgs/share/fiducial_vlam_msgs/cmake") 18 | set(orca_description_DIR "${PROJECT_SOURCE_DIR}/../../../install/orca_description/share/orca_description/cmake") 19 | set(orca_msgs_DIR "${PROJECT_SOURCE_DIR}/../../../install/orca_msgs/share/orca_msgs/cmake") 20 | set(orca_shared_DIR "${PROJECT_SOURCE_DIR}/../../../install/orca_shared/share/orca_shared/cmake") 21 | set(ros2_shared_DIR "${PROJECT_SOURCE_DIR}/../../../install/ros2_shared/share/ros2_shared/cmake") 22 | set(ukf_DIR "${PROJECT_SOURCE_DIR}/../../../install/ukf/share/ukf/cmake") 23 | endif() 24 | 25 | find_package(ament_cmake_auto REQUIRED) 26 | ament_auto_find_build_dependencies() 27 | 28 | ament_auto_add_executable(camera_info_publisher src/camera_info_publisher.cpp) 29 | 30 | ament_auto_add_executable(fiducial_localizer src/fiducial_localizer.cpp) 31 | 32 | ament_auto_add_executable(orb_slam2_localizer src/orb_slam2_localizer.cpp) 33 | 34 | # Load & run linters listed in package.xml 35 | if(BUILD_TESTING) 36 | find_package(ament_lint_auto REQUIRED) 37 | ament_lint_auto_find_test_dependencies() 38 | endif() 39 | 40 | ament_auto_package() -------------------------------------------------------------------------------- /orca_localize/README.md: -------------------------------------------------------------------------------- 1 | # Orca3 Localization 2 | 3 | Perhaps the biggest challenge in underwater robotics is localization: 4 | * radio signals (e.g., GPS) are absorbed rapidly in water, particularly seawater 5 | * you often can't see the seafloor 6 | * sonar is useful, but it is typically big, expensive and power-hungry 7 | ([although this is improving](https://bluerobotics.com/product-category/sensors-sonars-cameras/sonar/)) 8 | * inexpensive IMUs aren't good for more than a few meters, and the good ones are big and expensive 9 | 10 | Orca3 supports two SLAM (Simultaneous Localization And Mapping) systems: 11 | * [fiducial_vlam](https://github.com/ptrmu/fiducial_vlam) looks for ArUco markers in the environment 12 | using the forward-facing camera. This is useful for test environments like pools. 13 | * [orb_slam2_ros](https://github.com/clydemcqueen/orb_slam_2_ros/tree/clyde_rotate_pointcloud) looks for ORB features 14 | along the seafloor. It is configured to use a custom down-facing stereo camera. This works well 15 | in simulation, but it has not been tested in the field. 16 | 17 | ## FiducialLocalizer 18 | 19 | The `FiducialLocalizer` node does the following: 20 | * subscribe to `/fiducial_observations` (a list of marker observations) and `/forward_camera/camera_pose` 21 | (the pose of the camera generated by solvePnP or GTSAM) 22 | * reject poses generated by markers that are too far away (~2m). This avoids unstable solutions. 23 | * calculate the map->base_link transform 24 | * publish the map->odom transform 25 | 26 | No transforms are published until a marker is observed. After the first observation transforms are 27 | published at the target rate with updated timestamps, even if they are old. 28 | 29 | ### Parameters 30 | 31 | | Parameter | Type | Default | Notes | 32 | |---|---|---|---| 33 | | map_frame_id | string | map | Map frame id | 34 | | odom_frame_id | string | odom | Odom frame id | 35 | | camera_frame_id | string | camera_frame | Camera frame id | 36 | | publish_rate | int | 20 | How often to publish transforms, Hz | 37 | | wait_for_transform_ms | int | 500 | How long to wait for the odom->base transform, ms | 38 | | transform_expiration_ms | int | 1000 | How far ahead to post-date the map->odom transform, ms | 39 | | good_pose_distance | double | 2.0 | Maximum distance for a good marker observation, m | 40 | 41 | ## OrbSlam2Localizer 42 | 43 | The `OrbSlam2Localizer` node does the following: 44 | * subscribe to `/camera_pose` 45 | * calculate the map->base_link transform 46 | * publish the map->odom transform 47 | 48 | The systems starts as soon as there's a good pose. The simulated world has a depth of 4m and the 49 | mapping starts immediately. 50 | 51 | ### Parameters 52 | 53 | | Parameter | Type | Default | Notes | 54 | |---|---|---|---| 55 | | map_frame_id | string | map | Map frame id | 56 | | odom_frame_id | string | odom | Odom frame id | 57 | | camera_frame_id | string | camera_frame | Camera frame id | 58 | | publish_rate | int | 20 | How often to publish transforms, Hz | 59 | | transform_expiration_ms | int | 1000 | How far ahead to post-date the map->odom transform, ms | 60 | 61 | -------------------------------------------------------------------------------- /orca_localize/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | orca_localize 6 | 0.5.0 7 | Orca localizers 8 | 9 | Clyde McQueen 10 | MIT 11 | 12 | https://github.com/clydemcqueen/orca3.git 13 | https://github.com/clydemcqueen/orca3/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 | cv_bridge 36 | fiducial_vlam_msgs 37 | geometry_msgs 38 | image_geometry 39 | nav_msgs 40 | orca_msgs 41 | orca_shared 42 | rclcpp 43 | ros2_shared 44 | sensor_msgs 45 | tf2 46 | tf2_geometry_msgs 47 | tf2_ros 48 | 49 | 50 | ament_cmake 51 | 52 | 53 | -------------------------------------------------------------------------------- /orca_msgs/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | project(orca_msgs) 3 | 4 | if(NOT CMAKE_CXX_STANDARD) 5 | set(CMAKE_CXX_STANDARD 14) 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 | # Emulate colcon in CLion 13 | if($ENV{CLION_IDE}) 14 | find_package(fastrtps_cmake_module REQUIRED) 15 | set(FastRTPS_INCLUDE_DIR "/opt/ros/foxy/include") 16 | set(FastRTPS_LIBRARY_RELEASE "/opt/ros/foxy/lib/libfastrtps.so") 17 | endif() 18 | 19 | # Debugging: set _dump_all_variables to true 20 | set(_dump_all_variables false) 21 | if(_dump_all_variables) 22 | get_cmake_property(_variable_names VARIABLES) 23 | list(SORT _variable_names) 24 | foreach(_variable_name ${_variable_names}) 25 | message(STATUS "${_variable_name}=${${_variable_name}}") 26 | endforeach() 27 | endif() 28 | 29 | find_package(ament_cmake_auto REQUIRED) 30 | ament_auto_find_build_dependencies() 31 | 32 | # Generate ROS messages 33 | rosidl_generate_interfaces( 34 | orca_msgs 35 | msg/Barometer.msg 36 | msg/CameraTilt.msg 37 | msg/Depth.msg 38 | msg/Effort.msg 39 | msg/Lights.msg 40 | msg/Motion.msg 41 | msg/Pid.msg 42 | msg/Status.msg 43 | msg/Teleop.msg 44 | msg/Thrust.msg 45 | DEPENDENCIES geometry_msgs std_msgs 46 | ) 47 | 48 | ament_auto_package() -------------------------------------------------------------------------------- /orca_msgs/README.md: -------------------------------------------------------------------------------- 1 | # Orca3 messages 2 | 3 | | Message | Description | Publisher(s) | Subscriber(s) | 4 | |-----|--------|------|-----| 5 | | Barometer | Pressure and temperature | OrcaBarometerPlugin, BarometerNode | BaseController | 6 | | CameraTilt | BlueROV2 camera tilt | TeleopNode | DriverNode | 7 | | Depth | ENU depth in meters (+ is above the surface, - is below the surface) | BaseController | | 8 | | Effort | Force and torque divided by maximum force and torque (range \[-1, 1\]) | 9 | | Lights | BlueROV2 lights on / off | TeleopNode | DriverNode | 10 | | Motion | Output of motion model | BaseController | | 11 | | Pid | Output of PID controller | BaseController | | 12 | | Status | Hardware status | OrcaThrusterPlugin, DriverNode | | 13 | | Teleop | Arm or disarm the controller | TeleopNode | BaseController | 14 | | Thrust | Thruster pwm values | BaseController | OrcaThrusterPlugin, DriverNode | 15 | -------------------------------------------------------------------------------- /orca_msgs/msg/Barometer.msg: -------------------------------------------------------------------------------- 1 | std_msgs/Header header 2 | 3 | float64 pressure # Pascals 4 | float64 pressure_variance 5 | 6 | float64 temperature # Celsius -------------------------------------------------------------------------------- /orca_msgs/msg/CameraTilt.msg: -------------------------------------------------------------------------------- 1 | std_msgs/Header header 2 | 3 | uint16 TILT_45_UP=1100 4 | uint16 TILT_0=1500 5 | uint16 TILT_45_DOWN=1900 6 | uint16 camera_tilt_pwm 7 | -------------------------------------------------------------------------------- /orca_msgs/msg/Depth.msg: -------------------------------------------------------------------------------- 1 | std_msgs/Header header 2 | 3 | float64 z # m, 0 at the surface 4 | float64 z_variance 5 | -------------------------------------------------------------------------------- /orca_msgs/msg/Effort.msg: -------------------------------------------------------------------------------- 1 | geometry_msgs/Vector3 force 2 | geometry_msgs/Vector3 torque -------------------------------------------------------------------------------- /orca_msgs/msg/Lights.msg: -------------------------------------------------------------------------------- 1 | std_msgs/Header header 2 | 3 | uint16 LIGHTS_OFF=1100 4 | uint16 LIGHTS_FULL=1900 5 | uint16 brightness_pwm 6 | -------------------------------------------------------------------------------- /orca_msgs/msg/Motion.msg: -------------------------------------------------------------------------------- 1 | std_msgs/Header header 2 | 3 | # Inputs 4 | geometry_msgs/Twist cmd_vel 5 | float64 baro_z 6 | float64 dt 7 | 8 | # Outputs 9 | # Pose is in the world frame; acceleration, velocity, force and effort are in the robot frame 10 | 11 | geometry_msgs/Accel accel_model # Accel to move from v0 to v1 12 | geometry_msgs/Accel accel_drag # Accel to counteract drag 13 | geometry_msgs/Accel accel_hover # Accel to counteract buoyancy 14 | geometry_msgs/Accel accel_pid # Output of PID controller(s) 15 | geometry_msgs/Accel accel_total 16 | 17 | geometry_msgs/Twist vel 18 | 19 | geometry_msgs/Pose pose 20 | 21 | geometry_msgs/Wrench force 22 | orca_msgs/Effort effort # Force normalized to [-1, 1] 23 | -------------------------------------------------------------------------------- /orca_msgs/msg/Pid.msg: -------------------------------------------------------------------------------- 1 | std_msgs/Header header 2 | 3 | float64 target 4 | 5 | float64 dt 6 | float64 state 7 | float64 error 8 | float64 prev_error 9 | float64 integral 10 | float64 p_term 11 | float64 i_term 12 | float64 d_term 13 | float64 result 14 | -------------------------------------------------------------------------------- /orca_msgs/msg/Status.msg: -------------------------------------------------------------------------------- 1 | std_msgs/Header header 2 | 3 | float64 thrust_msg_lag 4 | float64 voltage 5 | 6 | uint32 STATUS_NONE=0 # Status not available (e.g., driver crashed) 7 | uint32 STATUS_READY=200 # Not receiving /thrust messages 8 | uint32 STATUS_RUNNING=201 # Receving /thrust messages 9 | uint32 STATUS_ABORT_HARDWARE=500 # Abort dive: hardware failure 10 | uint32 STATUS_ABORT_LOW_BATTERY=501 # Abort dive: low battery 11 | uint32 STATUS_ABORT_LEAK=502 # Abort dive: leak detected 12 | uint32 status 13 | -------------------------------------------------------------------------------- /orca_msgs/msg/Teleop.msg: -------------------------------------------------------------------------------- 1 | std_msgs/Header header 2 | 3 | bool armed 4 | bool hover_thrust 5 | bool pid_enabled -------------------------------------------------------------------------------- /orca_msgs/msg/Thrust.msg: -------------------------------------------------------------------------------- 1 | std_msgs/Header header 2 | 3 | uint16 THRUST_FULL_REV=1100 4 | uint16 THRUST_STOP=1500 5 | uint16 THRUST_FULL_FWD=1900 6 | uint16[] thrust 7 | -------------------------------------------------------------------------------- /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/orca3.git 13 | https://github.com/clydemcqueen/orca3/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.5) 2 | project(orca_nav2) 3 | 4 | if(NOT CMAKE_CXX_STANDARD) 5 | set(CMAKE_CXX_STANDARD 14) 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 | # Emulate colcon in CLion 13 | if($ENV{CLION_IDE}) 14 | find_package(fastrtps_cmake_module REQUIRED) 15 | set(FastRTPS_INCLUDE_DIR "/opt/ros/foxy/include") 16 | set(FastRTPS_LIBRARY_RELEASE "/opt/ros/foxy/lib/libfastrtps.so") 17 | set(orca_shared_DIR "${PROJECT_SOURCE_DIR}/../../../install/orca_shared/share/orca_shared/cmake") 18 | endif() 19 | 20 | find_package(ament_cmake_auto REQUIRED) 21 | ament_auto_find_build_dependencies() 22 | 23 | include_directories(include) 24 | 25 | ament_auto_add_library(pure_pursuit_controller_3d SHARED src/pure_pursuit_controller_3d.cpp) 26 | target_compile_definitions(pure_pursuit_controller_3d PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS") 27 | pluginlib_export_plugin_description_file(nav2_core pure_pursuit_controller_3d_plugin.xml) 28 | 29 | ament_auto_add_library(straight_line_planner_3d SHARED src/straight_line_planner_3d.cpp) 30 | target_compile_definitions(straight_line_planner_3d PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS") 31 | pluginlib_export_plugin_description_file(nav2_core straight_line_planner_3d_plugin.xml) 32 | 33 | ament_auto_add_library(goal_checker_3d SHARED src/goal_checker_3d.cpp) 34 | target_compile_definitions(goal_checker_3d PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS") 35 | pluginlib_export_plugin_description_file(nav2_core goal_checker_3d_plugin.xml) 36 | 37 | ament_auto_add_library(progress_checker_3d SHARED src/progress_checker_3d.cpp) 38 | target_compile_definitions(progress_checker_3d PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS") 39 | pluginlib_export_plugin_description_file(nav2_core progress_checker_3d_plugin.xml) 40 | 41 | # Load & run linters listed in package.xml 42 | if(BUILD_TESTING) 43 | find_package(ament_lint_auto REQUIRED) 44 | ament_lint_auto_find_test_dependencies() 45 | endif() 46 | 47 | install( 48 | FILES 49 | pure_pursuit_controller_3d_plugin.xml 50 | straight_line_planner_3d_plugin.xml 51 | goal_checker_3d_plugin.xml 52 | progress_checker_3d_plugin.xml 53 | DESTINATION share/${PROJECT_NAME} 54 | ) 55 | 56 | ament_auto_package() 57 | -------------------------------------------------------------------------------- /orca_nav2/README.md: -------------------------------------------------------------------------------- 1 | # Orca3 Nav2 Plugins 2 | 3 | The following Nav2 plugins provide basic 3D planning and control suitable for open water. 4 | They all ignore the global and local costmaps. 5 | They all largely ignore yaw, though the `PurePursuitController3D` will generate a value for yaw (`angular.z`) in `cmd_vel`. 6 | 7 | ## StraightLinePlanner3D 8 | 9 | The `StraightLinePlanner3D` plugin is similar to the 10 | [`StraightLinePlanner`](https://github.com/ros-planning/navigation2_tutorials/tree/master/nav2_straightline_planner) plugin. 11 | The planner always plans the vertical motion first, then the horizontal motion. 12 | 13 | Parameters: 14 | 15 | | Parameter | Type | Default | Notes | 16 | |---|---|---|---| 17 | | planning_dist | double | 0.1 | How far apart the plan poses are, m | 18 | 19 | ## PurePursuitController3D 20 | 21 | The `PurePursuitController3D` plugin is similar to the 22 | [`PurePursuitController`](https://github.com/ros-planning/navigation2_tutorials/tree/master/nav2_pure_pursuit_controller) plugin. 23 | It limits horizontal, vertical and yaw acceleration to reduce pitching and drift. 24 | The BlueROV2 frame is holonomic, but forward/backward drag is lower than the left/right drag, so the 25 | controller generates diff-drive motion (`linear.x`, `linear.z` and `angular.z`). 26 | 27 | Parameters: 28 | 29 | | Parameter | Type | Default | Notes | 30 | |---|---|---|---| 31 | | xy_vel | double | 0.4 | Desired horizontal velocity, m/s | 32 | | xy_accel | double | 0.4 | Max horizontal acceleration, m/s^2 | 33 | | z_vel | double | 0.2 | Desired vertical velocity, m/s | 34 | | z_accel | double | 0.2 | Max vertical acceleration, m/s^2 | 35 | | yaw_vel | double | 0.4 | Desired yaw velocity, r/s | 36 | | yaw_accel | double | 0.4 | Max yaw acceleration, r/s^2 | 37 | | lookahead_dist | double | 1.0 | Lookahead distance for pure pursuit algorithm, m | 38 | | transform_tolerance | double | 1.0 | How stale a transform can be and still be used, s | 39 | | goal_tolerance | double | 0.1 | Stop moving when goal is closer than goal_tolerance. Should be less than the values in GoalChecker3D | 40 | | tick_rate | double | 20 | The BT tick rate, used to calculate dt | 41 | 42 | ## ProgressChecker3D 43 | 44 | The `ProgressChecker3D` plugin is similar to the 45 | [`SimpleProgressChecker`](https://github.com/ros-planning/navigation2/tree/main/nav2_controller/plugins) plugin. 46 | 47 | Parameters: 48 | 49 | | Parameter | Type | Default | Notes | 50 | |---|---|---|---| 51 | | radius | double | 0.5 | Minimum distance to move, m | 52 | | time_allowance | double | 10.0 | Time allowed to move the minimum distance, s | 53 | 54 | ## GoalChecker3D 55 | 56 | The `GoalChecker3D` plugin is similar to the 57 | [`SimpleGoalChecker`](https://github.com/ros-planning/navigation2/tree/main/nav2_controller/plugins) plugin. 58 | 59 | Parameters: 60 | 61 | | Parameter | Type | Default | Notes | 62 | |---|---|---|---| 63 | | xy_goal_tolerance | double | 0.25 | Horizontal goal tolerance | 64 | | z_goal_tolerance | double | 0.25 | Vertical goal tolerance | 65 | -------------------------------------------------------------------------------- /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) 2021 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/orca3.git 13 | https://github.com/clydemcqueen/orca3/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 | -------------------------------------------------------------------------------- /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) 2021 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 | namespace orca_nav2 31 | { 32 | 33 | class GoalChecker3D : public nav2_core::GoalChecker 34 | { 35 | double xy_goal_tolerance_{}; 36 | double z_goal_tolerance_{}; 37 | 38 | // Most recent cmd_vel message 39 | geometry_msgs::msg::Twist cmd_vel_; 40 | 41 | rclcpp::Subscription::SharedPtr cmd_vel_sub_; 42 | 43 | public: 44 | void initialize( 45 | const rclcpp_lifecycle::LifecycleNode::WeakPtr & weak_parent, 46 | const std::string & plugin_name) override 47 | { 48 | auto parent = weak_parent.lock(); 49 | 50 | PARAMETER(parent, plugin_name, xy_goal_tolerance, 0.25) 51 | PARAMETER(parent, plugin_name, z_goal_tolerance, 0.25) 52 | 53 | cmd_vel_sub_ = parent->create_subscription( 54 | "cmd_vel", 1, 55 | [this](geometry_msgs::msg::Twist::ConstSharedPtr msg) // NOLINT 56 | { 57 | cmd_vel_ = *msg; 58 | }); 59 | 60 | RCLCPP_INFO(parent->get_logger(), "GoalChecker3D configured"); 61 | } 62 | 63 | void reset() override {} 64 | 65 | bool isGoalReached( 66 | const geometry_msgs::msg::Pose & query_pose, 67 | const geometry_msgs::msg::Pose & goal_pose, 68 | const geometry_msgs::msg::Twist &) override 69 | { 70 | // Wait for PurePursuitController3D to finish decelerating 71 | if (cmd_vel_.linear.x > 0 || cmd_vel_.linear.z > 0 || cmd_vel_.angular.z > 0) { 72 | return false; 73 | } 74 | 75 | double dx = query_pose.position.x - goal_pose.position.x; 76 | double dy = query_pose.position.y - goal_pose.position.y; 77 | double dz = query_pose.position.z - goal_pose.position.z; 78 | 79 | // Check xy position 80 | if (dx * dx + dy * dy > xy_goal_tolerance_ * xy_goal_tolerance_) { 81 | return false; 82 | } 83 | 84 | // Check z position 85 | return abs(dz) <= z_goal_tolerance_; 86 | } 87 | 88 | // Return tolerances for use by the controller (added in Galactic) 89 | bool getTolerances( 90 | geometry_msgs::msg::Pose & pose_tolerance, 91 | geometry_msgs::msg::Twist & vel_tolerance) override 92 | { 93 | double invalid_field = std::numeric_limits::lowest(); 94 | 95 | pose_tolerance.position.x = xy_goal_tolerance_; 96 | pose_tolerance.position.y = xy_goal_tolerance_; 97 | pose_tolerance.position.z = z_goal_tolerance_; 98 | 99 | pose_tolerance.orientation.w = invalid_field; 100 | pose_tolerance.orientation.x = invalid_field; 101 | pose_tolerance.orientation.y = invalid_field; 102 | pose_tolerance.orientation.z = invalid_field; 103 | 104 | vel_tolerance.linear.x = invalid_field; 105 | vel_tolerance.linear.y = invalid_field; 106 | vel_tolerance.linear.z = invalid_field; 107 | 108 | vel_tolerance.angular.x = invalid_field; 109 | vel_tolerance.angular.y = invalid_field; 110 | vel_tolerance.angular.z = invalid_field; 111 | 112 | return true; 113 | } 114 | }; 115 | 116 | } // namespace orca_nav2 117 | 118 | #include "pluginlib/class_list_macros.hpp" 119 | 120 | PLUGINLIB_EXPORT_CLASS(orca_nav2::GoalChecker3D, nav2_core::GoalChecker) 121 | -------------------------------------------------------------------------------- /orca_nav2/src/progress_checker_3d.cpp: -------------------------------------------------------------------------------- 1 | // MIT License 2 | // 3 | // Copyright (c) 2021 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/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.5) 2 | project(orca_shared) 3 | 4 | if(NOT CMAKE_CXX_STANDARD) 5 | set(CMAKE_CXX_STANDARD 14) 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 | # Emulate colcon in CLion 13 | if($ENV{CLION_IDE}) 14 | find_package(fastrtps_cmake_module REQUIRED) 15 | set(FastRTPS_INCLUDE_DIR "/opt/ros/foxy/include") 16 | set(FastRTPS_LIBRARY_RELEASE "/opt/ros/foxy/lib/libfastrtps.so") 17 | set(fiducial_vlam_msgs_DIR "${PROJECT_SOURCE_DIR}/../../../install/fiducial_vlam_msgs/share/fiducial_vlam_msgs/cmake") 18 | set(orca_msgs_DIR "${PROJECT_SOURCE_DIR}/../../../install/orca_msgs/share/orca_msgs/cmake") 19 | set(ros2_shared_DIR "${PROJECT_SOURCE_DIR}/../../../install/ros2_shared/share/ros2_shared/cmake") 20 | endif() 21 | 22 | find_package(ament_cmake_auto REQUIRED) 23 | ament_auto_find_build_dependencies() 24 | 25 | include_directories(include) 26 | 27 | ament_auto_add_library( 28 | orca_shared SHARED 29 | src/baro.cpp 30 | src/model.cpp 31 | src/pwm.cpp 32 | src/util.cpp 33 | ) 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 | ament_auto_package() -------------------------------------------------------------------------------- /orca_shared/include/orca_shared/baro.hpp: -------------------------------------------------------------------------------- 1 | // MIT License 2 | // 3 | // Copyright (c) 2021 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__BARO_HPP_ 24 | #define ORCA_SHARED__BARO_HPP_ 25 | 26 | #include "orca_shared/model.hpp" 27 | 28 | namespace orca 29 | { 30 | 31 | class Barometer 32 | { 33 | double atmospheric_pressure_{0}; 34 | 35 | public: 36 | double atmospheric_pressure() const {return atmospheric_pressure_;} 37 | 38 | bool initialized() const {return atmospheric_pressure_ > 0;} 39 | 40 | // Barometer must be initialized from a pressure and known depth 41 | void initialize(const Model & model, double pressure, double base_link_z); 42 | 43 | // Reset the barometer 44 | void clear() {atmospheric_pressure_ = 0;} 45 | 46 | // Given a pressure, return base_link.z, return 0 if not initialized 47 | double pressure_to_base_link_z(const Model & model, double pressure) const; 48 | }; 49 | 50 | } // namespace orca 51 | 52 | #endif // ORCA_SHARED__BARO_HPP_ 53 | -------------------------------------------------------------------------------- /orca_shared/include/orca_shared/pwm.hpp: -------------------------------------------------------------------------------- 1 | // MIT License 2 | // 3 | // Copyright (c) 2021 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 | #include "orca_msgs/msg/camera_tilt.hpp" 29 | #include "orca_msgs/msg/lights.hpp" 30 | 31 | namespace orca 32 | { 33 | //============================================================================= 34 | // Camera tilt servo 35 | //============================================================================= 36 | 37 | constexpr int TILT_MIN = -45; 38 | constexpr int TILT_MAX = 45; 39 | 40 | uint16_t tilt_to_pwm(int tilt); 41 | 42 | int pwm_to_tilt(uint16_t pwm); 43 | 44 | //============================================================================= 45 | // BlueRobotics Lumen subsea light 46 | //============================================================================= 47 | 48 | constexpr int BRIGHTNESS_MIN = 0; 49 | constexpr int BRIGHTNESS_MAX = 100; 50 | 51 | uint16_t brightness_to_pwm(int brightness); 52 | 53 | int pwm_to_brightness(uint16_t pwm); 54 | 55 | //============================================================================= 56 | // BlueRobotics T200 thruster + ESC 57 | //============================================================================= 58 | 59 | constexpr double THRUST_FULL_REV = -1.0; 60 | constexpr double THRUST_STOP = 0.0; 61 | constexpr double THRUST_FULL_FWD = 1.0; 62 | 63 | uint16_t effort_to_pwm(uint16_t thrust_dz_pwm, double effort); 64 | 65 | double pwm_to_effort(uint16_t thrust_dz_pwm, uint16_t pwm); 66 | 67 | } // namespace orca 68 | 69 | #endif // ORCA_SHARED__PWM_HPP_ 70 | -------------------------------------------------------------------------------- /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/orca3.git 13 | https://github.com/clydemcqueen/orca3/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 | fiducial_vlam_msgs 33 | geometry_msgs 34 | image_geometry 35 | orca_msgs 36 | rclcpp 37 | ros2_shared 38 | sensor_msgs 39 | tf2 40 | tf2_geometry_msgs 41 | tf2_ros 42 | 43 | 44 | ament_cmake 45 | 46 | 47 | -------------------------------------------------------------------------------- /orca_shared/src/baro.cpp: -------------------------------------------------------------------------------- 1 | // MIT License 2 | // 3 | // Copyright (c) 2021 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/baro.hpp" 24 | 25 | #include 26 | 27 | /*************************************************************** 28 | * When the sub is floating at the surface of the water: 29 | * base_link_z ~= -0.125 30 | * baro_link_z ~= -0.075 31 | */ 32 | 33 | namespace orca 34 | { 35 | 36 | // TODO(clyde): get from urdf or tf tree 37 | static const double baro_link_to_base_link_z = -0.05; 38 | 39 | void Barometer::initialize(const Model & model, double pressure, double base_link_z) 40 | { 41 | // Transform base_link to baro_link 42 | double baro_link_z = base_link_z - baro_link_to_base_link_z; 43 | 44 | // Initialize atmospheric pressure 45 | atmospheric_pressure_ = model.atmospheric_pressure(pressure, baro_link_z); 46 | } 47 | 48 | double Barometer::pressure_to_base_link_z(const Model & model, double pressure) const 49 | { 50 | if (!initialized()) { 51 | std::cout << "barometer is not initialized" << std::endl; 52 | return 0; 53 | } 54 | 55 | // Calc depth from pressure 56 | double baro_link_z = model.pressure_to_z(atmospheric_pressure_, pressure); 57 | 58 | // Transform baro_link to base_link 59 | double base_link_z = baro_link_z + baro_link_to_base_link_z; 60 | 61 | return base_link_z; 62 | } 63 | 64 | } // namespace orca 65 | -------------------------------------------------------------------------------- /orca_shared/src/model.cpp: -------------------------------------------------------------------------------- 1 | // MIT License 2 | // 3 | // Copyright (c) 2021 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) 2021 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 | #include "orca_msgs/msg/thrust.hpp" 27 | 28 | namespace orca 29 | { 30 | 31 | uint16_t tilt_to_pwm(const int tilt) 32 | { 33 | return orca::scale( 34 | tilt, TILT_MIN, TILT_MAX, 35 | orca_msgs::msg::CameraTilt::TILT_45_UP, orca_msgs::msg::CameraTilt::TILT_45_DOWN); 36 | } 37 | 38 | int pwm_to_tilt(const uint16_t pwm) 39 | { 40 | return orca::scale( 41 | pwm, orca_msgs::msg::CameraTilt::TILT_45_UP, 42 | orca_msgs::msg::CameraTilt::TILT_45_DOWN, 43 | TILT_MIN, TILT_MAX); 44 | } 45 | 46 | uint16_t brightness_to_pwm(const int brightness) 47 | { 48 | return orca::scale( 49 | brightness, BRIGHTNESS_MIN, BRIGHTNESS_MAX, 50 | orca_msgs::msg::Lights::LIGHTS_OFF, orca_msgs::msg::Lights::LIGHTS_FULL); 51 | } 52 | 53 | int pwm_to_brightness(const uint16_t pwm) 54 | { 55 | return orca::scale( 56 | pwm, orca_msgs::msg::Lights::LIGHTS_OFF, orca_msgs::msg::Lights::LIGHTS_FULL, 57 | BRIGHTNESS_MIN, BRIGHTNESS_MAX); 58 | } 59 | 60 | uint16_t effort_to_pwm(const uint16_t thrust_dz_pwm, const double effort) 61 | { 62 | uint16_t thrust_range_pwm = 400 - thrust_dz_pwm; 63 | 64 | return clamp( 65 | static_cast(orca_msgs::msg::Thrust::THRUST_STOP + 66 | (effort > THRUST_STOP ? thrust_dz_pwm : (effort < THRUST_STOP ? 67 | -thrust_dz_pwm : 0)) + 68 | std::round(effort * thrust_range_pwm)), 69 | orca_msgs::msg::Thrust::THRUST_FULL_REV, 70 | orca_msgs::msg::Thrust::THRUST_FULL_FWD); 71 | } 72 | 73 | double pwm_to_effort(const uint16_t thrust_dz_pwm, const uint16_t pwm) 74 | { 75 | uint16_t thrust_range_pwm = 400 - thrust_dz_pwm; 76 | 77 | return static_cast( 78 | pwm - orca_msgs::msg::Thrust::THRUST_STOP + 79 | (pwm > orca_msgs::msg::Thrust::THRUST_STOP ? -thrust_dz_pwm : 80 | (pwm < orca_msgs::msg::Thrust::THRUST_STOP ? thrust_dz_pwm : 0))) / 81 | thrust_range_pwm; 82 | } 83 | 84 | } // namespace orca 85 | -------------------------------------------------------------------------------- /orca_topside/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | project(orca_topside) 3 | 4 | if(NOT CMAKE_CXX_STANDARD) 5 | set(CMAKE_CXX_STANDARD 14) 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 | # Emulate colcon in CLion 13 | #if($ENV{CLION_IDE}) 14 | # set(gst_tools_DIR "${PROJECT_SOURCE_DIR}/../../../install/gst_tools/share/gst_tools/cmake") 15 | # set(orca_msgs_DIR "${PROJECT_SOURCE_DIR}/../../../install/orca_msgs/share/orca_msgs/cmake") 16 | # set(orca_shared_DIR "${PROJECT_SOURCE_DIR}/../../../install/orca_shared/share/orca_shared/cmake") 17 | # set(ros2_shared_DIR "${PROJECT_SOURCE_DIR}/../../../install/ros2_shared/share/ros2_shared/cmake") 18 | #endif() 19 | 20 | # A bit tricky to use ament + Qt5 + CMake + CLion, some resources: 21 | # https://doc.qt.io/qt-5.12/cmake-manual.html 22 | # https://www.jetbrains.com/help/clion/qt-tutorial.html 23 | 24 | set(CMAKE_AUTOMOC ON) 25 | set(CMAKE_AUTOUIC ON) 26 | set(CMAKE_AUTORCC ON) 27 | 28 | # Gstreamer doesn't provide CMake files 29 | find_package(PkgConfig) 30 | pkg_check_modules(GSTREAMER REQUIRED gstreamer-1.0) 31 | pkg_check_modules(GST_APP REQUIRED gstreamer-app-1.0) 32 | 33 | find_package(ament_cmake REQUIRED) 34 | find_package(camera_info_manager REQUIRED) 35 | find_package(gst_tools QUIET) 36 | find_package(h264_msgs REQUIRED) 37 | find_package(geometry_msgs REQUIRED) 38 | find_package(orb_slam2_ros REQUIRED) 39 | find_package(orca_msgs REQUIRED) 40 | find_package(orca_shared REQUIRED) 41 | find_package(rclcpp REQUIRED) 42 | find_package(ros2_shared REQUIRED) 43 | find_package(Qt5Widgets REQUIRED) 44 | 45 | include_directories(include) 46 | 47 | # Add the .h files so that MOC finds them 48 | add_executable( 49 | teleop_node 50 | src/fps_calculator.cpp 51 | src/gst_util.cpp 52 | src/gst_widget.cpp 53 | src/image_publisher.cpp 54 | src/image_widget.cpp 55 | src/node_spinner.cpp 56 | src/teleop_node.cpp 57 | src/teleop_main.cpp 58 | src/topside_layout.cpp 59 | src/topside_widget.cpp 60 | src/video_pipeline.cpp 61 | include/orca_topside/fps_calculator.hpp 62 | include/orca_topside/gst_util.hpp 63 | include/orca_topside/gst_widget.hpp 64 | include/orca_topside/image_publisher.hpp 65 | include/orca_topside/image_widget.hpp 66 | include/orca_topside/node_spinner.hpp 67 | include/orca_topside/teleop_node.hpp 68 | include/orca_topside/topside_layout.hpp 69 | include/orca_topside/topside_widget.hpp 70 | include/orca_topside/video_pipeline.hpp 71 | ) 72 | 73 | ament_target_dependencies( 74 | teleop_node 75 | camera_info_manager 76 | geometry_msgs 77 | GST_APP 78 | h264_msgs 79 | orb_slam2_ros 80 | orca_msgs 81 | orca_shared 82 | Qt5Widgets 83 | rclcpp 84 | ros2_shared 85 | ) 86 | 87 | if(gst_tools_FOUND) 88 | ament_target_dependencies(teleop_node gst_tools) 89 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DGST_TOOLS") 90 | endif() 91 | 92 | install( 93 | TARGETS teleop_node 94 | DESTINATION lib/${PROJECT_NAME} 95 | ) 96 | 97 | # Load & run linters listed in package.xml 98 | if(BUILD_TESTING) 99 | find_package(ament_lint_auto REQUIRED) 100 | ament_lint_auto_find_test_dependencies() 101 | endif() 102 | 103 | ament_package() -------------------------------------------------------------------------------- /orca_topside/README.md: -------------------------------------------------------------------------------- 1 | # Topside controller 2 | 3 | ![Topside controller](images/topside.png) 4 | 5 | The topside controller provides the following: 6 | * display and/or record video from 1 or more cameras 7 | * display system status 8 | * provide keyboard and XBox controller input for ROV operation 9 | 10 | See [teleop_node.hpp](include/orca_topside/teleop_node.hpp) for the list of parameters. 11 | 12 | The ROS-standard `teleop_twist_joy` node supports more joysticks and will drive the ROV 13 | by publishing `/cmd_vel` messages, but it will not set the camera tilt, 14 | turn on the lights, turn on the PID controller, etc. 15 | 16 | ## Display 17 | 18 | The top bar (roughly: sensors) displays the following: 19 | * Battery voltage 20 | * Depth (z) actual and target 21 | * SLAM status 22 | * Forward camera fps 23 | * Left camera fps 24 | * Right camera fps 25 | * SLAM debug image fps 26 | 27 | The bottom bar (roughly: controls) displays the following: 28 | * Disarmed vs armed 29 | * Float vs hold 30 | * Lights 0 (off) to 100 (full on) 31 | * Camera tilt 32 | * Forward (linear.x) velocity trim 33 | * Yaw (angular.z) velocity trim 34 | * Vertical (linear.z) velocity trim 35 | * Strafe (linear.y) velocity trim 36 | 37 | The main widget displays the images from the forward, left and right H.264 cameras, 38 | as well as the debug output of the ORB SLAM system. 39 | The various widgets must be enabled in the launch file. 40 | Control the main widget using the function keys: 41 | * F1, F2, F3 start / stop recording the forward, left or right camera 42 | * F4, F5, F6, F7 show / hide the forward, left, right and SLAM widgets 43 | * F8, F9, F10, F11 zoom the forward, left, right and SLAM widgets 44 | 45 | ## Additional keyboard controls 46 | 47 | * ! toggles between arm / disarm 48 | * @ toggles between hold / float 49 | * ( and ) control the lights 50 | * _ and + control the camera tilt 51 | * e, s, d, f and c are arranged to mimic the XBox left joystick. 52 | They increment and decrement the forward and yaw trim. 53 | The d key sets forward and yaw trim to 0. 54 | * i, j, k, l and , are arranged to mimic the XBox right joystick. 55 | They increment and decrement the vertical and strafe trim. 56 | The k key sets vertical and strafe trim to 0. 57 | 58 | It is possible, if a little clunky, to operate the ROV only using the keyboard. 59 | 60 | ## Xbox joystick controls 61 | 62 | * The menu button arms the ROV, the window button disarms the ROV 63 | * The left stick sets desired velocity forward / reverse and yaw left / yaw right 64 | * The right stick sets desired velocity up / down and strafe left / strafe right 65 | * Trim up / down adds or subtracts vertical velocity in 0.1 m/s increments 66 | * Trim left / right controls the lights 67 | * The logo button cancels vertical trim 68 | * Left and right bumper buttons controls the camera tilt in 15 degree increments 69 | * A selects _float_ (hover thrust and PID controller disabled) 70 | * B selects _hold_ (hover thrust and PID controller enabled) 71 | * X disables the right stick up / down motion. 72 | This makes it easy to strafe while holding a vertical position. 73 | Use trim to control vertical velocity and position. 74 | The default is disabled. 75 | * Y enables the right stick up / down motion 76 | 77 | ## Launch 78 | 79 | ~~~ 80 | cd ~/ros2/orca3_ws 81 | source install/setup.bash 82 | ros2 launch orca_bringup topside_launch.py 83 | ~~~ 84 | -------------------------------------------------------------------------------- /orca_topside/images/topside.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/clydemcqueen/orca3/02bfefe346cf04cce13e227b98a763241fdcf534/orca_topside/images/topside.png -------------------------------------------------------------------------------- /orca_topside/include/orca_topside/fps_calculator.hpp: -------------------------------------------------------------------------------- 1 | // MIT License 2 | // 3 | // Copyright (c) 2021 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_TOPSIDE__FPS_CALCULATOR_HPP_ 24 | #define ORCA_TOPSIDE__FPS_CALCULATOR_HPP_ 25 | 26 | #include 27 | #include 28 | 29 | #include "rclcpp/time.hpp" 30 | 31 | namespace orca_topside 32 | { 33 | 34 | // Multi-threaded framerate calculator 35 | // GStreamer pad callback calls push, Qt UI thread (shared with ROS thread) calls pop 36 | class FPSCalculator 37 | { 38 | std::queue stamps_; 39 | mutable std::mutex mutex_; 40 | 41 | void pop_old_impl(const rclcpp::Time & stamp); 42 | 43 | public: 44 | void push_new(const rclcpp::Time & stamp); 45 | void pop_old(const rclcpp::Time & stamp); 46 | int fps() const; 47 | }; 48 | 49 | } // namespace orca_topside 50 | 51 | #endif // ORCA_TOPSIDE__FPS_CALCULATOR_HPP_ 52 | -------------------------------------------------------------------------------- /orca_topside/include/orca_topside/gst_util.hpp: -------------------------------------------------------------------------------- 1 | // MIT License 2 | // 3 | // Copyright (c) 2021 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_TOPSIDE__GST_UTIL_HPP_ 24 | #define ORCA_TOPSIDE__GST_UTIL_HPP_ 25 | 26 | extern "C" { 27 | #include "gst/gst.h" 28 | } 29 | 30 | #include 31 | 32 | namespace gst_util 33 | { 34 | 35 | void copy_buffer(GstBuffer *buffer, std::vector& dest); 36 | 37 | } // namespace gst_util 38 | 39 | #endif // ORCA_TOPSIDE__GST_UTIL_HPP_ 40 | -------------------------------------------------------------------------------- /orca_topside/include/orca_topside/gst_widget.hpp: -------------------------------------------------------------------------------- 1 | // MIT License 2 | // 3 | // Copyright (c) 2021 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_TOPSIDE__GST_WIDGET_HPP_ 24 | #define ORCA_TOPSIDE__GST_WIDGET_HPP_ 25 | 26 | #include 27 | 28 | #include 29 | 30 | extern "C" { 31 | #include "gst/gst.h" 32 | #include "gst/app/gstappsink.h" 33 | } 34 | 35 | namespace orca_topside 36 | { 37 | 38 | // Poll a gstreamer appsink element for raw image data, and draw in a Qt widget 39 | class GstWidget : public QWidget 40 | { 41 | Q_OBJECT 42 | 43 | public: 44 | explicit GstWidget(GstElement *sink, QWidget *parent = nullptr); 45 | 46 | protected: 47 | void paintEvent(QPaintEvent *) override; 48 | 49 | private slots: 50 | void process_sample(); 51 | 52 | private: 53 | GstElement *sink_; 54 | gsize expected_size_; 55 | QImage *image_; 56 | }; 57 | 58 | } // namespace orca_topside 59 | 60 | #endif // ORCA_TOPSIDE__GST_WIDGET_HPP_ 61 | -------------------------------------------------------------------------------- /orca_topside/include/orca_topside/image_publisher.hpp: -------------------------------------------------------------------------------- 1 | // MIT License 2 | // 3 | // Copyright (c) 2021 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_TOPSIDE__IMAGE_PUBLISHER_HPP_ 24 | #define ORCA_TOPSIDE__IMAGE_PUBLISHER_HPP_ 25 | 26 | #include 27 | #include 28 | #include 29 | #include 30 | 31 | extern "C" { 32 | #include "gst/gst.h" 33 | #include "gst/app/gstappsink.h" 34 | } 35 | 36 | #include "camera_info_manager/camera_info_manager.hpp" 37 | #include "h264_msgs/msg/packet.hpp" 38 | #include "rclcpp/rclcpp.hpp" 39 | 40 | namespace orca_topside 41 | { 42 | 43 | class TeleopNode; 44 | 45 | // Poll a gstreamer appsink element for H264 data, and publish a ROS message 46 | class ImagePublisher 47 | { 48 | std::string topic_; 49 | TeleopNode *node_; 50 | sensor_msgs::msg::CameraInfo cam_info_; 51 | rclcpp::Publisher::SharedPtr h264_pub_; 52 | rclcpp::Publisher::SharedPtr cam_info_pub_; 53 | GstElement *pipeline_; 54 | GstElement *sink_; 55 | 56 | // Poll GStreamer on a separate thread 57 | std::thread pipeline_thread_; 58 | std::atomic stop_signal_; 59 | 60 | // Sequence number 61 | int seq_; 62 | 63 | void process_sample(); 64 | 65 | public: 66 | ImagePublisher(std::string topic, const std::string & cam_name, const std::string & cam_info_url, 67 | TeleopNode *node, bool sync, GstElement *pipeline, GstElement *upstream); 68 | 69 | ~ImagePublisher(); 70 | }; 71 | 72 | } // namespace orca_topside 73 | 74 | #endif // ORCA_TOPSIDE__IMAGE_PUBLISHER_HPP_ 75 | -------------------------------------------------------------------------------- /orca_topside/include/orca_topside/image_widget.hpp: -------------------------------------------------------------------------------- 1 | // MIT License 2 | // 3 | // Copyright (c) 2021 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_TOPSIDE__IMAGE_WIDGET_HPP_ 24 | #define ORCA_TOPSIDE__IMAGE_WIDGET_HPP_ 25 | 26 | #include 27 | 28 | #include 29 | #include 30 | #include 31 | 32 | #include "orca_topside/fps_calculator.hpp" 33 | #include "rclcpp/rclcpp.hpp" 34 | #include "sensor_msgs/msg/image.hpp" 35 | 36 | namespace orca_topside 37 | { 38 | 39 | class TeleopNode; 40 | 41 | // Draw ROS images in a Qt widget 42 | class ImageWidget : public QWidget 43 | { 44 | Q_OBJECT 45 | 46 | public: 47 | ImageWidget(std::shared_ptr node, std::string topic, QWidget *parent = nullptr); 48 | int fps() const { return fps_calculator_.fps(); } 49 | 50 | protected: 51 | void showEvent(QShowEvent *) override; 52 | void hideEvent(QHideEvent *) override; 53 | void paintEvent(QPaintEvent *) override; 54 | 55 | private: 56 | std::shared_ptr node_; 57 | std::string topic_; 58 | rclcpp::Subscription::SharedPtr image_sub_; 59 | QImage *image_; 60 | FPSCalculator fps_calculator_; 61 | }; 62 | 63 | } // namespace orca_topside 64 | 65 | #endif // ORCA_TOPSIDE__IMAGE_WIDGET_HPP_ 66 | -------------------------------------------------------------------------------- /orca_topside/include/orca_topside/node_spinner.hpp: -------------------------------------------------------------------------------- 1 | // MIT License 2 | // 3 | // Copyright (c) 2021 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_TOPSIDE__NODE_SPINNER_HPP_ 24 | #define ORCA_TOPSIDE__NODE_SPINNER_HPP_ 25 | 26 | #include 27 | 28 | #include 29 | 30 | #include "rclcpp/rclcpp.hpp" 31 | 32 | namespace orca_topside 33 | { 34 | 35 | class NodeSpinner : public QObject 36 | { 37 | Q_OBJECT 38 | 39 | public: 40 | NodeSpinner(std::shared_ptr node, std::function cleanup); 41 | 42 | private slots: 43 | void spin_some(); 44 | 45 | private: 46 | std::shared_ptr spinner_; 47 | std::function cleanup_; 48 | }; 49 | 50 | } // namespace orca_topside 51 | 52 | #endif // ORCA_TOPSIDE__NODE_SPINNER_HPP_ 53 | -------------------------------------------------------------------------------- /orca_topside/include/orca_topside/topside_layout.hpp: -------------------------------------------------------------------------------- 1 | // MIT License 2 | // 3 | // Copyright (c) 2021 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_TOPSIDE__TOPSIDE_LAYOUT_HPP_ 24 | #define ORCA_TOPSIDE__TOPSIDE_LAYOUT_HPP_ 25 | 26 | #include 27 | #include 28 | 29 | #include 30 | #include 31 | 32 | namespace orca_topside 33 | { 34 | 35 | // Arrange the various video widgets: 36 | // -- the first widget in the list is the main widget: it takes up as much space as possible 37 | // -- the other widgets are smaller and arranged around the border on top of the first widget 38 | // -- preserve the aspect ratio of each widget 39 | class TopsideLayout : public QLayout 40 | { 41 | public: 42 | TopsideLayout(); 43 | ~TopsideLayout() override; 44 | 45 | void addWidget(QWidget *widget, int width, int height, Qt::Alignment alignment); 46 | 47 | void set_main_widget(QWidget *widget); 48 | 49 | private: 50 | void addItem(QLayoutItem *item, int width, int height, Qt::Alignment alignment); 51 | 52 | void addItem(QLayoutItem *item) override; 53 | int count() const override; 54 | Qt::Orientations expandingDirections() const override; 55 | bool hasHeightForWidth() const override; 56 | QLayoutItem *itemAt(int index) const override; 57 | QSize minimumSize() const override; 58 | void setGeometry(const QRect & rect) override; 59 | QSize sizeHint() const override; 60 | QLayoutItem *takeAt(int index) override; 61 | 62 | struct ItemWrapper 63 | { 64 | ItemWrapper(QLayoutItem *_item, int _width, int _height, Qt::Alignment _alignment) 65 | { 66 | item = _item; 67 | width = _width; 68 | height = _height; 69 | alignment = _alignment; 70 | } 71 | 72 | ~ItemWrapper() 73 | { 74 | delete item; 75 | } 76 | 77 | QLayoutItem *item; 78 | int width; 79 | int height; 80 | Qt::Alignment alignment; 81 | }; 82 | 83 | enum SizeType 84 | { 85 | MinimumSize, SizeHint 86 | }; 87 | QSize calculate_size(SizeType sizeType) const; 88 | 89 | QList widgets_; 90 | }; 91 | 92 | } // namespace orca_topside 93 | 94 | #endif // ORCA_TOPSIDE__TOPSIDE_LAYOUT_HPP_ 95 | -------------------------------------------------------------------------------- /orca_topside/include/orca_topside/topside_widget.hpp: -------------------------------------------------------------------------------- 1 | // MIT License 2 | // 3 | // Copyright (c) 2021 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_TOPSIDE__TOPSIDE_WIDGET_HPP_ 24 | #define ORCA_TOPSIDE__TOPSIDE_WIDGET_HPP_ 25 | 26 | #include 27 | #include 28 | #include 29 | 30 | #include 31 | #include 32 | 33 | #include "orca_topside/gst_widget.hpp" 34 | #include "orca_topside/image_widget.hpp" 35 | #include "orca_topside/video_pipeline.hpp" 36 | #include "orb_slam2_ros/msg/status.hpp" 37 | 38 | namespace orca_topside 39 | { 40 | 41 | // A label that shows framerate and recording status 42 | class ImageLabel : public QLabel 43 | { 44 | Q_OBJECT 45 | 46 | public: 47 | explicit ImageLabel(std::string prefix); 48 | void set_info(int fps, bool recording); 49 | void set_info(const std::shared_ptr & pipeline); 50 | 51 | private: 52 | std::string prefix_; 53 | }; 54 | 55 | class TeleopNode; 56 | class TopsideLayout; 57 | 58 | // Main window 59 | class TopsideWidget : public QWidget 60 | { 61 | Q_OBJECT 62 | 63 | public: 64 | explicit TopsideWidget(std::shared_ptr node, QWidget *parent = nullptr); 65 | 66 | void about_to_quit(); 67 | 68 | void set_armed(bool armed); 69 | void set_hold(bool enabled); 70 | void set_tilt(int tilt); 71 | void set_depth(double target, double actual); 72 | void set_lights(int lights); 73 | void set_status(uint32_t status, double voltage); 74 | void set_slam(); 75 | void set_slam(const orb_slam2_ros::msg::Status & msg); 76 | void set_trim_x(double v); 77 | void set_trim_y(double v); 78 | void set_trim_z(double v); 79 | void set_trim_yaw(double v); 80 | 81 | void update_fcam_label_(); 82 | void update_lcam_label_(); 83 | void update_rcam_label_(); 84 | 85 | protected: 86 | void closeEvent(QCloseEvent *event) override; 87 | void keyPressEvent(QKeyEvent *event) override; 88 | 89 | private slots: 90 | void update_fps(); 91 | 92 | private: 93 | void set_main_widget(QWidget *widget); 94 | 95 | std::shared_ptr node_; 96 | GstWidget *fcam_widget_{}; 97 | GstWidget *lcam_widget_{}; 98 | GstWidget *rcam_widget_{}; 99 | ImageWidget *slam_image_widget_{}; 100 | TopsideLayout *cam_layout_{}; 101 | QLabel *armed_label_{}; 102 | QLabel *hold_label_{}; 103 | QLabel *depth_label_{}; 104 | QLabel *lights_label_{}; 105 | ImageLabel *fcam_label_{}; 106 | ImageLabel *lcam_label_{}; 107 | ImageLabel *rcam_label_{}; 108 | ImageLabel *slam_image_label_{}; 109 | QLabel *status_label_{}; 110 | QLabel *slam_label_{}; 111 | QLabel *tilt_label_{}; 112 | QLabel *trim_x_label_{}; 113 | QLabel *trim_y_label_{}; 114 | QLabel *trim_z_label_{}; 115 | QLabel *trim_yaw_label_{}; 116 | }; 117 | 118 | } // namespace orca_topside 119 | 120 | #endif // ORCA_TOPSIDE__TOPSIDE_WIDGET_HPP_ 121 | -------------------------------------------------------------------------------- /orca_topside/include/orca_topside/xbox.hpp: -------------------------------------------------------------------------------- 1 | // MIT License 2 | // 3 | // Copyright (c) 2021 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_TOPSIDE__XBOX_HPP_ 24 | #define ORCA_TOPSIDE__XBOX_HPP_ 25 | 26 | // XBox One joystick axes and buttons 27 | const int JOY_AXIS_LEFT_LR = 0; // Left stick left/right; 1.0 is left and -1.0 is right 28 | const int JOY_AXIS_LEFT_FB = 1; // Left stick forward/back; 1.0 is forward and -1.0 is back 29 | const int JOY_AXIS_LEFT_TRIGGER = 2; // Left trigger 30 | const int JOY_AXIS_RIGHT_LR = 3; /* Right stick left/right; 1.0 is left and -1.0 is right*/ 31 | const int JOY_AXIS_RIGHT_FB = 4; // Right stick forward/back; 1.0 is forward and -1.0 is back 32 | const int JOY_AXIS_RIGHT_TRIGGER = 5; // Right trigger 33 | const int JOY_AXIS_TRIM_LR = 6; // Trim left/right; 1.0 for left and -1.0 for right 34 | const int JOY_AXIS_TRIM_FB = 7; // Trim forward/back; 1.0 for forward and -1.0 for back 35 | const int JOY_BUTTON_A = 0; // A button 36 | const int JOY_BUTTON_B = 1; // B button 37 | const int JOY_BUTTON_X = 2; // X button 38 | const int JOY_BUTTON_Y = 3; // Y button 39 | const int JOY_BUTTON_LEFT_BUMPER = 4; // Left bumper 40 | const int JOY_BUTTON_RIGHT_BUMPER = 5; // Right bumper 41 | const int JOY_BUTTON_VIEW = 6; // View button 42 | const int JOY_BUTTON_MENU = 7; // Menu button 43 | const int JOY_BUTTON_LOGO = 8; // XBox logo button 44 | const int JOY_BUTTON_LEFT_STICK = 9; // Left stick button 45 | const int JOY_BUTTON_RIGHT_STICK = 10; // Right stick button 46 | 47 | #endif // ORCA_TOPSIDE__XBOX_HPP_ 48 | -------------------------------------------------------------------------------- /orca_topside/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | orca_topside 6 | 0.1.0 7 | Orca Topside Control 8 | 9 | Clyde McQueen 10 | MIT 11 | 12 | https://github.com/clydemcqueen/orca3.git 13 | https://github.com/clydemcqueen/orca3/issues 14 | 15 | Clyde McQueen 16 | 17 | ament_cmake 18 | 19 | ament_lint_auto 20 | ament_cmake_copyright 21 | ament_cmake_cppcheck 22 | ament_cmake_cpplint 23 | ament_cmake_flake8 24 | ament_cmake_lint_cmake 25 | ament_cmake_pep257 26 | ament_cmake_xmllint 27 | 28 | 32 | 33 | camera_info_manager 34 | geometry_msgs 35 | h264_msgs 36 | libgstreamer1.0-dev 37 | libgstreamer-plugins-base1.0-dev 38 | orb_slam2_ros 39 | orca_msgs 40 | orca_shared 41 | rclcpp 42 | ros2_shared 43 | 44 | 45 | 46 | 47 | ament_cmake 48 | 49 | 50 | -------------------------------------------------------------------------------- /orca_topside/src/fps_calculator.cpp: -------------------------------------------------------------------------------- 1 | // MIT License 2 | // 3 | // Copyright (c) 2021 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_topside/fps_calculator.hpp" 24 | 25 | namespace orca_topside 26 | { 27 | 28 | // Caller must lock the mutex before calling 29 | void FPSCalculator::pop_old_impl(const rclcpp::Time & stamp) 30 | { 31 | while (!stamps_.empty() && stamp - stamps_.front() > rclcpp::Duration(1, 0)) { 32 | stamps_.pop(); 33 | } 34 | } 35 | 36 | void FPSCalculator::push_new(const rclcpp::Time & stamp) 37 | { 38 | std::lock_guard lock(mutex_); 39 | stamps_.push(stamp); 40 | pop_old_impl(stamp); 41 | } 42 | 43 | void FPSCalculator::pop_old(const rclcpp::Time & stamp) 44 | { 45 | std::lock_guard lock(mutex_); 46 | pop_old_impl(stamp); 47 | } 48 | 49 | int FPSCalculator::fps() const 50 | { 51 | std::lock_guard lock(mutex_); 52 | return static_cast(stamps_.size()); 53 | } 54 | 55 | } // namespace orca_topside 56 | -------------------------------------------------------------------------------- /orca_topside/src/gst_util.cpp: -------------------------------------------------------------------------------- 1 | // MIT License 2 | // 3 | // Copyright (c) 2021 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_topside/gst_util.hpp" 27 | 28 | // Copy all memory segments in a GstBuffer into dest 29 | void gst_util::copy_buffer(GstBuffer *buffer, std::vector& dest) 30 | { 31 | auto num_segments = static_cast(gst_buffer_n_memory(buffer)); 32 | gsize copied = 0; 33 | for (int i = 0; i < num_segments; ++i) { 34 | GstMemory *segment = gst_buffer_get_memory(buffer, i); 35 | GstMapInfo segment_info; 36 | gst_memory_map(segment, &segment_info, GST_MAP_READ); 37 | 38 | std::copy(segment_info.data, segment_info.data + segment_info.size, 39 | dest.begin() + (int64_t) copied); 40 | copied += segment_info.size; 41 | 42 | gst_memory_unmap(segment, &segment_info); 43 | gst_memory_unref(segment); 44 | } 45 | } 46 | -------------------------------------------------------------------------------- /orca_topside/src/image_widget.cpp: -------------------------------------------------------------------------------- 1 | // MIT License 2 | // 3 | // Copyright (c) 2021 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_topside/image_widget.hpp" 24 | 25 | #include 26 | 27 | #include 28 | #include 29 | #include 30 | 31 | #include "cv_bridge/cv_bridge.h" 32 | #include "orca_topside/teleop_node.hpp" 33 | 34 | namespace orca_topside 35 | { 36 | 37 | ImageWidget::ImageWidget(std::shared_ptr node, std::string topic, QWidget *parent): 38 | QWidget(parent), 39 | node_(std::move(node)), 40 | topic_(std::move(topic)), 41 | image_(nullptr) 42 | { 43 | (void) image_sub_; 44 | } 45 | 46 | void ImageWidget::showEvent(QShowEvent *event) 47 | { 48 | (void) event; 49 | 50 | image_sub_ = node_->create_subscription(topic_, 10, 51 | [this](sensor_msgs::msg::Image::ConstSharedPtr msg) // NOLINT 52 | { 53 | if (msg->encoding != "bgr8") { 54 | RCLCPP_ERROR_ONCE(node_->get_logger(), "unsupported image format '%s'", msg->encoding.c_str()); // NOLINT 55 | return; 56 | } 57 | 58 | // Hack: Qt doesn't support Format_BGR888 until 5.14. It turns out that orb_slam2_ros writes 59 | // green on a mono image, so Format_RGB888 will work just as well. 60 | if (!image_) { 61 | RCLCPP_INFO(node_->get_logger(), "%s width %d, height %d", topic_.c_str(), msg->width, msg->height); // NOLINT 62 | image_ = new QImage(static_cast(msg->width), static_cast(msg->height), 63 | QImage::Format_RGB888); 64 | } 65 | 66 | // Copy data from gstreamer memory segment to Qt image 67 | memcpy(image_->scanLine(0), &msg->data[0], msg->width * msg->height * 3); 68 | 69 | // Call paintEvent() 70 | update(); 71 | 72 | fps_calculator_.push_new(msg->header.stamp); 73 | }); 74 | } 75 | 76 | void ImageWidget::hideEvent(QHideEvent *event) 77 | { 78 | (void) event; 79 | 80 | image_sub_ = nullptr; 81 | } 82 | 83 | void ImageWidget::paintEvent(QPaintEvent *event) 84 | { 85 | (void) event; 86 | 87 | QRect target(QPoint(0, 0), size()); 88 | QPainter painter(this); 89 | 90 | if (image_) { 91 | painter.drawImage(target, *image_); 92 | } else { 93 | painter.fillRect(target, Qt::GlobalColor::black); 94 | } 95 | } 96 | 97 | } // namespace orca_topside 98 | -------------------------------------------------------------------------------- /orca_topside/src/node_spinner.cpp: -------------------------------------------------------------------------------- 1 | // MIT License 2 | // 3 | // Copyright (c) 2021 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_topside/node_spinner.hpp" 24 | 25 | #include 26 | 27 | #include 28 | #include 29 | 30 | namespace orca_topside 31 | { 32 | 33 | NodeSpinner::NodeSpinner(std::shared_ptr node, std::function cleanup): 34 | cleanup_(std::move(cleanup)) 35 | { 36 | spinner_ = std::make_shared(); 37 | spinner_->add_node(static_cast>(std::move(node))); 38 | 39 | auto timer = new QTimer(this); 40 | connect(timer, &QTimer::timeout, this, QOverload<>::of(&NodeSpinner::spin_some)); 41 | timer->start(20); 42 | } 43 | 44 | void NodeSpinner::spin_some() 45 | { 46 | if (rclcpp::ok()) { 47 | spinner_->spin_some(std::chrono::milliseconds(10)); 48 | } else { 49 | cleanup_(); 50 | QApplication::exit(0); 51 | } 52 | } 53 | 54 | } // namespace orca_topside 55 | -------------------------------------------------------------------------------- /orca_topside/src/teleop_main.cpp: -------------------------------------------------------------------------------- 1 | // MIT License 2 | // 3 | // Copyright (c) 2021 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_topside/teleop_node.hpp" 24 | 25 | #include 26 | #include 27 | 28 | #include 29 | 30 | #include "orca_topside/node_spinner.hpp" 31 | #include "orca_topside/topside_widget.hpp" 32 | 33 | #define STYLESHEET \ 34 | "QWidget {background-color : black;}" \ 35 | "QLabel {background-color : black; color : white; font-family: sans-serif; " \ 36 | "font-size: 24px;text-align: center;}" 37 | 38 | int main(int argc, char *argv[]) 39 | { 40 | rclcpp::init(argc, argv); 41 | auto node = std::make_shared(); 42 | 43 | if (node->cxt().show_window_) { 44 | // Display, keyboard and joystick input 45 | QApplication app(argc, argv); 46 | QCoreApplication::setApplicationName("Orca Topside Control"); 47 | QGuiApplication::setApplicationDisplayName(QCoreApplication::applicationName()); 48 | QCoreApplication::setApplicationVersion(QT_VERSION_STR); 49 | 50 | app.setStyleSheet(STYLESHEET); 51 | 52 | auto topside_widget = new orca_topside::TopsideWidget(node); 53 | const QRect availableGeometry = QApplication::desktop()->availableGeometry(topside_widget); 54 | topside_widget->resize(availableGeometry.width() / 6, availableGeometry.height() / 4); 55 | topside_widget->show(); 56 | 57 | node->set_view(topside_widget); 58 | 59 | // Two ways to kill TeleopNode: 60 | // 61 | // 1. Hit the QWindow close box. 62 | // 2. Type ctrl-C in the CLI. The spinner will do the cleanup and quit the app. 63 | 64 | // Spin the ROS node in the same thread as the Qt app 65 | orca_topside::NodeSpinner spinner(node, [topside_widget]() 66 | { 67 | // Close open mp4 files 68 | topside_widget->about_to_quit(); 69 | }); 70 | 71 | return QApplication::exec(); 72 | } else { 73 | // Joystick input, no display 74 | rclcpp::spin(node); 75 | rclcpp::shutdown(); 76 | return 0; 77 | } 78 | } 79 | -------------------------------------------------------------------------------- /setup.bash: -------------------------------------------------------------------------------- 1 | # Handy script, run `. src/orca3/setup.bash` from the workspace root 2 | 3 | # Clear the decks 4 | unset AMENT_PREFIX_PATH 5 | unset CMAKE_PREFIX_PATH 6 | unset COLCON_PREFIX_PATH 7 | unset GAZEBO_MASTER_URI 8 | unset GAZEBO_MODEL_DATABASE_URI 9 | unset GAZEBO_MODEL_PATH 10 | unset GAZEBO_PLUGIN_PATH 11 | unset GAZEBO_RESOURCE_PATH 12 | unset LD_LIBRARY_PATH 13 | unset PYTHONPATH 14 | 15 | # ROS distro and overlay 16 | . /opt/ros/$ROS_DISTRO/setup.bash 17 | . install/local_setup.bash 18 | 19 | # Force logging to stdout, not stderr 20 | export RCUTILS_LOGGING_USE_STDOUT=1 21 | export RCUTILS_CONSOLE_OUTPUT_FORMAT='[{severity}] {name}: {message}' 22 | 23 | # Gazebo 24 | . /usr/share/gazebo/setup.sh 25 | 26 | # Gazebo model path 27 | export GAZEBO_MODEL_PATH=$GAZEBO_MODEL_PATH:$PWD/install/sim_fiducial/share/sim_fiducial/models 28 | export GAZEBO_MODEL_PATH=$GAZEBO_MODEL_PATH:$PWD/install/orca_gazebo/share/orca_gazebo/models 29 | 30 | # GTSAM 31 | # export CMAKE_PREFIX_PATH=~/lib/gtsam/install/lib/cmake/GTSAM:$CMAKE_PREFIX_PATH 32 | # export LD_LIBRARY_PATH=~/lib/gtsam/install/lib/:$LD_LIBRARY_PATH 33 | 34 | # OpenCV 4.4 35 | # export CMAKE_PREFIX_PATH=~/opencv/install/opencv_4_4:$CMAKE_PREFIX_PATH 36 | # export LD_LIBRARY_PATH=~/opencv/install/opencv_4_4/lib:$LD_LIBRARY_PATH 37 | -------------------------------------------------------------------------------- /workspace.repos: -------------------------------------------------------------------------------- 1 | repositories: 2 | ptrmu/fiducial_vlam: 3 | type: git 4 | url: https://github.com/ptrmu/fiducial_vlam 5 | version: master 6 | clydemcqueen/h264_image_transport: 7 | type: git 8 | url: https://github.com/clydemcqueen/h264_image_transport 9 | version: master 10 | clydemcqueen/orb_slam_2_ros: 11 | type: git 12 | url: https://github.com/clydemcqueen/orb_slam_2_ros 13 | version: clyde_h264_stereo_galactic 14 | ptrmu/ros2_shared: 15 | type: git 16 | url: https://github.com/ptrmu/ros2_shared 17 | version: master 18 | clydemcqueen/sim_fiducial: 19 | type: git 20 | url: https://github.com/clydemcqueen/sim_fiducial 21 | version: master 22 | clydemcqueen/stereo_decoder: 23 | type: git 24 | url: https://github.com/clydemcqueen/stereo_decoder 25 | version: main 26 | clydemcqueen/ukf: 27 | type: git 28 | url: https://github.com/clydemcqueen/ukf 29 | version: master 30 | --------------------------------------------------------------------------------