diff --git a/.devcontainer/nouveau/Dockerfile b/.devcontainer/nouveau/Dockerfile index 8122286a..da3846e0 100644 --- a/.devcontainer/nouveau/Dockerfile +++ b/.devcontainer/nouveau/Dockerfile @@ -18,8 +18,7 @@ RUN sudo apt-get -q update \ # Install debugging/linting Python packages RUN python3 -m pip install \ - pre-commit \ - mypy + pre-commit # Disable the setuputils installation warning # This prevents us from needing to pin the setuputils version (which doesn't always work) diff --git a/.devcontainer/nvidia/Dockerfile b/.devcontainer/nvidia/Dockerfile index 1c55005d..8a2fad0f 100644 --- a/.devcontainer/nvidia/Dockerfile +++ b/.devcontainer/nvidia/Dockerfile @@ -18,8 +18,7 @@ RUN sudo apt-get -q update \ # Install debugging/linting Python packages RUN python3 -m pip install \ - pre-commit \ - mypy + pre-commit # Disable the setuputils installation warning # This prevents us from needing to pin the setuputils version (which doesn't always work) diff --git a/.devcontainer/nvidia/devcontainer.json b/.devcontainer/nvidia/devcontainer.json index 27f58aa6..08a5714d 100644 --- a/.devcontainer/nvidia/devcontainer.json +++ b/.devcontainer/nvidia/devcontainer.json @@ -1,6 +1,6 @@ { "name": "NVIDIA Dev Container", - "dockerFile": "Dockerfile", + "dockerFile": "../../.docker/Dockerfile", "context": "../..", "workspaceMount": "source=${localWorkspaceFolder},target=/home/ubuntu/ws_blue/src/blue,type=bind", "workspaceFolder": "/home/ubuntu/ws_blue/src/blue", diff --git a/.devcontainer/robot/Dockerfile b/.devcontainer/robot/Dockerfile index cd3d2818..1059230c 100644 --- a/.devcontainer/robot/Dockerfile +++ b/.devcontainer/robot/Dockerfile @@ -23,8 +23,7 @@ RUN sudo apt-get -q update \ # Install debugging/linting Python packages RUN python3 -m pip install \ - pre-commit \ - mypy + pre-commit # Disable the setuputils installation warning # This prevents us from needing to pin the setuputils version (which doesn't always work) diff --git a/.docker/Dockerfile b/.docker/Dockerfile index 9f38a9ee..2a59dc7f 100644 --- a/.docker/Dockerfile +++ b/.docker/Dockerfile @@ -1,4 +1,4 @@ -ARG ROS_DISTRO=rolling +ARG ROS_DISTRO=jazzy FROM ros:$ROS_DISTRO-ros-base AS ci ENV DEBIAN_FRONTEND=noninteractive @@ -13,7 +13,7 @@ RUN apt-get -q update \ git \ sudo \ clang \ - clang-format-14 \ + clang-format-18 \ clang-tidy \ clang-tools \ python3-pip \ @@ -27,6 +27,22 @@ RUN apt-get -q update \ && apt-get clean -y \ && rm -rf /var/lib/apt/lists/* +RUN apt-get -q update \ + && apt-get -q -y upgrade \ + && apt-get -q install --no-install-recommends -y \ + git \ + sudo \ + clang \ + wget \ + python3-pip \ + python3-dev \ + python3-venv \ + apt-utils \ + software-properties-common \ + && apt-get autoremove -y \ + && apt-get clean -y \ + && rm -rf /var/lib/apt/lists/* + # Install all ROS dependencies for _just_ blue # (we have not imported other repos from .repos files) RUN apt-get -q update \ @@ -86,35 +102,34 @@ ENV USER_WORKSPACE=/home/$USERNAME/ws_blue WORKDIR $USER_WORKSPACE COPY --chown=$USER_UID:$USER_GID . src/blue -# Install the Python requirements that aren't available as rosdeps -RUN python3 -m pip install -r $(pwd)/src/blue/requirements-build.txt - -# Install gstreamer +# Install GStreamer RUN sudo apt-get -q update \ && sudo apt-get -q -y upgrade \ && sudo apt-get -q install --no-install-recommends -y \ - python3-gi \ - gstreamer1.0-tools \ gir1.2-gstreamer-1.0 \ - gir1.2-gst-plugins-base-1.0 \ + libgstreamer1.0-dev \ + libgstreamer-plugins-base1.0-dev \ + libgstreamer-plugins-bad1.0-dev \ + gstreamer1.0-plugins-base \ gstreamer1.0-plugins-good \ - gstreamer1.0-plugins-ugly \ gstreamer1.0-plugins-bad \ + gstreamer1.0-plugins-ugly \ gstreamer1.0-libav \ - libgstreamer1.0-dev \ + gstreamer1.0-tools \ + gstreamer1.0-x \ + gstreamer1.0-alsa \ gstreamer1.0-gl \ - libgstreamer-plugins-base1.0-dev \ + gstreamer1.0-gtk3 \ + gstreamer1.0-qt5 \ + gstreamer1.0-pulseaudio \ + libcairo2-dev \ + libgif-dev \ + libjpeg-dev \ + python3-pip \ && sudo apt-get autoremove -y \ && sudo apt-get clean -y \ && sudo rm -rf /var/lib/apt/lists/* -# Manually install MAVROS from source in the ws_blue/ workspace -WORKDIR $USER_WORKSPACE/src/ -ARG MAVROS_RELEASE=ros2 -ARG MAVLINK_RELEASE=release/rolling/mavlink -RUN git clone --depth 1 -b ${MAVROS_RELEASE} https://github.com/mavlink/mavros.git -RUN git clone --depth 1 --recursive -b ${MAVLINK_RELEASE} https://github.com/ros2-gbp/mavlink-gbp-release.git mavlink - WORKDIR $USER_WORKSPACE RUN sudo apt-get -q update \ && sudo apt-get -q -y upgrade \ @@ -200,23 +215,6 @@ RUN [ "/bin/bash" , "-c" , " \ && cmake .. -DCMAKE_BUILD_TYPE=RelWithDebInfo \ && make -j4" ] -# Install ros_gz and other project dependencies -WORKDIR $USER_WORKSPACE -RUN sudo apt-get -q update \ - && sudo apt-get -q -y upgrade \ - && vcs import src < src/blue/sim.repos \ - && rosdep update \ - && rosdep install -y --from-paths src --ignore-src --rosdistro ${ROS_DISTRO} \ - && sudo apt-get autoremove -y \ - && sudo apt-get clean -y \ - && sudo rm -rf /var/lib/apt/lists/* - -# For users that build this on a laptop or system with limited RAM, -# Modify the 'colcon build' line to be 'MAKEFLAGS="-j1 -l1" colcon build' -# This will limit the amount of RAM that colcon is allowed to use -RUN . "/opt/ros/${ROS_DISTRO}/setup.sh" \ - && colcon build - # Setup the simulation environment variables RUN <> /home/$USERNAME/.bashrc diff --git a/.dockerignore b/.dockerignore index ae633710..fcf49a1e 100644 --- a/.dockerignore +++ b/.dockerignore @@ -3,10 +3,8 @@ # Except the following !blue_bringup -!blue_localization !blue_description !blue_demos !blue.repos !sim.repos !.docker/entrypoints -!requirements-build.txt diff --git a/.github/ISSUE_TEMPLATE/bug-report.yaml b/.github/ISSUE_TEMPLATE/bug-report.yaml index 8ba16ae6..6509674f 100644 --- a/.github/ISSUE_TEMPLATE/bug-report.yaml +++ b/.github/ISSUE_TEMPLATE/bug-report.yaml @@ -1,7 +1,8 @@ name: Bug Report description: Report a bug. -title: "[BUG]: " -labels: [bug, needs triage] +title: "" +labels: [needs triage] +type: Bug body: - type: markdown diff --git a/.github/ISSUE_TEMPLATE/documentation.yaml b/.github/ISSUE_TEMPLATE/documentation.yaml index c44b1e6b..8ff2026f 100644 --- a/.github/ISSUE_TEMPLATE/documentation.yaml +++ b/.github/ISSUE_TEMPLATE/documentation.yaml @@ -1,7 +1,8 @@ name: Documentation Improvement description: Report an issue related to the Blue documentation. -title: "[DOC]: " -labels: [documentation, needs triage] +title: "" +labels: [needs triage] +type: Documentation body: - type: markdown diff --git a/.github/ISSUE_TEMPLATE/feature-request.yaml b/.github/ISSUE_TEMPLATE/feature-request.yaml index d2022388..da1648fc 100644 --- a/.github/ISSUE_TEMPLATE/feature-request.yaml +++ b/.github/ISSUE_TEMPLATE/feature-request.yaml @@ -1,7 +1,8 @@ name: Feature Request description: Suggest a new idea for the project. -title: "[FEATURE]: " -labels: [enhancement, needs triage] +title: "" +labels: [needs triage] +type: Feature body: - type: markdown diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index ab1548fc..371a8006 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -10,6 +10,11 @@ repos: hooks: - id: codespell + - repo: https://github.com/BlankSpruce/gersemi + rev: 0.13.5 + hooks: + - id: gersemi + - repo: https://github.com/pre-commit/pre-commit-hooks rev: v4.4.0 hooks: @@ -20,9 +25,7 @@ repos: - id: check-yaml - id: check-xml - id: check-merge-conflict - - id: check-symlinks - id: debug-statements - - id: destroyed-symlinks - id: detect-private-key - id: end-of-file-fixer - id: mixed-line-ending diff --git a/README.md b/README.md index b536c209..91b08b0c 100644 --- a/README.md +++ b/README.md @@ -9,12 +9,12 @@ the project [documentation](https://robotic-decision-making-lab.github.io/blue/) The main features of Blue include: - Support for custom controllers implemented using [auv_controllers](https://github.com/Robotic-Decision-Making-Lab/auv_controllers) - and localization algorithms implemented using [marine_localization](https://github.com/Robotic-Decision-Making-Lab/marine_localization) - Vehicle models and configurations for simulation using Gazebo - CI/CD pipelines to help you deploy your software in the same environment that you performed development and testing in - A development environment that includes many of the tools that you need to develop software for underwater vehicles +- First-party drivers for common underwater sensors ## Citation diff --git a/blue.repos b/blue.repos index 20a32377..ff96e039 100644 --- a/blue.repos +++ b/blue.repos @@ -19,3 +19,8 @@ repositories: type: git url: https://github.com/Robotic-Decision-Making-Lab/mobile_to_maritime.git version: main + + reach: + type: git + url: https://github.com/Robotic-Decision-Making-Lab/reach.git + version: main diff --git a/blue_bringup/CMakeLists.txt b/blue_bringup/CMakeLists.txt index 2ae4f3b3..ac42b158 100644 --- a/blue_bringup/CMakeLists.txt +++ b/blue_bringup/CMakeLists.txt @@ -3,9 +3,6 @@ project(blue_bringup) find_package(ament_cmake REQUIRED) -install( - DIRECTORY launch - DESTINATION share/blue_bringup -) +install(DIRECTORY launch DESTINATION share/blue_bringup) ament_package() diff --git a/blue_bringup/launch/bluerov2/bluerov2.launch.yaml b/blue_bringup/launch/bluerov2/bluerov2.launch.yaml index 7781fd27..b49093d6 100644 --- a/blue_bringup/launch/bluerov2/bluerov2.launch.yaml +++ b/blue_bringup/launch/bluerov2/bluerov2.launch.yaml @@ -5,22 +5,6 @@ launch: value: bluerov2 # Arguments - - arg: - name: use_camera - default: "false" - - - arg: - name: use_mocap - default: "false" - - - arg: - name: localization_source - default: gazebo - choice: - - value: gazebo - - value: mocap - - value: camera - - arg: name: use_sim default: "false" @@ -53,10 +37,6 @@ launch: name: ardusub_params_file default: $(find-pkg-share blue_description)/config/$(var model_name)/ardusub.parm - - arg: - name: localization_file - default: $(find-pkg-share blue_description)/config/$(var model_name)/localization.yaml - # Load the description file - let: name: description_file @@ -104,20 +84,6 @@ launch: - name: use_manager value: $(var use_manager) - - include: - file: $(find-pkg-share blue_localization)/localization.launch.py - arg: - - name: localization_source - value: $(var localization_source) - - name: use_camera - value: $(var use_camera) - - name: use_mocap - value: $(var use_mocap) - - name: use_sim_time - value: $(var use_sim) - - name: config_filepath - value: $(var localization_file) - - include: file: $(find-pkg-share blue_bringup)/launch/$(var model_name)/thrusters.launch.yaml diff --git a/blue_bringup/launch/bluerov2_heavy/bluerov2_heavy.launch.yaml b/blue_bringup/launch/bluerov2_heavy/bluerov2_heavy.launch.yaml index 8d8f8de7..904d2812 100644 --- a/blue_bringup/launch/bluerov2_heavy/bluerov2_heavy.launch.yaml +++ b/blue_bringup/launch/bluerov2_heavy/bluerov2_heavy.launch.yaml @@ -5,22 +5,6 @@ launch: value: bluerov2_heavy # Arguments - - arg: - name: use_camera - default: "false" - - - arg: - name: use_mocap - default: "false" - - - arg: - name: localization_source - default: gazebo - choice: - - value: gazebo - - value: mocap - - value: camera - - arg: name: use_sim default: "false" @@ -53,10 +37,6 @@ launch: name: ardusub_params_file default: $(find-pkg-share blue_description)/config/$(var model_name)/ardusub.parm - - arg: - name: localization_file - default: $(find-pkg-share blue_description)/config/$(var model_name)/localization.yaml - # Load the description file - let: name: description_file @@ -104,20 +84,6 @@ launch: - name: use_manager value: $(var use_manager) - - include: - file: $(find-pkg-share blue_localization)/localization.launch.py - arg: - - name: localization_source - value: $(var localization_source) - - name: use_camera - value: $(var use_camera) - - name: use_mocap - value: $(var use_mocap) - - name: use_sim_time - value: $(var use_sim) - - name: config_filepath - value: $(var localization_file) - - include: file: $(find-pkg-share blue_bringup)/launch/$(var model_name)/thrusters.launch.yaml diff --git a/blue_bringup/launch/bluerov2_heavy_reach/bluerov2_heavy_reach.launch.yaml b/blue_bringup/launch/bluerov2_heavy_reach/bluerov2_heavy_reach.launch.yaml index 1464a0a8..b98b18e6 100644 --- a/blue_bringup/launch/bluerov2_heavy_reach/bluerov2_heavy_reach.launch.yaml +++ b/blue_bringup/launch/bluerov2_heavy_reach/bluerov2_heavy_reach.launch.yaml @@ -5,22 +5,6 @@ launch: value: bluerov2_heavy_reach # Arguments - - arg: - name: use_camera - default: "false" - - - arg: - name: use_mocap - default: "false" - - - arg: - name: localization_source - default: gazebo - choice: - - value: gazebo - - value: mocap - - value: camera - - arg: name: use_sim default: "false" @@ -53,10 +37,6 @@ launch: name: ardusub_params_file default: $(find-pkg-share blue_description)/config/$(var model_name)/ardusub.parm - - arg: - name: localization_file - default: $(find-pkg-share blue_description)/config/$(var model_name)/localization.yaml - # Load the description file - let: name: description_file @@ -104,20 +84,6 @@ launch: - name: use_manager value: $(var use_manager) - - include: - file: $(find-pkg-share blue_localization)/localization.launch.py - arg: - - name: localization_source - value: $(var localization_source) - - name: use_camera - value: $(var use_camera) - - name: use_mocap - value: $(var use_mocap) - - name: use_sim_time - value: $(var use_sim) - - name: config_filepath - value: $(var localization_file) - - include: file: $(find-pkg-share blue_bringup)/launch/$(var model_name)/thrusters.launch.yaml diff --git a/blue_demos/CMakeLists.txt b/blue_demos/CMakeLists.txt index 977d6090..3afa405d 100644 --- a/blue_demos/CMakeLists.txt +++ b/blue_demos/CMakeLists.txt @@ -4,16 +4,24 @@ project(blue_demos) find_package(ament_cmake REQUIRED) install( - DIRECTORY control_integration/launch - control_integration/config - control_integration/description - DESTINATION share/blue_demos/control_integration + DIRECTORY + control_integration/launch + control_integration/config + control_integration/description + DESTINATION share/blue_demos/control_integration ) install( - DIRECTORY teleoperation/launch - teleoperation/config - DESTINATION share/blue_demos/teleoperation + DIRECTORY teleoperation/launch teleoperation/config + DESTINATION share/blue_demos/teleoperation +) + +install( + DIRECTORY + manipulator_systems/launch + manipulator_systems/config + manipulator_systems/description + DESTINATION share/blue_demos/manipulator_systems ) ament_package() diff --git a/blue_demos/README.md b/blue_demos/README.md index 2321f0dc..12be3735 100644 --- a/blue_demos/README.md +++ b/blue_demos/README.md @@ -6,3 +6,4 @@ relevant tutorials: - [ros2_control integration example](https://robotic-decision-making-lab.github.io/blue/tutorials/control) - [Teleoperation example](https://robotic-decision-making-lab.github.io/blue/tutorials/teleop) +- [Manipulator systems example](https://robotic-decision-making-lab.github.io/blue/tutorials/uvms) diff --git a/blue_demos/control_integration/config/bluerov2_controllers.yaml b/blue_demos/control_integration/config/bluerov2_controllers.yaml index a7ea4dbe..ba0f16b2 100644 --- a/blue_demos/control_integration/config/bluerov2_controllers.yaml +++ b/blue_demos/control_integration/config/bluerov2_controllers.yaml @@ -2,8 +2,8 @@ controller_manager: ros__parameters: update_rate: 100 # Hz - integral_sliding_mode_controller: - type: velocity_controllers/IntegralSlidingModeController + adaptive_integral_terminal_sliding_mode_controller: + type: velocity_controllers/AdaptiveIntegralTerminalSlidingModeController thruster_allocation_matrix_controller: type: thruster_allocation_matrix_controller/ThrusterAllocationMatrixController @@ -26,27 +26,48 @@ controller_manager: thruster6_controller: type: thruster_controllers/PolynomialThrustCurveController -integral_sliding_mode_controller: +adaptive_integral_terminal_sliding_mode_controller: ros__parameters: - use_external_measured_states: true reference_controller: thruster_allocation_matrix_controller + use_external_measured_states: true gains: - rho: 15.0 - lambda: 100.0 - Kp: [5.0, 5.0, 5.0, 5.0, 5.0, 10.0] - tf: - base_frame: "base_link_fsd" - odom_frame: "map_ned" - hydrodynamics: - mass: 10.0 - weight: 98.1 - buoyancy: 100.06 - moments_of_inertia: [0.16, 0.16, 0.16] - added_mass: [-5.50, -12.70, -14.60, -0.12, -0.12, -0.12] - center_of_buoyancy: [0.0, 0.0, 0.0] - center_of_gravity: [0.0, 0.0, 0.0] - linear_damping: [-4.03, -6.22, -5.18, -0.07, -0.07, -0.07] - quadratic_damping: [-18.18, -21.66, -36.99, -1.55, -1.55, -1.55] + lambda: 0.9 + x: + alpha: 12.0 + k1_min: 0.1 + k2: 0.4 + k_theta: 2.0 + mu: 0.01 + y: + alpha: 12.0 + k1_min: 0.1 + k2: 0.4 + k_theta: 2.0 + mu: 0.01 + z: + alpha: 12.0 + k1_min: 0.4 + k2: 0.8 + k_theta: 2.0 + mu: 0.01 + rx: + alpha: 0.5 + k1_min: 0.1 + k2: 0.4 + k_theta: 2.0 + mu: 0.01 + ry: + alpha: 12.0 + k1_min: 0.1 + k2: 1.0 + k_theta: 2.0 + mu: 0.01 + rz: + alpha: 12.0 + k1_min: 0.1 + k2: 0.4 + k_theta: 2.0 + mu: 0.01 thruster_allocation_matrix_controller: ros__parameters: diff --git a/blue_demos/control_integration/config/bluerov2_heavy_controllers.yaml b/blue_demos/control_integration/config/bluerov2_heavy_controllers.yaml index 5b05daaf..fdb2f4b7 100644 --- a/blue_demos/control_integration/config/bluerov2_heavy_controllers.yaml +++ b/blue_demos/control_integration/config/bluerov2_heavy_controllers.yaml @@ -2,8 +2,8 @@ controller_manager: ros__parameters: update_rate: 100 # Hz - integral_sliding_mode_controller: - type: velocity_controllers/IntegralSlidingModeController + adaptive_integral_terminal_sliding_mode_controller: + type: velocity_controllers/AdaptiveIntegralTerminalSlidingModeController thruster_allocation_matrix_controller: type: thruster_allocation_matrix_controller/ThrusterAllocationMatrixController @@ -32,27 +32,48 @@ controller_manager: thruster8_controller: type: thruster_controllers/PolynomialThrustCurveController -integral_sliding_mode_controller: +adaptive_integral_terminal_sliding_mode_controller: ros__parameters: - use_external_measured_states: true reference_controller: thruster_allocation_matrix_controller + use_external_measured_states: true gains: - rho: 20.0 - lambda: 200.0 - Kp: [10.0, 10.0, 6.0, 3.0, 6.0, 10.0] - tf: - base_frame: "base_link_fsd" - odom_frame: "map_ned" - hydrodynamics: - mass: 13.5 - weight: 114.80 - buoyancy: 112.80 - moments_of_inertia: [0.16, 0.16, 0.16] - added_mass: [-5.50, -12.70, -14.60, -0.12, -0.12, -0.12] - center_of_buoyancy: [0.0, 0.0, 0.0] - center_of_gravity: [0.0, 0.0, 0.0] - linear_damping: [-4.03, -6.22, -5.18, -0.07, -0.07, -0.07] - quadratic_damping: [-18.18, -21.66, -36.99, -1.55, -1.55, -1.55] + lambda: 0.9 + x: + alpha: 12.0 + k1_min: 0.1 + k2: 0.4 + k_theta: 2.0 + mu: 0.01 + y: + alpha: 12.0 + k1_min: 0.1 + k2: 0.4 + k_theta: 2.0 + mu: 0.01 + z: + alpha: 12.0 + k1_min: 0.4 + k2: 0.8 + k_theta: 2.0 + mu: 0.01 + rx: + alpha: 0.5 + k1_min: 0.1 + k2: 0.4 + k_theta: 2.0 + mu: 0.01 + ry: + alpha: 12.0 + k1_min: 0.1 + k2: 1.0 + k_theta: 2.0 + mu: 0.01 + rz: + alpha: 12.0 + k1_min: 0.1 + k2: 0.4 + k_theta: 2.0 + mu: 0.01 thruster_allocation_matrix_controller: ros__parameters: diff --git a/blue_demos/control_integration/config/bluerov2_heavy_transforms.yaml b/blue_demos/control_integration/config/bluerov2_heavy_transforms.yaml new file mode 100644 index 00000000..c03ec2ff --- /dev/null +++ b/blue_demos/control_integration/config/bluerov2_heavy_transforms.yaml @@ -0,0 +1,12 @@ +/control_integration/message_transforms: + ros__parameters: + + incoming_topics: + - /model/bluerov2_heavy/odometry + + transforms: + /model/bluerov2_heavy/odometry: + outgoing_topic: /adaptive_integral_terminal_sliding_mode_controller/system_state + message_type: nav_msgs/msg/Odometry + frame_id: map_fsd + child_frame_id: base_link_fsd diff --git a/blue_demos/control_integration/config/bluerov2_transforms.yaml b/blue_demos/control_integration/config/bluerov2_transforms.yaml new file mode 100644 index 00000000..058890a1 --- /dev/null +++ b/blue_demos/control_integration/config/bluerov2_transforms.yaml @@ -0,0 +1,12 @@ +/control_integration/message_transforms: + ros__parameters: + + incoming_topics: + - /model/bluerov2/odometry + + transforms: + /model/bluerov2/odometry: + outgoing_topic: /adaptive_integral_terminal_sliding_mode_controller/system_state + message_type: nav_msgs/msg/Odometry + frame_id: map_fsd + child_frame_id: base_link_fsd diff --git a/blue_demos/control_integration/config/transforms.yaml b/blue_demos/control_integration/config/transforms.yaml deleted file mode 100644 index 6f562a34..00000000 --- a/blue_demos/control_integration/config/transforms.yaml +++ /dev/null @@ -1,11 +0,0 @@ -/control_integration/message_transforms: - ros__parameters: - - incoming_topics: - - /mavros/local_position/velocity_body - - transforms: - /mavros/local_position/velocity_body: - outgoing_topic: /integral_sliding_mode_controller/system_state - message_type: geometry_msgs/msg/TwistStamped - frame_id: base_link_fsd diff --git a/blue_demos/control_integration/description/ros2_control/bluerov2.model.xacro b/blue_demos/control_integration/description/ros2_control/bluerov2.model.xacro new file mode 100644 index 00000000..353354bb --- /dev/null +++ b/blue_demos/control_integration/description/ros2_control/bluerov2.model.xacro @@ -0,0 +1,25 @@ + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/blue_demos/control_integration/description/ros2_control/bluerov2.ros2_control.xacro b/blue_demos/control_integration/description/ros2_control/bluerov2.ros2_control.xacro index 63e80692..8552b56a 100644 --- a/blue_demos/control_integration/description/ros2_control/bluerov2.ros2_control.xacro +++ b/blue_demos/control_integration/description/ros2_control/bluerov2.ros2_control.xacro @@ -3,7 +3,6 @@ - thruster_hardware/ThrusterHardware ${max_set_param_attempts} @@ -50,7 +49,6 @@ 6 - diff --git a/blue_demos/control_integration/description/ros2_control/bluerov2_heavy.model.xacro b/blue_demos/control_integration/description/ros2_control/bluerov2_heavy.model.xacro new file mode 100644 index 00000000..4c063398 --- /dev/null +++ b/blue_demos/control_integration/description/ros2_control/bluerov2_heavy.model.xacro @@ -0,0 +1,25 @@ + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/blue_demos/control_integration/description/ros2_control/bluerov2_heavy.ros2_control.xacro b/blue_demos/control_integration/description/ros2_control/bluerov2_heavy.ros2_control.xacro index 0ab9119a..a4f2dd27 100644 --- a/blue_demos/control_integration/description/ros2_control/bluerov2_heavy.ros2_control.xacro +++ b/blue_demos/control_integration/description/ros2_control/bluerov2_heavy.ros2_control.xacro @@ -3,7 +3,6 @@ - thruster_hardware/ThrusterHardware ${max_set_param_attempts} @@ -64,7 +63,6 @@ 8 - diff --git a/blue_demos/control_integration/description/urdf/bluerov2.config.xacro b/blue_demos/control_integration/description/urdf/bluerov2.config.xacro index 55a225f9..ce4b26e5 100644 --- a/blue_demos/control_integration/description/urdf/bluerov2.config.xacro +++ b/blue_demos/control_integration/description/urdf/bluerov2.config.xacro @@ -1,22 +1,20 @@ + + + + + - - - - - - - + diff --git a/blue_demos/control_integration/description/urdf/bluerov2_heavy.config.xacro b/blue_demos/control_integration/description/urdf/bluerov2_heavy.config.xacro index ece97666..be98b6af 100644 --- a/blue_demos/control_integration/description/urdf/bluerov2_heavy.config.xacro +++ b/blue_demos/control_integration/description/urdf/bluerov2_heavy.config.xacro @@ -1,22 +1,20 @@ + + + + + - - - - - - - + diff --git a/blue_demos/control_integration/launch/bluerov2_controllers.launch.py b/blue_demos/control_integration/launch/bluerov2_controllers.launch.py index e37dc412..b6a682ac 100644 --- a/blue_demos/control_integration/launch/bluerov2_controllers.launch.py +++ b/blue_demos/control_integration/launch/bluerov2_controllers.launch.py @@ -25,7 +25,7 @@ RegisterEventHandler, ) from launch.event_handlers import OnProcessExit -from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.launch_description_sources import FrontendLaunchDescriptionSource from launch.substitutions import PathJoinSubstitution, TextSubstitution from launch_ros.actions import Node from launch_ros.substitutions import FindPackageShare @@ -52,14 +52,14 @@ def generate_launch_description() -> LaunchDescription: ), ] - # The ISMC expects state information to be provided in the FSD frame + # The velocity controller expects state information to be provided in the FSD frame message_transformer = IncludeLaunchDescription( - PythonLaunchDescriptionSource( + FrontendLaunchDescriptionSource( PathJoinSubstitution( [ FindPackageShare("message_transforms"), "launch", - "message_transforms.launch.py", + "message_transforms.launch.yaml", ] ) ), @@ -69,7 +69,7 @@ def generate_launch_description() -> LaunchDescription: FindPackageShare("blue_demos"), "control_integration", "config", - "transforms.yaml", + "bluerov2_transforms.yaml", ] ), "ns": TextSubstitution(text="control_integration"), @@ -90,30 +90,27 @@ def generate_launch_description() -> LaunchDescription: ] ), ], - remappings=[ - ("/controller_manager/robot_description", "/robot_description"), - ], ) + def make_controller_args(name): + cm = ["--controller-manager", ["", "controller_manager"]] + controller_timeout = ["--controller-manager-timeout", "120"] + switch_timeout = ["--switch-timeout", "100"] + return [name, *cm, *controller_timeout, *switch_timeout] + velocity_controller_spawner = Node( package="controller_manager", executable="spawner", - arguments=[ - "integral_sliding_mode_controller", - "--controller-manager", - ["", "controller_manager"], - ], + arguments=make_controller_args( + "adaptive_integral_terminal_sliding_mode_controller" + ), ) thruster_spawners = [ Node( package="controller_manager", executable="spawner", - arguments=[ - f"thruster{i + 1}_controller", - "--controller-manager", - ["", "controller_manager"], - ], + arguments=make_controller_args(f"thruster{i + 1}_controller"), ) for i in range(6) # BlueROV2 has 6 thrusters ] diff --git a/blue_demos/control_integration/launch/bluerov2_heavy_controllers.launch.py b/blue_demos/control_integration/launch/bluerov2_heavy_controllers.launch.py index 3f6ab9a9..15608214 100644 --- a/blue_demos/control_integration/launch/bluerov2_heavy_controllers.launch.py +++ b/blue_demos/control_integration/launch/bluerov2_heavy_controllers.launch.py @@ -25,7 +25,7 @@ RegisterEventHandler, ) from launch.event_handlers import OnProcessExit -from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.launch_description_sources import FrontendLaunchDescriptionSource from launch.substitutions import PathJoinSubstitution, TextSubstitution from launch_ros.actions import Node from launch_ros.substitutions import FindPackageShare @@ -51,14 +51,14 @@ def generate_launch_description() -> LaunchDescription: ), ] - # The ISMC expects state information to be provided in the FSD frame + # The velocity controller expects state information to be provided in the FSD frame message_transformer = IncludeLaunchDescription( - PythonLaunchDescriptionSource( + FrontendLaunchDescriptionSource( PathJoinSubstitution( [ FindPackageShare("message_transforms"), "launch", - "message_transforms.launch.py", + "message_transforms.launch.yaml", ] ) ), @@ -68,7 +68,7 @@ def generate_launch_description() -> LaunchDescription: FindPackageShare("blue_demos"), "control_integration", "config", - "transforms.yaml", + "bluerov2_heavy_transforms.yaml", ] ), "ns": TextSubstitution(text="control_integration"), @@ -89,30 +89,27 @@ def generate_launch_description() -> LaunchDescription: ] ), ], - remappings=[ - ("/controller_manager/robot_description", "/robot_description"), - ], ) + def make_controller_args(name): + cm = ["--controller-manager", ["", "controller_manager"]] + controller_timeout = ["--controller-manager-timeout", "120"] + switch_timeout = ["--switch-timeout", "100"] + return [name, *cm, *controller_timeout, *switch_timeout] + velocity_controller_spawner = Node( package="controller_manager", executable="spawner", - arguments=[ - "integral_sliding_mode_controller", - "--controller-manager", - ["", "controller_manager"], - ], + arguments=make_controller_args( + "adaptive_integral_terminal_sliding_mode_controller" + ), ) thruster_spawners = [ Node( package="controller_manager", executable="spawner", - arguments=[ - f"thruster{i + 1}_controller", - "--controller-manager", - ["", "controller_manager"], - ], + arguments=make_controller_args(f"thruster{i + 1}_controller"), ) for i in range(8) # BlueROV2 Heavy has 8 thrusters ] diff --git a/blue_demos/manipulator_systems/config/initial_positions.yaml b/blue_demos/manipulator_systems/config/initial_positions.yaml new file mode 100644 index 00000000..c3fa71d2 --- /dev/null +++ b/blue_demos/manipulator_systems/config/initial_positions.yaml @@ -0,0 +1,7 @@ +# Default initial positions for the alpha ros2_control system +initial_positions: + axis_a: 0 + axis_b: 0 + axis_c: 1.57080 + axis_d: 0.01 + axis_e: 3.14159 diff --git a/blue_demos/manipulator_systems/config/uvms_controllers.yaml b/blue_demos/manipulator_systems/config/uvms_controllers.yaml new file mode 100644 index 00000000..574ae818 --- /dev/null +++ b/blue_demos/manipulator_systems/config/uvms_controllers.yaml @@ -0,0 +1,207 @@ +controller_manager: + ros__parameters: + update_rate: 100 # Hz + + end_effector_trajectory_controller: + type: end_effector_trajectory_controller/EndEffectorTrajectoryController + + adaptive_integral_terminal_sliding_mode_controller: + type: velocity_controllers/AdaptiveIntegralTerminalSlidingModeController + + thruster_allocation_matrix_controller: + type: thruster_allocation_matrix_controller/ThrusterAllocationMatrixController + + thruster1_controller: + type: thruster_controllers/GazeboPassthroughController + + thruster2_controller: + type: thruster_controllers/GazeboPassthroughController + + thruster3_controller: + type: thruster_controllers/GazeboPassthroughController + + thruster4_controller: + type: thruster_controllers/GazeboPassthroughController + + thruster5_controller: + type: thruster_controllers/GazeboPassthroughController + + thruster6_controller: + type: thruster_controllers/GazeboPassthroughController + + thruster7_controller: + type: thruster_controllers/GazeboPassthroughController + + thruster8_controller: + type: thruster_controllers/GazeboPassthroughController + + joint_state_broadcaster: + type: joint_state_broadcaster/JointStateBroadcaster + + tcp_position_controller: + type: forward_command_controller/ForwardCommandController + + whole_body_controller: + type: whole_body_controllers/IKController + +end_effector_trajectory_controller: + ros__parameters: + path_tolerance: 0.0 + goal_tolerance: 0.05 + lookup_end_effector_transform: true + reference_controller: whole_body_controller + odom_frame_id: map_ned + end_effector_frame_id: alpha_tcp + +whole_body_controller: + ros__parameters: + ik_solver: task_priority_solver + command_interfaces: + - velocity + state_interfaces: + - position + controlled_joints: + - alpha_axis_e + - alpha_axis_d + - alpha_axis_c + - alpha_axis_b + use_external_measured_vehicle_states: true + vehicle_reference_controller: adaptive_integral_terminal_sliding_mode_controller + task_priority_solver: + end_effector_frame_id: alpha_tcp + constrained_joints: + - alpha_axis_c + - alpha_axis_d + - alpha_axis_e + end_effector_pose_task: + gain: 0.38 + joint_limit_task: + alpha_axis_c: + safety_tolerance: 0.78539816 + activation_threshold: 0.087 + gain: 0.2 + alpha_axis_d: + safety_tolerance: 1.57079633 + activation_threshold: 0.087 + gain: 0.2 + alpha_axis_e: + safety_tolerance: 1.74532925 + activation_threshold: 0.17453292 + gain: 0.2 + +adaptive_integral_terminal_sliding_mode_controller: + ros__parameters: + reference_controller: thruster_allocation_matrix_controller + use_external_measured_states: true + gains: + lambda: 0.3 + x: + alpha: 12.0 + k1_min: 0.1 + k2: 0.4 + k_theta: 2.0 + mu: 0.01 + y: + alpha: 12.0 + k1_min: 0.1 + k2: 0.4 + k_theta: 2.0 + mu: 0.01 + z: + alpha: 16.0 + k1_min: 0.1 + k2: 0.4 + k_theta: 2.0 + mu: 0.01 + rx: + alpha: 0.5 + k1_min: 0.1 + k2: 0.4 + k_theta: 2.0 + mu: 0.01 + ry: + alpha: 20.0 + k1_min: 0.1 + k2: 1.0 + k_theta: 2.0 + mu: 0.01 + rz: + alpha: 14.0 + k1_min: 0.1 + k2: 0.4 + k_theta: 2.0 + mu: 0.01 + +thruster_allocation_matrix_controller: + ros__parameters: + thrusters: + - thruster1_joint + - thruster2_joint + - thruster3_joint + - thruster4_joint + - thruster5_joint + - thruster6_joint + - thruster7_joint + - thruster8_joint + reference_controllers: + - thruster1_controller + - thruster2_controller + - thruster3_controller + - thruster4_controller + - thruster5_controller + - thruster6_controller + - thruster7_controller + - thruster8_controller + tam: + x: [ -0.707, -0.707, 0.707, 0.707, 0.0, 0.0, 0.0, 0.0] + y: [ 0.707, -0.707, 0.707, -0.707, 0.0, 0.0, 0.0, 0.0] + z: [ 0.0, 0.0, 0.0, 0.0, 1.0, 1.0, 1.0, 1.0] + rx: [ 0.0, 0.0, 0.0, 0.0, -0.21805, 0.21805, -0.21805, 0.21805] + ry: [ 0.0, 0.0, 0.0, 0.0, -0.12, -0.12, 0.12, 0.12] + rz: [0.1888, -0.1888, -0.1888, 0.1888, 0.0, 0.0, 0.0, 0.0] + +thruster1_controller: + ros__parameters: + thruster: thruster1_joint + topic: /model/uvms/joint/thruster1_joint/cmd_thrust + +thruster2_controller: + ros__parameters: + thruster: thruster2_joint + topic: /model/uvms/joint/thruster2_joint/cmd_thrust + +thruster3_controller: + ros__parameters: + thruster: thruster3_joint + topic: /model/uvms/joint/thruster3_joint/cmd_thrust + +thruster4_controller: + ros__parameters: + thruster: thruster4_joint + topic: /model/uvms/joint/thruster4_joint/cmd_thrust + +thruster5_controller: + ros__parameters: + thruster: thruster5_joint + topic: /model/uvms/joint/thruster5_joint/cmd_thrust + +thruster6_controller: + ros__parameters: + thruster: thruster6_joint + topic: /model/uvms/joint/thruster6_joint/cmd_thrust + +thruster7_controller: + ros__parameters: + thruster: thruster7_joint + topic: /model/uvms/joint/thruster7_joint/cmd_thrust + +thruster8_controller: + ros__parameters: + thruster: thruster8_joint + topic: /model/uvms/joint/thruster8_joint/cmd_thrust + +tcp_position_controller: + ros__parameters: + joints: + - alpha_axis_a + interface_name: position diff --git a/blue_demos/manipulator_systems/config/uvms_transforms.yaml b/blue_demos/manipulator_systems/config/uvms_transforms.yaml new file mode 100644 index 00000000..78d50acb --- /dev/null +++ b/blue_demos/manipulator_systems/config/uvms_transforms.yaml @@ -0,0 +1,17 @@ +/manipulator_systems/message_transforms: + ros__parameters: + + incoming_topics: + - /model/uvms/odometry + - /model/target/pose + + transforms: + /model/uvms/odometry: + outgoing_topic: /model/uvms/odometry_ned + message_type: nav_msgs/msg/Odometry + frame_id: map_fsd + child_frame_id: base_link_fsd + + /model/target/pose: + outgoing_topic: /model/target/pose_ned + message_type: geometry_msgs/msg/Pose diff --git a/blue_demos/manipulator_systems/description/ros2_control/uvms.model.xacro b/blue_demos/manipulator_systems/description/ros2_control/uvms.model.xacro new file mode 100644 index 00000000..47c28728 --- /dev/null +++ b/blue_demos/manipulator_systems/description/ros2_control/uvms.model.xacro @@ -0,0 +1,25 @@ + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/blue_demos/manipulator_systems/description/ros2_control/uvms.ros2_control.xacro b/blue_demos/manipulator_systems/description/ros2_control/uvms.ros2_control.xacro new file mode 100644 index 00000000..1b207afb --- /dev/null +++ b/blue_demos/manipulator_systems/description/ros2_control/uvms.ros2_control.xacro @@ -0,0 +1,126 @@ + + + + + + + + + + + gz_ros2_control/GazeboSimSystem + + + + 1 + + 0 + 0.015 + + + -0.003 + 0.003 + + + + ${initial_positions['axis_a']} + + + 0.0 + + + 0.0 + + + + + 2 + + 0 + 3.22 + + + -0.8726646 + 0.8726646 + + + + ${initial_positions['axis_b']} + + + 0.0 + + + 0.0 + + + + + 3 + + 0 + 3.22 + + + -0.5235988 + 0.5235988 + + + + ${initial_positions['axis_c']} + + + 0.0 + + + 0.0 + + + + + 4 + + 0 + 3.49 + + + -0.5235988 + 0.5235988 + + + + ${initial_positions['axis_d']} + + + 0.0 + + + 0.0 + + + + + 5 + + 0 + 6.14 + + + -0.5235988 + 0.5235988 + + + + ${initial_positions['axis_e']} + + + 0.0 + + + 0.0 + + + + + + diff --git a/blue_demos/manipulator_systems/description/sdf/target.sdf b/blue_demos/manipulator_systems/description/sdf/target.sdf new file mode 100644 index 00000000..d92c95e0 --- /dev/null +++ b/blue_demos/manipulator_systems/description/sdf/target.sdf @@ -0,0 +1,28 @@ + + + + true + + + + + 0.03 + + + + 0.3 0.0 0.0 1 + 0.8 0.0 0.0 1 + 0.1 0.1 0.1 1 + 0 0 0 1 + + + + + false + false + false + true + true + + + diff --git a/blue_demos/manipulator_systems/description/urdf/uvms.config.xacro b/blue_demos/manipulator_systems/description/urdf/uvms.config.xacro new file mode 100644 index 00000000..18d06837 --- /dev/null +++ b/blue_demos/manipulator_systems/description/urdf/uvms.config.xacro @@ -0,0 +1,21 @@ + + + + + + + + + + + + + + + + + + + + diff --git a/blue_demos/manipulator_systems/description/urdf/uvms.gazebo.xacro b/blue_demos/manipulator_systems/description/urdf/uvms.gazebo.xacro new file mode 100644 index 00000000..674da209 --- /dev/null +++ b/blue_demos/manipulator_systems/description/urdf/uvms.gazebo.xacro @@ -0,0 +1,80 @@ + + + + + + + ${controllers_file} + + + + /adaptive_integral_terminal_sliding_mode_controller/system_state:=/model/uvms/odometry_ned + /whole_body_controller/vehicle_state:=/model/uvms/odometry_ned + /whole_body_controller/reference:=/model/target/pose_ned + + + + + + + ${prefix}base_footprint + + 0 + 0 + 0 + 0 + 0 + 0 + + 0 + 0 + 0 + 0 + 0 + 0 + + -58.42 + -55.137 + -124.818 + -4.0 + -4.0 + -4.0 + + + + + + map + ${prefix}base_link + 3 + 100 + + + + + + false + true + false + false + false + true + + + + + + + + + + + + + + + diff --git a/blue_demos/manipulator_systems/description/urdf/uvms.urdf.xacro b/blue_demos/manipulator_systems/description/urdf/uvms.urdf.xacro new file mode 100644 index 00000000..cf6d5ec9 --- /dev/null +++ b/blue_demos/manipulator_systems/description/urdf/uvms.urdf.xacro @@ -0,0 +1,56 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/blue_demos/manipulator_systems/launch/bridges.launch.yaml b/blue_demos/manipulator_systems/launch/bridges.launch.yaml new file mode 100644 index 00000000..a2bd0f39 --- /dev/null +++ b/blue_demos/manipulator_systems/launch/bridges.launch.yaml @@ -0,0 +1,68 @@ +launch: + - node: + pkg: ros_gz_bridge + exec: parameter_bridge + args: /clock@rosgraph_msgs/msg/Clock[gz.msgs.Clock + + - node: + pkg: ros_gz_bridge + exec: parameter_bridge + args: /camera/image_raw@sensor_msgs/msg/Image[gz.msgs.Image + + - node: + pkg: ros_gz_bridge + exec: parameter_bridge + args: /model/$(var gz_model_name)/odometry@nav_msgs/msg/Odometry[gz.msgs.Odometry + + - node: + pkg: ros_gz_bridge + exec: parameter_bridge + args: /model/$(var gz_model_name)/pose@tf2_msgs/msg/TFMessage[gz.msgs.Pose_V + remap: + - from: /model/$(var gz_model_name)/pose + to: /tf + + - node: + pkg: ros_gz_bridge + exec: parameter_bridge + args: /model/target/pose@geometry_msgs/msg/Pose[gz.msgs.Pose + + - node: + pkg: ros_gz_bridge + exec: parameter_bridge + args: /model/uvms/joint/thruster1_joint/cmd_thrust@std_msgs/msg/Float64]gz.msgs.Double + + - node: + pkg: ros_gz_bridge + exec: parameter_bridge + args: /model/uvms/joint/thruster2_joint/cmd_thrust@std_msgs/msg/Float64]gz.msgs.Double + + - node: + pkg: ros_gz_bridge + exec: parameter_bridge + args: /model/uvms/joint/thruster3_joint/cmd_thrust@std_msgs/msg/Float64]gz.msgs.Double + + - node: + pkg: ros_gz_bridge + exec: parameter_bridge + args: /model/uvms/joint/thruster4_joint/cmd_thrust@std_msgs/msg/Float64]gz.msgs.Double + + - node: + pkg: ros_gz_bridge + exec: parameter_bridge + args: /model/uvms/joint/thruster5_joint/cmd_thrust@std_msgs/msg/Float64]gz.msgs.Double + + - node: + pkg: ros_gz_bridge + exec: parameter_bridge + args: /model/uvms/joint/thruster6_joint/cmd_thrust@std_msgs/msg/Float64]gz.msgs.Double + + - node: + pkg: ros_gz_bridge + exec: parameter_bridge + args: /model/uvms/joint/thruster7_joint/cmd_thrust@std_msgs/msg/Float64]gz.msgs.Double + + - node: + pkg: ros_gz_bridge + exec: parameter_bridge + args: /model/uvms/joint/thruster8_joint/cmd_thrust@std_msgs/msg/Float64]gz.msgs.Double diff --git a/blue_demos/manipulator_systems/launch/interactive_uvms_demo.launch.yaml b/blue_demos/manipulator_systems/launch/interactive_uvms_demo.launch.yaml new file mode 100644 index 00000000..a3a0838d --- /dev/null +++ b/blue_demos/manipulator_systems/launch/interactive_uvms_demo.launch.yaml @@ -0,0 +1,42 @@ +launch: + - let: + name: gz_model_name + value: "uvms" + + - let: + name: gz_world_file + value: $(find-pkg-share blue_description)/gazebo/worlds/underwater.world + + - let: + name: controllers_file + value: $(find-pkg-share blue_demos)/manipulator_systems/config/uvms_controllers.yaml + + - let: + name: description_file + value: $(find-pkg-share blue_demos)/manipulator_systems/description/urdf/uvms.config.xacro + + - let: + name: robot_description + value: $(command 'xacro $(var description_file) controllers_file:=$(var controllers_file)') + + - include: + file: $(find-pkg-share blue_demos)/manipulator_systems/launch/bridges.launch.yaml + + - include: + file: $(find-pkg-share blue_bringup)/launch/bluerov2_heavy_reach/thrusters.launch.yaml + + - include: + file: $(find-pkg-share blue_demos)/manipulator_systems/launch/uvms_base.launch.py + arg: + - name: gz_model_name + value: $(var gz_model_name) + - name: gz_world_file + value: $(var gz_world_file) + - name: robot_description + value: $(var robot_description) + - name: controllers_file + value: $(var controllers_file) + - name: use_trajectory_control + value: "false" + - name: use_target + value: "true" diff --git a/blue_demos/manipulator_systems/launch/publish_trajectory.launch.yaml b/blue_demos/manipulator_systems/launch/publish_trajectory.launch.yaml new file mode 100644 index 00000000..ffc5d671 --- /dev/null +++ b/blue_demos/manipulator_systems/launch/publish_trajectory.launch.yaml @@ -0,0 +1,49 @@ +launch: + - executable: + cmd: > + ros2 topic pub /end_effector_trajectory_controller/trajectory auv_control_msgs/msg/EndEffectorTrajectory '{ + header: { + stamp: { + sec: 0, + nanosec: 0 + }, + frame_id: "alpha_tcp" + }, + points: [ + { + point: { + position: {x: 0.0, y: 0.0, z: 0.5}, + orientation: {x: 1.0, y: 0.0, z: 0.0, w: 0.0} + }, + time_from_start: {sec: 5, nanosec: 0} + }, + { + point: { + position: {x: 1.0, y: 0.0, z: 0.5}, + orientation: {x: 1.0, y: 0.0, z: 0.0, w: 0.0} + }, + time_from_start: {sec: 10, nanosec: 0} + }, + { + point: { + position: {x: 1.0, y: 1.0, z: 0.5}, + orientation: {x: 1.0, y: 0.0, z: 0.0, w: 0.0} + }, + time_from_start: {sec: 15, nanosec: 0} + }, + { + point: { + position: {x: 0.0, y: 1.0, z: 0.5}, + orientation: {x: 1.0, y: 0.0, z: 0.0, w: 0.0} + }, + time_from_start: {sec: 20, nanosec: 0} + }, + { + point: { + position: {x: 0.0, y: 0.0, z: 0.5}, + orientation: {x: 1.0, y: 0.0, z: 0.0, w: 0.0} + }, + time_from_start: {sec: 25, nanosec: 0} + } + ] + }' --once diff --git a/blue_demos/manipulator_systems/launch/send_trajectory_action.launch.yaml b/blue_demos/manipulator_systems/launch/send_trajectory_action.launch.yaml new file mode 100644 index 00000000..5efffb6a --- /dev/null +++ b/blue_demos/manipulator_systems/launch/send_trajectory_action.launch.yaml @@ -0,0 +1,50 @@ +launch: + - executable: + cmd: > + ros2 action send_goal /end_effector_trajectory_controller/follow_trajectory auv_control_msgs/action/FollowEndEffectorTrajectory '{ + trajectory: { + header: { + stamp: {sec: 0, nanosec: 0}, + frame_id: 'alpha_tcp' + }, + points: [ + { + point: { + position: {x: 0.0, y: 0.0, z: 0.5}, + orientation: {x: 1.0, y: 0.0, z: 0.0, w: 0.0} + }, + time_from_start: {sec: 5, nanosec: 0} + }, + { + point: { + position: {x: 1.0, y: 0.0, z: 0.5}, + orientation: {x: 1.0, y: 0.0, z: 0.0, w: 0.0} + }, + time_from_start: {sec: 10, nanosec: 0} + }, + { + point: { + position: {x: 1.0, y: 1.0, z: 0.5}, + orientation: {x: 1.0, y: 0.0, z: 0.0, w: 0.0} + }, + time_from_start: {sec: 15, nanosec: 0} + }, + { + point: { + position: {x: 0.0, y: 1.0, z: 0.5}, + orientation: {x: 1.0, y: 0.0, z: 0.0, w: 0.0} + }, + time_from_start: {sec: 20, nanosec: 0} + }, + { + point: { + position: {x: 0.0, y: 0.0, z: 0.5}, + orientation: {x: 1.0, y: 0.0, z: 0.0, w: 0.0} + }, + time_from_start: {sec: 25, nanosec: 0} + } + ] + }, + path_tolerance: 0.0, + goal_tolerance: 0.05, + }' diff --git a/blue_demos/manipulator_systems/launch/trajectory_controller_demo.launch.yaml b/blue_demos/manipulator_systems/launch/trajectory_controller_demo.launch.yaml new file mode 100644 index 00000000..bb74d429 --- /dev/null +++ b/blue_demos/manipulator_systems/launch/trajectory_controller_demo.launch.yaml @@ -0,0 +1,45 @@ +launch: + - let: + name: gz_model_name + value: "uvms" + + - let: + name: gz_world_file + value: $(find-pkg-share blue_description)/gazebo/worlds/underwater.world + + - let: + name: controllers_file + value: $(find-pkg-share blue_demos)/manipulator_systems/config/uvms_controllers.yaml + + - let: + name: description_file + value: $(find-pkg-share blue_demos)/manipulator_systems/description/urdf/uvms.config.xacro + + - let: + name: robot_description + value: $(command 'xacro $(var description_file) controllers_file:=$(var controllers_file)') + + - include: + file: $(find-pkg-share blue_demos)/manipulator_systems/launch/bridges.launch.yaml + + - include: + file: $(find-pkg-share message_transforms)/launch/tf.launch.yaml + + - include: + file: $(find-pkg-share blue_bringup)/launch/bluerov2_heavy_reach/thrusters.launch.yaml + + - include: + file: $(find-pkg-share blue_demos)/manipulator_systems/launch/uvms_base.launch.py + arg: + - name: gz_model_name + value: $(var gz_model_name) + - name: gz_world_file + value: $(var gz_world_file) + - name: robot_description + value: $(var robot_description) + - name: controllers_file + value: $(var controllers_file) + - name: use_trajectory_control + value: "true" + - name: use_target + value: "false" diff --git a/blue_demos/manipulator_systems/launch/uvms_base.launch.py b/blue_demos/manipulator_systems/launch/uvms_base.launch.py new file mode 100644 index 00000000..d3fe09b6 --- /dev/null +++ b/blue_demos/manipulator_systems/launch/uvms_base.launch.py @@ -0,0 +1,287 @@ +# Copyright 2025, Evan Palmer +# +# Permission is hereby granted, free of charge, to any person obtaining a copy +# of this software and associated documentation files (the "Software"), to deal +# in the Software without restriction, including without limitation the rights +# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +# copies of the Software, and to permit persons to whom the Software is +# furnished to do so, subject to the following conditions: +# +# The above copyright notice and this permission notice shall be included in +# all copies or substantial portions of the Software. +# +# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL +# THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +# THE SOFTWARE. + +import os + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import ( + DeclareLaunchArgument, + ExecuteProcess, + IncludeLaunchDescription, + RegisterEventHandler, +) +from launch.conditions import IfCondition +from launch.event_handlers import OnExecutionComplete, OnProcessExit +from launch.launch_description_sources import ( + FrontendLaunchDescriptionSource, + PythonLaunchDescriptionSource, +) +from launch.substitutions import ( + LaunchConfiguration, + TextSubstitution, +) +from launch_ros.actions import Node + + +def generate_launch_description() -> LaunchDescription: + args = [ + DeclareLaunchArgument("gz_model_name"), + DeclareLaunchArgument("gz_world_file"), + DeclareLaunchArgument("controllers_file"), + DeclareLaunchArgument("robot_description"), + DeclareLaunchArgument("use_trajectory_control"), + DeclareLaunchArgument("use_target"), + ] + + robot_state_publisher = Node( + package="robot_state_publisher", + executable="robot_state_publisher", + output="both", + parameters=[ + { + "robot_description": LaunchConfiguration("robot_description"), + "use_sim_time": True, + } + ], + ) + + # Gazebo launch + gz_spawner = Node( + package="ros_gz_sim", + executable="create", + arguments=[ + "-name", + LaunchConfiguration("gz_model_name"), + "-topic", + "robot_description", + ], + output="screen", + ) + + gz_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + [ + os.path.join( + get_package_share_directory("ros_gz_sim"), + "launch", + "gz_sim.launch.py", + ), + ] + ), + launch_arguments=[ + ( + "gz_args", + ["-v 4 -r", " ", LaunchConfiguration("gz_world_file")], + ) + ], + ) + + # the velocity controller expects state information to be provided in the FSD frame + message_transformer = IncludeLaunchDescription( + FrontendLaunchDescriptionSource( + os.path.join( + get_package_share_directory("message_transforms"), + "launch", + "message_transforms.launch.yaml", + ) + ), + launch_arguments={ + "parameters_file": os.path.join( + get_package_share_directory("blue_demos"), + "manipulator_systems", + "config", + "uvms_transforms.yaml", + ), + "ns": TextSubstitution(text="manipulator_systems"), + }.items(), + ) + + # spawn a target pose for the whole body controller + spawn_pose = ExecuteProcess( + cmd=[ + "ros2", + "run", + "ros_gz_sim", + "create", + "-file", + os.path.join( + get_package_share_directory("blue_demos"), + "manipulator_systems", + "description", + "sdf", + "target.sdf", + ), + "-name", + "target", + "-x", + "1", + "-y", + "0", + "-z", + "-1", + ], + output="screen", + condition=IfCondition(LaunchConfiguration("use_target")), + ) + + delay_target_spawner_after_gz_spawner = RegisterEventHandler( + event_handler=OnProcessExit(target_action=gz_spawner, on_exit=[spawn_pose]) + ) + + def make_controller_args(name): + cm = ["--controller-manager", ["", "controller_manager"]] + controller_timeout = ["--controller-manager-timeout", "120"] + switch_timeout = ["--switch-timeout", "100"] + return [name, *cm, *controller_timeout, *switch_timeout] + + velocity_controller_spawner = Node( + package="controller_manager", + executable="spawner", + arguments=make_controller_args( + "adaptive_integral_terminal_sliding_mode_controller" + ), + ) + + tam_controller_spawner = Node( + package="controller_manager", + executable="spawner", + arguments=make_controller_args("thruster_allocation_matrix_controller"), + ) + + thruster_controller_spawners = [ + Node( + package="controller_manager", + executable="spawner", + arguments=make_controller_args(f"thruster{i + 1}_controller"), + ) + for i in range(8) + ] + + joint_state_broadcaster_spawner = Node( + package="controller_manager", + executable="spawner", + arguments=make_controller_args("joint_state_broadcaster"), + ) + + tcp_controller_spawner = Node( + package="controller_manager", + executable="spawner", + arguments=make_controller_args("tcp_position_controller"), + ) + + whole_body_controller_spawner = Node( + package="controller_manager", + executable="spawner", + arguments=make_controller_args("whole_body_controller"), + ) + + trajectory_controller_spawner = Node( + package="controller_manager", + executable="spawner", + arguments=make_controller_args("end_effector_trajectory_controller"), + condition=IfCondition(LaunchConfiguration("use_trajectory_control")), + ) + + # launch the thrusters sequentially + delay_thruster_controller_spawners = [] + for i, thruster_spawner in enumerate(thruster_controller_spawners): + if not len(delay_thruster_controller_spawners): + delay_thruster_controller_spawners.append( + RegisterEventHandler( + event_handler=OnProcessExit( + target_action=gz_spawner, on_exit=[thruster_spawner] + ) + ) + ) + else: + delay_thruster_controller_spawners.append( + RegisterEventHandler( + event_handler=OnExecutionComplete( + target_action=thruster_controller_spawners[i - 1], + on_completion=[thruster_spawner], + ) + ) + ) + + # launch the TAM controller after the thruster controllers + delay_tam_controller_spawner_after_thruster_controller_spawners = ( + RegisterEventHandler( + event_handler=OnExecutionComplete( + target_action=thruster_controller_spawners[-1], + on_completion=[tam_controller_spawner], + ) + ) + ) + + # launch the velocity controller after the TAM controller + delay_velocity_controller_spawner_after_tam_controller_spawner = ( + RegisterEventHandler( + event_handler=OnExecutionComplete( + target_action=tam_controller_spawner, + on_completion=[velocity_controller_spawner], + ) + ) + ) + + delay_jsb_spawner_after_gz_spawner = RegisterEventHandler( + event_handler=OnProcessExit( + target_action=gz_spawner, on_exit=[joint_state_broadcaster_spawner] + ) + ) + + delay_tcp_controller_spawner_after_jsb_spawner = RegisterEventHandler( + event_handler=OnExecutionComplete( + target_action=joint_state_broadcaster_spawner, + on_completion=[tcp_controller_spawner], + ) + ) + + delay_wbc_spawner_after_velocity_controller_spawner = RegisterEventHandler( + event_handler=OnExecutionComplete( + target_action=velocity_controller_spawner, + on_completion=[whole_body_controller_spawner], + ) + ) + + delay_trajectory_controller_spawner_after_wbc_spawner = RegisterEventHandler( + event_handler=OnExecutionComplete( + target_action=whole_body_controller_spawner, + on_completion=[trajectory_controller_spawner], + ), + ) + + return LaunchDescription( + [ + *args, + message_transformer, + robot_state_publisher, + gz_spawner, + gz_launch, + delay_target_spawner_after_gz_spawner, + *delay_thruster_controller_spawners, + delay_tam_controller_spawner_after_thruster_controller_spawners, + delay_velocity_controller_spawner_after_tam_controller_spawner, + delay_jsb_spawner_after_gz_spawner, + delay_tcp_controller_spawner_after_jsb_spawner, + delay_wbc_spawner_after_velocity_controller_spawner, + delay_trajectory_controller_spawner_after_wbc_spawner, + ] + ) diff --git a/blue_demos/package.xml b/blue_demos/package.xml index a27a7dea..e700f049 100644 --- a/blue_demos/package.xml +++ b/blue_demos/package.xml @@ -19,6 +19,8 @@ teleop_twist_keyboard joy_teleop joy_linux + ros_gz + robot_localization ament_cmake diff --git a/blue_demos/teleoperation/config/transforms.yaml b/blue_demos/teleoperation/config/transforms.yaml index 00c04540..61bcc030 100644 --- a/blue_demos/teleoperation/config/transforms.yaml +++ b/blue_demos/teleoperation/config/transforms.yaml @@ -6,5 +6,5 @@ message_transforms: transforms: /cmd_vel: - outgoing_topic: /integral_sliding_mode_controller/reference + outgoing_topic: /adaptive_integral_terminal_sliding_mode_controller/reference message_type: geometry_msgs/msg/Twist diff --git a/blue_demos/teleoperation/launch/joy_teleop.launch.yaml b/blue_demos/teleoperation/launch/joy_teleop.launch.yaml index 966f3620..af05a886 100644 --- a/blue_demos/teleoperation/launch/joy_teleop.launch.yaml +++ b/blue_demos/teleoperation/launch/joy_teleop.launch.yaml @@ -23,7 +23,7 @@ launch: - from: $(var joy_file) - include: - file: $(find-pkg-share message_transforms)/launch/message_transforms.launch.py + file: $(find-pkg-share message_transforms)/launch/message_transforms.launch.yaml arg: - name: parameters_file value: $(var transforms_file) diff --git a/blue_description/CMakeLists.txt b/blue_description/CMakeLists.txt index cc68f270..afcea954 100644 --- a/blue_description/CMakeLists.txt +++ b/blue_description/CMakeLists.txt @@ -4,8 +4,8 @@ project(blue_description) find_package(ament_cmake REQUIRED) install( - DIRECTORY config gazebo meshes description rviz - DESTINATION share/blue_description + DIRECTORY config gazebo meshes description rviz + DESTINATION share/blue_description ) ament_environment_hooks("${CMAKE_CURRENT_SOURCE_DIR}/hooks/hooks.dsv.in") diff --git a/blue_description/config/bluerov2/localization.yaml b/blue_description/config/bluerov2/localization.yaml deleted file mode 100644 index 76ba091e..00000000 --- a/blue_description/config/bluerov2/localization.yaml +++ /dev/null @@ -1,34 +0,0 @@ -camera: - ros__parameters: - port: 5600 - distortion_model: "plumb_bob" - frame: - height: 1080 - width: 1920 - camera_matrix: - [1078.17559, 0.0, 1010.57086, - 0.0, 1076.46176, 463.06243, - 0.0, 0.0, 1.0] - distortion_coefficients: - [0.019645, 0.007271, -0.004324, -0.001628, 0.000000] - projection_matrix: - [1108.25366, 0.0, 1003.75555, 0.0, - 0.0, 1108.39001, 456.92861, 0.0, - 0.0, 0.0, 1.0, 0.0] - -qualisys_mocap: - ros__parameters: - ip: "192.168.254.1" - port: 22223 - version: "1.22" - body: "bluerov" - -qualisys_localizer: - ros__parameters: - update_rate: 30.0 - body: "bluerov" # This should be the same as the body parameter setting for the qualisys_mocap node - -gazebo_localizer: - ros__parameters: - update_rate: 30.0 - gazebo_odom_topic: "/model/bluerov2/odometry" diff --git a/blue_description/config/bluerov2_heavy/localization.yaml b/blue_description/config/bluerov2_heavy/localization.yaml deleted file mode 100644 index 5cbec6d4..00000000 --- a/blue_description/config/bluerov2_heavy/localization.yaml +++ /dev/null @@ -1,34 +0,0 @@ -camera: - ros__parameters: - port: 5600 - distortion_model: "plumb_bob" - frame: - height: 1080 - width: 1920 - camera_matrix: - [1078.17559, 0.0, 1010.57086, - 0.0, 1076.46176, 463.06243, - 0.0, 0.0, 1.0] - distortion_coefficients: - [0.019645, 0.007271, -0.004324, -0.001628, 0.000000] - projection_matrix: - [1108.25366, 0.0, 1003.75555, 0.0, - 0.0, 1108.39001, 456.92861, 0.0, - 0.0, 0.0, 1.0, 0.0] - -qualisys_mocap: - ros__parameters: - ip: "192.168.254.1" - port: 22223 - version: "1.22" - body: "bluerov" - -qualisys_localizer: - ros__parameters: - update_rate: 30.0 - body: "bluerov" # This should be the same as the body parameter setting for the qualisys_mocap node - -gazebo_localizer: - ros__parameters: - update_rate: 30.0 - gazebo_odom_topic: "/model/bluerov2_heavy/odometry" diff --git a/blue_description/config/bluerov2_heavy_reach/localization.yaml b/blue_description/config/bluerov2_heavy_reach/localization.yaml deleted file mode 100644 index 53e8cb8b..00000000 --- a/blue_description/config/bluerov2_heavy_reach/localization.yaml +++ /dev/null @@ -1,34 +0,0 @@ -camera: - ros__parameters: - port: 5600 - distortion_model: "plumb_bob" - frame: - height: 1080 - width: 1920 - camera_matrix: - [1078.17559, 0.0, 1010.57086, - 0.0, 1076.46176, 463.06243, - 0.0, 0.0, 1.0] - distortion_coefficients: - [0.019645, 0.007271, -0.004324, -0.001628, 0.000000] - projection_matrix: - [1108.25366, 0.0, 1003.75555, 0.0, - 0.0, 1108.39001, 456.92861, 0.0, - 0.0, 0.0, 1.0, 0.0] - -qualisys_mocap: - ros__parameters: - ip: "192.168.254.1" - port: 22223 - version: "1.22" - body: "bluerov" - -qualisys_localizer: - ros__parameters: - update_rate: 30.0 - body: "bluerov" # This should be the same as the body parameter setting for the qualisys_mocap node - -gazebo_localizer: - ros__parameters: - update_rate: 30.0 - gazebo_odom_topic: "/model/bluerov2_heavy_reach/odometry" diff --git a/blue_description/description/bluerov2/gazebo.xacro b/blue_description/description/bluerov2/gazebo.xacro index f8938e36..791f152b 100644 --- a/blue_description/description/bluerov2/gazebo.xacro +++ b/blue_description/description/bluerov2/gazebo.xacro @@ -5,7 +5,7 @@ - + 127.0.0.1 9002 5 @@ -32,6 +32,7 @@ -0.5 100 + ${prefix}thruster2_joint 1100 @@ -41,6 +42,7 @@ -0.5 100 + ${prefix}thruster3_joint 1100 @@ -50,6 +52,7 @@ -0.5 100 + ${prefix}thruster4_joint 1100 @@ -59,6 +62,7 @@ -0.5 100 + ${prefix}thruster5_joint 1100 @@ -68,6 +72,7 @@ -0.5 100 + ${prefix}thruster6_joint 1100 diff --git a/blue_description/description/bluerov2_heavy/gazebo.xacro b/blue_description/description/bluerov2_heavy/gazebo.xacro index 011dc392..74fb2eb5 100644 --- a/blue_description/description/bluerov2_heavy/gazebo.xacro +++ b/blue_description/description/bluerov2_heavy/gazebo.xacro @@ -5,7 +5,7 @@ - + 127.0.0.1 9002 5 diff --git a/blue_description/description/bluerov2_heavy_reach/gazebo.xacro b/blue_description/description/bluerov2_heavy_reach/gazebo.xacro index c121cfe4..e078af93 100644 --- a/blue_description/description/bluerov2_heavy_reach/gazebo.xacro +++ b/blue_description/description/bluerov2_heavy_reach/gazebo.xacro @@ -5,7 +5,7 @@ - + 127.0.0.1 9002 5 @@ -139,9 +139,7 @@ - + map ${prefix}base_link 3 diff --git a/blue_description/gazebo/worlds/underwater.world b/blue_description/gazebo/worlds/underwater.world index 22e75d54..b0d386e9 100644 --- a/blue_description/gazebo/worlds/underwater.world +++ b/blue_description/gazebo/worlds/underwater.world @@ -2,28 +2,12 @@ - - - - - - - - - - - + + + + - + 1000 diff --git a/blue_localization/LICENSE b/blue_localization/LICENSE deleted file mode 100644 index 30e8e2ec..00000000 --- a/blue_localization/LICENSE +++ /dev/null @@ -1,17 +0,0 @@ -Permission is hereby granted, free of charge, to any person obtaining a copy -of this software and associated documentation files (the "Software"), to deal -in the Software without restriction, including without limitation the rights -to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -copies of the Software, and to permit persons to whom the Software is -furnished to do so, subject to the following conditions: - -The above copyright notice and this permission notice shall be included in -all copies or substantial portions of the Software. - -THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL -THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN -THE SOFTWARE. diff --git a/blue_localization/blue_localization/__init__.py b/blue_localization/blue_localization/__init__.py deleted file mode 100644 index e69de29b..00000000 diff --git a/blue_localization/blue_localization/localizer.py b/blue_localization/blue_localization/localizer.py deleted file mode 100644 index a12a65f5..00000000 --- a/blue_localization/blue_localization/localizer.py +++ /dev/null @@ -1,674 +0,0 @@ -# Copyright 2023, Evan Palmer -# -# Permission is hereby granted, free of charge, to any person obtaining a copy -# of this software and associated documentation files (the "Software"), to deal -# in the Software without restriction, including without limitation the rights -# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -# copies of the Software, and to permit persons to whom the Software is -# furnished to do so, subject to the following conditions: -# -# The above copyright notice and this permission notice shall be included in -# all copies or substantial portions of the Software. -# -# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL -# THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN -# THE SOFTWARE. - -import time -from abc import ABC, abstractmethod -from collections import deque -from typing import Any, Deque - -import cv2 -import numpy as np -import rclpy -import tf2_geometry_msgs # noqa -from cv_bridge import CvBridge -from geometry_msgs.msg import ( - Pose, - PoseStamped, - PoseWithCovarianceStamped, - TwistStamped, - TwistWithCovarianceStamped, -) -from nav_msgs.msg import Odometry -from rclpy.callback_groups import MutuallyExclusiveCallbackGroup -from rclpy.executors import MultiThreadedExecutor -from rclpy.node import Node -from rclpy.qos import ( - DurabilityPolicy, - HistoryPolicy, - QoSProfile, - ReliabilityPolicy, - qos_profile_default, - qos_profile_sensor_data, -) -from scipy.spatial.transform import Rotation as R -from sensor_msgs.msg import CameraInfo, Image -from tf2_ros import TransformException # type: ignore -from tf2_ros import Time -from tf2_ros.buffer import Buffer -from tf2_ros.transform_listener import TransformListener - - -class Localizer(Node, ABC): - """Base class for implementing a visual localization interface.""" - - MAP_FRAME = "map" - MAP_NED_FRAME = "map_ned" - BASE_LINK_FRAME = "base_link" - BASE_LINK_FRD_FRAME = "base_link_frd" - CAMERA_FRAME = "camera_link" - - def __init__(self, node_name: str) -> None: - """Create a new localizer. - - Args: - node_name: The name of the ROS 2 node. - """ - Node.__init__(self, node_name) - ABC.__init__(self) - - self.declare_parameter("update_rate", 30.0) - - # Provide access to TF2 - self.tf_buffer = Buffer() - self.tf_listener = TransformListener(self.tf_buffer, self) - - # Publish the current state at the provided rate. Note that, if the localizer - # receives state messages at a lower rate, the state will be published at the - # rate at which it is received (basically just a low-pass filter). The reason - # for applying a filter is to ensure that high-frequency state updates don't - # overload the FCU. - self._state = None - self._update_rate = 1 / ( - self.get_parameter("update_rate").get_parameter_value().double_value - ) - self._last_update = time.time() - self.update_state_timer = self.create_timer( - self._update_rate, self._publish_wrapper, MutuallyExclusiveCallbackGroup() - ) - - @property - def state(self) -> Any: - """Get the current state obtained by a localizer. - - Returns: - The current state. - """ - return self._state - - @state.setter - def state(self, state: Any) -> None: - """Set the current state to be published by the EKF. - - Args: - state: The current state. - """ - self._last_update = time.time() - self._state = state - - def _publish_wrapper(self) -> None: - """Publish the state at the max allowable frequency. - - If the state hasn't been updated since the last loop iteration, don't publish - the state again: only publish a state once. - """ - if self.state is None or time.time() - self._last_update > self._update_rate: - return - - self.publish() - - @abstractmethod - def publish(self) -> None: - """Publish the state to the ArduSub EKF. - - This is automatically called by the localizer timer and should not be called - manually. - """ - ... - - -class PoseLocalizer(Localizer): - """Interface for sending pose estimates to the ArduSub EKF.""" - - def __init__(self, node_name: str) -> None: - """Create a new pose localizer. - - Args: - node_name: The name of the localizer node. - """ - super().__init__(node_name) - - # Poses are sent to the ArduPilot EKF - self.vision_pose_pub = self.create_publisher( - PoseStamped, - "/mavros/vision_pose/pose", - qos_profile_default, - ) - self.vision_pose_cov_pub = self.create_publisher( - PoseWithCovarianceStamped, - "/mavros/vision_pose/pose_cov", - qos_profile_default, - ) - - def publish(self) -> None: - """Publish a pose message to the ArduSub EKF.""" - if isinstance(self.state, PoseStamped): - self.vision_pose_pub.publish(self.state) - elif isinstance(self.state, PoseWithCovarianceStamped): - self.vision_pose_cov_pub.publish(self.state) - else: - raise TypeError( - "Invalid state type provided for publishing. Expected one of" - f" {PoseStamped.__name__}, {PoseWithCovarianceStamped.__name__}: got" - f" {self.state.__class__.__name__}" - ) - - -class TwistLocalizer(Localizer): - """Interface for sending pose estimates to the ArduSub EKF.""" - - def __init__(self, node_name: str) -> None: - """Create a new pose localizer. - - Args: - node_name: The name of the localizer node. - """ - super().__init__(node_name) - - # Twists are sent to the ArduPilot EKF - self.vision_speed_pub = self.create_publisher( - TwistStamped, "/mavros/vision_speed/speed", qos_profile_default - ) - self.vision_speed_cov_pub = self.create_publisher( - TwistWithCovarianceStamped, - "/mavros/vision_speed/speed_cov", - qos_profile_default, - ) - - def publish(self) -> None: - """Publish a twist message to the ArduSub EKF.""" - if isinstance(self.state, TwistStamped): - self.vision_speed_pub.publish(self.state) - elif isinstance(self.state, TwistWithCovarianceStamped): - self.vision_speed_cov_pub.publish(self.state) - else: - raise TypeError( - "Invalid state type provided for publishing. Expected one of" - f" {TwistStamped.__name__}, {TwistWithCovarianceStamped.__name__}: got" - f" {self.state.__class__.__name__}" - ) - - -class ArucoMarkerLocalizer(PoseLocalizer): - """Performs localization using ArUco markers.""" - - ARUCO_MARKER_TYPES = [ - cv2.aruco.DICT_4X4_50, - cv2.aruco.DICT_4X4_100, - cv2.aruco.DICT_4X4_250, - cv2.aruco.DICT_4X4_1000, - cv2.aruco.DICT_5X5_50, - cv2.aruco.DICT_5X5_100, - cv2.aruco.DICT_5X5_250, - cv2.aruco.DICT_5X5_1000, - cv2.aruco.DICT_6X6_50, - cv2.aruco.DICT_6X6_100, - cv2.aruco.DICT_6X6_250, - cv2.aruco.DICT_6X6_1000, - cv2.aruco.DICT_7X7_50, - cv2.aruco.DICT_7X7_100, - cv2.aruco.DICT_7X7_250, - cv2.aruco.DICT_7X7_1000, - cv2.aruco.DICT_ARUCO_ORIGINAL, - ] - - def __init__(self) -> None: - """Create a new ArUco marker localizer.""" - super().__init__("aruco_marker_localizer") - - self.bridge = CvBridge() - self.camera_info: CameraInfo | None = None - - self.camera_info_sub = self.create_subscription( - CameraInfo, - "/camera/camera_info", - self.get_camera_info_cb, - QoSProfile( - reliability=ReliabilityPolicy.RELIABLE, - durability=DurabilityPolicy.TRANSIENT_LOCAL, - history=HistoryPolicy.KEEP_LAST, - depth=1, - ), - ) - self.camera_sub = self.create_subscription( - Image, - "/camera/image_raw", - self.update_pose_cb, - qos_profile_sensor_data, - ) - - def get_camera_info_cb(self, info: CameraInfo) -> None: - """Get the camera info from the camera. - - Args: - info: The camera meta information. - """ - self.camera_info = info - - def detect_markers(self, frame: np.ndarray) -> tuple[Any, Any] | None: - """Detect any ArUco markers in the frame. - - All markers in a frame should be the same type of ArUco marker - (e.g., 4x4 50) if multiple are expected to be in-frame. - - Args: - frame: The video frame containing ArUco markers. - - Returns: - A list of marker corners and IDs. If no markers were found, returns None. - """ - # Check each tag type, breaking when we find one that works - for tag_type in self.ARUCO_MARKER_TYPES: - aruco_dict = cv2.aruco.Dictionary_get(tag_type) - aruco_params = cv2.aruco.DetectorParameters_create() - - try: - # Return the corners and ids if we find the correct tag type - corners, ids, _ = cv2.aruco.detectMarkers( - frame, aruco_dict, parameters=aruco_params - ) - - if len(ids) > 0: - return corners, ids - - except Exception: - continue - - # Nothing was found - return None - - def get_camera_pose(self, frame: np.ndarray) -> tuple[Any, Any, int] | None: - """Get the pose of the camera relative to any ArUco markers detected. - - If multiple markers are detected, then the "largest" marker will be used to - determine the pose of the camera. - - Args: - frame: The camera frame containing ArUco markers. - - Returns: - The rotation vector and translation vector of the camera in the marker - frame and the ID of the marker detected. If no marker was detected, - returns None. - """ - # Wait to process frames until we get the camera meta info - if self.camera_info is None: - return None - - # Convert to greyscale image then try to detect the tag(s) - gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY) - detection = self.detect_markers(gray) - - if detection is None: - return None - - corners, ids = detection - - # If there are multiple markers, get the marker with the "longest" side, where - # "longest" should be interpreted as the relative size in the image - side_lengths = [ - abs(corner[0][0][0] - corner[0][2][0]) - + abs(corner[0][0][1] - corner[0][2][1]) - for corner in corners - ] - - min_side_idx = side_lengths.index(max(side_lengths)) - min_marker_id = ids[min_side_idx] - - camera_matrix = np.array(self.camera_info.k, dtype=np.float64).reshape(3, 4) - projection_matrix = np.array(self.camera_info.d, dtype=np.float64).reshape(1, 5) - - # Get the estimated pose - rot_vec, trans_vec, _ = cv2.aruco.estimatePoseSingleMarkers( - corners[min_side_idx], min_marker_id, camera_matrix, projection_matrix - ) - - return rot_vec, trans_vec, min_marker_id - - def update_pose_cb(self, frame: Image) -> None: - """Get the camera pose relative to the marker and send to the ArduSub EKF. - - Args: - frame: The BlueROV2 camera frame. - """ - # Get the pose of the camera in the `marker` frame - camera_pose = self.get_camera_pose(self.bridge.imgmsg_to_cv2(frame)) - - # If there was no marker in the image, exit early - if camera_pose is None: - self.get_logger().debug( - "An ArUco marker could not be detected in the current image" - ) - return - - rot_vec, trans_vec, marker_id = camera_pose - - # Convert the pose into a PoseStamped message - pose = PoseStamped() - - pose.header.frame_id = f"marker_{marker_id}" - pose.header.stamp = self.get_clock().now().to_msg() - - ( - pose.pose.position.x, - pose.pose.position.y, - pose.pose.position.z, - ) = trans_vec.squeeze() - - rot_mat, _ = cv2.Rodrigues(rot_vec) - - ( - pose.pose.orientation.x, - pose.pose.orientation.y, - pose.pose.orientation.z, - pose.pose.orientation.w, - ) = R.from_matrix(rot_mat).as_quat() - - # Transform the pose from the `marker` frame to the `map` frame - try: - pose = self.tf_buffer.transform(pose, self.MAP_FRAME) - except TransformException as e: - self.get_logger().warning( - f"Could not transform from frame marker_{marker_id} to map: {e}" - ) - return - - # The pose now represents the transformation from the map frame to the - # camera frame, but we need to publish the transformation from the map frame - # to the base_link frame - - # Start by getting the camera to base_link transform - try: - tf_camera_to_base = self.tf_buffer.lookup_transform( - self.CAMERA_FRAME, self.BASE_LINK_FRAME, Time() - ) - except TransformException as e: - self.get_logger().warning(f"Could not access transform: {e}") - return - - # Convert the tf into a homogeneous tf matrix - tf_camera_to_base_mat = np.eye(4) - tf_camera_to_base_mat[:3, :3] = R.from_quat( - [ - tf_camera_to_base.transform.rotation.x, - tf_camera_to_base.transform.rotation.y, - tf_camera_to_base.transform.rotation.z, - tf_camera_to_base.transform.rotation.w, - ] - ).as_matrix() - tf_camera_to_base_mat[:3, 3] = np.array( - [ - tf_camera_to_base.transform.translation.x, - tf_camera_to_base.transform.translation.y, - tf_camera_to_base.transform.translation.z, - ] - ) - - # Convert the pose back into a matrix - tf_map_to_camera_mat = np.eye(4) - tf_map_to_camera_mat[:3, :3] = R.from_quat( - [ - pose.pose.orientation.x, # type: ignore - pose.pose.orientation.y, # type: ignore - pose.pose.orientation.z, # type: ignore - pose.pose.orientation.w, # type: ignore - ] - ).as_matrix() - tf_map_to_camera_mat[:3, 3] = np.array( - [ - pose.pose.position.x, # type: ignore - pose.pose.position.y, # type: ignore - pose.pose.position.z, # type: ignore - ] - ) - - # Calculate the new transform - tf_map_to_base_mat = tf_camera_to_base_mat @ tf_map_to_camera_mat - - # Update the pose using the new transform - ( - pose.pose.position.x, # type: ignore - pose.pose.position.y, # type: ignore - pose.pose.position.z, # type: ignore - ) = tf_map_to_base_mat[3:, 3] - - ( - pose.pose.orientation.x, # type: ignore - pose.pose.orientation.y, # type: ignore - pose.pose.orientation.z, # type: ignore - pose.pose.orientation.w, # type: ignore - ) = R.from_matrix(tf_map_to_base_mat[:3, :3]).as_quat() - - self.state = pose - - -class QualisysLocalizer(PoseLocalizer): - """Localize the BlueROV2 using the Qualisys motion capture system.""" - - def __init__(self) -> None: - """Create a new Qualisys motion capture localizer.""" - super().__init__("qualisys_localizer") - - self.declare_parameter("body", "bluerov") - self.declare_parameter("filter_len", 20) - - body = self.get_parameter("body").get_parameter_value().string_value - filter_len = ( - self.get_parameter("filter_len").get_parameter_value().integer_value - ) - - self.mocap_sub = self.create_subscription( - PoseStamped, - f"/blue/mocap/qualisys/{body}", - self.update_pose_cb, - qos_profile_sensor_data, - ) - - # Publish to the MoCap interface instead of the default pose interface - self.mocap_pose_pub = self.create_publisher( - PoseStamped, - "/mavros/mocap/pose", - qos_profile_default, - ) - - # Store the pose information in a buffer and apply an LWMA filter to it - self.pose_buffer: Deque[np.ndarray] = deque(maxlen=filter_len) - - @staticmethod - def check_isnan(pose: PoseStamped) -> bool: - """Check if a pose message has NaN values. - - NaN values are not uncommon when dealing with MoCap data. - - Args: - pose: The message to check for NaN values. - - Returns: - Whether or not the message has any NaN values. - """ - # Check the position - if np.isnan( - np.min( - np.array( - [pose.pose.position.x, pose.pose.position.y, pose.pose.position.z] - ) - ) - ): - return False - - # Check the orientation - if np.isnan( - np.min( - np.array( - [ - pose.pose.orientation.x, - pose.pose.orientation.y, - pose.pose.orientation.z, - pose.pose.orientation.w, - ] - ) - ) - ): - return False - - return True - - def publish(self) -> None: - """Publish the current MoCap state. - - This overrides the default PoseLocalizer publish interface to send the pose - state information to the MAVROS MoCap plugin. - """ - self.mocap_pose_pub.publish(self.state) - - def update_pose_cb(self, pose: PoseStamped) -> None: - """Proxy the pose to the ArduSub EKF. - - We need to do some filtering here to handle the noise from the measurements. - The filter that we apply in this case is the LWMA filter. - - Args: - pose: The pose of the BlueROV2 identified by the motion capture system. - """ - # Check if any of the values in the array are NaN; if they are, then - # discard the reading - if not self.check_isnan(pose): - return - - def pose_to_array(pose: Pose) -> np.ndarray: - ar = np.zeros(6) - ar[:3] = [pose.position.x, pose.position.y, pose.position.z] - ar[3:] = R.from_quat( - [ - pose.orientation.x, - pose.orientation.y, - pose.orientation.z, - pose.orientation.w, - ] - ).as_euler("xyz") - - return ar - - # Convert the pose message into an array for filtering - pose_ar = pose_to_array(pose.pose) - - # Add the pose to the circular buffer - self.pose_buffer.append(pose_ar) - - # Wait until our buffer is full to start publishing the state information - if len(self.pose_buffer) < self.pose_buffer.maxlen: # type: ignore - return - - def lwma(measurements: np.ndarray) -> np.ndarray: - # Get the linear weights - weights = np.arange(len(measurements)) + 1 - - # Apply the LWMA filter and return - return np.array( - [ - np.sum(np.prod(np.vstack((axis, weights)), axis=0)) - / np.sum(weights) - for axis in measurements.T - ] - ) - - filtered_pose_ar = lwma(np.array(self.pose_buffer)) - - def array_to_pose(ar: np.ndarray) -> Pose: - pose = Pose() - pose.position.x, pose.position.y, pose.position.z = ar[:3] - ( - pose.orientation.x, - pose.orientation.y, - pose.orientation.z, - pose.orientation.w, - ) = R.from_euler("xyz", ar[3:]).as_quat() - return pose - - # Update the pose to be the new filtered pose - pose.pose = array_to_pose(filtered_pose_ar) - - self.state = pose - - -class GazeboLocalizer(PoseLocalizer): - """Localize the BlueROV2 using the Gazebo ground-truth data.""" - - def __init__(self) -> None: - """Create a new Gazebo localizer.""" - super().__init__("gazebo_localizer") - - # We need to know the topic to stream from - self.declare_parameter("gazebo_odom_topic", "") - - # Subscribe to that topic so that we can proxy messages to the ArduSub EKF - odom_topic = ( - self.get_parameter("gazebo_odom_topic").get_parameter_value().string_value - ) - self.odom_sub = self.create_subscription( - Odometry, odom_topic, self.update_odom_cb, qos_profile_sensor_data - ) - - def update_odom_cb(self, msg: Odometry) -> None: - """Proxy the pose data from the Gazebo odometry ground-truth data. - - Args: - msg: The Gazebo ground-truth odometry for the BlueROV2. - """ - pose_cov = PoseWithCovarianceStamped() - pose_cov.header = msg.header - pose_cov.pose = msg.pose - - self.state = pose_cov - - -def main_aruco(args: list[str] | None = None): - """Run the ArUco marker detector.""" - rclpy.init(args=args) - - node = ArucoMarkerLocalizer() - executor = MultiThreadedExecutor() - rclpy.spin(node, executor) - - node.destroy_node() - rclpy.shutdown() - - -def main_qualisys(args: list[str] | None = None): - """Run the Qualisys localizer.""" - rclpy.init(args=args) - - node = QualisysLocalizer() - executor = MultiThreadedExecutor() - rclpy.spin(node, executor) - - node.destroy_node() - rclpy.shutdown() - - -def main_gazebo(args: list[str] | None = None): - """Run the Gazebo localizer.""" - rclpy.init(args=args) - - node = GazeboLocalizer() - executor = MultiThreadedExecutor() - rclpy.spin(node, executor) - - node.destroy_node() - rclpy.shutdown() diff --git a/blue_localization/blue_localization/source.py b/blue_localization/blue_localization/source.py deleted file mode 100644 index 65e6ee3f..00000000 --- a/blue_localization/blue_localization/source.py +++ /dev/null @@ -1,363 +0,0 @@ -# Copyright 2023, Evan Palmer -# -# Permission is hereby granted, free of charge, to any person obtaining a copy -# of this software and associated documentation files (the "Software"), to deal -# in the Software without restriction, including without limitation the rights -# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -# copies of the Software, and to permit persons to whom the Software is -# furnished to do so, subject to the following conditions: -# -# The above copyright notice and this permission notice shall be included in -# all copies or substantial portions of the Software. -# -# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL -# THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN -# THE SOFTWARE. - -import asyncio -import xml.etree.ElementTree as ET -from abc import ABC -from typing import Any - -import gi -import numpy as np -import qtm -import rclpy -from cv_bridge import CvBridge -from geometry_msgs.msg import PoseStamped -from rclpy.node import Node -from rclpy.qos import ( - DurabilityPolicy, - HistoryPolicy, - QoSProfile, - ReliabilityPolicy, - qos_profile_sensor_data, -) -from scipy.spatial.transform import Rotation as R -from sensor_msgs.msg import CameraInfo, Image - -gi.require_version("Gst", "1.0") -from gi.repository import Gst # noqa # type: ignore - - -class Source(Node, ABC): - """Base class for a localization source (e.g., camera, sonar, etc.).""" - - def __init__(self, node_name: str) -> None: - """Create a new localization source. - - Args: - node_name: The name of the ROS 2 node. - """ - Node.__init__(self, node_name) - ABC.__init__(self) - - -class Camera(Source): - """BlueROV2 camera source. - - The camera source uses GStreamer to proxy the BlueROV2 camera stream (i.e., frames - received from GStreamer are converted to ROS ``Image`` messages and republished for - other packages to use). - """ - - def __init__(self) -> None: - """Create a new Camera source.""" - super().__init__("camera") - - self.bridge = CvBridge() - - self.declare_parameter("port", 5600) - self.declare_parameter("camera_matrix", list(np.zeros(9))) - self.declare_parameter("projection_matrix", list(np.zeros(12))) - self.declare_parameter("distortion_coefficients", list(np.zeros(5))) - self.declare_parameter("frame.height", 1080) - self.declare_parameter("frame.width", 1920) - self.declare_parameter("distortion_model", "plumb_bob") - - # Get the camera intrinsics - camera_matrix = ( - self.get_parameter("camera_matrix").get_parameter_value().double_array_value - ) - projection_matrix = ( - self.get_parameter("projection_matrix") - .get_parameter_value() - .double_array_value - ) - distortion_coefficients = ( - self.get_parameter("distortion_coefficients") - .get_parameter_value() - .double_array_value - ) - frame_height = ( - self.get_parameter("frame.height").get_parameter_value().integer_value - ) - frame_width = ( - self.get_parameter("frame.width").get_parameter_value().integer_value - ) - distortion_model = ( - self.get_parameter("distortion_model").get_parameter_value().string_value - ) - - # Create a message with the camera info - camera_info = CameraInfo() - camera_info.header.stamp = self.get_clock().now().to_msg() - camera_info.header.frame_id = "camera_link" - camera_info.height = frame_height - camera_info.width = frame_width - camera_info.distortion_model = distortion_model - camera_info.d = distortion_coefficients - camera_info.k = camera_matrix - camera_info.p = projection_matrix - - self.camera_info_pub = self.create_publisher( - CameraInfo, - "/camera/camera_info", - QoSProfile( - reliability=ReliabilityPolicy.RELIABLE, - durability=DurabilityPolicy.TRANSIENT_LOCAL, - history=HistoryPolicy.KEEP_LAST, - depth=1, - ), - ) - - # Go ahead and publish the camera info now - # This uses transient local durability, so this message will persist for - # subscribers - self.camera_info_pub.publish(camera_info) - - self.camera_frame_pub = self.create_publisher( - Image, "/camera/image_raw", qos_profile_sensor_data - ) - - # Start the GStreamer stream - self.video_pipe, self.video_sink = self.init_stream( - self.get_parameter("port").get_parameter_value().integer_value - ) - - def init_stream(self, port: int) -> tuple[Any, Any]: - """Initialize a GStreamer video stream interface. - - GStreamer is used to receive video frames from the BlueROV2 for processing. - - Args: - port: The port over which the video feed is being streamed. - - Returns: - The video pipe and sink. - """ - Gst.init(None) - - video_source = f"udpsrc port={port}" - video_codec = ( - "! application/x-rtp, payload=96 ! rtph264depay ! h264parse ! avdec_h264" - ) - video_decode = ( - "! decodebin ! videoconvert ! video/x-raw,format=(string)BGR ! videoconvert" - ) - video_sink_conf = ( - "! appsink emit-signals=true sync=false max-buffers=2 drop=true" - ) - - command = " ".join([video_source, video_codec, video_decode, video_sink_conf]) - - video_pipe = Gst.parse_launch(command) - video_pipe.set_state(Gst.State.PLAYING) - - video_sink = video_pipe.get_by_name("appsink0") - - def proxy_frame_cb(sink: Any) -> Any: - # Convert from a GStreamer frame to a ROS 2 message and publish - frame = self.gst_to_opencv(sink.emit("pull-sample")) - self.camera_frame_pub.publish(self.bridge.cv2_to_imgmsg(frame)) - return Gst.FlowReturn.OK - - video_sink.connect("new-sample", proxy_frame_cb) - - return video_pipe, video_sink - - @staticmethod - def gst_to_opencv(frame: Any) -> np.ndarray: - """Convert a GStreamer frame to an array. - - Args: - frame: The GStreamer frame to convert. - - Returns: - The GStreamer video frame as an array. - """ - buf = frame.get_buffer() - caps = frame.get_caps() - - return np.ndarray( - ( - caps.get_structure(0).get_value("height"), - caps.get_structure(0).get_value("width"), - 3, - ), - buffer=buf.extract_dup(0, buf.get_size()), - dtype=np.uint8, - ) - - -class QualisysMotionCapture(Source): - """Qualisys motion capture system source. - - The Qualisys motion capture source provides an ROS 2 wrapper for the Qualisys - Python SDK. The source streams the pose of the bluerov body and republishes the pose - as a `PoseStamped` message. - """ - - def __init__(self) -> None: - """Create a new Qualisys motion capture source.""" - super().__init__("qualisys_mocap") - - self.declare_parameter("ip", "192.168.254.1") - self.declare_parameter("port", 22223) - self.declare_parameter("version", "1.22") - self.declare_parameter("body", "ROV") - - # Load the parameters - self.ip = self.get_parameter("ip").get_parameter_value().string_value - self.port = self.get_parameter("port").get_parameter_value().integer_value - self.version = self.get_parameter("version").get_parameter_value().string_value - self.body = self.get_parameter("body").get_parameter_value().string_value - - # Publish the pose using the name of the body as the topic - self.mocap_pub = self.create_publisher( - PoseStamped, f"/blue/mocap/qualisys/{self.body}", qos_profile_sensor_data - ) - - @staticmethod - def create_body_index(params: str) -> dict[str, int]: - """Create a name to index dictionary from the 6-D parameters. - - This is used to retrieve the specific body of interest from a packet. - - Args: - params: The 6-D parameters to use for generating the mapping. - - Returns: - A mapping from the body name to its respective index in a packet. - """ - xml = ET.fromstring(params) - - body_to_index = {} - for index, body in enumerate(xml.findall("*/Body/Name")): - if body is not None: - body_to_index[body.text.strip()] = index # type: ignore - - return body_to_index - - async def run_mocap(self) -> None: - """Run the motion capture system coroutine. - - This implementation is inspired by the `stream_6dof_example.py` example included - with the Qualisys Python SDK. - """ - connection = await qtm.connect(self.ip, self.port, self.version) - - # Normally we would want to raise an exception here, but because the exception - # is returned as part of the future object--which doesn't get captured until - # keyboard interrupt due to the `spinning` method--we are going to notify - # the users with a log message and return. - if connection is None: - self.get_logger().error( - "The Qualisys motion capture source failed to establish a connection" - f" at the address '{self.ip}:{self.port}' using version" - f" '{self.version}'" - ) - return - - # Load the 6D parameters - params = await connection.get_parameters(parameters=["6d"]) - body_index = self.create_body_index(params) - - if self.body not in body_index: - self.get_logger().error( - f"The body '{self.body}' is not available. Please make sure that the" - " body has been properly defined and is enabled." - ) - return - - # Create a callback to bind to the frame stream - def proxy_pose_cb(packet: qtm.QRTPacket) -> None: - _, bodies = packet.get_6d() # type: ignore - - position, rotation = bodies[body_index[self.body]] - - pose_msg = PoseStamped() - - pose_msg.header.frame_id = "map" - pose_msg.header.stamp = self.get_clock().now().to_msg() - - # Convert from mm to m and save the position to the message - ( - pose_msg.pose.position.x, - pose_msg.pose.position.y, - pose_msg.pose.position.z, - ) = (position.x / 1000, position.y / 1000, position.z / 1000) - - # Convert from a column-major rotation matrix to a quaternion - ( - pose_msg.pose.orientation.x, - pose_msg.pose.orientation.y, - pose_msg.pose.orientation.z, - pose_msg.pose.orientation.w, - ) = R.from_matrix(np.array(rotation.matrix).reshape((3, 3)).T).as_quat() - - self.mocap_pub.publish(pose_msg) - - # Stream the mocap 6D pose - await connection.stream_frames(components=["6d"], on_packet=proxy_pose_cb) - - -async def spinning(node: Node) -> None: - """Spin the ROS 2 node as an asyncio Task. - - Args: - node: The ROS 2 node to spin. - """ - while rclpy.ok(): - rclpy.spin_once(node, timeout_sec=0.01) - await asyncio.sleep(0.0001) - - -def main_camera(args: list[str] | None = None): - """Run the camera source.""" - rclpy.init(args=args) - - node = Camera() - rclpy.spin(node) - - node.destroy_node() - rclpy.shutdown() - - -def main_qualisys_mocap(args: list[str] | None = None): - """Run the Qualisys motion capture source.""" - - async def run_mocap(args: list[str] | None, loop: asyncio.AbstractEventLoop): - """Run the Qualisys motion capture system.""" - rclpy.init(args=args) - - node = QualisysMotionCapture() - - spin_task = loop.create_task(spinning(node)) - mocap_task = loop.create_task(node.run_mocap()) - - try: - await spin_task - except KeyboardInterrupt: - spin_task.cancel() - mocap_task.cancel() - - node.destroy_node() - rclpy.shutdown() - - loop = asyncio.new_event_loop() - loop.run_until_complete(run_mocap(args, loop)) diff --git a/blue_localization/launch/localization.launch.py b/blue_localization/launch/localization.launch.py deleted file mode 100644 index 0020667a..00000000 --- a/blue_localization/launch/localization.launch.py +++ /dev/null @@ -1,177 +0,0 @@ -# Copyright 2023, Evan Palmer -# -# Permission is hereby granted, free of charge, to any person obtaining a copy -# of this software and associated documentation files (the "Software"), to deal -# in the Software without restriction, including without limitation the rights -# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -# copies of the Software, and to permit persons to whom the Software is -# furnished to do so, subject to the following conditions: -# -# The above copyright notice and this permission notice shall be included in -# all copies or substantial portions of the Software. -# -# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL -# THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN -# THE SOFTWARE. - -from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription -from launch.conditions import IfCondition -from launch.launch_description_sources import PythonLaunchDescriptionSource -from launch.substitutions import ( - LaunchConfiguration, - PathJoinSubstitution, - PythonExpression, -) -from launch_ros.actions import Node -from launch_ros.substitutions import FindPackageShare - - -def generate_launch_description() -> LaunchDescription: - """Generate a launch description for the localization package. - - Returns: - The localization ROS 2 launch description. - """ - args = [ - DeclareLaunchArgument( - "config_filepath", - default_value=None, - description="The path to the configuration YAML file.", - ), - DeclareLaunchArgument( - "localization_source", - default_value="mocap", - choices=["mocap", "camera", "gazebo"], - description="The localization source to stream from.", - ), - DeclareLaunchArgument( - "use_camera", - default_value="false", - description=( - "Launch the BlueROV2 camera stream. This is automatically set to true" - " when using the camera for localization." - ), - ), - DeclareLaunchArgument( - "use_mocap", - default_value="false", - description=( - "Launch the Qualisys motion capture stream. This is automatically" - " set to true when using the motion capture system for localization." - ), - ), - DeclareLaunchArgument( - "use_sim_time", - default_value="false", - description=("Use the simulated Gazebo clock."), - ), - ] - - localization_source = LaunchConfiguration("localization_source") - use_camera = LaunchConfiguration("use_camera") - use_mocap = LaunchConfiguration("use_mocap") - use_sim_time = LaunchConfiguration("use_sim_time") - - nodes = [ - Node( - package="blue_localization", - executable="camera", - name="camera", - output="both", - parameters=[ - LaunchConfiguration("config_filepath"), - {"use_sim_time": use_sim_time}, - ], - condition=IfCondition( - PythonExpression( - [ - "'", - localization_source, - "' == 'camera' or '", - use_camera, - "' == 'true'", - ] - ) - ), - ), - Node( - package="blue_localization", - executable="aruco_marker_localizer", - name="aruco_marker_localizer", - output="both", - parameters=[ - LaunchConfiguration("config_filepath"), - {"use_sim_time": use_sim_time}, - ], - condition=IfCondition( - PythonExpression(["'", localization_source, "' == 'camera'"]) - ), - ), - Node( - package="blue_localization", - executable="qualisys_mocap", - name="qualisys_mocap", - output="both", - parameters=[ - LaunchConfiguration("config_filepath"), - {"use_sim_time": use_sim_time}, - ], - condition=IfCondition( - PythonExpression( - [ - "'", - localization_source, - "' == 'mocap' or '", - use_mocap, - "' == 'true'", - ] - ) - ), - ), - Node( - package="blue_localization", - executable="qualisys_localizer", - name="qualisys_localizer", - output="both", - parameters=[ - LaunchConfiguration("config_filepath"), - {"use_sim_time": use_sim_time}, - ], - condition=IfCondition( - PythonExpression(["'", localization_source, "' == 'mocap'"]) - ), - ), - Node( - package="blue_localization", - executable="gazebo_localizer", - name="gazebo_localizer", - output="both", - parameters=[ - LaunchConfiguration("config_filepath"), - {"use_sim_time": use_sim_time}, - ], - condition=IfCondition( - PythonExpression(["'", localization_source, "' == 'gazebo'"]) - ), - ), - ] - - includes = [ - IncludeLaunchDescription( - PythonLaunchDescriptionSource( - PathJoinSubstitution( - [FindPackageShare("blue_localization"), "markers.launch.py"] - ) - ), - condition=IfCondition( - PythonExpression(["'", localization_source, "' == 'camera'"]) - ), - ) - ] - - return LaunchDescription(args + nodes + includes) diff --git a/blue_localization/launch/markers.launch.py b/blue_localization/launch/markers.launch.py deleted file mode 100644 index f90f4d2b..00000000 --- a/blue_localization/launch/markers.launch.py +++ /dev/null @@ -1,58 +0,0 @@ -# Copyright 2023, Evan Palmer -# -# Permission is hereby granted, free of charge, to any person obtaining a copy -# of this software and associated documentation files (the "Software"), to deal -# in the Software without restriction, including without limitation the rights -# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -# copies of the Software, and to permit persons to whom the Software is -# furnished to do so, subject to the following conditions: -# -# The above copyright notice and this permission notice shall be included in -# all copies or substantial portions of the Software. -# -# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL -# THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN -# THE SOFTWARE. - -from launch import LaunchDescription -from launch_ros.actions import Node - - -def generate_launch_description() -> LaunchDescription: - """Generate a launch description for the ArUco marker TF broadcasters. - - Returns: - The ArUco marker TF broadcaster ROS 2 launch description. - """ - nodes = [ - Node( - package="tf2_ros", - executable="static_transform_publisher", - name="marker_00_to_map_tf_broadcaster", - arguments=[ - "--x", - "0.0", - "--y", - "0.0", - "--z", - "0.0", - "--roll", - "0", - "--pitch", - "0", - "--yaw", - "0", - "--frame-id", - "map", - "--child-frame-id", - "marker_00", - ], - output="both", - ) - ] - - return LaunchDescription(nodes) diff --git a/blue_localization/package.xml b/blue_localization/package.xml deleted file mode 100644 index 1e054ea3..00000000 --- a/blue_localization/package.xml +++ /dev/null @@ -1,41 +0,0 @@ - - - - - blue_localization - 0.0.1 - Localization interface used to provide visual odometry estimates to the ArduPilot EKF - - Evan Palmer - MIT - - https://github.com/Robotic-Decision-Making-Lab/blue.git - https://github.com/Robotic-Decision-Making-Lab/blue/issues - - Evan Palmer - - mavros - mavros_extras - tf_transformations - - rclpy - sensor_msgs - std_msgs - python3-opencv - python3-numpy - python3-scipy - mavros_msgs - tf2 - tf2_geometry_msgs - cv_bridge - tf2_ros - - ament_copyright - ament_flake8 - ament_pep257 - python3-pytest - - - ament_python - - diff --git a/blue_localization/resource/blue_localization b/blue_localization/resource/blue_localization deleted file mode 100644 index e69de29b..00000000 diff --git a/blue_localization/setup.cfg b/blue_localization/setup.cfg deleted file mode 100644 index 09efd4b8..00000000 --- a/blue_localization/setup.cfg +++ /dev/null @@ -1,4 +0,0 @@ -[develop] -script_dir=$base/lib/blue_localization -[install] -install_scripts=$base/lib/blue_localization diff --git a/blue_localization/setup.py b/blue_localization/setup.py deleted file mode 100644 index 638ab6df..00000000 --- a/blue_localization/setup.py +++ /dev/null @@ -1,56 +0,0 @@ -# Copyright 2023, Evan Palmer -# -# Permission is hereby granted, free of charge, to any person obtaining a copy -# of this software and associated documentation files (the "Software"), to deal -# in the Software without restriction, including without limitation the rights -# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -# copies of the Software, and to permit persons to whom the Software is -# furnished to do so, subject to the following conditions: -# -# The above copyright notice and this permission notice shall be included in -# all copies or substantial portions of the Software. -# -# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL -# THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN -# THE SOFTWARE. - -import os -from glob import glob - -from setuptools import find_packages, setup - -package_name = "blue_localization" - -setup( - name=package_name, - version="0.0.1", - packages=find_packages(exclude=["test"]), - data_files=[ - ("share/ament_index/resource_index/packages", ["resource/" + package_name]), - ("share/" + package_name, ["package.xml"]), - (os.path.join("share", package_name), glob("launch/*.launch.py")), - ], - install_requires=["setuptools", "numpy", "transforms3d", "opencv-python", "qtm"], - zip_safe=True, - maintainer="Evan Palmer", - maintainer_email="evanp922@gmail.com", - description=( - "Localization interface used to provide visual odometry estimates to" - " the ArduPilot EKF." - ), - license="MIT", - tests_require=["pytest"], - entry_points={ - "console_scripts": [ - "camera = blue_localization.source:main_camera", - "qualisys_mocap = blue_localization.source:main_qualisys_mocap", - "aruco_marker_localizer = blue_localization.localizer:main_aruco", - "qualisys_localizer = blue_localization.localizer:main_qualisys", - "gazebo_localizer = blue_localization.localizer:main_gazebo", - ], - }, -) diff --git a/blue_localization/test/test_copyright.py b/blue_localization/test/test_copyright.py deleted file mode 100644 index 8f18fa4b..00000000 --- a/blue_localization/test/test_copyright.py +++ /dev/null @@ -1,27 +0,0 @@ -# Copyright 2015 Open Source Robotics Foundation, Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -import pytest -from ament_copyright.main import main - - -# Remove the `skip` decorator once the source file(s) have a copyright header -@pytest.mark.skip( - reason="No copyright header has been placed in the generated source file." -) -@pytest.mark.copyright -@pytest.mark.linter -def test_copyright(): - rc = main(argv=[".", "test"]) - assert rc == 0, "Found errors" diff --git a/docs/docs/contributing.mdx b/docs/docs/contributing.mdx index 743d3135..003737f7 100644 --- a/docs/docs/contributing.mdx +++ b/docs/docs/contributing.mdx @@ -1,5 +1,5 @@ --- -sidebar_position: 5 +sidebar_position: 6 title: Contributing --- diff --git a/docs/docs/installation.mdx b/docs/docs/installation.mdx index 4ed73794..226a8b81 100644 --- a/docs/docs/installation.mdx +++ b/docs/docs/installation.mdx @@ -5,8 +5,8 @@ sidebar_position: 3 # Installation Blue is currently supported on Linux and is available for the ROS distributions -Humble and Iron. Blue can be installed from source or using one of -the provided Docker images. If you plan to use Blue for simulation and have +Humble and Jazzy (recommended). Blue can be installed from source or using one +of the provided Docker images. If you plan to use Blue for simulation and have a system with limited compute power, we recommend installing Blue from source. For all other cases, we recommend installing Blue using Docker. @@ -22,21 +22,21 @@ Once you have successfully installed Docker and logged into the GitHub Container Registry, you can install Blue by pulling one of the provided [Docker images](https://github.com/Robotic-Decision-Making-Lab/blue/pkgs/container/blue). A complete list of the provided images is documented below (where `*` should be -replaced with the desired ROS distribution, e.g., `humble-robot`): +replaced with the desired ROS distribution, e.g., `jazzy-robot`): | Supported ROS Versions | Image Tag | Build Architectures | Description | | ---------------------- | --------- | ------------------- | ----------- | -| Humble, Iron | *-robot | `amd64`, `arm64` | A bare-bones image that includes all dependencies without simulation or visualization features. This can be used to deploy Blue to hardware. | -| Humble, Iron | *-desktop | `amd64` | Image that includes all dependencies, including simulation and visualization features. This can be used for development, testing, and top-side deployment. | -| Humble, Iron | *-desktop-nvidia | `amd64` | Extension of the `*-desktop` image that provides support for NVIDIA GPU drivers. This image is *strongly* recommended for systems that have an NVIDIA GPU, and requires the [NVIDIA Container Toolkit](https://docs.nvidia.com/datacenter/cloud-native/container-toolkit/latest/install-guide.html). | +| Jazzy, Humble | *-robot | `amd64`, `arm64` | A bare-bones image that includes all dependencies without simulation or visualization features. This can be used to deploy Blue to hardware. | +| Jazzy, Humble | *-desktop | `amd64` | Image that includes all dependencies, including simulation and visualization features. This can be used for development, testing, and top-side deployment. | +| Jazzy, Humble | *-desktop-nvidia | `amd64` | Extension of the `*-desktop` image that provides support for NVIDIA GPU drivers. This image is *strongly* recommended for systems that have an NVIDIA GPU, and requires the [NVIDIA Container Toolkit](https://docs.nvidia.com/datacenter/cloud-native/container-toolkit/latest/install-guide.html). | If you plan to use either the `*-desktop` or `*-desktop-nvidia` images, you may use the provided [Docker Compose scripts](https://github.com/Robotic-Decision-Making-Lab/blue/tree/main/.docker/compose) -to install and run Blue. For instance, to pull and run the `iron-desktop` image, +to install and run Blue. For instance, to pull and run the `jazzy-desktop` image, first run the command: ```bash -wget https://raw.githubusercontent.com/Robotic-Decision-Making-Lab/blue/iron/.docker/compose/nouveau-desktop.yaml && \ +wget https://raw.githubusercontent.com/Robotic-Decision-Making-Lab/blue/jazzy/.docker/compose/nouveau-desktop.yaml && \ docker compose -f nouveau-desktop.yaml up ``` @@ -58,10 +58,11 @@ and `*-desktop-nvidia` images as base images. To use the development containers, first verify that you can run Visual Studio Code inside a Docker container by following the tutorial provided [here](https://code.visualstudio.com/docs/devcontainers/tutorial). Once you have successfully verified that you can run Visual Studio Code inside a -Docker container, open the Blue repository in Visual Studio Code and select the option -to reopen the folder inside a container when prompted. This will automatically -build the development container and install all necessary dependencies. Once the -container is built, you can start developing your own packages! +Docker container, open the Blue repository in Visual Studio Code and select the +option to reopen the folder inside a container when prompted. This will +automatically build the development container and install all necessary +dependencies. Once the container is built, you can start developing your own +packages! ## Source installation diff --git a/docs/docs/overview.mdx b/docs/docs/overview.mdx index c6751443..6b7e06a0 100644 --- a/docs/docs/overview.mdx +++ b/docs/docs/overview.mdx @@ -26,17 +26,11 @@ following features: vehicles, such as ROS 2, Gazebo, linters, and formatters. This helps you avoid spending resources setting up your development environment and focus on writing your robotics algorithms 😀 -4. **Custom control and localization algorithms**: Blue supports custom -controllers implemented using [auv_controllers](https://github.com/Robotic-Decision-Making-Lab/auv_controllers) -and localization algorithms implemented using [blue_localization](https://github.com/Robotic-Decision-Making-Lab/blue/tree/main/blue_localization). - -:::info - -The features implemented in `blue_localization` are currently being ported to -[marine_localization](https://github.com/Robotic-Decision-Making-Lab/marine_localization). -Keep an eye on that repository for the latest changes and updates. - -::: +4. **Custom control algorithms**: Blue supports custom + controllers implemented using [auv_controllers](https://github.com/Robotic-Decision-Making-Lab/auv_controllers). +5. **Localization**: We have implemented several drivers for common underwater + sensors, and integrate these sensors using the [robot_localization](https://docs.ros.org/en/noetic/api/robot_localization/html/configuring_robot_localization.html) + package. See [Localization](/sensors) for more information. ## Recommended workflow @@ -64,13 +58,14 @@ Several vehicle models have been implemented and tested with Blue in
- The BlueROV2 is a popular lightweight underwater vehicle made by [Blue Robotics](https://bluerobotics.com/) - that is used for hobby, research, and commercial applications. + The BlueROV2 is a popular lightweight underwater vehicle made by + [Blue Robotics](https://bluerobotics.com/) that is used for hobby, + research, and commercial applications. - The BlueROV2 Heavy is an extension to the BlueROV2 made by [Blue Robotics](https://bluerobotics.com/) - that includes additional thrusters and a larger frame to improve the - vehicle's manueverability. + The BlueROV2 Heavy is an extension to the BlueROV2 made by + [Blue Robotics](https://bluerobotics.com/) that includes additional + thrusters and a larger frame to improve the vehicle's manueverability. The BlueROV2 Heavy Reach modifies the BlueROV2 Heavy by moving the @@ -80,9 +75,12 @@ Several vehicle models have been implemented and tested with Blue in
-## Alternative projects +## Related projects -If you find that Blue does not meet your needs, we recommend exploring the following alternative projects: +If you find that Blue does not meet your needs, we recommend exploring the +following related projects: - [Orca4](https://github.com/clydemcqueen/orca4) - [Marine Vehicle Packages (MVP)](https://github.com/uri-ocean-robotics/mvp_readme) +- [UVMS Simulator](https://github.com/edxmorgan/uvms-simulator) +- [CentraleNantesROV bluerov2](https://github.com/CentraleNantesROV/bluerov2) diff --git a/docs/docs/sensors.mdx b/docs/docs/sensors.mdx new file mode 100644 index 00000000..f5b63070 --- /dev/null +++ b/docs/docs/sensors.mdx @@ -0,0 +1,34 @@ +--- +sidebar_position: 4 +title: Localization +--- + +This page documents supported ROS 2 drivers for sensors that are commonly used +in underwater robotics research. The [first-party sensors](#first-party-sensors) +can be integrated with ROS 2 packages such as [robot_localization](https://github.com/cra-ros-pkg/robot_localization) +for localization. + +## First-party drivers + +Below is a list of sensor drivers developed by our lab that are supported by +Blue. + +| Sensor | Driver | Supported ROS Versions | +| --------------------------------------------------------------------- | --------------------------------------------------------------------------------------------------- | ---------------------- | +| [Water Linked DVL-A50](https://waterlinked.com/dvl) | [waterlinked_dvl](https://github.com/Robotic-Decision-Making-Lab/waterlinked_dvl) | Jazzy | +| [Water Linked Underwater GPS](https://waterlinked.com/underwater-gps) | [waterlinked_ugps](https://github.com/Robotic-Decision-Making-Lab/waterlinked_ugps) | Jazzy | +| [Barlus UW-S5-3PBX10 Underwater Camera](https://barluscam.com/) | [barlus_underwater_camera](https://github.com/Robotic-Decision-Making-Lab/barlus_underwater_camera) | Jazzy | + +## Third-party drivers + +Below is a list of underwater sensor drivers that have been developed by the +community. This list is not exhaustive and not all drivers have been tested +with Blue; however, we encourage you to try them out and provide feedback to +the authors. + +| Sensor | Driver | Supported ROS Versions | Authors | +| --------------------------------------------------------------------------------------------------------- | ----------------------------------------------------------------------------------- | ---------------------- | ----------------------------------------------------------------------------------------------- | +| [Blueprint Subsea Oculus Sonar](https://www.blueprintsubsea.com/oculus/) | [liboculus](https://github.com/apl-ocean-engineering/liboculus) | Noetic | [University of Washington Applied Physics Laboratory](https://github.com/apl-ocean-engineering) | +| [BlueROV Camera](https://bluerobotics.com/product-category/sensors-cameras/cameras/) | [blue_rov_camera](https://gitlab.com/apl-ocean-engineering/bluerov/blue_rov_camera) | Jazzy, Humble | [University of Washington Applied Physics Laboratory](https://github.com/apl-ocean-engineering) | +| [Blue Robotics Ping 360 Sonar](https://bluerobotics.com/store/sonars/imaging-sonars/ping360-sonar-r1-rp/) | [ping360_sonar](https://github.com/CentraleNantesRobotics/ping360_sonar) | Foxy | [Robotics Group at Centrale Nantes](https://github.com/CentraleNantesRobotics) | +| Motion Capture Systems | [MOCAP4ROS2](https://mocap4ros2-project.github.io/index.html) | Jazzy, Humble | [MOCAP4ROS2-Project](https://github.com/MOCAP4ROS2-Project) | diff --git a/docs/docs/tutorials/_category_.json b/docs/docs/tutorials/_category_.json index b9dada94..ae26fa51 100644 --- a/docs/docs/tutorials/_category_.json +++ b/docs/docs/tutorials/_category_.json @@ -1,4 +1,4 @@ { "label": "Tutorials", - "position": 4 + "position": 5 } diff --git a/docs/docs/tutorials/control.mdx b/docs/docs/tutorials/control.mdx index dd168e0b..d9813425 100644 --- a/docs/docs/tutorials/control.mdx +++ b/docs/docs/tutorials/control.mdx @@ -29,7 +29,7 @@ Docker as described in the [installation instructions](/installation). 1. Launch the demo BlueROV2 in simulation by running the following command: ```bash - ros2 launch blue_demos bluerov2_demo.launch.yaml use_sim:=true + ros2 launch blue_demos bluerov2_heavy_demo.launch.yaml use_sim:=true ``` 2. Wait for Gazebo, ArduSub, and MAVROS to fully load. Typically, the `param` @@ -43,13 +43,15 @@ Docker as described in the [installation instructions](/installation). 3. Open a new terminal and launch the demo control framework: ```bash - ros2 launch blue_demos bluerov2_controllers.launch.py use_sim:=true + ros2 launch blue_demos bluerov2_heavy_controllers.launch.py use_sim:=true ``` This should launch a set of cascading controllers, concluding with the - `integral_sliding_mode_controller` (ISMC) controller. The control framework - uses the `thruster_hardware/ThrusterHardware` system interface to enable - thruster-level control with ArduSub. + `adaptive_integral_terminal_sliding_mode_controller` (AITSMC) controller. + The control framework uses the `thruster_hardware/ThrusterHardware` system + interface to enable thruster-level control with ArduSub and the + `topic_sensors/OdomSensor` sensor interface convert ROS 2 messages into + a ros2_control compatible format. :::tip @@ -67,7 +69,7 @@ Docker as described in the [installation instructions](/installation). The output should resemble: ``` - integral_sliding_mode_controller[velocity_controllers/IntegralSlidingModeController] active + adaptive_integral_terminal_sliding_mode_controller[velocity_controllers/AdaptiveIntegralTerminalSlidingModeController] active thruster_allocation_matrix_controller[thruster_allocation_matrix_controller/ThrusterAllocationMatrixController] active thruster_1_controller[thruster_controllers/PolynomialThrustCurveController] active thruster_2_controller[thruster_controllers/PolynomialThrustCurveController] active @@ -75,6 +77,8 @@ Docker as described in the [installation instructions](/installation). thruster_4_controller[thruster_controllers/PolynomialThrustCurveController] active thruster_5_controller[thruster_controllers/PolynomialThrustCurveController] active thruster_6_controller[thruster_controllers/PolynomialThrustCurveController] active + thruster_7_controller[thruster_controllers/PolynomialThrustCurveController] active + thruster_8_controller[thruster_controllers/PolynomialThrustCurveController] active ``` :::tip @@ -82,7 +86,7 @@ Docker as described in the [installation instructions](/installation). Many of the controllers implemented in `auv_controllers` support dynamically modifying controller parameters and gains. We encourage you to take advantage of this feature to tune the controllers to your specific vehicle. - This helps avoid the need to recompile and relauch the system for each + This helps avoid the need to recompile and relaunch the system for each change that you make. ::: diff --git a/docs/docs/tutorials/simulation.mdx b/docs/docs/tutorials/simulation.mdx index 8eaae54f..0d6291c8 100644 --- a/docs/docs/tutorials/simulation.mdx +++ b/docs/docs/tutorials/simulation.mdx @@ -3,6 +3,8 @@ sidebar_position: 1 title: Running Blue in Simulation --- +import Sim from '@site/static/gif/sim.gif'; + This tutorial describes how to launch an implemented vehicle in Gazebo with the Gazebo hydrodynamics plugins. Our tests have found that Gazebo provides a good representation of underwater vehicle dynamics. Alternative, *untested* @@ -12,7 +14,11 @@ underwater simulation options that can be used with ROS 2 include: * [HoloOcean](https://holoocean.readthedocs.io/en/master/) We recommend exploring each simulation option to determine what best fits your -project's needs. +project's needs. You can find more information about recent underwater +simulators in our ICRA 2025 workshop [AQ²UASIM: Advancing Quantitative and +Qualitative Simulators for Marine Applications](https://sites.google.com/view/aq2uasim) + +Simulation demo gif ## Dependencies @@ -38,7 +44,7 @@ where `` should be replaced with the name of the desired vehicle. For instance, to launch Gazebo using the BlueROV2, run ```bash -ros2 launch blue_bringup bluerov2.launch.yaml use_sim:=true +ros2 launch blue_bringup bluerov2_heavy.launch.yaml use_sim:=true ``` This should bringup Gazebo, [ArduSub](https://github.com/ArduPilot/ardupilot), diff --git a/docs/docs/tutorials/teleop.mdx b/docs/docs/tutorials/teleop.mdx index 6e468a91..a4a8fd5c 100644 --- a/docs/docs/tutorials/teleop.mdx +++ b/docs/docs/tutorials/teleop.mdx @@ -3,11 +3,15 @@ sidebar_position: 3 title: Teleoperation --- +import Teleop from '@site/static/gif/teleop.gif'; + This page provides a collection of tutorials that describe how to teleoperate a simulated or real vehicle. Prior to starting these tutorials, you should have completed the [Running Blue in Simulation](/tutorials/simulation) tutorial and the [Integrating Custom Controllers](/tutorials/control) tutorial. +Teleoperation demo gif + ## Keyboard teleoperation This tutorial describes how to teleoperate a simulated BlueROV2 using your @@ -31,14 +35,14 @@ Docker as described in the [installation instructions](/installation). 1. Launch the demo Dependencies in simulation by running the following command: ```bash - ros2 launch blue_demos bluerov2_demo.launch.yaml use_sim:=true + ros2 launch blue_demos bluerov2_heavy_demo.launch.yaml use_sim:=true ``` 2. Once Gazebo, ArduSub, and MAVROS have fully loaded, open a new terminal and launch the demo control framework: ```bash - ros2 launch blue_demos bluerov2_controllers.launch.py use_sim:=true + ros2 launch blue_demos bluerov2_heavy_controllers.launch.py use_sim:=true ``` 3. Open a new terminal and launch the `teleop_twist_keyboard` node: @@ -48,20 +52,20 @@ Docker as described in the [installation instructions](/installation). ``` 4. The `teleop_twist_keyboard` node will publish velocity commands according to - [REP-105](https://ros.org/reps/rep-0105.html); however, the launched ISMC + [REP-105](https://ros.org/reps/rep-0105.html); however, the launched AITSMC adheres to the maritime conventions recorded in [REP-156](https://github.com/ros-infrastructure/rep/pull/398). To convert the velocity commands to the appropriate convention, run the `message_transforms` node in a new terminal: ```bash - ros2 launch message_transforms message_transforms.launch.py parameters_file:= + ros2 launch message_transforms message_transforms.launch.yaml parameters_file:= ``` where `` should be replaced with the path to the `transforms.yaml` file in the `blue_demos` package, e.g., ```bash - ros2 launch message_transforms message_transforms.launch.py parameters_file:=./blue_demos/teleoperation/config/transforms.yaml + ros2 launch message_transforms message_transforms.launch.yaml parameters_file:=./blue_demos/teleoperation/config/transforms.yaml ``` 5. You should now be able to teleoperate the BlueROV2 using your keyboard. @@ -114,14 +118,14 @@ installed on your local machine **outside** of the container. 1. Launch the demo BlueROV2 in simulation by running the following command: ```bash - ros2 launch blue_demos bluerov2_demo.launch.yaml use_sim:=true + ros2 launch blue_demos bluerov2_heavy_demo.launch.yaml use_sim:=true ``` 2. Once Gazebo, ArduSub, and MAVROS have fully loaded, open a new terminal and launch the demo control framework: ```bash - ros2 launch blue_demos bluerov2_controllers.launch.py use_sim:=true + ros2 launch blue_demos bluerov2_heavy_controllers.launch.py use_sim:=true ``` 3. Open a new terminal and launch the ROS 2 joystick driver using the provided diff --git a/docs/docs/tutorials/uvms.mdx b/docs/docs/tutorials/uvms.mdx new file mode 100644 index 00000000..d58478b6 --- /dev/null +++ b/docs/docs/tutorials/uvms.mdx @@ -0,0 +1,105 @@ +--- +sidebar_position: 4 +title: Manipulator Systems +--- + +import UVMS from '@site/static/gif/uvms.gif'; + + +This tutorial shows how to use [auv_controllers](https://github.com/Robotic-Decision-Making-Lab/auv_controllers) +to control underwater vehicle manipulator systems (UVMS). The tutorial will use +the BlueROV2 Heavy with an affixed Reach Alpha 5 manipulator. The files used +for this tutorial are available in the `blue_demos/manipulator_systems` +directory. + +This tutorial includes two demonstrations. In the first demonstration, we show +how to integrate reactive whole-body control using closed-loop task priority +inverse kinematics control. The second demonstration extends the first by +showing how to perform end effector trajectory tracking. + +UVMS demo gif + +## Dependencies + +The following ROS 2 dependencies are required for this tutorial: + +* Gazebo Harmonic or newer +* [auv_controllers](https://github.com/Robotic-Decision-Making-Lab/auv_controllers) +* [reach](https://github.com/Robotic-Decision-Making-Lab/reach) + +These dependencies will be met by default if you have installed Blue with +Docker as described in the [installation instructions](/installation). + +### Interactive whole-body control + +The first demonstration leverages the auv_controllers `ik_controller` with the +`task_priority_solver`. The controller has been configured to dynamically track +a target that has been placed in the world. The target can be translated or +rotated to change the end effector pose. + +#### Tutorial steps + +1. Launch the demo in simulation by running the following command: + + ```bash + ros2 launch blue_demos interactive_uvms_demo.launch.yaml + ``` + +2. Once Gazebo and the controllers have fully loaded, you should see the UVMS + Gazebo with a red spherical object that represents the target end effector + pose. You can translate and rotate the object to change the target end + effector pose. + +### End effector trajectory tracking + +The second demonstration extends the [Interactive whole-body control](#interactive-whole-body-control) +demonstration by integrating the auv_controllers +`end_effector_trajectory_controller`. This controller is used to interpolate +end effector motion plans using linear and spherical linear interpolation. + +#### Tutorial steps + +1. Launch the demo by running the following command: + + ```bash + ros2 launch blue_demos trajectory_controller_demo.launch.yaml + ``` + +2. Once Gazebo and the controllers have fully loaded, you can send a reference + trajectory to the controller by running the following command in a new + terminal: + + ```bash + ros2 topic pub /end_effector_trajectory_controller/trajectory auv_control_msgs/msg/EndEffectorTrajectory '{}' + ``` + + A pre-configured trajectory is available, and can be sent to the controller + by running + + ```bash + ros2 launch blue_demos publish_trajectory.launch.yaml + ``` + + :::tip + + The `end_effector_trajectory_controller` uses the default system QoS policy, + which does not guarantee message reception. So if the vehicle does not + immediately respond to the command, try resending the message. + + ::: + +3. Similar to the ros2_controllers `joint_trajectory_controller`, the + `end_effector_trajectory_controller` supports sending trajectory messages + using messages (as done in the previous step) or actions. To send a + trajectory as an action goal, run the following command: + + ```bash + ros2 action send_goal /end_effector_trajectory_controller/follow_trajectory auv_control_msgs/action/FollowEndEffectorTrajectory '{}' + ``` + + A pre-configured action goal is available, and can be sent to the controller + by running + + ```bash + ros2 launch blue_demos send_trajectory_action.launch.yaml + ``` diff --git a/docs/static/gif/sim.gif b/docs/static/gif/sim.gif new file mode 100644 index 00000000..7c1c6c66 Binary files /dev/null and b/docs/static/gif/sim.gif differ diff --git a/docs/static/gif/teleop.gif b/docs/static/gif/teleop.gif new file mode 100644 index 00000000..63dd3ead Binary files /dev/null and b/docs/static/gif/teleop.gif differ diff --git a/docs/static/gif/uvms.gif b/docs/static/gif/uvms.gif new file mode 100644 index 00000000..98f76ef5 Binary files /dev/null and b/docs/static/gif/uvms.gif differ diff --git a/requirements-build.txt b/requirements-build.txt deleted file mode 100644 index 8b2288a2..00000000 --- a/requirements-build.txt +++ /dev/null @@ -1,3 +0,0 @@ -# Requirements unavailable as rosdeps: -# https://github.com/ros/rosdistro/blob/master/rosdep/python.yaml -qtm diff --git a/sim.repos b/sim.repos deleted file mode 100644 index 91c14acd..00000000 --- a/sim.repos +++ /dev/null @@ -1,11 +0,0 @@ -# -# Note this is _in addition to_ blue.repos -# (i.e. you need to vcs import both files independently) -# -repositories: - - # ROS-Gazebo integration - ros_gz: - type: git - url: https://github.com/gazebosim/ros_gz - version: ros2