From ee5eab349f839461fae58d56a48ab7e12a6b9f97 Mon Sep 17 00:00:00 2001 From: Evan Palmer Date: Fri, 15 Mar 2024 01:54:52 -0700 Subject: [PATCH 01/38] Started migration to new controller package and ardusub interface --- .devcontainer/nvidia/Dockerfile | 3 +- blue.repos | 15 + blue_bringup/launch/base.launch | 67 +++ blue_bringup/launch/base.launch.py | 48 +- .../bluerov2_heavy/bluerov2_heavy.launch.py | 16 +- .../bluerov2_heavy/bluerov2_heavy_launch.xml | 0 .../launch/bluerov2_heavy/control.launch.py | 168 ++++++ blue_bringup/launch/bluerov2_heavy/tf.launch | 35 ++ .../launch/bluerov2_heavy/tf.launch.py | 226 -------- blue_bringup/launch/sitl.launch | 12 + blue_bringup/launch/sitl.launch.py | 8 +- blue_bringup/package.xml | 3 +- blue_control/CMakeLists.txt | 92 ---- blue_control/LICENSE | 17 - .../include/blue_control/controller.hpp | 166 ------ blue_control/include/blue_control/ismc.hpp | 79 --- blue_control/launch/control.launch.py | 70 --- blue_control/package.xml | 38 -- blue_control/src/controller.cpp | 213 -------- blue_control/src/ismc.cpp | 264 --------- .../config/bluerov2_heavy/controllers.yaml | 189 ++++++- .../config/bluerov2_heavy/new_manager.yaml | 8 + .../xacro/bluerov2_heavy/config.xacro | 11 +- .../xacro/bluerov2_heavy/ros2_control.xacro | 71 +++ blue_dynamics/CMakeLists.txt | 92 ---- blue_dynamics/LICENSE | 17 - .../include/blue_dynamics/hydrodynamics.hpp | 304 ----------- .../blue_dynamics/thruster_dynamics.hpp | 90 ---- blue_dynamics/package.xml | 33 -- blue_dynamics/src/hydrodynamics.cpp | 180 ------- blue_dynamics/src/thruster_dynamics.cpp | 96 ---- blue_dynamics/test/test_hydrodynamics.cpp | 155 ------ blue_dynamics/test/test_thruster_dynamics.cpp | 72 --- blue_manager/LICENSE | 17 - blue_manager/blue_manager/manager.py | 508 ------------------ blue_manager/launch/manager.launch.py | 67 --- blue_manager/package.xml | 28 - blue_manager/resource/blue_manager | 0 blue_manager/setup.cfg | 4 - blue_manager/setup.py | 49 -- blue_manager/test/test_copyright.py | 27 - blue_utils/CMakeLists.txt | 68 --- blue_utils/LICENSE | 17 - blue_utils/include/blue_utils/eigen.hpp | 64 --- blue_utils/include/blue_utils/tf2.hpp | 95 ---- blue_utils/package.xml | 30 -- {blue_utils/scripts => scripts}/arm.sh | 0 {blue_utils/scripts => scripts}/disarm.sh | 0 {blue_utils/scripts => scripts}/send_cmd.sh | 0 {blue_utils/scripts => scripts}/send_pwm.sh | 0 .../start_keyboard_teleop.sh | 0 51 files changed, 558 insertions(+), 3274 deletions(-) create mode 100644 blue_bringup/launch/base.launch rename blue_manager/blue_manager/__init__.py => blue_bringup/launch/bluerov2_heavy/bluerov2_heavy_launch.xml (100%) create mode 100644 blue_bringup/launch/bluerov2_heavy/control.launch.py create mode 100644 blue_bringup/launch/bluerov2_heavy/tf.launch delete mode 100644 blue_bringup/launch/bluerov2_heavy/tf.launch.py create mode 100644 blue_bringup/launch/sitl.launch delete mode 100644 blue_control/CMakeLists.txt delete mode 100644 blue_control/LICENSE delete mode 100644 blue_control/include/blue_control/controller.hpp delete mode 100644 blue_control/include/blue_control/ismc.hpp delete mode 100644 blue_control/launch/control.launch.py delete mode 100644 blue_control/package.xml delete mode 100644 blue_control/src/controller.cpp delete mode 100644 blue_control/src/ismc.cpp create mode 100644 blue_description/config/bluerov2_heavy/new_manager.yaml create mode 100644 blue_description/xacro/bluerov2_heavy/ros2_control.xacro delete mode 100644 blue_dynamics/CMakeLists.txt delete mode 100644 blue_dynamics/LICENSE delete mode 100644 blue_dynamics/include/blue_dynamics/hydrodynamics.hpp delete mode 100644 blue_dynamics/include/blue_dynamics/thruster_dynamics.hpp delete mode 100644 blue_dynamics/package.xml delete mode 100644 blue_dynamics/src/hydrodynamics.cpp delete mode 100644 blue_dynamics/src/thruster_dynamics.cpp delete mode 100644 blue_dynamics/test/test_hydrodynamics.cpp delete mode 100644 blue_dynamics/test/test_thruster_dynamics.cpp delete mode 100644 blue_manager/LICENSE delete mode 100644 blue_manager/blue_manager/manager.py delete mode 100644 blue_manager/launch/manager.launch.py delete mode 100644 blue_manager/package.xml delete mode 100644 blue_manager/resource/blue_manager delete mode 100644 blue_manager/setup.cfg delete mode 100644 blue_manager/setup.py delete mode 100644 blue_manager/test/test_copyright.py delete mode 100644 blue_utils/CMakeLists.txt delete mode 100644 blue_utils/LICENSE delete mode 100644 blue_utils/include/blue_utils/eigen.hpp delete mode 100644 blue_utils/include/blue_utils/tf2.hpp delete mode 100644 blue_utils/package.xml rename {blue_utils/scripts => scripts}/arm.sh (100%) rename {blue_utils/scripts => scripts}/disarm.sh (100%) rename {blue_utils/scripts => scripts}/send_cmd.sh (100%) rename {blue_utils/scripts => scripts}/send_pwm.sh (100%) rename {blue_utils/scripts => scripts}/start_keyboard_teleop.sh (100%) diff --git a/.devcontainer/nvidia/Dockerfile b/.devcontainer/nvidia/Dockerfile index b2259379..4e98922b 100644 --- a/.devcontainer/nvidia/Dockerfile +++ b/.devcontainer/nvidia/Dockerfile @@ -1,4 +1,4 @@ -FROM ghcr.io/robotic-decision-making-lab/blue:rolling-desktop-nvidia +FROM ghcr.io/robotic-decision-making-lab/blue:iron-desktop-nvidia # Install ROS dependencies # This is done in a previous stage, but we include it again here in case anyone wants to @@ -10,6 +10,7 @@ WORKDIR $USER_WORKSPACE COPY --chown=$USER_UID:$USER_GID . src/blue RUN sudo apt-get -q update \ && sudo apt-get -q -y upgrade \ + && vcs import src < src/blue/blue.repos \ && rosdep update \ && rosdep install -y --from-paths src --ignore-src --rosdistro ${ROS_DISTRO} --skip-keys="gz-transport12 gz-sim7 gz-math7 gz-msgs9 gz-plugin2" \ && sudo apt-get autoremove -y \ diff --git a/blue.repos b/blue.repos index 6b184ee8..1c80c1ec 100644 --- a/blue.repos +++ b/blue.repos @@ -4,3 +4,18 @@ repositories: type: git url: https://github.com/gazebosim/ros_gz version: ros2 + + hydrodynamics: + type: git + url: https://github.com/Robotic-Decision-Making-Lab/hydrodynamics.git + version: main + + auv_controllers: + type: git + url: https://github.com/Robotic-Decision-Making-Lab/auv_controllers.git + version: develop + + ardusub_driver: + type: git + url: https://github.com/Robotic-Decision-Making-Lab/ardusub_driver.git + version: develop diff --git a/blue_bringup/launch/base.launch b/blue_bringup/launch/base.launch new file mode 100644 index 00000000..56e35d30 --- /dev/null +++ b/blue_bringup/launch/base.launch @@ -0,0 +1,67 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/blue_bringup/launch/base.launch.py b/blue_bringup/launch/base.launch.py index f822fdc4..b0bd9109 100644 --- a/blue_bringup/launch/base.launch.py +++ b/blue_bringup/launch/base.launch.py @@ -60,8 +60,8 @@ def generate_launch_description() -> LaunchDescription: ), DeclareLaunchArgument( "manager_file", - default_value="manager.yaml", - description="The BlueROV2 Heavy manager configuration file.", + default_value="new_manager.yaml", + description="The ArduSub manager configuration file.", ), DeclareLaunchArgument( "mavros_file", @@ -86,15 +86,6 @@ def generate_launch_description() -> LaunchDescription: default_value="", description="The world configuration to load if using Gazebo.", ), - DeclareLaunchArgument( - "controller", - default_value="ismc", - description=( - "The controller to use; this should be the same name as the" - " controller's executable." - ), - choices=["ismc"], - ), DeclareLaunchArgument( "localization_source", default_value="gazebo", @@ -248,25 +239,6 @@ def generate_launch_description() -> LaunchDescription: "use_sim_time": use_sim, }.items(), ), - IncludeLaunchDescription( - PythonLaunchDescriptionSource( - PathJoinSubstitution( - [FindPackageShare("blue_control"), "launch", "control.launch.py"] - ) - ), - launch_arguments={ - "config_filepath": PathJoinSubstitution( - [ - FindPackageShare(description_package), - "config", - configuration_type, - LaunchConfiguration("controllers_file"), - ] - ), - "controller": LaunchConfiguration("controller"), - "use_sim_time": use_sim, - }.items(), - ), IncludeLaunchDescription( PythonLaunchDescriptionSource( PathJoinSubstitution( @@ -288,22 +260,6 @@ def generate_launch_description() -> LaunchDescription: "use_sim_time": use_sim, }.items(), ), - IncludeLaunchDescription( - PythonLaunchDescriptionSource( - PathJoinSubstitution([FindPackageShare("blue_joy"), "joy.launch.py"]) - ), - launch_arguments={ - "config_filepath": PathJoinSubstitution( - [ - FindPackageShare(description_package), - "config", - LaunchConfiguration("joy_file"), - ] - ), - "controller": LaunchConfiguration("controller"), - "use_sim_time": use_sim, - }.items(), - ), ] return LaunchDescription(args + nodes + includes) diff --git a/blue_bringup/launch/bluerov2_heavy/bluerov2_heavy.launch.py b/blue_bringup/launch/bluerov2_heavy/bluerov2_heavy.launch.py index 1c01286a..a32263fe 100644 --- a/blue_bringup/launch/bluerov2_heavy/bluerov2_heavy.launch.py +++ b/blue_bringup/launch/bluerov2_heavy/bluerov2_heavy.launch.py @@ -21,6 +21,7 @@ from launch import LaunchDescription from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch_ros.actions import Node from launch.substitutions import ( Command, FindExecutable, @@ -37,15 +38,6 @@ def generate_launch_description() -> LaunchDescription: The launch description for the BlueROV2 base configuration. """ args = [ - DeclareLaunchArgument( - "controller", - default_value="ismc", - description=( - "The controller to use; this should be the same name as the" - " controller's executable." - ), - choices=["ismc"], - ), DeclareLaunchArgument( "localization_source", default_value="gazebo", @@ -94,7 +86,7 @@ def generate_launch_description() -> LaunchDescription: ), ] - robot_description = Command( + robot_description_content = Command( [ PathJoinSubstitution([FindExecutable(name="xacro")]), " ", @@ -114,6 +106,7 @@ def generate_launch_description() -> LaunchDescription: LaunchConfiguration("use_sim"), ] ) + robot_description = {"robot_description": robot_description_content} return LaunchDescription( [ @@ -126,7 +119,6 @@ def generate_launch_description() -> LaunchDescription: ), launch_arguments={ "configuration_type": "bluerov2_heavy", - "controller": LaunchConfiguration("controller"), "localization_source": LaunchConfiguration("localization_source"), "use_camera": LaunchConfiguration("use_camera"), "use_mocap": LaunchConfiguration("use_mocap"), @@ -137,7 +129,7 @@ def generate_launch_description() -> LaunchDescription: "gazebo_world_file", default="bluerov2_heavy_underwater.world" ), "prefix": LaunchConfiguration("prefix"), - "robot_description": robot_description, + "robot_description": robot_description_content, "use_joy": LaunchConfiguration("use_joy"), }.items(), ), diff --git a/blue_manager/blue_manager/__init__.py b/blue_bringup/launch/bluerov2_heavy/bluerov2_heavy_launch.xml similarity index 100% rename from blue_manager/blue_manager/__init__.py rename to blue_bringup/launch/bluerov2_heavy/bluerov2_heavy_launch.xml diff --git a/blue_bringup/launch/bluerov2_heavy/control.launch.py b/blue_bringup/launch/bluerov2_heavy/control.launch.py new file mode 100644 index 00000000..d248ee97 --- /dev/null +++ b/blue_bringup/launch/bluerov2_heavy/control.launch.py @@ -0,0 +1,168 @@ +# Copyright 2024, 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 +from launch_ros.actions import Node +from launch.substitutions import ( + Command, + FindExecutable, + LaunchConfiguration, + PathJoinSubstitution, +) +from launch_ros.substitutions import FindPackageShare +from launch.actions import RegisterEventHandler +from launch.event_handlers import OnProcessExit + + +def generate_launch_description() -> LaunchDescription: + """Generate a launch description to run the system. + + Returns: + The launch description for the BlueROV2 base configuration. + """ + args = [ + DeclareLaunchArgument( + "use_sim", + default_value="false", + description="Launch the Gazebo + ArduSub simulator.", + ), + DeclareLaunchArgument( + "prefix", + default_value="", + description=( + "The prefix of the model. This is useful for multi-robot setups." + " Expected format '/'." + ), + ), + ] + + robot_description_content = Command( + [ + PathJoinSubstitution([FindExecutable(name="xacro")]), + " ", + PathJoinSubstitution( + [ + FindPackageShare("blue_description"), + "xacro", + "bluerov2_heavy", + "config.xacro", + ] + ), + " ", + "prefix:=", + LaunchConfiguration("prefix"), + " ", + "use_sim:=", + LaunchConfiguration("use_sim"), + ] + ) + robot_description = {"robot_description": robot_description_content} + + controller_manager = Node( + package="controller_manager", + executable="ros2_control_node", + output="both", + parameters=[ + robot_description, + PathJoinSubstitution( + [ + FindPackageShare("blue_description"), + "config", + "bluerov2_heavy", + "controllers.yaml", + ] + ), + ], + ) + + velocity_controller_spawner = Node( + package="controller_manager", + executable="spawner", + arguments=[ + "integral_sliding_mode_controller", + "--controller-manager", + ["", "controller_manager"], + ], + ) + thruster_spawners = [ + Node( + package="controller_manager", + executable="spawner", + arguments=[ + f"thruster{i + 1}_controller", + "--controller-manager", + ["", "controller_manager"], + ], + ) + for i in range(8) + ] + + delay_thruster_spawners = [] + for i, thruster_spawner in enumerate(thruster_spawners): + if not len(delay_thruster_spawners): + delay_thruster_spawners.append( + thruster_spawner, + ) + else: + delay_thruster_spawners.append( + RegisterEventHandler( + event_handler=OnProcessExit( + target_action=thruster_spawners[i - 1], + on_exit=[thruster_spawner], + ) + ) + ) + + tam_controller_spawner = Node( + package="controller_manager", + executable="spawner", + arguments=[ + "thruster_allocation_matrix_controller", + "--controller-manager", + ["", "controller_manager"], + ], + ) + delay_tam_controller_spawner_after_thruster_controller_spawners = ( + RegisterEventHandler( + event_handler=OnProcessExit( + target_action=thruster_spawners[-1], + on_exit=[tam_controller_spawner], + ) + ) + ) + delay_velocity_controller_spawner_after_tam_controller_spawner = ( + RegisterEventHandler( + event_handler=OnProcessExit( + target_action=tam_controller_spawner, + on_exit=[velocity_controller_spawner], + ) + ) + ) + + return LaunchDescription( + [ + *args, + controller_manager, + *delay_thruster_spawners, + delay_tam_controller_spawner_after_thruster_controller_spawners, + delay_velocity_controller_spawner_after_tam_controller_spawner, + ] + ) diff --git a/blue_bringup/launch/bluerov2_heavy/tf.launch b/blue_bringup/launch/bluerov2_heavy/tf.launch new file mode 100644 index 00000000..01996453 --- /dev/null +++ b/blue_bringup/launch/bluerov2_heavy/tf.launch @@ -0,0 +1,35 @@ + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/blue_bringup/launch/bluerov2_heavy/tf.launch.py b/blue_bringup/launch/bluerov2_heavy/tf.launch.py deleted file mode 100644 index d0c4d0c2..00000000 --- a/blue_bringup/launch/bluerov2_heavy/tf.launch.py +++ /dev/null @@ -1,226 +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 -from launch.substitutions import LaunchConfiguration -from launch_ros.actions import Node - - -def generate_launch_description(): - """Generate a launch description for the static TF broadcasters.""" - args = [ - DeclareLaunchArgument( - "prefix", default_value="", description="The URDF model prefix." - ) - ] - - prefix = LaunchConfiguration("prefix") - - nodes = [ - Node( - package="tf2_ros", - executable="static_transform_publisher", - name="base_link_to_thruster1", - arguments=[ - "--x", - "0.14", - "--y", - "-0.092", - "--z", - "0.0", - "--roll", - "-1.571", - "--pitch", - "1.571", - "--yaw", - "-0.785", - "--frame-id", - [prefix, "base_link"], - "--child-frame-id", - [prefix, "thruster1"], - ], - output="both", - ), - Node( - package="tf2_ros", - executable="static_transform_publisher", - name="base_link_to_thruster2", - arguments=[ - "--x", - "0.14", - "--y", - "0.092", - "--z", - "0.0", - "--roll", - "-1.571", - "--pitch", - "1.571", - "--yaw", - "-2.356", - "--frame-id", - [prefix, "base_link"], - "--child-frame-id", - [prefix, "thruster2"], - ], - output="both", - ), - Node( - package="tf2_ros", - executable="static_transform_publisher", - name="base_link_to_thruster3", - arguments=[ - "--x", - "-0.15", - "--y", - "-0.092", - "--z", - "0.0", - "--roll", - "-1.571", - "--pitch", - "1.571", - "--yaw", - "0.785", - "--frame-id", - [prefix, "base_link"], - "--child-frame-id", - [prefix, "thruster3"], - ], - output="both", - ), - Node( - package="tf2_ros", - executable="static_transform_publisher", - name="base_link_to_thruster4", - arguments=[ - "--x", - "-0.15", - "--y", - "0.092", - "--z", - "0.0", - "--roll", - "-1.571", - "--pitch", - "1.571", - "--yaw", - "2.356", - "--frame-id", - [prefix, "base_link"], - "--child-frame-id", - [prefix, "thruster4"], - ], - output="both", - ), - Node( - package="tf2_ros", - executable="static_transform_publisher", - name="base_link_to_thruster5", - arguments=[ - "--x", - "0.118", - "--y", - "-0.215", - "--z", - "0.064", - "--frame-id", - [prefix, "base_link"], - "--child-frame-id", - [prefix, "thruster5"], - ], - output="both", - ), - Node( - package="tf2_ros", - executable="static_transform_publisher", - name="base_link_to_thruster6", - arguments=[ - "--x", - "0.118", - "--y", - "0.215", - "--z", - "0.064", - "--frame-id", - [prefix, "base_link"], - "--child-frame-id", - [prefix, "thruster6"], - ], - output="both", - ), - Node( - package="tf2_ros", - executable="static_transform_publisher", - name="base_link_to_thruster7", - arguments=[ - "--x", - "-0.118", - "--y", - "-0.215", - "--z", - "0.064", - "--frame-id", - [prefix, "base_link"], - "--child-frame-id", - [prefix, "thruster7"], - ], - output="both", - ), - Node( - package="tf2_ros", - executable="static_transform_publisher", - name="base_link_to_thruster8", - arguments=[ - "--x", - "-0.118", - "--y", - "0.215", - "--z", - "0.064", - "--frame-id", - [prefix, "base_link"], - "--child-frame-id", - [prefix, "thruster8"], - ], - output="both", - ), - Node( - package="tf2_ros", - executable="static_transform_publisher", - name="base_link_to_base_footprint", - arguments=[ - "--x", - "-0.0", - "--y", - "0.0", - "--z", - "0.0", - "--frame-id", - [prefix, "base_link"], - "--child-frame-id", - [prefix, "base_footprint"], - ], - output="both", - ), - ] - - return LaunchDescription(args + nodes) diff --git a/blue_bringup/launch/sitl.launch b/blue_bringup/launch/sitl.launch new file mode 100644 index 00000000..095722ef --- /dev/null +++ b/blue_bringup/launch/sitl.launch @@ -0,0 +1,12 @@ + + + + + + + + + + + diff --git a/blue_bringup/launch/sitl.launch.py b/blue_bringup/launch/sitl.launch.py index 0d1678d2..3b826711 100644 --- a/blue_bringup/launch/sitl.launch.py +++ b/blue_bringup/launch/sitl.launch.py @@ -86,7 +86,7 @@ def generate_launch_description() -> LaunchDescription: # Clock (IGN -> ROS 2) "/clock@rosgraph_msgs/msg/Clock[gz.msgs.Clock" ], - output="both", + # output="both", ), Node( package="ros_gz_bridge", @@ -101,7 +101,7 @@ def generate_launch_description() -> LaunchDescription: "/odometry@nav_msgs/msg/Odometry[gz.msgs.Odometry", ], ], - output="both", + # output="both", ), Node( package="ros_gz_bridge", @@ -114,13 +114,13 @@ def generate_launch_description() -> LaunchDescription: "/odometry@nav_msgs/msg/Odometry[gz.msgs.Odometry", ], ], - output="both", + # output="both", ), Node( package="ros_gz_sim", executable="create", arguments=["-name", configuration_type, "-topic", "robot_description"], - output="both", + # output="both", ), ] diff --git a/blue_bringup/package.xml b/blue_bringup/package.xml index a36b9cc6..2cd4bb08 100644 --- a/blue_bringup/package.xml +++ b/blue_bringup/package.xml @@ -16,8 +16,7 @@ mavros mavros_extras - python3-matplotlib - teleop_twist_keyboard + ros2launch ament_copyright ament_flake8 diff --git a/blue_control/CMakeLists.txt b/blue_control/CMakeLists.txt deleted file mode 100644 index 5308775f..00000000 --- a/blue_control/CMakeLists.txt +++ /dev/null @@ -1,92 +0,0 @@ -cmake_minimum_required(VERSION 3.8) -project(blue_control) - -# Default to C99 -if(NOT CMAKE_C_STANDARD) - set(CMAKE_C_STANDARD 99) -endif() - -# Default to C++ 17 -if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 17) -endif() - -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic) -endif() - -set(THIS_PACKAGE_INCLUDE_DEPENDS - rclcpp - ament_cmake - blue_dynamics - blue_utils - nav_msgs - sensor_msgs - mavros_msgs - geometry_msgs - std_srvs - eigen3_cmake_module - Eigen3 - tf2_eigen - tf2_geometry_msgs -) - -foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) - find_package(${Dependency} REQUIRED) -endforeach() - -set(CUSTOM_CONTROLLER_HEADERS - include/blue_control/ismc.hpp -) - -set(CUSTOM_CONTROLLER_SOURCES - src/ismc.cpp -) - -include_directories( - include -) - -# Create a library for the base_controller class -add_library(base_controller include/blue_control/controller.hpp src/controller.cpp) -ament_target_dependencies(base_controller ${THIS_PACKAGE_INCLUDE_DEPENDS}) - -# Create a library for the custom controllers; link this to the base controller -add_library(custom_controllers ${CUSTOM_CONTROLLER_HEADERS} ${CUSTOM_CONTROLLER_SOURCES}) -ament_target_dependencies(custom_controllers ${THIS_PACKAGE_INCLUDE_DEPENDS}) -target_link_libraries(custom_controllers base_controller) - -# Add executables for the custom controllers -add_executable(ismc src/ismc.cpp) -ament_target_dependencies(ismc ${THIS_PACKAGE_INCLUDE_DEPENDS}) -target_link_libraries(ismc custom_controllers) - -# Install -install( - TARGETS base_controller custom_controllers ismc - DESTINATION lib/${PROJECT_NAME} -) - -install(DIRECTORY - include/ - DESTINATION include -) - -install(DIRECTORY - launch - DESTINATION share/${PROJECT_NAME}/ -) - -if(BUILD_TESTING) - find_package(ament_lint_auto REQUIRED) - - # Run linters found in package.xml except those below - set(ament_cmake_copyright_FOUND TRUE) - set(ament_cmake_uncrustify_FOUND TRUE) - set(ament_cmake_pep257_FOUND TRUE) - set(ament_cmake_flake8_FOUND TRUE) - - ament_lint_auto_find_test_dependencies() -endif() - -ament_package() diff --git a/blue_control/LICENSE b/blue_control/LICENSE deleted file mode 100644 index 30e8e2ec..00000000 --- a/blue_control/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_control/include/blue_control/controller.hpp b/blue_control/include/blue_control/controller.hpp deleted file mode 100644 index 8e9462f0..00000000 --- a/blue_control/include/blue_control/controller.hpp +++ /dev/null @@ -1,166 +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. - -#pragma once - -#include -#include -#include -#include -#include - -#include "blue_dynamics/hydrodynamics.hpp" -#include "geometry_msgs/msg/accel.hpp" -#include "geometry_msgs/msg/accel_stamped.hpp" -#include "geometry_msgs/msg/pose_stamped.hpp" -#include "geometry_msgs/msg/transform_stamped.hpp" -#include "mavros_msgs/msg/override_rc_in.hpp" -#include "nav_msgs/msg/odometry.hpp" -#include "rclcpp/rclcpp.hpp" -#include "sensor_msgs/msg/battery_state.hpp" -#include "std_srvs/srv/set_bool.hpp" -#include "tf2_ros/buffer.h" -#include "tf2_ros/transform_broadcaster.h" -#include "tf2_ros/transform_listener.h" - -namespace blue::control -{ - -/** - * @brief A base class for custom BlueROV2 controllers. - */ -class Controller : public rclcpp::Node -{ -public: - /** - * @brief Construct a new Controller object. - * - * @param node_name The name of the ROS node. - */ - explicit Controller(const std::string & node_name); - -protected: - /** - * @brief Function executed when the controller is armed. - */ - virtual void onArm() = 0; - - /** - * @brief Function executed when the controller is disarmed. - */ - virtual void onDisarm() = 0; - - /** - * @brief Update the current control inputs to the thrusters - * - * @return mavros_msgs::msg::OverrideRCIn - */ - virtual mavros_msgs::msg::OverrideRCIn calculateControlInput() = 0; - - /** - * @brief A collection of the hydrodynamic parameters for the BlueROV2. - */ - blue::dynamics::HydrodynamicParameters hydrodynamics_; - - /** - * @brief The thruster configuration matrix for the BlueROV2. - */ - Eigen::MatrixXd tcm_; - - /** - * @brief The current state of the battery. - * - * @note This can be used to approximate the thrust curve according to the current battery - * voltage. - */ - sensor_msgs::msg::BatteryState battery_state_; - - /** - * @brief The current pose and twist of the BlueROV2. - */ - nav_msgs::msg::Odometry odom_; - - /** - * @brief The current acceleration of the BlueROV2. - * - * @note This is not provided by ArduSub directly and is calculated using finite differencing. - */ - geometry_msgs::msg::Accel accel_; - - /** - * @brief The total time (s) between control loop iterations - * - * @note This can be useful when calculating integral terms for the controller. - */ - double dt_{0.0}; - - // TF2 - std::shared_ptr tf_buffer_; - std::unique_ptr tf_broadcaster_; - -private: - /** - * @brief Enable/disable the controller. - * - * @details This method enables/disables sending RC Override messages to the BlueROV2. - * - * @param request The request to enable/disable the controller. - * @param response The result of the arming request. - */ - void armControllerCb( - std::shared_ptr request, - std::shared_ptr response); - - /** - * @brief Handle the incoming odometry messages. - * - * @note This message will be published after MAVROS and all of it's plugins are loaded. - * - * @param msg The current odometry message. - */ - void updateOdomCb(nav_msgs::msg::Odometry::ConstSharedPtr msg); - - // Manages whether or not control inputs are sent to ArduSub - bool armed_{false}; - - // TF2 - std::unique_ptr tf_listener_; - - // Publishers - rclcpp::Publisher::SharedPtr rc_override_pub_; - rclcpp::Publisher::SharedPtr accel_pub_; - - // Subscribers - rclcpp::Subscription::SharedPtr battery_state_sub_; - rclcpp::Subscription::SharedPtr odom_sub_; - rclcpp::Subscription::SharedPtr ardu_pose_sub_; - - // Callback groups - rclcpp::CallbackGroup::SharedPtr control_loop_cb_group_; - - // Timers - rclcpp::TimerBase::SharedPtr control_loop_timer_; - rclcpp::TimerBase::SharedPtr set_message_rate_timer_; - - // Services - rclcpp::Service::SharedPtr arm_srv_; -}; - -} // namespace blue::control diff --git a/blue_control/include/blue_control/ismc.hpp b/blue_control/include/blue_control/ismc.hpp deleted file mode 100644 index ebb2ed00..00000000 --- a/blue_control/include/blue_control/ismc.hpp +++ /dev/null @@ -1,79 +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. - -#pragma once - -#include - -#include "blue_control/controller.hpp" -#include "blue_dynamics/thruster_dynamics.hpp" -#include "blue_utils/eigen.hpp" -#include "geometry_msgs/msg/twist.hpp" -#include "geometry_msgs/msg/twist_stamped.hpp" -#include "geometry_msgs/msg/wrench_stamped.hpp" -#include "mavros_msgs/msg/override_rc_in.hpp" - -namespace blue::control -{ - -/** - * @brief Integral sliding mode controller for the BlueROV2. - */ -class ISMC : public Controller -{ -public: - /** - * @brief Construct a new ISMC object. - */ - ISMC(); - -protected: - void onArm() override; - void onDisarm() override; - mavros_msgs::msg::OverrideRCIn calculateControlInput() override; - -private: - // ISMC gains - Eigen::Matrix6d integral_gain_; - Eigen::Matrix6d proportional_gain_; - Eigen::Matrix6d derivative_gain_; - double sliding_gain_; - double boundary_thickness_; - - // Error terms - Eigen::Vector6d initial_velocity_error_; - Eigen::Vector6d initial_acceleration_error_; - Eigen::Vector6d total_velocity_error_; - - // Control whether or not the battery state is used when converting thrust to PWM - bool use_battery_state_; - - // Reference signal - geometry_msgs::msg::Twist cmd_; - - // Publishers - rclcpp::Publisher::SharedPtr desired_wrench_pub_; - rclcpp::Publisher::SharedPtr velocity_error_pub_; - - // Subscribers - rclcpp::Subscription::SharedPtr cmd_sub_; -}; - -} // namespace blue::control diff --git a/blue_control/launch/control.launch.py b/blue_control/launch/control.launch.py deleted file mode 100644 index 0ac5a3a1..00000000 --- a/blue_control/launch/control.launch.py +++ /dev/null @@ -1,70 +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 -from launch.substitutions import LaunchConfiguration -from launch_ros.actions import Node - - -def generate_launch_description() -> LaunchDescription: - """Generate a launch description for the Blue control interface. - - Returns: - LaunchDescription: The Blue control launch description. - """ - args = [ - DeclareLaunchArgument( - "config_filepath", - default_value=None, - description="The path to the configuration YAML file.", - ), - DeclareLaunchArgument( - "controller", - default_value="ismc", - description=( - "The controller to use; this should be the same name as the" - " controller's executable." - ), - choices=["ismc"], - ), - DeclareLaunchArgument( - "use_sim_time", - default_value="false", - description=("Use the simulated Gazebo clock."), - ), - ] - - controller = LaunchConfiguration("controller") - - nodes = [ - Node( - package="blue_control", - executable=controller, - name=controller, - output="both", - parameters=[ - LaunchConfiguration("config_filepath"), - {"use_sim_time": LaunchConfiguration("use_sim_time")}, - ], - ), - ] - - return LaunchDescription(args + nodes) diff --git a/blue_control/package.xml b/blue_control/package.xml deleted file mode 100644 index f0dcbfc0..00000000 --- a/blue_control/package.xml +++ /dev/null @@ -1,38 +0,0 @@ - - - - - blue_control - 0.0.1 - Control interface for the BlueROV2 - - Evan Palmer - MIT - - https://github.com/Robotic-Decision-Making-Lab/blue.git - https://github.com/Robotic-Decision-Making-Lab/blue/issues - - Evan Palmer - - nav_msgs - sensor_msgs - mavros_msgs - rclcpp - eigen - std_srvs - tf2_eigen - tf2 - tf2_geometry_msgs - tf2_ros - blue_dynamics - blue_utils - - ament_cmake - - ament_lint_auto - ament_lint_common - - - ament_cmake - - diff --git a/blue_control/src/controller.cpp b/blue_control/src/controller.cpp deleted file mode 100644 index 04c825af..00000000 --- a/blue_control/src/controller.cpp +++ /dev/null @@ -1,213 +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. - -#include "blue_control/controller.hpp" - -#include - -#include "blue_utils/eigen.hpp" -#include "blue_utils/tf2.hpp" - -namespace blue::control -{ - -Controller::Controller(const std::string & node_name) -: Node(node_name) -{ - // Declare ROS parameters - this->declare_parameter("mass", 13.5); - this->declare_parameter("buoyancy", 112.80); - this->declare_parameter("weight", 114.80); - this->declare_parameter("center_of_gravity", std::vector({0.0, 0.0, 0.0})); - this->declare_parameter("center_of_buoyancy", std::vector({0.0, 0.0, 0.0})); - this->declare_parameter("ocean_current", std::vector({0.0, 0.0, 0.0, 0.0, 0.0, 0.0})); - this->declare_parameter("num_thrusters", 8); - this->declare_parameter("control_rate", 200.0); - this->declare_parameter("inertia_tensor_coeff", std::vector({0.16, 0.16, 0.16})); - this->declare_parameter( - "added_mass_coeff", std::vector({-5.50, -12.70, -14.60, -0.12, -0.12, -0.12})); - this->declare_parameter( - "linear_damping_coeff", std::vector({-4.03, -6.22, -5.18, -0.07, -0.07, -0.07})); - this->declare_parameter( - "quadratic_damping_coeff", std::vector({-18.18, -21.66, -36.99, -1.55, -1.55, -1.55})); - this->declare_parameter( - "frame", std::vector({true, true, false, false, true, false, false, true})); - - // clang-format off - this->declare_parameter( - "tcm", std::vector({-0.707, -0.707, 0.707, 0.707, 0.0, 0.0, 0.0, 0.0, - 0.707, -0.707, 0.707, -0.707, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 1.0, -1.0, -1.0, 1.0, - 0.0, 0.0, 0.0, 0.0, -0.218, -0.218, 0.218, 0.218, - 0.0, 0.0, 0.0, 0.0, -0.12, 0.12, -0.12, 0.12, - 0.1888, -0.1888, -0.1888, 0.1888, 0.0, 0.0, 0.0, 0.0})); - // clang-format on - - // Get hydrodynamic parameters - const double mass = this->get_parameter("mass").as_double(); - const double buoyancy = this->get_parameter("buoyancy").as_double(); - const double weight = this->get_parameter("weight").as_double(); - const Eigen::Vector3d inertia_tensor_coeff = blue::utility::vectorToEigen( - this->get_parameter("inertia_tensor_coeff").as_double_array(), 3, 1); - const Eigen::Matrix added_mass_coeff = blue::utility::vectorToEigen( - this->get_parameter("added_mass_coeff").as_double_array(), 6, 1); - const Eigen::Matrix linear_damping_coeff = blue::utility::vectorToEigen( - this->get_parameter("linear_damping_coeff").as_double_array(), 6, 1); - const Eigen::Matrix quadratic_damping_coeff = blue::utility::vectorToEigen( - this->get_parameter("quadratic_damping_coeff").as_double_array(), 6, 1); - const Eigen::Vector3d center_of_gravity = blue::utility::vectorToEigen( - this->get_parameter("center_of_gravity").as_double_array(), 3, 1); - const Eigen::Vector3d center_of_buoyancy = blue::utility::vectorToEigen( - this->get_parameter("center_of_buoyancy").as_double_array(), 3, 1); - const Eigen::Matrix ocean_current = blue::utility::vectorToEigen( - this->get_parameter("ocean_current").as_double_array(), 6, 1); - - // Get the thruster configuration matrix - std::vector tcm_vec = this->get_parameter("tcm").as_double_array(); - const int num_thrusters = this->get_parameter("num_thrusters").as_int(); - tcm_ = blue::utility::vectorToEigen( - tcm_vec, static_cast(tcm_vec.size() / num_thrusters), num_thrusters); - - // Initialize the hydrodynamic parameters - hydrodynamics_ = blue::dynamics::HydrodynamicParameters( - blue::dynamics::Inertia(mass, inertia_tensor_coeff, added_mass_coeff), - blue::dynamics::Coriolis(mass, inertia_tensor_coeff, added_mass_coeff), - blue::dynamics::Damping(linear_damping_coeff, quadratic_damping_coeff), - blue::dynamics::RestoringForces(weight, buoyancy, center_of_buoyancy, center_of_gravity), - blue::dynamics::CurrentEffects(ocean_current)); - - // Setup the ROS things - tf_buffer_ = std::make_shared(this->get_clock()); - tf_listener_ = std::make_unique(*tf_buffer_); - tf_broadcaster_ = std::make_unique(this); - - accel_pub_ = this->create_publisher( - "/blue/state/accel", rclcpp::SensorDataQoS()); - rc_override_pub_ = this->create_publisher( - "mavros/rc/override", rclcpp::SystemDefaultsQoS()); - - // clang-tidy and ROS conflict when creating subscriptions with ConstSharedPtr - // NOLINTBEGIN(performance-unnecessary-value-param) - battery_state_sub_ = this->create_subscription( - "/mavros/battery", rclcpp::SensorDataQoS(), - [this](sensor_msgs::msg::BatteryState::ConstSharedPtr msg) -> void { battery_state_ = *msg; }); - - odom_sub_ = this->create_subscription( - "/mavros/local_position/odom", rclcpp::SensorDataQoS(), - [this](nav_msgs::msg::Odometry::ConstSharedPtr msg) -> void { updateOdomCb(msg); }); - - arm_srv_ = this->create_service( - "blue/cmd/arm", - [this]( - const std::shared_ptr request, - std::shared_ptr response) -> void { - armControllerCb(request, response); - }); - // NOLINTEND(performance-unnecessary-value-param) - - // Convert the control loop frequency to seconds - dt_ = 1 / this->get_parameter("control_rate").as_double(); - - // Give the control loop its own callback group to avoid issues with long callbacks in the - // default callback group - control_loop_timer_ = this->create_wall_timer( - std::chrono::duration(dt_), - [this]() -> void { - if (armed_) { - rc_override_pub_->publish(calculateControlInput()); - } - }, - control_loop_cb_group_); -} - -void Controller::armControllerCb( - const std::shared_ptr request, // NOLINT - std::shared_ptr response) // NOLINT -{ - if (request->data) { - // Run the controller arming function prior to actually arming the controller - // This makes sure that any processing that needs to happen before the controller is *actually* - // armed can occur - onArm(); - - // Arm the controller - armed_ = true; - response->success = true; - response->message = "Controller armed."; - RCLCPP_WARN(this->get_logger(), "Custom BlueROV2 controller armed."); - } else { - // Disarm the controller - armed_ = false; - response->success = true; - response->message = "Controller disarmed."; - RCLCPP_WARN(this->get_logger(), "Custom BlueROV2 controller disarmed."); - - // Run the controller disarming function after the controller has been fully disarmed - onDisarm(); - } -} - -void Controller::updateOdomCb(nav_msgs::msg::Odometry::ConstSharedPtr msg) // NOLINT -{ - // Get the duration between the readings - rclcpp::Time prev_stamp(odom_.header.stamp.sec, odom_.header.stamp.nanosec); - rclcpp::Time current_stamp(msg->header.stamp.sec, msg->header.stamp.nanosec); - const double dt = (current_stamp - prev_stamp).seconds(); - - // Calculate the current acceleration using finite differencing and publish it for debugging - geometry_msgs::msg::Accel accel; - accel.linear.x = (msg->twist.twist.linear.x - odom_.twist.twist.linear.x) / dt; - accel.linear.y = (msg->twist.twist.linear.y - odom_.twist.twist.linear.y) / dt; - accel.linear.z = (msg->twist.twist.linear.z - odom_.twist.twist.linear.z) / dt; - accel.angular.x = (msg->twist.twist.angular.x - odom_.twist.twist.angular.x) / dt; - accel.angular.y = (msg->twist.twist.angular.y - odom_.twist.twist.angular.y) / dt; - accel.angular.z = (msg->twist.twist.angular.z - odom_.twist.twist.angular.z) / dt; - - accel_ = accel; - - geometry_msgs::msg::AccelStamped accel_stamped; - accel_stamped.header.frame_id = blue::transforms::kBaseLinkFrameId; - accel_stamped.header.stamp = this->get_clock()->now(); - accel_stamped.accel = accel_; - - accel_pub_->publish(accel_stamped); - - // Publish the current transformation from the map frame to the base_link frame - geometry_msgs::msg::TransformStamped tf; - tf.header.frame_id = "map"; - tf.header.stamp = this->get_clock()->now(); - tf.child_frame_id = "base_link"; - - tf.transform.translation.x = msg->pose.pose.position.x; - tf.transform.translation.y = msg->pose.pose.position.y; - tf.transform.translation.z = msg->pose.pose.position.z; - - tf.transform.rotation.x = msg->pose.pose.orientation.x; - tf.transform.rotation.y = msg->pose.pose.orientation.y; - tf.transform.rotation.z = msg->pose.pose.orientation.z; - tf.transform.rotation.w = msg->pose.pose.orientation.w; - - tf_broadcaster_->sendTransform(tf); - - // Update the current Odometry reading - odom_ = *msg; -} - -} // namespace blue::control diff --git a/blue_control/src/ismc.cpp b/blue_control/src/ismc.cpp deleted file mode 100644 index 5ab030f1..00000000 --- a/blue_control/src/ismc.cpp +++ /dev/null @@ -1,264 +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. - -#include "blue_control/ismc.hpp" - -#include -#include - -#include "blue_dynamics/thruster_dynamics.hpp" -#include "blue_utils/eigen.hpp" -#include "blue_utils/tf2.hpp" -#include "tf2/transform_datatypes.h" -#include "tf2_eigen/tf2_eigen.hpp" -#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" - -namespace blue::control -{ - -ISMC::ISMC() -: Controller("ismc"), - initial_velocity_error_(Eigen::Vector6d::Zero()), - initial_acceleration_error_(Eigen::Vector6d::Zero()), - total_velocity_error_(Eigen::Vector6d::Zero()) -{ - // Declare the ROS parameters specific to this controller - this->declare_parameter("integral_gain", std::vector({1.0, 1.0, 1.0, 1.0, 1.0, 1.0})); - this->declare_parameter("proportional_gain", std::vector({1.0, 1.0, 1.0, 1.0, 1.0, 1.0})); - this->declare_parameter("derivative_gain", std::vector({1.0, 1.0, 1.0, 1.0, 1.0, 1.0})); - this->declare_parameter("sliding_gain", 20.0); - this->declare_parameter("boundary_thickness", 100.0); - this->declare_parameter("use_battery_state", false); - - // Get the gain matrices - Eigen::VectorXd integral_gain_coeff = blue::utility::vectorToEigen( - this->get_parameter("integral_gain").as_double_array(), 6, 1); - Eigen::VectorXd proportional_gain_coeff = blue::utility::vectorToEigen( - this->get_parameter("proportional_gain").as_double_array(), 6, 1); - Eigen::VectorXd derivative_gain_coeff = blue::utility::vectorToEigen( - this->get_parameter("derivative_gain").as_double_array(), 6, 1); - - integral_gain_ = integral_gain_coeff.asDiagonal().toDenseMatrix(); - proportional_gain_ = proportional_gain_coeff.asDiagonal().toDenseMatrix(); - derivative_gain_ = derivative_gain_coeff.asDiagonal().toDenseMatrix(); - sliding_gain_ = this->get_parameter("sliding_gain").as_double(); - boundary_thickness_ = this->get_parameter("boundary_thickness").as_double(); - use_battery_state_ = this->get_parameter("use_battery_state").as_bool(); - - // Publish the desired wrench and errors to help with tuning and visualization - std::stringstream wrench_ss; - wrench_ss << "/blue/" << this->get_name() << "/desired_wrench"; - std::stringstream twist_ss; - twist_ss << "/blue/" << this->get_name() << "/velocity_error"; - desired_wrench_pub_ = this->create_publisher( - wrench_ss.str(), rclcpp::SensorDataQoS()); - velocity_error_pub_ = this->create_publisher( - twist_ss.str(), rclcpp::SensorDataQoS()); - - // Update the reference signal when a new command is received - std::stringstream topic_ss; - topic_ss << "/blue/" << this->get_name() << "/cmd_vel"; - cmd_sub_ = this->create_subscription( - topic_ss.str(), rclcpp::QoS(1), - [this](geometry_msgs::msg::Twist::ConstSharedPtr msg) -> void // NOLINT - { cmd_ = *msg; }); -} - -void ISMC::onArm() -{ - // Reset the total velocity error - total_velocity_error_ = Eigen::Vector6d::Zero(); - - // Reset the initial conditions - initial_velocity_error_ = Eigen::Vector6d::Zero(); - initial_acceleration_error_ = Eigen::Vector6d::Zero(); - - // We need to calculate the initial conditions for the controller now. This includes the - // initial velocity and acceleration errors - - // Start by calculating the velocity error i.c. - Eigen::Vector6d velocity; - tf2::fromMsg(odom_.twist.twist, velocity); - - tf2::fromMsg(cmd_, initial_velocity_error_); - initial_velocity_error_ -= velocity; - - // Now calculate the acceleration error i.c. - // Assume that the desired acceleration is 0 - Eigen::Vector6d accel; - tf2::fromMsg(accel_, accel); - initial_acceleration_error_ -= accel; -}; - -void ISMC::onDisarm() -{ - // Reset the total velocity error on disarm just to be safe - total_velocity_error_ = Eigen::Vector6d::Zero(); - - // Reset the initial conditions too - initial_velocity_error_ = Eigen::Vector6d::Zero(); - initial_acceleration_error_ = Eigen::Vector6d::Zero(); -}; - -mavros_msgs::msg::OverrideRCIn ISMC::calculateControlInput() -{ - Eigen::Vector6d velocity; - tf2::fromMsg(odom_.twist.twist, velocity); - - // Calculate the velocity error - Eigen::Vector6d velocity_error; - tf2::fromMsg(cmd_, velocity_error); - velocity_error -= velocity; - - // Calculate the acceleration error; assume that the desired acceleration is 0 - Eigen::Vector6d accel_error; - tf2::fromMsg(accel_, accel_error); - accel_error *= -1; - - // Publish the velocity error to help with debugging - geometry_msgs::msg::TwistStamped velocity_error_msg; - velocity_error_msg.header.frame_id = blue::transforms::kBaseLinkFrameId; - velocity_error_msg.header.stamp = this->get_clock()->now(); - velocity_error_msg.twist = tf2::toMsg(velocity_error); - velocity_error_pub_->publish(velocity_error_msg); - - // Get the current rotation of the vehicle in the inertial (map) frame - // This is effectively the transformation of the orientation to the body (base_link) frame - Eigen::Quaterniond orientation; - tf2::fromMsg(odom_.pose.pose.orientation, orientation); - - // Make sure to update the velocity error integral term BEFORE calculating the sliding surface - // (the integral is up to time "t") - total_velocity_error_ += velocity_error * dt_; - - // Calculate the sliding surface - Eigen::Vector6d surface = - proportional_gain_ * velocity_error + integral_gain_ * total_velocity_error_ + - derivative_gain_ * accel_error - proportional_gain_ * initial_velocity_error_ - - initial_acceleration_error_; - - // Apply the sign function to the surface - // We use the tanh function to help reduce some of the chatter - surface = surface.unaryExpr([this](double x) { return tanh(x / boundary_thickness_); }); - - // Calculate the computed torque control - Eigen::Vector6d tau0 = - hydrodynamics_.inertia.getInertia() * - (proportional_gain_ * velocity_error + integral_gain_ * total_velocity_error_ + - derivative_gain_ * accel_error) + - (hydrodynamics_.coriolis.calculateCoriolis(velocity) + - hydrodynamics_.damping.calculateDamping(velocity)) * - velocity + - hydrodynamics_.restoring_forces.calculateRestoringForces(orientation.toRotationMatrix()); - - // Calculate the disturbance rejection torque - Eigen::Vector6d tau1 = -sliding_gain_ * surface; - - Eigen::Vector6d forces = tau0 + tau1; - - // Publish the desired wrench for debugging purposes - geometry_msgs::msg::WrenchStamped wrench; - wrench.header.frame_id = blue::transforms::kBaseLinkFrameId; - wrench.header.stamp = this->get_clock()->now(); - wrench.wrench = tf2::toMsg2(forces); - desired_wrench_pub_->publish(wrench); - - // Initialize an OverrideRCIn message with no change for the PWM values - mavros_msgs::msg::OverrideRCIn msg; - - // Set all channels to "NOCHANGE" by default - for (uint16_t & channel : msg.channels) { - channel = mavros_msgs::msg::OverrideRCIn::CHAN_NOCHANGE; - } - - // The torques have been calculated for the base_link frame, but we need to transform them - // to the base_link_frd frame for ArduSub. - geometry_msgs::msg::TransformStamped transform; - - try { - transform = tf_buffer_->lookupTransform( - blue::transforms::kBaseLinkFrdFrameId, blue::transforms::kBaseLinkFrameId, - tf2::TimePointZero); - } - catch (const tf2::TransformException & e) { - RCLCPP_INFO( // NOLINT - this->get_logger(), "Could not transform from %s to %s: %s", - blue::transforms::kBaseLinkFrameId, blue::transforms::kBaseLinkFrdFrameId, e.what()); - return msg; - } - - geometry_msgs::msg::WrenchStamped wrench_frd; - tf2::doTransform(wrench, wrench_frd, transform); - tf2::fromMsg(wrench_frd.wrench, forces); - - // Multiply the desired forces by the pseudoinverse of the thruster configuration matrix - // The size of this vector will depend on the number of thrusters so we don't assign it to a - // fixed-size matrix - Eigen::VectorXd thruster_forces = tcm_.completeOrthogonalDecomposition().pseudoInverse() * forces; - - // Convert the thruster forces to PWM values - Eigen::VectorXi pwms; - - if (use_battery_state_) { - pwms = thruster_forces.unaryExpr([this](double x) { - return blue::dynamics::calculatePwmFromThrustSurface(x, battery_state_.voltage); - }); - } else { - pwms = thruster_forces.unaryExpr( - [this](double x) { return blue::dynamics::calculatePwmFromThrustCurve(x); }); - } - - // Calculate the deadzone band - std::tuple deadband; - - if (use_battery_state_) { - deadband = blue::dynamics::calculateDeadZone(battery_state_.voltage); - } else { - deadband = blue::dynamics::calculateDeadZone(); - } - - // Set the PWM values - for (int i = 0; i < pwms.size(); i++) { - auto pwm = static_cast(pwms[i]); - - // Apply the deadband to the PWM values - if (pwm > std::get<0>(deadband) && pwm < std::get<1>(deadband)) { - pwm = blue::dynamics::kNoThrustPwm; - } - - // Clamp the PWM to the valid PWM range - msg.channels[i] = std::clamp(pwm, blue::dynamics::kMinPwm, blue::dynamics::kMaxPwm); - } - - return msg; -} - -} // namespace blue::control - -int main(int argc, char ** argv) -{ - rclcpp::init(argc, argv); - auto node = std::make_shared(); - rclcpp::executors::MultiThreadedExecutor executor; - executor.add_node(node); - executor.spin(); - rclcpp::shutdown(); - return 0; -} diff --git a/blue_description/config/bluerov2_heavy/controllers.yaml b/blue_description/config/bluerov2_heavy/controllers.yaml index be9aa292..a31ca128 100644 --- a/blue_description/config/bluerov2_heavy/controllers.yaml +++ b/blue_description/config/bluerov2_heavy/controllers.yaml @@ -1,26 +1,163 @@ -ismc: - ros__parameters: - mass: 13.5 - buoyancy: 112.80 - weight: 114.80 - inertia_tensor_coeff: [0.16, 0.16, 0.16] - added_mass_coeff: [-5.50, -12.70, -14.60, -0.12, -0.12, -0.12] - linear_damping_coeff: [-4.03, -6.22, -5.18, -0.07, -0.07, -0.07] - quadratic_damping_coeff: [-18.18, -21.66, -36.99, -1.55, -1.55, -1.55] - center_of_gravity: [0.0, 0.0, 0.0] - center_of_buoyancy: [0.0, 0.0, 0.0] - ocean_current: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - num_thrusters: 8 - tcm: [-0.707, -0.707, 0.707, 0.707, 0.0, 0.0, 0.0, 0.0, - 0.707, -0.707, 0.707, -0.707, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 1.0, 1.0, 1.0, 1.0, - 0.0, 0.0, 0.0, 0.0, -0.21805, 0.21805, -0.21805, 0.21805, - 0.0, 0.0, 0.0, 0.0, -0.12, -0.12, 0.12, 0.12, - 0.1888, -0.1888, -0.1888, 0.1888, 0.0, 0.0, 0.0 , 0.0] - control_rate: 100.0 - proportional_gain: [10.0, 10.0, 6.0, 3.0, 6.0, 10.0] - integral_gain: [0.01, 0.01, 0.01, 0.01, 0.01, 0.01] - derivative_gain: [0.1, 0.1, 0.1, 0.1, 0.1, 0.1] - sliding_gain: 20.0 - boundary_thickness: 200.0 - use_battery_state: false +controller_manager: + ros__parameters: + update_rate: 100 # Hz + + integral_sliding_mode_controller: + type: velocity_controllers/IntegralSlidingModeController + + thruster_allocation_matrix_controller: + type: thruster_allocation_matrix_controller/ThrusterAllocationMatrixController + + thruster1_controller: + type: thruster_controllers/PolynomialThrustCurveController + + thruster2_controller: + type: thruster_controllers/PolynomialThrustCurveController + + thruster3_controller: + type: thruster_controllers/PolynomialThrustCurveController + + thruster4_controller: + type: thruster_controllers/PolynomialThrustCurveController + + thruster5_controller: + type: thruster_controllers/PolynomialThrustCurveController + + thruster6_controller: + type: thruster_controllers/PolynomialThrustCurveController + + thruster7_controller: + type: thruster_controllers/PolynomialThrustCurveController + + thruster8_controller: + type: thruster_controllers/PolynomialThrustCurveController + +integral_sliding_mode_controller: + ros__parameters: + use_external_measured_states: true + reference_controller: thruster_allocation_matrix_controller + gains: + rho: 20.0 + lambda: 200.0 + Kp: [4.0, 4.0, 9.0, 3.0, 3.0, 7.0] + tf: + base_frame: "base_link" + odom_frame: "map" + 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] + +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 + min_thrust: -40.0 + max_thrust: 60.0 + min_deadband: 1470 + max_deadband: 1530 + neutral_pwm: 1500 + thrust_curve_coefficients: [1498.0, 12.01, -0.04731, -0.002098, 0.00002251] + +thruster2_controller: + ros__parameters: + thruster: thruster2_joint + min_thrust: -40.0 + max_thrust: 60.0 + min_deadband: 1470 + max_deadband: 1530 + neutral_pwm: 1500 + thrust_curve_coefficients: [1498.0, 12.01, -0.04731, -0.002098, 0.00002251] + +thruster3_controller: + ros__parameters: + thruster: thruster3_joint + min_thrust: -40.0 + max_thrust: 60.0 + min_deadband: 1470 + max_deadband: 1530 + neutral_pwm: 1500 + thrust_curve_coefficients: [1498.0, 12.01, -0.04731, -0.002098, 0.00002251] + +thruster4_controller: + ros__parameters: + thruster: thruster4_joint + min_thrust: -40.0 + max_thrust: 60.0 + min_deadband: 1470 + max_deadband: 1530 + neutral_pwm: 1500 + thrust_curve_coefficients: [1498.0, 12.01, -0.04731, -0.002098, 0.00002251] + +thruster5_controller: + ros__parameters: + thruster: thruster5_joint + min_thrust: -40.0 + max_thrust: 60.0 + min_deadband: 1470 + max_deadband: 1530 + neutral_pwm: 1500 + thrust_curve_coefficients: [1498.0, 12.01, -0.04731, -0.002098, 0.00002251] + +thruster6_controller: + ros__parameters: + thruster: thruster6_joint + min_thrust: -40.0 + max_thrust: 60.0 + min_deadband: 1470 + max_deadband: 1530 + neutral_pwm: 1500 + thrust_curve_coefficients: [1498.0, 12.01, -0.04731, -0.002098, 0.00002251] + +thruster7_controller: + ros__parameters: + thruster: thruster7_joint + min_thrust: -40.0 + max_thrust: 60.0 + min_deadband: 1470 + max_deadband: 1530 + neutral_pwm: 1500 + thrust_curve_coefficients: [1498.0, 12.01, -0.04731, -0.002098, 0.00002251] + +thruster8_controller: + ros__parameters: + thruster: thruster8_joint + min_thrust: -40.0 + max_thrust: 60.0 + min_deadband: 1470 + max_deadband: 1530 + neutral_pwm: 1500 + thrust_curve_coefficients: [1498.0, 12.01, -0.04731, -0.002098, 0.00002251] diff --git a/blue_description/config/bluerov2_heavy/new_manager.yaml b/blue_description/config/bluerov2_heavy/new_manager.yaml new file mode 100644 index 00000000..10b7ad06 --- /dev/null +++ b/blue_description/config/bluerov2_heavy/new_manager.yaml @@ -0,0 +1,8 @@ +ardusub_manager: + ros__parameters: + message_intervals: + ids: [31, 32] + rates: [200.0, 200.0] + set_ekf_origin: false + set_home_position: false + publish_tf: true diff --git a/blue_description/xacro/bluerov2_heavy/config.xacro b/blue_description/xacro/bluerov2_heavy/config.xacro index 86503cda..ef0a3a6f 100644 --- a/blue_description/xacro/bluerov2_heavy/config.xacro +++ b/blue_description/xacro/bluerov2_heavy/config.xacro @@ -5,16 +5,17 @@ - - + - + + + + + diff --git a/blue_description/xacro/bluerov2_heavy/ros2_control.xacro b/blue_description/xacro/bluerov2_heavy/ros2_control.xacro new file mode 100644 index 00000000..0ab9119a --- /dev/null +++ b/blue_description/xacro/bluerov2_heavy/ros2_control.xacro @@ -0,0 +1,71 @@ + + + + + + + + thruster_hardware/ThrusterHardware + ${max_set_param_attempts} + + + + SERVO1_FUNCTION + 33 + 1 + + + + + SERVO2_FUNCTION + 34 + 2 + + + + + SERVO3_FUNCTION + 35 + 3 + + + + + SERVO4_FUNCTION + 36 + 4 + + + + + SERVO5_FUNCTION + 37 + 5 + + + + + SERVO6_FUNCTION + 38 + 6 + + + + + SERVO7_FUNCTION + 39 + 7 + + + + + SERVO8_FUNCTION + 40 + 8 + + + + + + + diff --git a/blue_dynamics/CMakeLists.txt b/blue_dynamics/CMakeLists.txt deleted file mode 100644 index 095ac857..00000000 --- a/blue_dynamics/CMakeLists.txt +++ /dev/null @@ -1,92 +0,0 @@ -cmake_minimum_required(VERSION 3.8) -project(blue_dynamics) - -# Default to C99 -if(NOT CMAKE_C_STANDARD) - set(CMAKE_C_STANDARD 99) -endif() - -# Default to C++ 17 -if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 17) -endif() - -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic) -endif() - -set(THIS_PACKAGE_INCLUDE_DEPENDS - rclcpp - ament_cmake - eigen3_cmake_module - Eigen3 - blue_utils -) - -foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) - find_package(${Dependency} REQUIRED) -endforeach() - -include_directories( - include -) - -add_library(${PROJECT_NAME} SHARED - src/hydrodynamics.cpp - src/thruster_dynamics.cpp -) - -target_link_libraries(${PROJECT_NAME} - ${rclcpp_LIBRARIES} - blue_utils::blue_utils -) -target_include_directories(${PROJECT_NAME} PUBLIC - "$" - "$" -) -ament_target_dependencies(${PROJECT_NAME} ${THIS_PACKAGE_INCLUDE_DEPENDS}) - -install( - DIRECTORY include/ - DESTINATION include/${PROJECT_NAME} -) - -install( - TARGETS ${PROJECT_NAME} - EXPORT "export_${PROJECT_NAME}" - RUNTIME DESTINATION bin - LIBRARY DESTINATION lib - ARCHIVE DESTINATION lib - INCLUDES DESTINATION include -) - -if(BUILD_TESTING) - find_package(ament_cmake_gtest REQUIRED) - find_package(ament_lint_auto REQUIRED) - - # Run linters found in package.xml except those below - set(ament_cmake_copyright_FOUND TRUE) - set(ament_cmake_uncrustify_FOUND TRUE) - set(ament_cmake_pep257_FOUND TRUE) - set(ament_cmake_flake8_FOUND TRUE) - - ament_lint_auto_find_test_dependencies() - - # Setup unit tests - ament_add_gtest( - test_hydrodynamics - test/test_hydrodynamics.cpp - ) - target_link_libraries(test_hydrodynamics ${PROJECT_NAME}) - - ament_add_gtest( - test_thruster_dynamics - test/test_thruster_dynamics.cpp - ) - target_link_libraries(test_thruster_dynamics ${PROJECT_NAME}) -endif() - -ament_export_targets("export_${PROJECT_NAME}" HAS_LIBRARY_TARGET) -ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS}) - -ament_package() diff --git a/blue_dynamics/LICENSE b/blue_dynamics/LICENSE deleted file mode 100644 index 30e8e2ec..00000000 --- a/blue_dynamics/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_dynamics/include/blue_dynamics/hydrodynamics.hpp b/blue_dynamics/include/blue_dynamics/hydrodynamics.hpp deleted file mode 100644 index 44747599..00000000 --- a/blue_dynamics/include/blue_dynamics/hydrodynamics.hpp +++ /dev/null @@ -1,304 +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. - -#pragma once - -#include - -#include "blue_utils/eigen.hpp" - -namespace blue::dynamics -{ - -/** - * @brief Create a skew-symmetric matrix from the provided coefficients. - * - * @param a1 Coefficient one. - * @param a2 Coefficient two. - * @param a3 Coefficient three. - * @return A skew-symmetric matrix. - */ -[[nodiscard]] Eigen::Matrix3d createSkewSymmetricMatrix(double a1, double a2, double a3); - -/** - * @brief Create a skew-symmetric matrix from the provided vector. - * - * @param vec The vector whose coefficients will be used to create the skew-symmetric matrix. - * @return A skew-symmetric matrix. - */ -[[nodiscard]] Eigen::Matrix3d createSkewSymmetricMatrix(const Eigen::Vector3d & vec); - -/** - * @brief The inertia of the vehicle. - */ -class Inertia -{ -public: - /** - * @brief Default constructor for the inertia object. - */ - Inertia() = default; - /** - * @brief Construct a new Inertia object. - * - * @param mass The total mass of the vehicle (kg). - * @param inertia_tensor_coeff The inertia tensor coefficients `(I_xx, I_yy, I_zz)` (kg m^2). - * @param added_mass_coeff The added mass coefficients `(X_dot{u}, Y_dot{v}, Z_dot{w}, K_dot{p}, - * M_dot{q}, N_dot{r})`. - */ - Inertia( - double mass, const Eigen::Vector3d & inertia_tensor_coeff, - const Eigen::Vector6d & added_mass_coeff); - - /** - * @brief Get the vehicle's inertia matrix. - * - * @details The inertia matrix `M` is the sum of the rigid body mass `M_RB` and the added mass - * `M_A` such that `M = M_RB + M_A`. The definition used for the rigid body inertia matrix `M_RB` - * is provided by Thor I. Fossen's textbook "Handbook of Marine Craft Hydrodynamics and Motion - * Control" in Equation 3.44. Note that, in this model, we define the body frame to be coincident - * with the center of mass, such that r_g^b = 0. The result is that `M_RB` is a diagonal matrix. - * The definition used for the added mass inertia matrix `M_A` is provided by Gianluca Antonelli's - * textbook "Underwater Robots" in Section 2.4.1. - * - * @return The inertia matrix `M`. - */ - [[nodiscard]] Eigen::Matrix6d getInertia() const; - -private: - Eigen::Matrix6d inertia_matrix_; -}; - -/** - * @brief The Coriolis and centripetal forces acting on the vehicle. - */ -class Coriolis -{ -public: - /** - * @brief Default constructor for the Coriolis object. - */ - Coriolis() = default; - - /** - * @brief Construct a new Coriolis object. - * - * @param mass The total mass of the vehicle (kg). - * @param inertia_tensor_coeff The inertia tensor coefficients `(I_xx, I_yy, I_zz)` (kg m^2). - * @param added_mass_coeff The added mass coefficients `(X_dot{u}, Y_dot{v}, Z_dot{w}, K_dot{p}, - * M_dot{q}, N_dot{r})`. - */ - Coriolis( - double mass, const Eigen::Vector3d & inertia_tensor_coeff, Eigen::Vector6d added_mass_coeff); - - /** - * @brief Calculate the Coriolis and centripetal forces for the vehicle. - * - * @details The Coriolis and centripetal force matrix `C` is the sum of the rigid body Coriolis - * forces `C_RB` and the added Coriolis forces `C_A` such that `C = C_RB + C_A`. - * - * @param velocity The current velocity of the vehicle in the body frame. - * @return The Coriolis and centripetal force matrix `C`. - */ - [[nodiscard]] Eigen::Matrix6d calculateCoriolis(const Eigen::Vector6d & velocity) const; - - /** - * @brief Calculate the rigid body Coriolis matrix. - * - * @details The definition of the rigid body Coriolis-centripetal matrix `C_RB` used in this work - * is provided by Thor I. Fossen's textbook "Handbook of Marine Craft Hydrodynamics and Motion - * Control" in Equation 3.57. Note that, in this model, we define the body frame to be coincident - * with the center of mass, such that r_g^b = 0. - * - * @note The preferred entrypoint for calculating the Coriolis matrix is through the - * `calculateCoriolis` method; however, this method is made accessible for advanced users who wish - * to calculate the rigid body Coriolis matrix directly (which may be necessary when calculating - * the dynamics using the relative velocity). - * - * @param angular_velocity The current angular velocity of the vehicle in the body frame (rad/s). - * @return The rigid body Coriolis matrix `C_RB`. - */ - [[nodiscard]] Eigen::Matrix6d calculateRigidBodyCoriolis( - const Eigen::Vector3d & angular_velocity) const; - - /** - * @brief Calculate the added Coriolis matrix. - * - * @details The definition of the added Coriolis-centripetal matrix `C_A` used in this work is - * provided by Gianluca Antonelli's "Underwater Robots" in Section 2.4.1. - * - * @note The preferred entrypoint for calculating the Coriolis matrix is through the - * `calculateCoriolis` method; however, this method is made accessible for advanced users who wish - * to calculate the added Coriolis matrix directly (which may be necessary when calculating - * the dynamics using the relative velocity). - * - * @param velocity The current velocity of the vehicle in the body frame. - * @return The added Coriolis matrix `C_A`. - */ - [[nodiscard]] Eigen::Matrix6d calculateAddedCoriolis(const Eigen::Vector6d & velocity) const; - -private: - double mass_{0.0}; - Eigen::Matrix3d moments_; - Eigen::Vector6d added_mass_coeff_; -}; - -/** - * @brief The rigid-body damping forces acting on the vehicle. - */ -class Damping -{ -public: - /** - * @brief Default constructor for the Damping object. - */ - Damping() = default; - - /** - * @brief Construct a new Damping object. - * - * @param linear_damping_coeff The linear damping coefficients `(X_u, Y_v, Z_w, K_p, M_q, N_r)`. - * @param quadratic_damping_coeff The nonlinear damping coefficients `(X_u|u|, Y_v|v|, Z_w|w|, - * K_p|p|, M_q|q|, N_r|r|)`. - */ - Damping(const Eigen::Vector6d & linear_damping_coeff, Eigen::Vector6d quadratic_damping_coeff); - - /** - * @brief Calculate the damping forces for the vehicle. - * - * @details The rigid body damping matrix `D` is defined as the sum of the linear and nonlinear - * damping coefficients. The definition of the linear and nonlinear damping matrices used in this - * work is provided by Gianluca Antonelli's "Underwater Robots" in Section 2.4.2. - * - * @param velocity The current velocity of the vehicle in the body frame. - * @return The damping matrix `D`. - */ - [[nodiscard]] Eigen::Matrix6d calculateDamping(const Eigen::Vector6d & velocity) const; - -private: - Eigen::Matrix6d linear_damping_; - Eigen::Vector6d quadratic_damping_coeff_; - - /** - * @brief Calculate the nonlinear damping matrix. - * - * @param velocity The current velocity of the vehicle in the body frame. - * @return The nonlinear damping matrix. - */ - [[nodiscard]] Eigen::Matrix6d calculateNonlinearDamping(const Eigen::Vector6d & velocity) const; -}; - -/** - * @brief The restoring forces acting on the vehicle. - */ -class RestoringForces -{ -public: - /** - * @brief Default constructor for the RestoringForces object. - */ - RestoringForces() = default; - - /** - * @brief Construct a new RestoringForces object. - * - * @note The gravitational force vector `g` is given as [0, 0, 9.81]^T m/s^2. - * - * @param weight The weight of the vehicle (N). This should be defined according to the definition - * of `W` provided in Gianluca Antonelli's "Underwater Robots" in Section 2.5. - * @param buoyancy The buoyancy force acting on the vehicle. This should be defined according to - * the definition of `B` provided in Gianluca Antonelli's "Underwater Robots" in - * Section 2.5. - * @param center_of_buoyancy The center of buoyancy of the vehicle relative to the body frame. - * @param center_of_gravity The center of gravity of the vehicle relative to the body frame. - */ - RestoringForces( - double weight, double buoyancy, Eigen::Vector3d center_of_buoyancy, - Eigen::Vector3d center_of_gravity); - - /** - * @brief Calculate the restoring forces for the vehicle. - * - * @param rotation The current rotation of the vehicle in the inertial frame. - * @return The vector of restoring forces. - */ - [[nodiscard]] Eigen::Vector6d calculateRestoringForces(const Eigen::Matrix3d & rotation) const; - -private: - double weight_{0.0}; - double buoyancy_{0.0}; - Eigen::Vector3d center_of_buoyancy_; - Eigen::Vector3d center_of_gravity_; -}; - -/** - * @brief The velocity of the fluid in which the vehicle is operating. - */ -class CurrentEffects -{ -public: - /** - * @brief Default constructor for the CurrentEffects object. - */ - CurrentEffects() = default; - - /** - * @brief Construct a new CurrentEffects object - * - * @param current_velocity The velocity of the fluid in the inertial frame. - */ - explicit CurrentEffects(Eigen::Vector6d current_velocity); - - /** - * @brief Calculate the current in the body frame. - * - * @param rotation The current rotation of the vehicle in the inertial frame. - * @return The velocity of the current in the body frame. - */ - [[nodiscard]] Eigen::Vector6d calculateCurrentEffects(const Eigen::Matrix3d & rotation) const; - -private: - Eigen::Vector6d current_velocity_; -}; - -/** - * @brief The hydrodynamic parameters for an underwater vehicle. - * - * @note This implementation draws from Thor I. Fossen's "Handbook of Marine Craft Hydrodynamics - * and Motion Control" (2011) and Gianluca Antonelli's "Underwater Robots" (2014) for the - * hydrodynamic model. - * - */ -struct HydrodynamicParameters -{ - Inertia inertia; - Coriolis coriolis; - Damping damping; - RestoringForces restoring_forces; - CurrentEffects current_effects; - - HydrodynamicParameters() = default; - - HydrodynamicParameters( - Inertia inertia, Coriolis coriolis, Damping damping, RestoringForces restoring_forces, - CurrentEffects current_effects); -}; - -} // namespace blue::dynamics diff --git a/blue_dynamics/include/blue_dynamics/thruster_dynamics.hpp b/blue_dynamics/include/blue_dynamics/thruster_dynamics.hpp deleted file mode 100644 index 18d0486a..00000000 --- a/blue_dynamics/include/blue_dynamics/thruster_dynamics.hpp +++ /dev/null @@ -1,90 +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. - -#pragma once - -#include -#include - -namespace blue::dynamics -{ - -/** - * @brief The minimum PWM value that can be sent to the T200 thrusters. - */ -const uint16_t kMinPwm = 1100; - -/** - * @brief The maximum PWM value that can be sent to the T200 thrusters. - */ -const uint16_t kMaxPwm = 1900; - -/** - * @brief A standard PWM value for no thrust. - * - * @note This can be used when the thruster is not being used, or when the desired thrust falls - * within the deadband zone. - */ -const uint16_t kNoThrustPwm = 1500; - -/** - * @brief Calculate the T200 deadzone band for a given voltage. - * - * @param voltage The current battery voltage. - * @return The minimum and maximum PWM values for the deadzone band. - */ -[[nodiscard]] std::tuple calculateDeadZone(double voltage); - -/** - * @brief Calculate the general T200 deadzone band. - * - * @return The minimum and maximum PWM values for the deadzone band. - */ -[[nodiscard]] std::tuple calculateDeadZone(); - -/** - * @brief Approximate the PWM input needed to achieve a desired force given the current battery - * voltage. - * - * @note This is a simplified technique for calculating the PWM input. A more accurate method would - * perform a characterization of the thruster dynamics. The issue with that solution, however, is - * that there is no feedback from the BlueROV2 thrusters to use as the state variables for the - * system. - * - * @param force The desired force (N). - * @param voltage The current battery voltage (V). - * @return The PWM input needed to achieve the desired force. - */ -[[nodiscard]] int calculatePwmFromThrustSurface(double force, double voltage); - -/** - * @brief Approximate the PWM input needed to achieve a desired force. - * - * @note This is a simplified technique for calculating the PWM input. A more accurate method would - * perform a characterization of the thruster dynamics. The issue with that solution, however, is - * that there is no feedback from the BlueROV2 thrusters to use as the state variables for the - * system. - * - * @param force The desired force (N). - * @return The PWM input needed to achieve the desired force. - */ -[[nodiscard]] int calculatePwmFromThrustCurve(double force); - -} // namespace blue::dynamics diff --git a/blue_dynamics/package.xml b/blue_dynamics/package.xml deleted file mode 100644 index 5c968f00..00000000 --- a/blue_dynamics/package.xml +++ /dev/null @@ -1,33 +0,0 @@ - - - - - blue_dynamics - 0.0.1 - Library used to define and calculate the hydrodynamics of the BlueROV2 - - Evan Palmer - MIT - - https://github.com/Robotic-Decision-Making-Lab/blue.git - https://github.com/Robotic-Decision-Making-Lab/blue/issues - - Evan Palmer - - ament_cmake - eigen3_cmake_module - - eigen - blue_utils - - eigen3_cmake_module - - ament_cmake_gtest - ament_cmake_gmock - ament_lint_auto - ament_lint_common - - - ament_cmake - - diff --git a/blue_dynamics/src/hydrodynamics.cpp b/blue_dynamics/src/hydrodynamics.cpp deleted file mode 100644 index c001ff9e..00000000 --- a/blue_dynamics/src/hydrodynamics.cpp +++ /dev/null @@ -1,180 +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. - -#include "blue_dynamics/hydrodynamics.hpp" - -namespace blue::dynamics -{ - -[[nodiscard]] Eigen::Matrix3d createSkewSymmetricMatrix(double a1, double a2, double a3) -{ - Eigen::Matrix3d mat; - mat << 0, -a3, a2, a3, 0, -a1, -a2, a1, 0; - - return mat; -} - -[[nodiscard]] Eigen::Matrix3d createSkewSymmetricMatrix(const Eigen::Vector3d & vec) -{ - Eigen::Matrix3d mat; - mat << 0, -vec(2), vec(1), vec(2), 0, -vec(0), -vec(1), vec(0), 0; - - return mat; -} - -Inertia::Inertia( - double mass, const Eigen::Vector3d & inertia_tensor_coeff, - const Eigen::Vector6d & added_mass_coeff) -{ - Eigen::Matrix6d rigid_body = Eigen::Matrix6d::Zero(); - - rigid_body.topLeftCorner(3, 3) = mass * Eigen::Matrix3d::Identity(); - rigid_body.bottomRightCorner(3, 3) = inertia_tensor_coeff.asDiagonal().toDenseMatrix(); - - Eigen::Matrix6d added_mass = -added_mass_coeff.asDiagonal().toDenseMatrix(); - - inertia_matrix_ = rigid_body + added_mass; -} - -[[nodiscard]] Eigen::Matrix6d Inertia::getInertia() const { return inertia_matrix_; } - -Coriolis::Coriolis( - double mass, const Eigen::Vector3d & inertia_tensor_coeff, Eigen::Vector6d added_mass_coeff) -: mass_(mass), - moments_(inertia_tensor_coeff.asDiagonal().toDenseMatrix()), - added_mass_coeff_(std::move(added_mass_coeff)) -{ -} - -[[nodiscard]] Eigen::Matrix6d Coriolis::calculateCoriolis(const Eigen::Vector6d & velocity) const -{ - // The rigid body Coriolis matrix only needs the angular velocity - const Eigen::Vector3d angular_velocity = velocity.bottomRows(3); - - return calculateRigidBodyCoriolis(angular_velocity) + calculateAddedCoriolis(velocity); -} - -[[nodiscard]] Eigen::Matrix6d Coriolis::calculateRigidBodyCoriolis( - const Eigen::Vector3d & angular_velocity) const -{ - Eigen::Matrix6d rigid = Eigen::Matrix6d::Zero(); - - const Eigen::Vector3d moments_v2 = moments_ * angular_velocity; - - rigid.topLeftCorner(3, 3) = mass_ * createSkewSymmetricMatrix(angular_velocity); - rigid.bottomRightCorner(3, 3) = -createSkewSymmetricMatrix(moments_v2); - - return rigid; -} - -[[nodiscard]] Eigen::Matrix6d Coriolis::calculateAddedCoriolis( - const Eigen::Vector6d & velocity) const -{ - Eigen::Matrix6d added = Eigen::Matrix6d::Zero(); - - Eigen::Matrix3d linear_vel = createSkewSymmetricMatrix( - added_mass_coeff_(0) * velocity(0), added_mass_coeff_(1) * velocity(1), - added_mass_coeff_(2) * velocity(2)); - - Eigen::Matrix3d angular_vel = createSkewSymmetricMatrix( - added_mass_coeff_(3) * velocity(3), added_mass_coeff_(4) * velocity(4), - added_mass_coeff_(5) * velocity(5)); - - added.topRightCorner(3, 3) = linear_vel; - added.bottomLeftCorner(3, 3) = linear_vel; - added.bottomRightCorner(3, 3) = angular_vel; - - return added; -} - -Damping::Damping( - const Eigen::Vector6d & linear_damping_coeff, Eigen::Vector6d quadratic_damping_coeff) -: linear_damping_(-linear_damping_coeff.asDiagonal().toDenseMatrix()), - quadratic_damping_coeff_(std::move(quadratic_damping_coeff)) -{ -} - -[[nodiscard]] Eigen::Matrix6d Damping::calculateDamping(const Eigen::Vector6d & velocity) const -{ - return linear_damping_ + calculateNonlinearDamping(velocity); -} - -[[nodiscard]] Eigen::Matrix6d Damping::calculateNonlinearDamping( - const Eigen::Vector6d & velocity) const -{ - return -(quadratic_damping_coeff_.asDiagonal().toDenseMatrix() * velocity.cwiseAbs()) - .asDiagonal() - .toDenseMatrix(); -} - -RestoringForces::RestoringForces( - double weight, double buoyancy, Eigen::Vector3d center_of_buoyancy, - Eigen::Vector3d center_of_gravity) -: weight_(weight), - buoyancy_(buoyancy), - center_of_buoyancy_(std::move(center_of_buoyancy)), - center_of_gravity_(std::move(center_of_gravity)) -{ -} - -[[nodiscard]] Eigen::Vector6d RestoringForces::calculateRestoringForces( - const Eigen::Matrix3d & rotation) const -{ - const Eigen::Vector3d fg(0, 0, weight_); - const Eigen::Vector3d fb(0, 0, -buoyancy_); - - Eigen::Vector6d g_rb; - - g_rb.topRows(3) = rotation * (fg + fb); - g_rb.bottomRows(3) = - center_of_gravity_.cross(rotation * fg) + center_of_buoyancy_.cross(rotation * fb); - - g_rb *= -1; - - return g_rb; -} - -CurrentEffects::CurrentEffects(Eigen::Vector6d current_velocity) -: current_velocity_(std::move(current_velocity)) -{ -} - -[[nodiscard]] Eigen::Vector6d CurrentEffects::calculateCurrentEffects( - const Eigen::Matrix3d & rotation) const -{ - Eigen::Vector6d rotated_current_effects; - rotated_current_effects.topRows(3) = rotation * current_velocity_.topRows(3); - rotated_current_effects.bottomRows(3) = rotation * current_velocity_.bottomRows(3); - - return rotated_current_effects; -} - -HydrodynamicParameters::HydrodynamicParameters( - Inertia inertia, Coriolis coriolis, Damping damping, RestoringForces restoring_forces, - CurrentEffects current_effects) -: inertia(std::move(inertia)), - coriolis(std::move(coriolis)), - damping(std::move(damping)), - restoring_forces(std::move(restoring_forces)), - current_effects(std::move(current_effects)) -{ -} - -} // namespace blue::dynamics diff --git a/blue_dynamics/src/thruster_dynamics.cpp b/blue_dynamics/src/thruster_dynamics.cpp deleted file mode 100644 index f7774bd1..00000000 --- a/blue_dynamics/src/thruster_dynamics.cpp +++ /dev/null @@ -1,96 +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. - -#include "blue_dynamics/thruster_dynamics.hpp" - -#include -#include - -namespace blue::dynamics -{ - -[[nodiscard]] std::tuple calculateDeadZone(double voltage) -{ - // The minimum PWM in the deadzone increases as the voltage increases - // This is best represented by a 3rd order polynomial - const int min_deadzone = static_cast(std::round( - 0.00926 * std::pow(voltage, 3) - 0.488 * std::pow(voltage, 2) + 9.749 * voltage + 1401.84)); - - // The maximum PWM in the deadzone decreases as the voltage increases - // This is best represented by a 4th order polynomial - const int max_deadzone = static_cast(std::round( - -0.005208 * std::pow(voltage, 4) + 0.3264 * std::pow(voltage, 3) - - 7.354 * std::pow(voltage, 2) + 69.087 * voltage + 1310.19)); - - return std::tuple(min_deadzone, max_deadzone); -} - -[[nodiscard]] std::tuple calculateDeadZone() -{ - // The deadband zone was identified using the thrust curve data with an 18v battery - return std::tuple(1470, 1530); -} - -[[nodiscard]] int calculatePwmFromThrustSurface(double force, double voltage) -{ - // Coefficients for the surface identified by fitting a surface to the thrust curves for the - // voltages 10, 12, 14, 16, 18, 20 using Matlab's `fit` function with the `Poly23` surface - const double p00 = 1439; - const double p10 = 7.621; - const double p01 = 42.06; - const double p20 = -0.2692; - const double p11 = -3.278; - const double p02 = -0.1122; - const double p21 = 0.08778; - const double p12 = 0.006153; - const double p03 = -0.001667; - - const double pwm = p00 + p10 * voltage + p01 * force + p20 * std::pow(voltage, 2) + - p11 * voltage * force + p02 * std::pow(force, 2) + - p21 * std::pow(voltage, 2) * force + p12 * voltage * std::pow(force, 2) + - p03 * std::pow(force, 3); - - return static_cast(std::round(pwm)); -} - -[[nodiscard]] int calculatePwmFromThrustCurve(double force) -{ - // The thrust curve is only accurate over the following force range, so we restrict the input - // forces to that range - const double min_force = -40.0; - const double max_force = 60.0; - - force = std::clamp(force, min_force, max_force); - - // Coefficients for the 4th-degree polynomial fit to the thrust curve for the T200 run using - // a battery at 18v. The polynomial was identified using Matlab's `fit` function. - const double p00 = 1498; - const double p01 = 12.01; - const double p02 = -0.04731; - const double p03 = -0.002098; - const double p04 = 0.00002251; - - const double pwm = p00 + p01 * force + p02 * std::pow(force, 2) + p03 * std::pow(force, 3) + - p04 * std::pow(force, 4); - - return static_cast(std::round(pwm)); -} - -} // namespace blue::dynamics diff --git a/blue_dynamics/test/test_hydrodynamics.cpp b/blue_dynamics/test/test_hydrodynamics.cpp deleted file mode 100644 index a1b3ac22..00000000 --- a/blue_dynamics/test/test_hydrodynamics.cpp +++ /dev/null @@ -1,155 +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. - -#include - -#include -#include - -#include "blue_dynamics/hydrodynamics.hpp" -#include "blue_utils/eigen.hpp" - -namespace blue::dynamics::test -{ - -using blue::dynamics::Coriolis; -using blue::dynamics::Damping; -using blue::dynamics::Inertia; -using blue::dynamics::RestoringForces; - -TEST(HydrodynamicsTest, TestInertia) -{ - const double mass = 5.0; - - const Eigen::Vector3d inertia_coeff(1, 2, 3); - - Eigen::Vector6d added_mass_coeff; // NOLINT - added_mass_coeff << 1, 2, 3, 4, 5, 6; - - const Inertia inertia = Inertia(mass, inertia_coeff, added_mass_coeff); - - Eigen::Vector6d expected; // NOLINT - expected << 4, 3, 2, -3, -3, -3; - - const Eigen::Matrix6d expected_matrix = expected.asDiagonal().toDenseMatrix(); - const Eigen::Matrix6d actual = inertia.getInertia(); - - ASSERT_TRUE(actual.isApprox(expected_matrix)); -} - -TEST(HydrodynamicsTest, TestCoriolis) -{ - const double mass = 5.0; - - const Eigen::Vector3d inertia_coeff(1, 2, 3); - - Eigen::Vector6d added_mass_coeff; // NOLINT - added_mass_coeff << 1, 2, 3, 4, 5, 6; - - const Coriolis coriolis = Coriolis(mass, inertia_coeff, added_mass_coeff); - - Eigen::Vector6d velocity; // NOLINT - velocity << 2, 2, 2, 2, 2, 2; - - Eigen::Matrix3d tl_rb; - tl_rb << 0, -10, 10, 10, 0, -10, -10, 10, 0; - - Eigen::Matrix3d br_rb; - br_rb << 0, 6, -4, -6, 0, 2, 4, -2, 0; - - Eigen::Matrix6d rigid = Eigen::Matrix6d::Zero(); // NOLINT - rigid.topLeftCorner(3, 3) = tl_rb; - rigid.bottomRightCorner(3, 3) = br_rb; - - Eigen::Matrix3d tr_bl_a; - tr_bl_a << 0, -6, 4, 6, 0, -2, -4, 2, 0; - - Eigen::Matrix3d br_a; - br_a << 0, -12, 10, 12, 0, -8, -10, 8, 0; - - Eigen::Matrix6d added = Eigen::Matrix6d::Zero(); // NOLINT - added.topRightCorner(3, 3) = tr_bl_a; - added.bottomLeftCorner(3, 3) = tr_bl_a; - added.bottomRightCorner(3, 3) = br_a; - - Eigen::Matrix6d expected = rigid + added; - - const Eigen::Matrix6d actual = coriolis.calculateCoriolis(velocity); - - ASSERT_TRUE(actual.isApprox(expected)); -} - -TEST(HydrodynamicsTest, TestDamping) -{ - Eigen::Vector6d linear_damping_coeff; // NOLINT - linear_damping_coeff << 1, 2, 3, 4, 5, 6; - - Eigen::Vector6d quadratic_damping_coeff; // NOLINT - quadratic_damping_coeff << 1, 2, 3, 4, 5, 6; - - const Damping damping = Damping(linear_damping_coeff, quadratic_damping_coeff); - - Eigen::Vector6d velocity; // NOLINT - velocity << 2, 2, 2, 2, 2, 2; - - Eigen::Vector6d linear; // NOLINT - linear << -1, -2, -3, -4, -5, -6; - - Eigen::Vector6d quadratic; // NOLINT - quadratic << -2, -4, -6, -8, -10, -12; - - const Eigen::Matrix6d expected = - linear.asDiagonal().toDenseMatrix() + quadratic.asDiagonal().toDenseMatrix(); - const Eigen::Matrix6d actual = damping.calculateDamping(velocity); - - ASSERT_TRUE(actual.isApprox(expected)); -} - -TEST(HydrodynamicsTest, TestRestoringForces) -{ - const double weight = 5.0; - const double buoyancy = 7.0; - - const Eigen::Vector3d center_of_buoyancy(1, 2, 3); - const Eigen::Vector3d center_of_gravity(1, 2, 3); - - const RestoringForces restoring_forces = - RestoringForces(weight, buoyancy, center_of_buoyancy, center_of_gravity); - - const Eigen::Quaterniond orientation(1, 0, 0, 0); - const Eigen::Matrix3d rot = orientation.toRotationMatrix(); - - Eigen::Vector6d expected; // NOLINT - expected << 0, 0, 2, 4, -2, 0; - - const Eigen::Vector6d actual = restoring_forces.calculateRestoringForces(rot); - - ASSERT_TRUE(actual.isApprox(expected)); -} - -} // namespace blue::dynamics::test - -int main(int argc, char ** argv) -{ - ::testing::InitGoogleTest(&argc, argv); - const int result = RUN_ALL_TESTS(); - - return result; -} diff --git a/blue_dynamics/test/test_thruster_dynamics.cpp b/blue_dynamics/test/test_thruster_dynamics.cpp deleted file mode 100644 index 83e8fc3a..00000000 --- a/blue_dynamics/test/test_thruster_dynamics.cpp +++ /dev/null @@ -1,72 +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. - -#include - -#include "blue_dynamics/thruster_dynamics.hpp" - -namespace blue::dynamics::test -{ - -using blue::dynamics::calculateDeadZone; -using blue::dynamics::calculatePwmFromThrustSurface; - -TEST(ThrusterDynamicsTest, TestDeadzoneModel) -{ - // These are measurements obtained from the BlueROV2 T200 characterization - const int expected_min = 1476; - const int expected_max = 1528; - - const double voltage = 20.0; - - // Calculate the deadzone using the battery voltage - std::tuple actual = calculateDeadZone(voltage); - - ASSERT_NEAR(expected_min, std::get<0>(actual), 1); - ASSERT_NEAR(expected_max, std::get<1>(actual), 1); -} - -TEST(ThrusterDynamicsTest, TestThrustModel) -{ - // These are measurements obtained from the BlueROV2 T200 characterization - const int expected_pwm = 1280; - const double force = -1.86 * 9.8; // Convert from KgF to N - const double voltage = 20.0; - - // Calculate the PWM values using the battery voltage - int actual = calculatePwmFromThrustSurface(force, voltage); - - ASSERT_NEAR(expected_pwm, actual, 15); - - // Now calculate the PWM values without the battery voltage - actual = calculatePwmFromThrustCurve(force); - - ASSERT_NEAR(expected_pwm, actual, 15); -} - -} // namespace blue::dynamics::test - -int main(int argc, char ** argv) -{ - ::testing::InitGoogleTest(&argc, argv); - const int result = RUN_ALL_TESTS(); - - return result; -} diff --git a/blue_manager/LICENSE b/blue_manager/LICENSE deleted file mode 100644 index 30e8e2ec..00000000 --- a/blue_manager/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_manager/blue_manager/manager.py b/blue_manager/blue_manager/manager.py deleted file mode 100644 index f13e8252..00000000 --- a/blue_manager/blue_manager/manager.py +++ /dev/null @@ -1,508 +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 copy import deepcopy - -import rclpy -from geographic_msgs.msg import GeoPoint, GeoPointStamped -from mavros_msgs.msg import OverrideRCIn, ParamEvent -from mavros_msgs.srv import CommandHome, MessageInterval -from rcl_interfaces.msg import Parameter, ParameterValue -from rcl_interfaces.srv import SetParameters -from rclpy.callback_groups import ReentrantCallbackGroup -from rclpy.executors import MultiThreadedExecutor -from rclpy.node import Node -from rclpy.qos import qos_profile_default, qos_profile_parameter_events -from std_srvs.srv import SetBool - - -class Manager(Node): - """Provides an interface between custom controllers and the BlueROV2.""" - - STOPPED_PWM = 1500 - - def __init__(self) -> None: - """Create a new control manager.""" - super().__init__("blue_manager") - - self.declare_parameter("num_thrusters", 8) - self.declare_parameter("backup_params_file", "") - self.declare_parameters( - namespace="message_intervals", - parameters=[ # type: ignore - ("ids", [31, 32]), - ("rates", [100.0, 100.0]), - ("request_interval", 30.0), - ], - ) - self.declare_parameters( - namespace="mode_change", - parameters=[("timeout", 1.0), ("retries", 3)], # type: ignore - ) - self.declare_parameters( - namespace="ekf_origin", - parameters=[ # type: ignore - ("latitude", 44.65870), - ("longitude", -124.06556), - ("altitude", 0.0), - ], - ) - self.declare_parameters( - namespace="home_position", - parameters=[ # type: ignore - ("latitude", 44.65870), - ("longitude", -124.06556), - ("altitude", 0.0), - ("yaw", 270.0), - ("request_interval", 30.0), - ], - ) - - self.passthrough_enabled = False - self.num_thrusters = ( - self.get_parameter("num_thrusters").get_parameter_value().integer_value - ) - self.timeout = ( - self.get_parameter("mode_change.timeout").get_parameter_value().double_value - ) - self.retries = ( - self.get_parameter("mode_change.retries") - .get_parameter_value() - .integer_value - ) - - # We need a reentrant callback group to get synchronous calls to services from - # within service callbacks. - reentrant_callback_group = ReentrantCallbackGroup() - - self.thruster_params_backup: dict[str, Parameter | None] = { - f"SERVO{i}_FUNCTION": None for i in range(1, self.num_thrusters + 1) - } - - # Try to load the backup parameters from a file - backup_filepath = ( - self.get_parameter("backup_params_file").get_parameter_value().string_value - ) - if not self.backup_thruster_params_from_file(backup_filepath): - self.get_logger().info( - "Failed to load all thruster parameters from the provided backup file." - " Attempting to load parameters from MAVROS." - ) - self.param_event_sub = self.create_subscription( - ParamEvent, - "/mavros/param/event", - self.backup_thruster_params_cb, - qos_profile_parameter_events, - ) - else: - self.get_logger().info( - "Successfully backed up the thruster parameters from the parameters" - " file." - ) - - # Publishers - self.override_rc_in_pub = self.create_publisher( - OverrideRCIn, "/mavros/rc/override", qos_profile_default - ) - self.gp_origin_pub = self.create_publisher( - GeoPointStamped, - "/mavros/global_position/set_gp_origin", - qos_profile_default, - ) - - # Services - self.set_pwm_passthrough_srv = self.create_service( - SetBool, - "/blue/cmd/enable_passthrough", - self.set_rc_passthrough_mode_cb, - callback_group=reentrant_callback_group, - ) - - # Service clients - def wait_for_client(client) -> None: - while not client.wait_for_service(timeout_sec=1.0): - self.get_logger().info(f"Waiting for {client.srv_name}...") - - self.set_param_srv_client = self.create_client( - SetParameters, - "/mavros/param/set_parameters", - callback_group=reentrant_callback_group, - ) - self.set_message_rates_client = self.create_client( - MessageInterval, "/mavros/set_message_interval" - ) - self.set_home_pos_client = self.create_client( - CommandHome, "/mavros/cmd/set_home" - ) - - wait_for_client(self.set_param_srv_client) - wait_for_client(self.set_message_rates_client) - wait_for_client(self.set_home_pos_client) - - # Set the intervals at which the desired MAVLink messages are sent - def set_message_rates( - message_ids: list[int], message_rates: list[float] - ) -> None: - for msg, rate in zip(message_ids, message_rates): - self.set_message_rates_client.call_async( - MessageInterval.Request(message_id=msg, message_rate=rate) - ) - - message_request_rate = ( - self.get_parameter("message_intervals.request_interval") - .get_parameter_value() - .double_value - ) - message_ids = list( - self.get_parameter("message_intervals.ids") - .get_parameter_value() - .integer_array_value - ) - message_rates = list( - self.get_parameter("message_intervals.rates") - .get_parameter_value() - .double_array_value - ) - - # We set the intervals periodically to handle the situation in which - # users launch QGC which requests different rates on boot. - # Inspiration for this is taken from the Orca4 project here: - # https://github.com/clydemcqueen/orca4/blob/77152829e1d65781717ca55379c229145d6006e9/orca_base/src/manager.cpp#L407 - self.message_rate_timer = self.create_timer( - message_request_rate, lambda: set_message_rates(message_ids, message_rates) - ) - - # Set the home position - def set_home_pos(lat: float, lon: float, alt: float, yaw: float) -> None: - self.set_home_pos_client.call_async( - CommandHome.Request( - current_gps=False, - yaw=yaw, - latitude=lat, - longitude=lon, - altitude=alt, - ) - ) - - # Set the home position. Some folks have discussed that this is necessary for - # GUIDED mode - home_lat = ( - self.get_parameter("home_position.latitude") - .get_parameter_value() - .double_value - ) - home_lon = ( - self.get_parameter("home_position.longitude") - .get_parameter_value() - .double_value - ) - home_alt = ( - self.get_parameter("home_position.altitude") - .get_parameter_value() - .double_value - ) - home_yaw = ( - self.get_parameter("home_position.yaw").get_parameter_value().double_value - ) - hp_request_rate = ( - self.get_parameter("home_position.request_interval") - .get_parameter_value() - .double_value - ) - - # Similar to the message rates, we set the home position periodically to handle - # the case in which the home position is set by QGC to a different location - self.home_position_timer = self.create_timer( - hp_request_rate, - lambda: set_home_pos(home_lat, home_lon, home_alt, home_yaw), - ) - - # Now, set the EKF origin. This is necessary to enable GUIDED mode and other - # autonomy features with ArduSub - origin_lat = ( - self.get_parameter("ekf_origin.latitude").get_parameter_value().double_value - ) - origin_lon = ( - self.get_parameter("ekf_origin.longitude") - .get_parameter_value() - .double_value - ) - origin_alt = ( - self.get_parameter("ekf_origin.altitude").get_parameter_value().double_value - ) - - # Normally, we would like to set the QoS policy to use transient local - # durability, but MAVROS uses the volitile durability setting for its - # subscriber. Consequently, we need to publish this every once-in-a-while - # to make sure that it gets set - self.set_ekf_origin_timer = self.create_timer( - 15.0, - lambda: self.set_ekf_origin_cb( - GeoPoint(latitude=origin_lat, longitude=origin_lon, altitude=origin_alt) - ), - ) - - @property - def backup_params_saved(self) -> bool: - """Whether or not the thruster parameters are backed up. - - Returns: - Whether or not the parameters are backed up. - """ - return None not in self.thruster_params_backup.values() - - def backup_thruster_params_from_file(self, backup_filepath: str) -> bool: - """Backup thruster parameters from a parameters file. - - Args: - backup_filepath: The full path to the backup file to load. - - Returns: - Whether or not the parameters were successfully backed up from the file. - """ - if not backup_filepath: - return False - - with open(backup_filepath, "r") as ardusub_params: - params = ardusub_params.readlines() - - for line in params: - line = line.rstrip() - split = line.split(" ") - - try: - param_id = split[0] - except IndexError: - continue - - try: - param_value = int(split[1]) - except (ValueError, IndexError): - continue - - if param_id in self.thruster_params_backup.keys(): - self.thruster_params_backup[param_id] = Parameter( - name=param_id, - value=ParameterValue(type=2, integer_value=param_value), - ) - - return self.backup_params_saved - - def backup_thruster_params_cb(self, event: ParamEvent) -> None: - """Backup the default thruster parameter values. - - MAVROS publishes the parameter values of all ArduSub parameters when - populating the parameter server. We subscribe to this topic to receive the - messages at the same time as MAVROS so that we can avoid duplicating requests - to the FCU. - - Args: - event: The default parameter loading event triggered by MAVROS. - """ - if ( - event.param_id in self.thruster_params_backup - and self.thruster_params_backup[event.param_id] is None - ): - self.thruster_params_backup[event.param_id] = Parameter( - name=event.param_id, value=event.value - ) - - if self.backup_params_saved: - self.get_logger().info( - "Successfully backed up the thruster parameters." - ) - - def set_rc(self, pwm: list[int]) -> None: - """Set the PWM values of the thrusters. - - Args: - pwm: The PWM values to set the thruster to. The length of the provided list - must be equal to the number of thrusters. - - Returns: - The result of the PWM setting call. - """ - if len(pwm) != self.num_thrusters: - raise ValueError( - "The length of the PWM input must equal the number of thrusters." - ) - - # Change the values of only the thruster channels - channels = pwm + [OverrideRCIn.CHAN_NOCHANGE] * (18 - self.num_thrusters) - self.override_rc_in_pub.publish(OverrideRCIn(channels=channels)) - - def stop_thrusters(self) -> None: - """Stop all thrusters.""" - self.get_logger().warning("Stopping all BlueROV2 thrusters.") - self.set_rc([self.STOPPED_PWM] * self.num_thrusters) - - def set_thruster_params(self, params: list[Parameter]) -> bool: - """Set the thruster parameters. - - Args: - params: The ArduSub parameters to set. - - Returns: - True if the parameters were successfully set, False otherwise. - """ - # We would actually prefer to set all of the parameters atomically, but - # this functionality is not currently supported by MAVROS - response: SetParameters.Response = self.set_param_srv_client.call( - SetParameters.Request(parameters=params) - ) - return all([result.successful for result in response.results]) - - def set_rc_passthrough_mode_cb( - self, request: SetBool.Request, response: SetBool.Response - ) -> SetBool.Response: - """Set the RC Passthrough mode. - - RC Passthrough mode enables users to control the BlueROV2 thrusters directly - using the RC channels. It is important that users disable their RC transmitter - prior to enabling RC Passthrough mode to avoid sending conflicting commands to - the thrusters. - - Args: - request: The request to enable/disable RC passthrough mode. - response: The result of the request. - - Returns: - The result of the request. - """ - if request.data: - if self.passthrough_enabled: - response.success = True - response.message = "The system is already in RC Passthrough mode." - return response - - if not self.thruster_params_backup: - response.success = False - response.message = ( - "The thrusters cannot be set to RC Passthrough mode without first" - " being successfully backed up." - ) - return response - - self.get_logger().warning( - "Attempting to switch to the RC Passthrough flight mode. All ArduSub" - " arming and failsafe procedures will be disabled upon success." - ) - - passthrough_params = deepcopy(self.thruster_params_backup) - - # Set the servo mode to "RC Passthrough" - # This disables the arming and failsafe features, but now lets us send PWM - # values to the thrusters without any mixing - try: - for param in passthrough_params.values(): - param.value.integer_value = 1 # type: ignore - except AttributeError: - response.success = False - response.message = ( - "Failed to switch to RC Passthrough mode. Please ensure that all" - " ArduSub parameters have been loaded prior to attempting to" - " switch modes." - ) - return response - - for _ in range(self.retries): - self.passthrough_enabled = self.set_thruster_params( - list(passthrough_params.values()) # type: ignore - ) - response.success = self.passthrough_enabled - - if response.success: - break - - if response.success: - response.message = "Successfully switched to RC Passthrough mode." - - self.stop_thrusters() - else: - response.message = "Failed to switch to RC Passthrough mode." - else: - if not self.thruster_params_backup: - response.success = False - response.message = ( - "The thruster backup parameters have not yet been stored." - ) - - if not self.passthrough_enabled: - response.success = True - response.message = ( - "The system was not in the RC Passthrough mode to start with." - ) - return response - - self.stop_thrusters() - - self.get_logger().warning("Attempting to disable RC Passthrough mode.") - - for _ in range(self.retries): - self.passthrough_enabled = not self.set_thruster_params( - list(self.thruster_params_backup.values()) # type: ignore - ) - - response.success = not self.passthrough_enabled - - if response.success: - break - - if response.success: - response.message = "Successfully left RC Passthrough mode." - else: - self.get_logger().warning( - "Failed to leave the RC Passthrough mode. If failure persists," - " the following backup parameters may be restored manually using" - f" QGC: {self.thruster_params_backup}" - ) - response.message = ( - "Failed to disable RC Passthrough mode. Good luck soldier." - ) - - self.get_logger().info(response.message) - - return response - - def set_ekf_origin_cb(self, origin: GeoPoint) -> None: - """Set the EKF origin. - - This is required for navigation on a vehicle with one of the provided - localizers. - - Args: - origin: The EKF origin to set. - """ - origin_stamped = GeoPointStamped() - origin_stamped.header.stamp = self.get_clock().now().to_msg() - origin_stamped.position = origin - self.gp_origin_pub.publish(origin_stamped) - - -def main(args: list[str] | None = None): - """Run the ROV manager.""" - rclpy.init(args=args) - - node = Manager() - executor = MultiThreadedExecutor() - rclpy.spin(node, executor) - - node.destroy_node() - rclpy.shutdown() diff --git a/blue_manager/launch/manager.launch.py b/blue_manager/launch/manager.launch.py deleted file mode 100644 index ed5fdaa2..00000000 --- a/blue_manager/launch/manager.launch.py +++ /dev/null @@ -1,67 +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 -from launch.substitutions import LaunchConfiguration -from launch_ros.actions import Node - - -def generate_launch_description() -> LaunchDescription: - """Generate a launch description for the Blue manager interface. - - Returns: - LaunchDescription: The Blue manager launch description. - """ - args = [ - DeclareLaunchArgument( - "config_filepath", - default_value=None, - description="The path to the manager configuration YAML file.", - ), - DeclareLaunchArgument( - "use_sim_time", - default_value="false", - description="Use the simulated Gazebo clock.", - ), - DeclareLaunchArgument( - "backup_params_file", - default_value="", - description="A configuration file with the ArduSub thruster parameters.", - ), - ] - - nodes = [ - Node( - package="blue_manager", - executable="blue_manager", - name="blue_manager", - output="both", - parameters=[ - LaunchConfiguration("config_filepath"), - { - "backup_params_file": LaunchConfiguration("backup_params_file"), - "use_sim_time": LaunchConfiguration("use_sim_time"), - }, - ], - ), - ] - - return LaunchDescription(args + nodes) diff --git a/blue_manager/package.xml b/blue_manager/package.xml deleted file mode 100644 index 25bf07b9..00000000 --- a/blue_manager/package.xml +++ /dev/null @@ -1,28 +0,0 @@ - - - - - blue_manager - 0.0.1 - An interface for enabling individual thruster control on the BlueROV2. - - Evan Palmer - MIT - - https://github.com/Robotic-Decision-Making-Lab/blue.git - https://github.com/Robotic-Decision-Making-Lab/blue/issues - - Evan Palmer - - mavros_msgs - std_srvs - - ament_copyright - ament_flake8 - ament_pep257 - python3-pytest - - - ament_python - - diff --git a/blue_manager/resource/blue_manager b/blue_manager/resource/blue_manager deleted file mode 100644 index e69de29b..00000000 diff --git a/blue_manager/setup.cfg b/blue_manager/setup.cfg deleted file mode 100644 index 036fce1c..00000000 --- a/blue_manager/setup.cfg +++ /dev/null @@ -1,4 +0,0 @@ -[develop] -script_dir=$base/lib/blue_manager -[install] -install_scripts=$base/lib/blue_manager diff --git a/blue_manager/setup.py b/blue_manager/setup.py deleted file mode 100644 index 0c071598..00000000 --- a/blue_manager/setup.py +++ /dev/null @@ -1,49 +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 setup - -package_name = "blue_manager" - -setup( - name=package_name, - version="0.0.1", - packages=[package_name], - 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"], - zip_safe=True, - maintainer="Evan Palmer", - maintainer_email="evanp922@gmail.com", - description=( - "An interface for enabling individual thruster control on the BlueROV2." - ), - license="MIT", - tests_require=["pytest"], - entry_points={ - "console_scripts": ["blue_manager = blue_manager.manager:main"], - }, -) diff --git a/blue_manager/test/test_copyright.py b/blue_manager/test/test_copyright.py deleted file mode 100644 index 8f18fa4b..00000000 --- a/blue_manager/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/blue_utils/CMakeLists.txt b/blue_utils/CMakeLists.txt deleted file mode 100644 index bbfd6a46..00000000 --- a/blue_utils/CMakeLists.txt +++ /dev/null @@ -1,68 +0,0 @@ -cmake_minimum_required(VERSION 3.8) -project(blue_utils) - -# Default to C99 -if(NOT CMAKE_C_STANDARD) - set(CMAKE_C_STANDARD 99) -endif() - -# Default to C++ 17 -if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 17) -endif() - -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic) -endif() - -set(THIS_PACKAGE_INCLUDE_DEPENDS - rclcpp - ament_cmake - geometry_msgs - eigen3_cmake_module - Eigen3 -) - -foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) - find_package(${Dependency} REQUIRED) -endforeach() - -add_library(${PROJECT_NAME} INTERFACE) - -target_link_libraries(${PROJECT_NAME} INTERFACE) -target_include_directories(${PROJECT_NAME} INTERFACE - "$" - "$" -) -ament_target_dependencies(${PROJECT_NAME} INTERFACE ${THIS_PACKAGE_INCLUDE_DEPENDS}) - -install( - DIRECTORY include/ - DESTINATION include/${PROJECT_NAME} -) - -install( - TARGETS ${PROJECT_NAME} - EXPORT "export_${PROJECT_NAME}" - RUNTIME DESTINATION bin - LIBRARY DESTINATION lib - ARCHIVE DESTINATION lib - INCLUDES DESTINATION include -) - -if(BUILD_TESTING) - find_package(ament_lint_auto REQUIRED) - - # Run linters found in package.xml except those below - set(ament_cmake_copyright_FOUND TRUE) - set(ament_cmake_uncrustify_FOUND TRUE) - set(ament_cmake_pep257_FOUND TRUE) - set(ament_cmake_flake8_FOUND TRUE) - - ament_lint_auto_find_test_dependencies() -endif() - -ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS}) -ament_export_targets("export_${PROJECT_NAME}") - -ament_package() diff --git a/blue_utils/LICENSE b/blue_utils/LICENSE deleted file mode 100644 index 30e8e2ec..00000000 --- a/blue_utils/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_utils/include/blue_utils/eigen.hpp b/blue_utils/include/blue_utils/eigen.hpp deleted file mode 100644 index 0c096045..00000000 --- a/blue_utils/include/blue_utils/eigen.hpp +++ /dev/null @@ -1,64 +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. - -#pragma once - -#include -#include - -/// @cond -namespace Eigen -{ - -// Extend the Eigen namespace to include commonly used matrix types -using Matrix6d = Eigen::Matrix; -using Vector6d = Eigen::Matrix; - -} // namespace Eigen -/// @endcond - -namespace blue::utility -{ - -/** - * @brief Convert an std::vector into an Eigen::Matrix. - * - * @tparam T The type of values held by the vector. - * @tparam major The order to copy over the elements in (e.g., ``Eigen::RowMajor``). - * @param rows The number of rows in the resulting matrix. - * @param cols The number of columns in the resulting matrix. - * @param vec The vector to convert into a matrix. - * @return The converted Eigen matrix. - */ -template -inline Eigen::Matrix vectorToEigen( - const std::vector & vec, int rows, int cols) -{ - // While it would be preferable to define the rows and cols as template parameters, the - // primary use-case for this method within the scope of this implementation is to use it with - // ROS 2 parameters which are not always known at compile time (e.g., TCM). Therefore, the rows - // and cols are made to be function parameters. - typedef const Eigen::Matrix M; - Eigen::Map mat(vec.data(), rows, cols); - - return mat; -} - -} // namespace blue::utility diff --git a/blue_utils/include/blue_utils/tf2.hpp b/blue_utils/include/blue_utils/tf2.hpp deleted file mode 100644 index d9e5cd56..00000000 --- a/blue_utils/include/blue_utils/tf2.hpp +++ /dev/null @@ -1,95 +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. - -#pragma once - -#include -#include - -#include "blue_utils/eigen.hpp" -#include "geometry_msgs/msg/accel.hpp" -#include "geometry_msgs/msg/wrench.hpp" - -/// @cond -namespace tf2 -{ - -// Extend the tf2 namespace to include some commonly used conversions - -/** - * @brief Convert a geometry_msgs::msg::Accel message into an Eigen vector. - * - * @param in The Accel message to convert. - * @param out The Eigen vector that should be populated with the Accel data. - */ -inline void fromMsg(const geometry_msgs::msg::Accel & in, Eigen::Vector6d & out) -{ - Eigen::Vector6d v; - v << in.linear.x, in.linear.y, in.linear.z, in.angular.x, in.angular.y, in.angular.z; - out = v; -} - -/** - * @brief Convert a geometry_msgs::msg::Wrench into an Eigen vector. - * - * @param in The Wrench message to convert. - * @param out The Eigen vector that should be populated with the Wrench data. - */ -inline void fromMsg(const geometry_msgs::msg::Wrench & in, Eigen::Vector6d & out) -{ - Eigen::Vector6d v; - v << in.force.x, in.force.y, in.force.z, in.torque.x, in.torque.y, in.torque.z; - out = v; -} - -/** - * @brief Convert an Eigen vector into a geometry_msgs::msg::Wrench message. - * - * @note This method was renamed to avoid overloading conflicts with the Twist declaration. This - * is consistent with previous naming applied in the tf2_eigen project. - * - * @param in The Eigen vector to convert into a Wrench message. - * @return geometry_msgs::msg::Wrench - */ -inline geometry_msgs::msg::Wrench toMsg2(const Eigen::Vector6d & in) -{ - geometry_msgs::msg::Wrench msg; - msg.force.x = in[0]; - msg.force.y = in[1]; - msg.force.z = in[2]; - msg.torque.x = in[3]; - msg.torque.y = in[4]; - msg.torque.z = in[5]; - return msg; -} - -} // namespace tf2 -/// @endcond - -namespace blue::transforms -{ - -// Coordinate frame IDs -constexpr char * kMapFrameId = "map"; -constexpr char * kMapNedFrameId = "map_ned"; -constexpr char * kBaseLinkFrameId = "base_link"; -constexpr char * kBaseLinkFrdFrameId = "base_link_frd"; - -} // namespace blue::transforms diff --git a/blue_utils/package.xml b/blue_utils/package.xml deleted file mode 100644 index 4e844de7..00000000 --- a/blue_utils/package.xml +++ /dev/null @@ -1,30 +0,0 @@ - - - - - blue_utils - 0.0.1 - Common helper functions and types - - Evan Palmer - MIT - - https://github.com/Robotic-Decision-Making-Lab/blue.git - https://github.com/Robotic-Decision-Making-Lab/blue/issues - - Evan Palmer - - ament_cmake - eigen3_cmake_module - - eigen - - eigen3_cmake_module - - ament_lint_auto - ament_lint_common - - - ament_cmake - - diff --git a/blue_utils/scripts/arm.sh b/scripts/arm.sh similarity index 100% rename from blue_utils/scripts/arm.sh rename to scripts/arm.sh diff --git a/blue_utils/scripts/disarm.sh b/scripts/disarm.sh similarity index 100% rename from blue_utils/scripts/disarm.sh rename to scripts/disarm.sh diff --git a/blue_utils/scripts/send_cmd.sh b/scripts/send_cmd.sh similarity index 100% rename from blue_utils/scripts/send_cmd.sh rename to scripts/send_cmd.sh diff --git a/blue_utils/scripts/send_pwm.sh b/scripts/send_pwm.sh similarity index 100% rename from blue_utils/scripts/send_pwm.sh rename to scripts/send_pwm.sh diff --git a/blue_utils/scripts/start_keyboard_teleop.sh b/scripts/start_keyboard_teleop.sh similarity index 100% rename from blue_utils/scripts/start_keyboard_teleop.sh rename to scripts/start_keyboard_teleop.sh From f46a572db32f5d65e8d6b14a214447272bf38c75 Mon Sep 17 00:00:00 2001 From: Evan Palmer Date: Fri, 22 Mar 2024 16:01:44 -0700 Subject: [PATCH 02/38] Replaced launch files with yaml --- blue_bringup/launch/base.launch | 67 ----- blue_bringup/launch/base.launch.py | 265 ------------------ blue_bringup/launch/base.launch.yaml | 126 +++++++++ .../launch/bluerov2/bluerov2.launch.py | 158 ----------- .../launch/bluerov2/bluerov2.launch.yaml | 58 ++++ blue_bringup/launch/bluerov2/tf.launch.py | 190 ------------- .../launch/bluerov2/thrusters.launch.yaml | 37 +++ .../bluerov2_heavy/bluerov2_heavy.launch.py | 150 ---------- .../bluerov2_heavy/bluerov2_heavy.launch.yaml | 58 ++++ .../bluerov2_heavy/bluerov2_heavy_launch.xml | 0 blue_bringup/launch/bluerov2_heavy/tf.launch | 35 --- .../bluerov2_heavy/thrusters.launch.yaml | 49 ++++ .../bluerov2_heavy_reach.launch.py | 159 ----------- .../bluerov2_heavy_reach.launch.yaml | 58 ++++ .../launch/bluerov2_heavy_reach/tf.launch.py | 226 --------------- .../thrusters.launch.yaml | 49 ++++ blue_bringup/launch/sitl.launch | 12 - blue_bringup/launch/sitl.launch.py | 174 ------------ blue_bringup/launch/sitl.launch.yaml | 49 ++++ blue_bringup/launch/tf.launch.yaml | 34 +++ 20 files changed, 518 insertions(+), 1436 deletions(-) delete mode 100644 blue_bringup/launch/base.launch delete mode 100644 blue_bringup/launch/base.launch.py create mode 100644 blue_bringup/launch/base.launch.yaml delete mode 100644 blue_bringup/launch/bluerov2/bluerov2.launch.py create mode 100644 blue_bringup/launch/bluerov2/bluerov2.launch.yaml delete mode 100644 blue_bringup/launch/bluerov2/tf.launch.py create mode 100644 blue_bringup/launch/bluerov2/thrusters.launch.yaml delete mode 100644 blue_bringup/launch/bluerov2_heavy/bluerov2_heavy.launch.py create mode 100644 blue_bringup/launch/bluerov2_heavy/bluerov2_heavy.launch.yaml delete mode 100644 blue_bringup/launch/bluerov2_heavy/bluerov2_heavy_launch.xml delete mode 100644 blue_bringup/launch/bluerov2_heavy/tf.launch create mode 100644 blue_bringup/launch/bluerov2_heavy/thrusters.launch.yaml delete mode 100644 blue_bringup/launch/bluerov2_heavy_reach/bluerov2_heavy_reach.launch.py create mode 100644 blue_bringup/launch/bluerov2_heavy_reach/bluerov2_heavy_reach.launch.yaml delete mode 100644 blue_bringup/launch/bluerov2_heavy_reach/tf.launch.py create mode 100644 blue_bringup/launch/bluerov2_heavy_reach/thrusters.launch.yaml delete mode 100644 blue_bringup/launch/sitl.launch delete mode 100644 blue_bringup/launch/sitl.launch.py create mode 100644 blue_bringup/launch/sitl.launch.yaml create mode 100644 blue_bringup/launch/tf.launch.yaml diff --git a/blue_bringup/launch/base.launch b/blue_bringup/launch/base.launch deleted file mode 100644 index 56e35d30..00000000 --- a/blue_bringup/launch/base.launch +++ /dev/null @@ -1,67 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/blue_bringup/launch/base.launch.py b/blue_bringup/launch/base.launch.py deleted file mode 100644 index b0bd9109..00000000 --- a/blue_bringup/launch/base.launch.py +++ /dev/null @@ -1,265 +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 -from launch_ros.actions import Node -from launch_ros.substitutions import FindPackageShare - - -def generate_launch_description() -> LaunchDescription: - """Generate a launch description to run the system. - - Returns: - The base launch file for BlueROV2 configurations. - """ - args = [ - DeclareLaunchArgument( - "description_package", - default_value="blue_description", - description=( - "The description package with the blue configuration files. This is" - " typically not set, but is available in case another description" - " package has been defined." - ), - ), - DeclareLaunchArgument( - "configuration_type", - default_value="bluerov2_heavy", - description="The BlueROV2 configuration type to load.", - ), - DeclareLaunchArgument( - "controllers_file", - default_value="controllers.yaml", - description="The BlueROV2 Heavy controller configuration file.", - ), - DeclareLaunchArgument( - "localization_file", - default_value="localization.yaml", - description="The BlueROV2 Heavy localization configuration file.", - ), - DeclareLaunchArgument( - "manager_file", - default_value="new_manager.yaml", - description="The ArduSub manager configuration file.", - ), - DeclareLaunchArgument( - "mavros_file", - default_value="mavros.yaml", - description="The MAVROS configuration file.", - ), - DeclareLaunchArgument( - "ardusub_params_file", - default_value="ardusub.parm", - description=( - "The ArduSub parameters that the BlueROV2 should use if running in" - " simulation." - ), - ), - DeclareLaunchArgument( - "joy_file", - default_value="joy_teleop.yaml", - description="The joystick controller configuration file.", - ), - DeclareLaunchArgument( - "gazebo_world_file", - default_value="", - description="The world configuration to load if using Gazebo.", - ), - DeclareLaunchArgument( - "localization_source", - default_value="gazebo", - 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", - default_value="false", - description="Launch the Gazebo + ArduSub simulator.", - ), - DeclareLaunchArgument( - "use_rviz", - default_value="false", - description="Launch RViz2.", - ), - DeclareLaunchArgument( - "use_joy", default_value="false", description="Use a joystick controller." - ), - DeclareLaunchArgument( - "rviz_config", - default_value="", - description="The RViz2 configuration file to load.", - ), - DeclareLaunchArgument( - "prefix", - default_value="", - description=( - "The prefix of the model. This is useful for multi-robot setups." - " Expected format '/'." - ), - ), - DeclareLaunchArgument( - "robot_description", - default_value="", - description="The model URDF file.", - ), - ] - - description_package = LaunchConfiguration("description_package") - configuration_type = LaunchConfiguration("configuration_type") - use_sim = LaunchConfiguration("use_sim") - robot_description = LaunchConfiguration("robot_description") - - nodes = [ - Node( - package="mavros", - executable="mavros_node", - output="both", - parameters=[ - PathJoinSubstitution( - [ - FindPackageShare(description_package), - "config", - LaunchConfiguration("mavros_file"), - ] - ), - {"use_sim_time": use_sim}, - ], - ), - Node( - package="robot_state_publisher", - executable="robot_state_publisher", - output="both", - parameters=[ - {"use_sim_time": use_sim, "robot_description": robot_description} - ], - ), - Node( - package="rviz2", - executable="rviz2", - name="rviz2", - output="both", - arguments=[ - "-d", - PathJoinSubstitution( - [ - FindPackageShare(description_package), - "rviz", - LaunchConfiguration("rviz_config"), - ] - ), - ], - parameters=[ - {"use_sim_time": use_sim, "robot_description": robot_description} - ], - condition=IfCondition(LaunchConfiguration("use_rviz")), - ), - ] - - includes = [ - IncludeLaunchDescription( - PythonLaunchDescriptionSource( - [ - PathJoinSubstitution( - [ - FindPackageShare("blue_bringup"), - "launch", - "sitl.launch.py", - ] - ) - ] - ), - launch_arguments={ - "description_package": description_package, - "configuration_type": configuration_type, - "ardusub_params_file": LaunchConfiguration("ardusub_params_file"), - "gazebo_world_file": LaunchConfiguration("gazebo_world_file"), - }.items(), - condition=IfCondition(use_sim), - ), - IncludeLaunchDescription( - PythonLaunchDescriptionSource( - PathJoinSubstitution( - [FindPackageShare("blue_manager"), "manager.launch.py"] - ) - ), - launch_arguments={ - "config_filepath": PathJoinSubstitution( - [ - FindPackageShare(description_package), - "config", - configuration_type, - LaunchConfiguration("manager_file"), - ] - ), - "backup_params_file": PathJoinSubstitution( - [ - FindPackageShare(description_package), - "config", - configuration_type, - LaunchConfiguration("ardusub_params_file"), - ] - ), - "use_sim_time": use_sim, - }.items(), - ), - IncludeLaunchDescription( - PythonLaunchDescriptionSource( - PathJoinSubstitution( - [FindPackageShare("blue_localization"), "localization.launch.py"] - ) - ), - launch_arguments={ - "config_filepath": PathJoinSubstitution( - [ - FindPackageShare(description_package), - "config", - configuration_type, - LaunchConfiguration("localization_file"), - ] - ), - "localization_source": LaunchConfiguration("localization_source"), - "use_mocap": LaunchConfiguration("use_mocap"), - "use_camera": LaunchConfiguration("use_camera"), - "use_sim_time": use_sim, - }.items(), - ), - ] - - return LaunchDescription(args + nodes + includes) diff --git a/blue_bringup/launch/base.launch.yaml b/blue_bringup/launch/base.launch.yaml new file mode 100644 index 00000000..e24ba9ae --- /dev/null +++ b/blue_bringup/launch/base.launch.yaml @@ -0,0 +1,126 @@ +launch: + + # Arguments + - arg: + name: description_package + default: blue_description + + - arg: + name: configuration_type + default: bluerov2_heavy + + - arg: + name: localization_file + default: $(find-pkg-share $(var description_package))/config/$(var configuration_type)/localization.yaml + + - arg: + name: mavros_file + default: $(find-pkg-share $(var description_package))/config/mavros.yaml + + - arg: + name: joy_file + default: $(find-pkg-share $(var description_package))/config/joy_teleop.yaml + + - arg: + name: gazebo_world_file + default: $(find-pkg-share $(var description_package))/gazebo/worlds/$(var configuration_type)_underwater.world + + - arg: + name: rviz_config + default: $(find-pkg-share $(var description_package))/rviz/$(var configuration_type).rviz + + - arg: + name: description_file + default: $(find-pkg-share $(var description_package))/xacro/$(var configuration_type)/config.xacro + + - arg: + name: ardusub_params_file + default: $(find-pkg-share $(var description_package))/config/$(var configuration_type)/ardusub.parm + + - 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" + + - arg: + name: use_rviz + default: "false" + + - arg: + name: use_joy + default: "false" + + # Load the description file + - let: + name: robot_description + value: $(command 'xacro $(var description_file) use_sim:=$(var use_sim)') + + # Nodes + - node: + pkg: mavros + exec: mavros_node + param: + - from: $(var mavros_file) + - name: use_sim_time + value: $(var use_sim) + + - node: + pkg: robot_state_publisher + exec: robot_state_publisher + param: + - name: robot_description + value: $(var robot_description) + - name: use_sim_time + value: $(var use_sim) + + - node: + pkg: rviz2 + exec: rviz2 + if: $(var use_rviz) + args: -d $(var rviz_config) + param: + - name: robot_description + value: $(var robot_description) + + # Includes + - include: + file: $(find-pkg-share blue_bringup)/launch/sitl.launch.yaml + if: $(var use_sim) + arg: + - name: description_package + value: $(var description_package) + - name: configuration_type + value: $(var configuration_type) + - name: gazebo_world_file + value: $(var gazebo_world_file) + - name: ardusub_params_file + value: $(var ardusub_params_file) + + - 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) diff --git a/blue_bringup/launch/bluerov2/bluerov2.launch.py b/blue_bringup/launch/bluerov2/bluerov2.launch.py deleted file mode 100644 index fcf0f2c1..00000000 --- a/blue_bringup/launch/bluerov2/bluerov2.launch.py +++ /dev/null @@ -1,158 +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.launch_description_sources import PythonLaunchDescriptionSource -from launch.substitutions import ( - Command, - FindExecutable, - LaunchConfiguration, - PathJoinSubstitution, -) -from launch_ros.substitutions import FindPackageShare - - -def generate_launch_description() -> LaunchDescription: - """Generate a launch description to run the system. - - Returns: - The launch description for the BlueROV2 base configuration. - """ - args = [ - DeclareLaunchArgument( - "controller", - default_value="ismc", - description=( - "The controller to use; this should be the same name as the" - " controller's executable." - ), - choices=["ismc"], - ), - DeclareLaunchArgument( - "localization_source", - default_value="gazebo", - 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", - default_value="false", - description="Launch the Gazebo + ArduSub simulator.", - ), - DeclareLaunchArgument( - "use_rviz", default_value="false", description="Launch RViz2." - ), - DeclareLaunchArgument( - "use_joy", default_value="false", description="Use a joystick controller." - ), - DeclareLaunchArgument( - "rviz_config", - default_value="bluerov2.rviz", - description="The RViz2 configuration file to load.", - ), - DeclareLaunchArgument( - "prefix", - default_value="", - description=( - "The prefix of the model. This is useful for multi-robot setups." - " Expected format '/'." - ), - ), - ] - - robot_description = Command( - [ - PathJoinSubstitution([FindExecutable(name="xacro")]), - " ", - PathJoinSubstitution( - [ - FindPackageShare("blue_description"), - "xacro", - "bluerov2", - "config.xacro", - ] - ), - " ", - "prefix:=", - LaunchConfiguration("prefix"), - " ", - "use_sim:=", - LaunchConfiguration("use_sim"), - ] - ) - - return LaunchDescription( - [ - *args, - IncludeLaunchDescription( - PythonLaunchDescriptionSource( - PathJoinSubstitution( - [FindPackageShare("blue_bringup"), "launch", "base.launch.py"] - ) - ), - launch_arguments={ - "configuration_type": "bluerov2", - "controller": LaunchConfiguration("controller"), - "localization_source": LaunchConfiguration("localization_source"), - "use_camera": LaunchConfiguration("use_camera"), - "use_mocap": LaunchConfiguration("use_mocap"), - "use_sim": LaunchConfiguration("use_sim"), - "use_rviz": LaunchConfiguration("use_rviz"), - "rviz_config": LaunchConfiguration("rviz_config"), - "gazebo_world_file": LaunchConfiguration( - "gazebo_world_file", default="bluerov2_underwater.world" - ), - "prefix": LaunchConfiguration("prefix"), - "robot_description": robot_description, - "use_joy": LaunchConfiguration("use_joy"), - }.items(), - ), - IncludeLaunchDescription( - PythonLaunchDescriptionSource( - PathJoinSubstitution( - [ - FindPackageShare("blue_bringup"), - "launch", - "bluerov2", - "tf.launch.py", - ] - ) - ), - launch_arguments={"prefix": LaunchConfiguration("prefix")}.items(), - ), - ] - ) diff --git a/blue_bringup/launch/bluerov2/bluerov2.launch.yaml b/blue_bringup/launch/bluerov2/bluerov2.launch.yaml new file mode 100644 index 00000000..e03e3d1f --- /dev/null +++ b/blue_bringup/launch/bluerov2/bluerov2.launch.yaml @@ -0,0 +1,58 @@ +launch: + + # Arguments + - arg: + name: localization_source + default: gazebo + + - arg: + name: use_camera + default: "false" + + - arg: + name: use_mocap + default: "false" + + - arg: + name: use_sim + default: "false" + + - arg: + name: use_rviz + default: "false" + + - arg: + name: use_joy + default: "false" + + - arg: + name: rviz_config + default: $(find-pkg-share blue_description)/rviz/bluerov2.rviz + + - arg: + name: gazebo_world_file + default: $(find-pkg-share blue_description)/gazebo/worlds/bluerov2_underwater.world + + # Includes + - include: + file: $(find-pkg-share blue_bringup)/launch/base.launch.yaml + arg: + - name: configuration_type + value: bluerov2 + - name: gazebo_world_file + value: $(var gazebo_world_file) + - name: localization_source + value: $(var localization_source) + - name: use_camera + value: $(var use_camera) + - name: use_mocap + value: $(var use_mocap) + - name: use_sim + value: $(var use_sim) + - name: use_rviz + value: $(var use_rviz) + - name: use_joy + value: $(var use_joy) + + - include: + file: $(find-pkg-share blue_bringup)/launch/bluerov2/thrusters.launch.yaml diff --git a/blue_bringup/launch/bluerov2/tf.launch.py b/blue_bringup/launch/bluerov2/tf.launch.py deleted file mode 100644 index d97fc19a..00000000 --- a/blue_bringup/launch/bluerov2/tf.launch.py +++ /dev/null @@ -1,190 +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 -from launch.substitutions import LaunchConfiguration -from launch_ros.actions import Node - - -def generate_launch_description(): - """Generate a launch description for the static TF broadcasters.""" - args = [ - DeclareLaunchArgument( - "prefix", default_value="", description="The URDF model prefix." - ), - ] - - prefix = LaunchConfiguration("prefix") - - nodes = [ - Node( - package="tf2_ros", - executable="static_transform_publisher", - name="base_link_to_thruster1", - arguments=[ - "--x", - "0.14", - "--y", - "-0.092", - "--z", - "0.0", - "--roll", - "-1.571", - "--pitch", - "1.571", - "--yaw", - "-0.785", - "--frame-id", - [prefix, "base_link"], - "--child-frame-id", - [prefix, "thruster1"], - ], - output="both", - ), - Node( - package="tf2_ros", - executable="static_transform_publisher", - name="base_link_to_thruster2", - arguments=[ - "--x", - "0.14", - "--y", - "0.092", - "--z", - "0.0", - "--roll", - "-1.571", - "--pitch", - "1.571", - "--yaw", - "-2.356", - "--frame-id", - [prefix, "base_link"], - "--child-frame-id", - [prefix, "thruster2"], - ], - output="both", - ), - Node( - package="tf2_ros", - executable="static_transform_publisher", - name="base_link_to_thruster3", - arguments=[ - "--x", - "-0.15", - "--y", - "-0.092", - "--z", - "0.0", - "--roll", - "-1.571", - "--pitch", - "1.571", - "--yaw", - "0.785", - "--frame-id", - [prefix, "base_link"], - "--child-frame-id", - [prefix, "thruster3"], - ], - output="both", - ), - Node( - package="tf2_ros", - executable="static_transform_publisher", - name="base_link_to_thruster4", - arguments=[ - "--x", - "-0.15", - "--y", - "0.092", - "--z", - "0.0", - "--roll", - "-1.571", - "--pitch", - "1.571", - "--yaw", - "2.356", - "--frame-id", - [prefix, "base_link"], - "--child-frame-id", - [prefix, "thruster4"], - ], - output="both", - ), - Node( - package="tf2_ros", - executable="static_transform_publisher", - name="base_link_to_thruster5", - arguments=[ - "--x", - "0.0", - "--y", - "-0.109", - "--z", - "0.077", - "--frame-id", - [prefix, "base_link"], - "--child-frame-id", - [prefix, "thruster5"], - ], - output="both", - ), - Node( - package="tf2_ros", - executable="static_transform_publisher", - name="base_link_to_thruster6", - arguments=[ - "--x", - "0.0", - "--y", - "0.109", - "--z", - "0.077", - "--frame-id", - [prefix, "base_link"], - "--child-frame-id", - [prefix, "thruster6"], - ], - output="both", - ), - Node( - package="tf2_ros", - executable="static_transform_publisher", - name="base_link_to_base_footprint", - arguments=[ - "--x", - "-0.0", - "--y", - "0.0", - "--z", - "0.0", - "--frame-id", - [prefix, "base_link"], - "--child-frame-id", - [prefix, "base_footprint"], - ], - output="both", - ), - ] - - return LaunchDescription(args + nodes) diff --git a/blue_bringup/launch/bluerov2/thrusters.launch.yaml b/blue_bringup/launch/bluerov2/thrusters.launch.yaml new file mode 100644 index 00000000..70643a93 --- /dev/null +++ b/blue_bringup/launch/bluerov2/thrusters.launch.yaml @@ -0,0 +1,37 @@ +launch: + + - node: + pkg: tf2_ros + exec: static_transform_publisher + name: base_link_to_thruster1 + args: --x 0.14 --y -0.092 --roll -1.571 --pitch 1.571 --yaw -0.785 --frame-id base_link --child-frame-id thruster1 + + - node: + pkg: tf2_ros + exec: static_transform_publisher + name: base_link_to_thruster2 + args: --x 0.14 --y 0.092 --roll -1.571 --pitch 1.571 --yaw -2.356 --frame-id base_link --child-frame-id thruster2 + + - node: + pkg: tf2_ros + exec: static_transform_publisher + name: base_link_to_thruster3 + args: --x -0.15 --y -0.092 --roll -1.571 --pitch 1.571 --yaw 0.785 --frame-id base_link --child-frame-id thruster3 + + - node: + pkg: tf2_ros + exec: static_transform_publisher + name: base_link_to_thruster4 + args: --x -0.15 --y 0.092 --roll -1.571 --pitch 1.571 --yaw 2.356 --frame-id base_link --child-frame-id thruster4 + + - node: + pkg: tf2_ros + exec: static_transform_publisher + name: base_link_to_thruster5 + args: --y -0.109 --z 0.077 --frame-id base_link --child-frame-id thruster5 + + - node: + pkg: tf2_ros + exec: static_transform_publisher + name: base_link_to_thruster6 + args: --y 0.109 --z 0.077 --frame-id base_link --child-frame-id thruster6 diff --git a/blue_bringup/launch/bluerov2_heavy/bluerov2_heavy.launch.py b/blue_bringup/launch/bluerov2_heavy/bluerov2_heavy.launch.py deleted file mode 100644 index a32263fe..00000000 --- a/blue_bringup/launch/bluerov2_heavy/bluerov2_heavy.launch.py +++ /dev/null @@ -1,150 +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.launch_description_sources import PythonLaunchDescriptionSource -from launch_ros.actions import Node -from launch.substitutions import ( - Command, - FindExecutable, - LaunchConfiguration, - PathJoinSubstitution, -) -from launch_ros.substitutions import FindPackageShare - - -def generate_launch_description() -> LaunchDescription: - """Generate a launch description to run the system. - - Returns: - The launch description for the BlueROV2 base configuration. - """ - args = [ - DeclareLaunchArgument( - "localization_source", - default_value="gazebo", - 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", - default_value="false", - description="Launch the Gazebo + ArduSub simulator.", - ), - DeclareLaunchArgument( - "use_rviz", default_value="false", description="Launch RViz2." - ), - DeclareLaunchArgument( - "use_joy", default_value="false", description="Use a joystick controller." - ), - DeclareLaunchArgument( - "rviz_config", - default_value="bluerov2_heavy.rviz", - description="The RViz2 configuration file to load.", - ), - DeclareLaunchArgument( - "prefix", - default_value="", - description=( - "The prefix of the model. This is useful for multi-robot setups." - " Expected format '/'." - ), - ), - ] - - robot_description_content = Command( - [ - PathJoinSubstitution([FindExecutable(name="xacro")]), - " ", - PathJoinSubstitution( - [ - FindPackageShare("blue_description"), - "xacro", - "bluerov2_heavy", - "config.xacro", - ] - ), - " ", - "prefix:=", - LaunchConfiguration("prefix"), - " ", - "use_sim:=", - LaunchConfiguration("use_sim"), - ] - ) - robot_description = {"robot_description": robot_description_content} - - return LaunchDescription( - [ - *args, - IncludeLaunchDescription( - PythonLaunchDescriptionSource( - PathJoinSubstitution( - [FindPackageShare("blue_bringup"), "launch", "base.launch.py"] - ) - ), - launch_arguments={ - "configuration_type": "bluerov2_heavy", - "localization_source": LaunchConfiguration("localization_source"), - "use_camera": LaunchConfiguration("use_camera"), - "use_mocap": LaunchConfiguration("use_mocap"), - "use_sim": LaunchConfiguration("use_sim"), - "use_rviz": LaunchConfiguration("use_rviz"), - "rviz_config": LaunchConfiguration("rviz_config"), - "gazebo_world_file": LaunchConfiguration( - "gazebo_world_file", default="bluerov2_heavy_underwater.world" - ), - "prefix": LaunchConfiguration("prefix"), - "robot_description": robot_description_content, - "use_joy": LaunchConfiguration("use_joy"), - }.items(), - ), - IncludeLaunchDescription( - PythonLaunchDescriptionSource( - PathJoinSubstitution( - [ - FindPackageShare("blue_bringup"), - "launch", - "bluerov2_heavy", - "tf.launch.py", - ] - ) - ), - launch_arguments={"prefix": LaunchConfiguration("prefix")}.items(), - ), - ] - ) diff --git a/blue_bringup/launch/bluerov2_heavy/bluerov2_heavy.launch.yaml b/blue_bringup/launch/bluerov2_heavy/bluerov2_heavy.launch.yaml new file mode 100644 index 00000000..9e768043 --- /dev/null +++ b/blue_bringup/launch/bluerov2_heavy/bluerov2_heavy.launch.yaml @@ -0,0 +1,58 @@ +launch: + + # Arguments + - arg: + name: localization_source + default: gazebo + + - arg: + name: use_camera + default: "false" + + - arg: + name: use_mocap + default: "false" + + - arg: + name: use_sim + default: "false" + + - arg: + name: use_rviz + default: "false" + + - arg: + name: use_joy + default: "false" + + - arg: + name: rviz_config + default: $(find-pkg-share blue_description)/rviz/bluerov2_heavy.rviz + + - arg: + name: gazebo_world_file + default: $(find-pkg-share blue_description)/gazebo/worlds/bluerov2_heavy_underwater.world + + # Includes + - include: + file: $(find-pkg-share blue_bringup)/launch/base.launch.yaml + arg: + - name: configuration_type + value: bluerov2_heavy + - name: gazebo_world_file + value: $(var gazebo_world_file) + - name: localization_source + value: $(var localization_source) + - name: use_camera + value: $(var use_camera) + - name: use_mocap + value: $(var use_mocap) + - name: use_sim + value: $(var use_sim) + - name: use_rviz + value: $(var use_rviz) + - name: use_joy + value: $(var use_joy) + + - include: + file: $(find-pkg-share blue_bringup)/launch/bluerov2_heavy/thrusters.launch.yaml diff --git a/blue_bringup/launch/bluerov2_heavy/bluerov2_heavy_launch.xml b/blue_bringup/launch/bluerov2_heavy/bluerov2_heavy_launch.xml deleted file mode 100644 index e69de29b..00000000 diff --git a/blue_bringup/launch/bluerov2_heavy/tf.launch b/blue_bringup/launch/bluerov2_heavy/tf.launch deleted file mode 100644 index 01996453..00000000 --- a/blue_bringup/launch/bluerov2_heavy/tf.launch +++ /dev/null @@ -1,35 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/blue_bringup/launch/bluerov2_heavy/thrusters.launch.yaml b/blue_bringup/launch/bluerov2_heavy/thrusters.launch.yaml new file mode 100644 index 00000000..dba2b883 --- /dev/null +++ b/blue_bringup/launch/bluerov2_heavy/thrusters.launch.yaml @@ -0,0 +1,49 @@ +launch: + + - node: + pkg: tf2_ros + exec: static_transform_publisher + name: base_link_to_thruster1 + args: --x 0.14 --y -0.092 --roll -1.571 --pitch 1.571 --yaw -0.785 --frame-id base_link --child-frame-id thruster1 + + - node: + pkg: tf2_ros + exec: static_transform_publisher + name: base_link_to_thruster2 + args: --x 0.14 --y 0.092 --roll -1.571 --pitch 1.571 --yaw -2.356 --frame-id base_link --child-frame-id thruster2 + + - node: + pkg: tf2_ros + exec: static_transform_publisher + name: base_link_to_thruster3 + args: --x -0.15 --y -0.092 --roll -1.571 --pitch 1.571 --yaw 0.785 --frame-id base_link --child-frame-id thruster3 + + - node: + pkg: tf2_ros + exec: static_transform_publisher + name: base_link_to_thruster4 + args: --x -0.15 --y 0.092 --roll -1.571 --pitch 1.571 --yaw 2.356 --frame-id base_link --child-frame-id thruster4 + + - node: + pkg: tf2_ros + exec: static_transform_publisher + name: base_link_to_thruster5 + args: --x 0.118 --y -0.215 --z 0.064 --frame-id base_link --child-frame-id thruster5 + + - node: + pkg: tf2_ros + exec: static_transform_publisher + name: base_link_to_thruster6 + args: --x 0.118 --y 0.215 --z 0.064 --frame-id base_link --child-frame-id thruster6 + + - node: + pkg: tf2_ros + exec: static_transform_publisher + name: base_link_to_thruster7 + args: --x -0.118 --y -0.215 --z 0.064 --frame-id base_link --child-frame-id thruster7 + + - node: + pkg: tf2_ros + exec: static_transform_publisher + name: base_link_to_thruster8 + args: --x -0.118 --y 0.215 --z 0.064 --frame-id base_link --child-frame-id thruster8 diff --git a/blue_bringup/launch/bluerov2_heavy_reach/bluerov2_heavy_reach.launch.py b/blue_bringup/launch/bluerov2_heavy_reach/bluerov2_heavy_reach.launch.py deleted file mode 100644 index 2201e297..00000000 --- a/blue_bringup/launch/bluerov2_heavy_reach/bluerov2_heavy_reach.launch.py +++ /dev/null @@ -1,159 +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.launch_description_sources import PythonLaunchDescriptionSource -from launch.substitutions import ( - Command, - FindExecutable, - LaunchConfiguration, - PathJoinSubstitution, -) -from launch_ros.substitutions import FindPackageShare - - -def generate_launch_description() -> LaunchDescription: - """Generate a launch description to run the system. - - Returns: - The launch description for the BlueROV2 base configuration. - """ - args = [ - DeclareLaunchArgument( - "controller", - default_value="ismc", - description=( - "The controller to use; this should be the same name as the" - " controller's executable." - ), - choices=["ismc"], - ), - DeclareLaunchArgument( - "localization_source", - default_value="gazebo", - 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", - default_value="false", - description="Launch the Gazebo + ArduSub simulator.", - ), - DeclareLaunchArgument( - "use_rviz", default_value="false", description="Launch RViz2." - ), - DeclareLaunchArgument( - "use_joy", default_value="false", description="Use a joystick controller." - ), - DeclareLaunchArgument( - "rviz_config", - default_value="bluerov2_heavy_reach.rviz", - description="The RViz2 configuration file to load.", - ), - DeclareLaunchArgument( - "prefix", - default_value="", - description=( - "The prefix of the model. This is useful for multi-robot setups." - " Expected format '/'." - ), - ), - ] - - robot_description = Command( - [ - PathJoinSubstitution([FindExecutable(name="xacro")]), - " ", - PathJoinSubstitution( - [ - FindPackageShare("blue_description"), - "xacro", - "bluerov2_heavy_reach", - "config.xacro", - ] - ), - " ", - "prefix:=", - LaunchConfiguration("prefix"), - " ", - "use_sim:=", - LaunchConfiguration("use_sim"), - ] - ) - - return LaunchDescription( - [ - *args, - IncludeLaunchDescription( - PythonLaunchDescriptionSource( - PathJoinSubstitution( - [FindPackageShare("blue_bringup"), "launch", "base.launch.py"] - ) - ), - launch_arguments={ - "configuration_type": "bluerov2_heavy_reach", - "controller": LaunchConfiguration("controller"), - "localization_source": LaunchConfiguration("localization_source"), - "use_camera": LaunchConfiguration("use_camera"), - "use_mocap": LaunchConfiguration("use_mocap"), - "use_sim": LaunchConfiguration("use_sim"), - "use_rviz": LaunchConfiguration("use_rviz"), - "rviz_config": LaunchConfiguration("rviz_config"), - "gazebo_world_file": LaunchConfiguration( - "gazebo_world_file", - default="bluerov2_heavy_reach_underwater.world", - ), # noqa - "prefix": LaunchConfiguration("prefix"), - "robot_description": robot_description, - "use_joy": LaunchConfiguration("use_joy"), - }.items(), - ), - IncludeLaunchDescription( - PythonLaunchDescriptionSource( - PathJoinSubstitution( - [ - FindPackageShare("blue_bringup"), - "launch", - "bluerov2_heavy_reach", - "tf.launch.py", - ] - ) - ), - launch_arguments={"prefix": LaunchConfiguration("prefix")}.items(), - ), - ] - ) 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 new file mode 100644 index 00000000..16167652 --- /dev/null +++ b/blue_bringup/launch/bluerov2_heavy_reach/bluerov2_heavy_reach.launch.yaml @@ -0,0 +1,58 @@ +launch: + + # Arguments + - arg: + name: localization_source + default: gazebo + + - arg: + name: use_camera + default: "false" + + - arg: + name: use_mocap + default: "false" + + - arg: + name: use_sim + default: "false" + + - arg: + name: use_rviz + default: "false" + + - arg: + name: use_joy + default: "false" + + - arg: + name: rviz_config + default: $(find-pkg-share blue_description)/rviz/bluerov2_heavy_reach.rviz + + - arg: + name: gazebo_world_file + default: $(find-pkg-share blue_description)/gazebo/worlds/bluerov2_heavy_reach_underwater.world + + # Includes + - include: + file: $(find-pkg-share blue_bringup)/launch/base.launch.yaml + arg: + - name: configuration_type + value: bluerov2_heavy_reach + - name: gazebo_world_file + value: $(var gazebo_world_file) + - name: localization_source + value: $(var localization_source) + - name: use_camera + value: $(var use_camera) + - name: use_mocap + value: $(var use_mocap) + - name: use_sim + value: $(var use_sim) + - name: use_rviz + value: $(var use_rviz) + - name: use_joy + value: $(var use_joy) + + - include: + file: $(find-pkg-share blue_bringup)/launch/bluerov2_heavy_reach/thrusters.launch.yaml diff --git a/blue_bringup/launch/bluerov2_heavy_reach/tf.launch.py b/blue_bringup/launch/bluerov2_heavy_reach/tf.launch.py deleted file mode 100644 index 4ae56499..00000000 --- a/blue_bringup/launch/bluerov2_heavy_reach/tf.launch.py +++ /dev/null @@ -1,226 +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 -from launch.substitutions import LaunchConfiguration -from launch_ros.actions import Node - - -def generate_launch_description(): - """Generate a launch description for the static TF broadcasters.""" - args = [ - DeclareLaunchArgument( - "prefix", default_value="", description="The URDF model prefix." - ) - ] - - prefix = LaunchConfiguration("prefix") - - nodes = [ - Node( - package="tf2_ros", - executable="static_transform_publisher", - name="base_link_to_thruster1", - arguments=[ - "--x", - "0.088", - "--y", - "-0.102", - "--z", - "-0.04", - "--roll", - "-1.571", - "--pitch", - "1.571", - "--yaw", - "-1.047", - "--frame-id", - [prefix, "base_link"], - "--child-frame-id", - [prefix, "thruster1"], - ], - output="both", - ), - Node( - package="tf2_ros", - executable="static_transform_publisher", - name="base_link_to_thruster2", - arguments=[ - "--x", - "0.088", - "--y", - "0.102", - "--z", - "-0.04", - "--roll", - "-1.571", - "--pitch", - "1.571", - "--yaw", - "-2.094", - "--frame-id", - [prefix, "base_link"], - "--child-frame-id", - [prefix, "thruster2"], - ], - output="both", - ), - Node( - package="tf2_ros", - executable="static_transform_publisher", - name="base_link_to_thruster3", - arguments=[ - "--x", - "-0.088", - "--y", - "-0.102", - "--z", - "-0.04", - "--roll", - "-1.571", - "--pitch", - "1.571", - "--yaw", - "1.047", - "--frame-id", - [prefix, "base_link"], - "--child-frame-id", - [prefix, "thruster3"], - ], - output="both", - ), - Node( - package="tf2_ros", - executable="static_transform_publisher", - name="base_link_to_thruster4", - arguments=[ - "--x", - "-0.088", - "--y", - "0.102", - "--z", - "-0.04", - "--roll", - "-1.571", - "--pitch", - "1.571", - "--yaw", - "2.094", - "--frame-id", - [prefix, "base_link"], - "--child-frame-id", - [prefix, "thruster4"], - ], - output="both", - ), - Node( - package="tf2_ros", - executable="static_transform_publisher", - name="base_link_to_thruster5", - arguments=[ - "--x", - "0.118", - "--y", - "-0.215", - "--z", - "0.064", - "--frame-id", - [prefix, "base_link"], - "--child-frame-id", - [prefix, "thruster5"], - ], - output="both", - ), - Node( - package="tf2_ros", - executable="static_transform_publisher", - name="base_link_to_thruster6", - arguments=[ - "--x", - "0.118", - "--y", - "0.215", - "--z", - "0.064", - "--frame-id", - [prefix, "base_link"], - "--child-frame-id", - [prefix, "thruster6"], - ], - output="both", - ), - Node( - package="tf2_ros", - executable="static_transform_publisher", - name="base_link_to_thruster7", - arguments=[ - "--x", - "-0.118", - "--y", - "-0.215", - "--z", - "0.064", - "--frame-id", - [prefix, "base_link"], - "--child-frame-id", - [prefix, "thruster7"], - ], - output="both", - ), - Node( - package="tf2_ros", - executable="static_transform_publisher", - name="base_link_to_thruster8", - arguments=[ - "--x", - "-0.118", - "--y", - "0.215", - "--z", - "0.064", - "--frame-id", - [prefix, "base_link"], - "--child-frame-id", - [prefix, "thruster8"], - ], - output="both", - ), - Node( - package="tf2_ros", - executable="static_transform_publisher", - name="base_link_to_base_footprint", - arguments=[ - "--x", - "-0.0", - "--y", - "0.0", - "--z", - "0.0", - "--frame-id", - [prefix, "base_link"], - "--child-frame-id", - [prefix, "base_footprint"], - ], - output="both", - ), - ] - - return LaunchDescription(args + nodes) diff --git a/blue_bringup/launch/bluerov2_heavy_reach/thrusters.launch.yaml b/blue_bringup/launch/bluerov2_heavy_reach/thrusters.launch.yaml new file mode 100644 index 00000000..6c6074ff --- /dev/null +++ b/blue_bringup/launch/bluerov2_heavy_reach/thrusters.launch.yaml @@ -0,0 +1,49 @@ +launch: + - node: + pkg: tf2_ros + exec: static_transform_publisher + name: base_link_to_thruster1 + args: --x 0.088 --y -0.102 --z -0.04 --roll -1.571 --pitch 1.571 --yaw -1.047 --frame-id base_link --child-frame-id thruster1 + + - node: + pkg: tf2_ros + exec: static_transform_publisher + name: base_link_to_thruster2 + args: --x 0.088 --y 0.102 --z -0.04 --roll -1.571 --pitch 1.571 --yaw -2.094 --frame-id base_link --child-frame-id thruster2 + + - node: + pkg: tf2_ros + exec: static_transform_publisher + name: base_link_to_thruster3 + args: --x -0.088 --y -0.102 --z -0.04 --roll -1.571 --pitch 1.571 --yaw 1.047 --frame-id base_link --child-frame-id thruster3 + + - node: + pkg: tf2_ros + exec: static_transform_publisher + name: base_link_to_thruster4 + args: --x -0.088 --y 0.102 --z -0.04 --roll -1.571 --pitch 1.571 --yaw 2.094 --frame-id base_link --child-frame-id thruster4 + + - node: + pkg: tf2_ros + exec: static_transform_publisher + name: base_link_to_thruster5 + args: --x 0.118 --y -0.215 --z 0.064 --frame-id base_link --child-frame-id thruster5 + + - node: + pkg: tf2_ros + exec: static_transform_publisher + name: base_link_to_thruster6 + args: --x 0.118 --y 0.215 --z 0.064 --frame-id base_link --child-frame-id thruster6 + + - node: + pkg: tf2_ros + exec: static_transform_publisher + name: base_link_to_thruster7 + args: --x -0.118 --y -0.215 --z 0.064 --frame-id base_link --child-frame-id thruster7 + + - node: + pkg: tf2_ros + exec: static_transform_publisher + name: base_link_to_thruster8 + args: --x -0.118 --y 0.215 --z 0.064 --frame-id base_link --child-frame-id thruster8 + diff --git a/blue_bringup/launch/sitl.launch b/blue_bringup/launch/sitl.launch deleted file mode 100644 index 095722ef..00000000 --- a/blue_bringup/launch/sitl.launch +++ /dev/null @@ -1,12 +0,0 @@ - - - - - - - - - - - diff --git a/blue_bringup/launch/sitl.launch.py b/blue_bringup/launch/sitl.launch.py deleted file mode 100644 index 3b826711..00000000 --- a/blue_bringup/launch/sitl.launch.py +++ /dev/null @@ -1,174 +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, - ExecuteProcess, - IncludeLaunchDescription, -) -from launch.launch_description_sources import PythonLaunchDescriptionSource -from launch.substitutions import LaunchConfiguration, PathJoinSubstitution -from launch_ros.actions import Node -from launch_ros.substitutions import FindPackageShare - - -def generate_launch_description() -> LaunchDescription: - """Generate a launch description to run the system. - - Returns: - The base launch file for BlueROV2 configurations. - """ - args = [ - DeclareLaunchArgument( - "description_package", - default_value="blue_description", - description=( - "The description package with the blue configuration files. This is" - " typically not set, but is available in case another description" - " package has been defined." - ), - ), - DeclareLaunchArgument( - "configuration_type", - default_value="bluerov2_heavy", - description="The BlueROV2 configuration type to load.", - ), - DeclareLaunchArgument( - "ardusub_params_file", - default_value="ardusub.parm", - description=( - "The ArduSub parameters that the BlueROV2 should use if running in" - " simulation." - ), - ), - DeclareLaunchArgument( - "gazebo_world_file", - default_value="", - description="The world configuration to load if using Gazebo.", - ), - ] - - description_package = LaunchConfiguration("description_package") - configuration_type = LaunchConfiguration("configuration_type") - - ardusub_params_filepath = PathJoinSubstitution( - [ - FindPackageShare(description_package), - "config", - configuration_type, - LaunchConfiguration("ardusub_params_file"), - ] - ) - - nodes = [ - Node( - package="ros_gz_bridge", - executable="parameter_bridge", - arguments=[ - # Clock (IGN -> ROS 2) - "/clock@rosgraph_msgs/msg/Clock[gz.msgs.Clock" - ], - # output="both", - ), - Node( - package="ros_gz_bridge", - executable="parameter_bridge", - arguments=[ - # Camera (IGN -> ROS 2) - "/camera/image_raw@sensor_msgs/msg/Image[gz.msgs.Image", - # Odom (IGN -> ROS 2) - [ - "/model/", - configuration_type, - "/odometry@nav_msgs/msg/Odometry[gz.msgs.Odometry", - ], - ], - # output="both", - ), - Node( - package="ros_gz_bridge", - executable="parameter_bridge", - arguments=[ - # Odom (IGN -> ROS 2) - [ - "/model/", - configuration_type, - "/odometry@nav_msgs/msg/Odometry[gz.msgs.Odometry", - ], - ], - # output="both", - ), - Node( - package="ros_gz_sim", - executable="create", - arguments=["-name", configuration_type, "-topic", "robot_description"], - # output="both", - ), - ] - - processes = [ - ExecuteProcess( - cmd=[ - "ardusub", - "-S", - "-w", - "-M", - "JSON", - "--defaults", - ardusub_params_filepath, - "-I0", - "--home", - "44.65870,-124.06556,0.0,270.0", # my not-so-secret surf spot - ], - output="both", - ), - ] - - includes = [ - IncludeLaunchDescription( - PythonLaunchDescriptionSource( - [ - PathJoinSubstitution( - [ - FindPackageShare("ros_gz_sim"), - "launch", - "gz_sim.launch.py", - ] - ) - ] - ), - launch_arguments=[ - ( - "gz_args", - [ - "-v", - "4", - " ", - "-r", - " ", - LaunchConfiguration("gazebo_world_file"), - ], - ) - ], - ), - ] - - return LaunchDescription(args + nodes + processes + includes) diff --git a/blue_bringup/launch/sitl.launch.yaml b/blue_bringup/launch/sitl.launch.yaml new file mode 100644 index 00000000..bacd79bf --- /dev/null +++ b/blue_bringup/launch/sitl.launch.yaml @@ -0,0 +1,49 @@ +launch: + # Arguments + - arg: + name: description_package + default: blue_description + + - arg: + name: configuration_type + default: bluerov2_heavy + + - arg: + name: gazebo_world_file + default: $(find-pkg-share $(var description_package))/gazebo/worlds/$(var configuration_type)_underwater.world + + - arg: + name: ardusub_params_file + default: $(find-pkg-share $(var description_package))/config/$(var configuration_type)/ardusub.parm + + # Nodes + - 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 configuration_type)/odometry@nav_msgs/msg/Odometry[gz.msgs.Odometry + + - node: + pkg: ros_gz_sim + exec: create + args: -name $(var configuration_type) -topic robot_description + + # Processes + - executable: + cmd: ardusub -S -w -M JSON --defaults $(var ardusub_params_file) -IO --home 44.65870,-124.06556,0.0,270.0 + + # Includes + - include: + file: $(find-pkg-share ros_gz_sim)/launch/gz_sim.launch.py + arg: + - name: gz_args + value: -v 4 -r $(var gazebo_world_file) diff --git a/blue_bringup/launch/tf.launch.yaml b/blue_bringup/launch/tf.launch.yaml new file mode 100644 index 00000000..eef3578c --- /dev/null +++ b/blue_bringup/launch/tf.launch.yaml @@ -0,0 +1,34 @@ +launch: + + # REP 156 Transforms + - node: + pkg: tf2_ros + exec: static_transform_publisher + name: base_link_to_base_link_fsd + arg: + - name: roll + value: 3.142 + - name: pitch + value: 0.0 + - name: yaw + value: 0.0 + - name: frame-id + value: base_link + - name: child-frame-id + value: base_link_fsd + + - node: + pkg: tf2_ros + exec: map + name: map_frd + arg: + - name: roll + value: 3.142 + - name: pitch + value: 0.0 + - name: yaw + value: 0.0 + - name: frame-id + value: base_link + - name: child-frame-id + value: base_link_fsd From 95e4e41f0c2fd31a8e7ecb80b3a9a0bc7c3e3b7a Mon Sep 17 00:00:00 2001 From: Evan Palmer Date: Sat, 23 Mar 2024 02:37:33 -0700 Subject: [PATCH 03/38] Started integrating new control framework --- blue.repos | 2 +- blue_bringup/CMakeLists.txt | 2 +- .../launch/ardusub/ardusub.launch.yaml | 43 ++++ .../launch/{ => ardusub}/base.launch.yaml | 26 +-- .../launch/{ => ardusub}/sitl.launch.yaml | 0 .../launch/bluerov2/bluerov2.launch.yaml | 2 +- .../bluerov2_heavy/bluerov2_heavy.launch.yaml | 2 +- ...ch.py => bluerov2_heavy_control.launch.py} | 38 +++- .../bluerov2_heavy_reach.launch.yaml | 2 +- blue_bringup/launch/tf.launch.yaml | 28 +-- .../ardusub_manager.yaml} | 3 +- .../config/{ => ardusub}/mavros.yaml | 7 +- .../config/bluerov2/controllers.yaml | 159 ++++++++++++--- blue_description/config/bluerov2/manager.yaml | 19 -- .../config/bluerov2_heavy/controllers.yaml | 4 +- .../config/bluerov2_heavy/manager.yaml | 19 -- .../bluerov2_heavy_reach/controllers.yaml | 189 +++++++++++++++--- .../config/bluerov2_heavy_reach/manager.yaml | 19 -- blue_description/xacro/bluerov2/config.xacro | 11 +- .../xacro/bluerov2/ros2_control.xacro | 57 ++++++ .../xacro/bluerov2_heavy_reach/config.xacro | 8 +- .../bluerov2_heavy_reach/ros2_control.xacro | 71 +++++++ 22 files changed, 527 insertions(+), 184 deletions(-) create mode 100644 blue_bringup/launch/ardusub/ardusub.launch.yaml rename blue_bringup/launch/{ => ardusub}/base.launch.yaml (81%) rename blue_bringup/launch/{ => ardusub}/sitl.launch.yaml (100%) rename blue_bringup/launch/bluerov2_heavy/{control.launch.py => bluerov2_heavy_control.launch.py} (85%) rename blue_description/config/{bluerov2_heavy/new_manager.yaml => ardusub/ardusub_manager.yaml} (68%) rename blue_description/config/{ => ardusub}/mavros.yaml (74%) delete mode 100644 blue_description/config/bluerov2/manager.yaml delete mode 100644 blue_description/config/bluerov2_heavy/manager.yaml delete mode 100644 blue_description/config/bluerov2_heavy_reach/manager.yaml create mode 100644 blue_description/xacro/bluerov2/ros2_control.xacro create mode 100644 blue_description/xacro/bluerov2_heavy_reach/ros2_control.xacro diff --git a/blue.repos b/blue.repos index 1c80c1ec..a189f517 100644 --- a/blue.repos +++ b/blue.repos @@ -13,7 +13,7 @@ repositories: auv_controllers: type: git url: https://github.com/Robotic-Decision-Making-Lab/auv_controllers.git - version: develop + version: main ardusub_driver: type: git diff --git a/blue_bringup/CMakeLists.txt b/blue_bringup/CMakeLists.txt index f9ff49aa..df18d9e5 100644 --- a/blue_bringup/CMakeLists.txt +++ b/blue_bringup/CMakeLists.txt @@ -13,7 +13,7 @@ endforeach() install( DIRECTORY launch - DESTINATION share/${PROJECT_NAME} + DESTINATION share/blue_bringup ) ament_package() diff --git a/blue_bringup/launch/ardusub/ardusub.launch.yaml b/blue_bringup/launch/ardusub/ardusub.launch.yaml new file mode 100644 index 00000000..c720c84a --- /dev/null +++ b/blue_bringup/launch/ardusub/ardusub.launch.yaml @@ -0,0 +1,43 @@ +launch: + + # Arguments + - arg: + name: description_package + default: blue_description + + - arg: + name: mavros_file + default: $(find-pkg-share $(var description_package))/config/ardusub/mavros.yaml + + - arg: + name: gazebo_world_file + + - arg: + name: ardusub_params_file + + - arg: + name: use_sim + default: "false" + + # Nodes + - node: + pkg: mavros + exec: mavros_node + param: + - from: $(var mavros_file) + - name: use_sim_time + value: $(var use_sim) + + # Includes + - include: + file: $(find-pkg-share blue_bringup)/launch/ardusub/sitl.launch.yaml + if: $(var use_sim) + arg: + - name: description_package + value: $(var description_package) + - name: configuration_type + value: $(var configuration_type) + - name: gazebo_world_file + value: $(var gazebo_world_file) + - name: ardusub_params_file + value: $(var ardusub_params_file) diff --git a/blue_bringup/launch/base.launch.yaml b/blue_bringup/launch/ardusub/base.launch.yaml similarity index 81% rename from blue_bringup/launch/base.launch.yaml rename to blue_bringup/launch/ardusub/base.launch.yaml index e24ba9ae..8027aebe 100644 --- a/blue_bringup/launch/base.launch.yaml +++ b/blue_bringup/launch/ardusub/base.launch.yaml @@ -15,7 +15,7 @@ launch: - arg: name: mavros_file - default: $(find-pkg-share $(var description_package))/config/mavros.yaml + default: $(find-pkg-share $(var description_package))/config/ardusub/mavros.yaml - arg: name: joy_file @@ -71,14 +71,6 @@ launch: value: $(command 'xacro $(var description_file) use_sim:=$(var use_sim)') # Nodes - - node: - pkg: mavros - exec: mavros_node - param: - - from: $(var mavros_file) - - name: use_sim_time - value: $(var use_sim) - - node: pkg: robot_state_publisher exec: robot_state_publisher @@ -98,19 +90,6 @@ launch: value: $(var robot_description) # Includes - - include: - file: $(find-pkg-share blue_bringup)/launch/sitl.launch.yaml - if: $(var use_sim) - arg: - - name: description_package - value: $(var description_package) - - name: configuration_type - value: $(var configuration_type) - - name: gazebo_world_file - value: $(var gazebo_world_file) - - name: ardusub_params_file - value: $(var ardusub_params_file) - - include: file: $(find-pkg-share blue_localization)/localization.launch.py arg: @@ -124,3 +103,6 @@ launch: value: $(var use_sim) - name: config_filepath value: $(var localization_file) + + - include: + file: $(find-pkg-share blue_bringup)/launch/tf.launch.yaml diff --git a/blue_bringup/launch/sitl.launch.yaml b/blue_bringup/launch/ardusub/sitl.launch.yaml similarity index 100% rename from blue_bringup/launch/sitl.launch.yaml rename to blue_bringup/launch/ardusub/sitl.launch.yaml diff --git a/blue_bringup/launch/bluerov2/bluerov2.launch.yaml b/blue_bringup/launch/bluerov2/bluerov2.launch.yaml index e03e3d1f..d005e754 100644 --- a/blue_bringup/launch/bluerov2/bluerov2.launch.yaml +++ b/blue_bringup/launch/bluerov2/bluerov2.launch.yaml @@ -35,7 +35,7 @@ launch: # Includes - include: - file: $(find-pkg-share blue_bringup)/launch/base.launch.yaml + file: $(find-pkg-share blue_bringup)/launch/ardusub/base.launch.yaml arg: - name: configuration_type value: bluerov2 diff --git a/blue_bringup/launch/bluerov2_heavy/bluerov2_heavy.launch.yaml b/blue_bringup/launch/bluerov2_heavy/bluerov2_heavy.launch.yaml index 9e768043..c46e01f0 100644 --- a/blue_bringup/launch/bluerov2_heavy/bluerov2_heavy.launch.yaml +++ b/blue_bringup/launch/bluerov2_heavy/bluerov2_heavy.launch.yaml @@ -35,7 +35,7 @@ launch: # Includes - include: - file: $(find-pkg-share blue_bringup)/launch/base.launch.yaml + file: $(find-pkg-share blue_bringup)/launch/ardusub/base.launch.yaml arg: - name: configuration_type value: bluerov2_heavy diff --git a/blue_bringup/launch/bluerov2_heavy/control.launch.py b/blue_bringup/launch/bluerov2_heavy/bluerov2_heavy_control.launch.py similarity index 85% rename from blue_bringup/launch/bluerov2_heavy/control.launch.py rename to blue_bringup/launch/bluerov2_heavy/bluerov2_heavy_control.launch.py index d248ee97..748c2ad2 100644 --- a/blue_bringup/launch/bluerov2_heavy/control.launch.py +++ b/blue_bringup/launch/bluerov2_heavy/bluerov2_heavy_control.launch.py @@ -19,7 +19,8 @@ # THE SOFTWARE. from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource from launch_ros.actions import Node from launch.substitutions import ( Command, @@ -39,11 +40,6 @@ def generate_launch_description() -> LaunchDescription: The launch description for the BlueROV2 base configuration. """ args = [ - DeclareLaunchArgument( - "use_sim", - default_value="false", - description="Launch the Gazebo + ArduSub simulator.", - ), DeclareLaunchArgument( "prefix", default_value="", @@ -52,6 +48,11 @@ def generate_launch_description() -> LaunchDescription: " Expected format '/'." ), ), + DeclareLaunchArgument( + "use_sim", + default_value="false", + description="Launch the Gazebo + ArduSub simulator.", + ), ] robot_description_content = Command( @@ -76,6 +77,30 @@ def generate_launch_description() -> LaunchDescription: ) robot_description = {"robot_description": robot_description_content} + ardusub_manager_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + [ + PathJoinSubstitution( + [ + FindPackageShare("ardusub_manager"), + "launch", + "ardusub_manager.launch.py", + ] + ) + ] + ), + launch_arguments={ + "ardusub_manager_file": PathJoinSubstitution( + [ + FindPackageShare("blue_description"), + "config", + "ardusub", + "ardusub_manager.yaml", + ] + ), + }.items(), + ) + controller_manager = Node( package="controller_manager", executable="ros2_control_node", @@ -160,6 +185,7 @@ def generate_launch_description() -> LaunchDescription: return LaunchDescription( [ *args, + ardusub_manager_launch, controller_manager, *delay_thruster_spawners, delay_tam_controller_spawner_after_thruster_controller_spawners, 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 16167652..b38ec559 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 @@ -35,7 +35,7 @@ launch: # Includes - include: - file: $(find-pkg-share blue_bringup)/launch/base.launch.yaml + file: $(find-pkg-share blue_bringup)/launch/ardusub/base.launch.yaml arg: - name: configuration_type value: bluerov2_heavy_reach diff --git a/blue_bringup/launch/tf.launch.yaml b/blue_bringup/launch/tf.launch.yaml index eef3578c..42f0af80 100644 --- a/blue_bringup/launch/tf.launch.yaml +++ b/blue_bringup/launch/tf.launch.yaml @@ -5,30 +5,10 @@ launch: pkg: tf2_ros exec: static_transform_publisher name: base_link_to_base_link_fsd - arg: - - name: roll - value: 3.142 - - name: pitch - value: 0.0 - - name: yaw - value: 0.0 - - name: frame-id - value: base_link - - name: child-frame-id - value: base_link_fsd + args: --roll 3.142 --frame-id base_link --child-frame-id base_link_fsd - node: pkg: tf2_ros - exec: map - name: map_frd - arg: - - name: roll - value: 3.142 - - name: pitch - value: 0.0 - - name: yaw - value: 0.0 - - name: frame-id - value: base_link - - name: child-frame-id - value: base_link_fsd + exec: static_transform_publisher + name: map_to_map_ned + args: --roll 3.142 --frame-id map --child-frame-id map_ned diff --git a/blue_description/config/bluerov2_heavy/new_manager.yaml b/blue_description/config/ardusub/ardusub_manager.yaml similarity index 68% rename from blue_description/config/bluerov2_heavy/new_manager.yaml rename to blue_description/config/ardusub/ardusub_manager.yaml index 10b7ad06..5af5b9ac 100644 --- a/blue_description/config/bluerov2_heavy/new_manager.yaml +++ b/blue_description/config/ardusub/ardusub_manager.yaml @@ -2,7 +2,6 @@ ardusub_manager: ros__parameters: message_intervals: ids: [31, 32] - rates: [200.0, 200.0] + rates: [100.0, 100.0] set_ekf_origin: false - set_home_position: false publish_tf: true diff --git a/blue_description/config/mavros.yaml b/blue_description/config/ardusub/mavros.yaml similarity index 74% rename from blue_description/config/mavros.yaml rename to blue_description/config/ardusub/mavros.yaml index e574662c..62994115 100644 --- a/blue_description/config/mavros.yaml +++ b/blue_description/config/ardusub/mavros.yaml @@ -7,11 +7,8 @@ mavros: - imu - rc_io - param - - vision* - local* - global_position - - home_position - - mocap_pose_estimate mavros_node: ros__parameters: @@ -20,6 +17,4 @@ mavros_node: mavros/local_position: ros__parameters: - frame_id: "map" - tf: - send: false + frame_id: map diff --git a/blue_description/config/bluerov2/controllers.yaml b/blue_description/config/bluerov2/controllers.yaml index cbad978a..f6076d1a 100644 --- a/blue_description/config/bluerov2/controllers.yaml +++ b/blue_description/config/bluerov2/controllers.yaml @@ -1,26 +1,133 @@ -ismc: - ros__parameters: - mass: 10.0 - buoyancy: 100.06 - weight: 98.1 - inertia_tensor_coeff: [0.16, 0.16, 0.16] - added_mass_coeff: [-5.50, -12.70, -14.57, -0.12, -0.12, -0.12] - linear_damping_coeff: [-4.03, -6.22, -5.18, -0.07, -0.07, -0.07] - quadratic_damping_coeff: [-18.18, -21.66, -36.99, -1.55, -1.55, -1.55] - center_of_gravity: [0.0, 0.0, 0.0] - center_of_buoyancy: [0.0, 0.0, 0.2] - ocean_current: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - num_thrusters: 6 - tcm: [ -0.707, -0.707, 0.707, 0.707, 0.0, 0.0, - 0.707, -0.707, 0.707, -0.707, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 1.0, 1.0, - 0.0, 0.0, 0.0, 0.0, -0.21805, 0.21805, - 0.0, 0.0, 0.0, 0.0, -0.12, -0.12, - 0.1888, -0.1888, -0.1888, 0.1888, 0.0, 0.0] - control_rate: 100.0 - proportional_gain: [10.0, 10.0, 6.0, 3.0, 6.0, 10.0] - integral_gain: [0.01, 0.01, 0.01, 0.01, 0.01, 0.01] - derivative_gain: [0.1, 0.1, 0.1, 0.1, 0.1, 0.1] - sliding_gain: 20.0 - boundary_thickness: 200.0 - use_battery_state: false +controller_manager: + ros__parameters: + update_rate: 100 # Hz + + integral_sliding_mode_controller: + type: velocity_controllers/IntegralSlidingModeController + + thruster_allocation_matrix_controller: + type: thruster_allocation_matrix_controller/ThrusterAllocationMatrixController + + thruster1_controller: + type: thruster_controllers/PolynomialThrustCurveController + + thruster2_controller: + type: thruster_controllers/PolynomialThrustCurveController + + thruster3_controller: + type: thruster_controllers/PolynomialThrustCurveController + + thruster4_controller: + type: thruster_controllers/PolynomialThrustCurveController + + thruster5_controller: + type: thruster_controllers/PolynomialThrustCurveController + + thruster6_controller: + type: thruster_controllers/PolynomialThrustCurveController + +integral_sliding_mode_controller: + ros__parameters: + use_external_measured_states: true + reference_controller: thruster_allocation_matrix_controller + gains: + rho: 20.0 + lambda: 200.0 + Kp: [4.0, 4.0, 9.0, 3.0, 3.0, 7.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] + +thruster_allocation_matrix_controller: + ros__parameters: + thrusters: + - thruster1_joint + - thruster2_joint + - thruster3_joint + - thruster4_joint + - thruster5_joint + - thruster6_joint + reference_controllers: + - thruster1_controller + - thruster2_controller + - thruster3_controller + - thruster4_controller + - thruster5_controller + - thruster6_controller + tam: + x: [ -0.707, -0.707, 0.707, 0.707, 0.0, 0.0] + y: [ 0.707, -0.707, 0.707, -0.707, 0.0, 0.0] + z: [ 0.0, 0.0, 0.0, 0.0, 1.0, 1.0] + rx: [ 0.0, 0.0, 0.0, 0.0, -0.21805, 0.21805] + ry: [ 0.0, 0.0, 0.0, 0.0, -0.12, -0.12] + rz: [0.1888, -0.1888, -0.1888, 0.1888, 0.0, 0.0] + +thruster1_controller: + ros__parameters: + thruster: thruster1_joint + min_thrust: -40.0 + max_thrust: 60.0 + min_deadband: 1470 + max_deadband: 1530 + neutral_pwm: 1500 + thrust_curve_coefficients: [1498.0, 12.01, -0.04731, -0.002098, 0.00002251] + +thruster2_controller: + ros__parameters: + thruster: thruster2_joint + min_thrust: -40.0 + max_thrust: 60.0 + min_deadband: 1470 + max_deadband: 1530 + neutral_pwm: 1500 + thrust_curve_coefficients: [1498.0, 12.01, -0.04731, -0.002098, 0.00002251] + +thruster3_controller: + ros__parameters: + thruster: thruster3_joint + min_thrust: -40.0 + max_thrust: 60.0 + min_deadband: 1470 + max_deadband: 1530 + neutral_pwm: 1500 + thrust_curve_coefficients: [1498.0, 12.01, -0.04731, -0.002098, 0.00002251] + +thruster4_controller: + ros__parameters: + thruster: thruster4_joint + min_thrust: -40.0 + max_thrust: 60.0 + min_deadband: 1470 + max_deadband: 1530 + neutral_pwm: 1500 + thrust_curve_coefficients: [1498.0, 12.01, -0.04731, -0.002098, 0.00002251] + +thruster5_controller: + ros__parameters: + thruster: thruster5_joint + min_thrust: -40.0 + max_thrust: 60.0 + min_deadband: 1470 + max_deadband: 1530 + neutral_pwm: 1500 + thrust_curve_coefficients: [1498.0, 12.01, -0.04731, -0.002098, 0.00002251] + +thruster6_controller: + ros__parameters: + thruster: thruster6_joint + min_thrust: -40.0 + max_thrust: 60.0 + min_deadband: 1470 + max_deadband: 1530 + neutral_pwm: 1500 + thrust_curve_coefficients: [1498.0, 12.01, -0.04731, -0.002098, 0.00002251] diff --git a/blue_description/config/bluerov2/manager.yaml b/blue_description/config/bluerov2/manager.yaml deleted file mode 100644 index 46d49477..00000000 --- a/blue_description/config/bluerov2/manager.yaml +++ /dev/null @@ -1,19 +0,0 @@ -blue_manager: - ros__parameters: - num_thrusters: 6 - mode_change: - timeout: 1.0 - retries: 3 - message_intervals: - ids: [31, 32] - rates: [200.0, 200.0] - request_interval: 30.0 - ekf_origin: - latitude: 44.65870 - longitude: -124.06556 - altitude: 0.0 - home_position: - latitude: 44.65870 - longitude: -124.06556 - altitude: 0.0 - request_interval: 30.0 diff --git a/blue_description/config/bluerov2_heavy/controllers.yaml b/blue_description/config/bluerov2_heavy/controllers.yaml index a31ca128..a8608511 100644 --- a/blue_description/config/bluerov2_heavy/controllers.yaml +++ b/blue_description/config/bluerov2_heavy/controllers.yaml @@ -41,8 +41,8 @@ integral_sliding_mode_controller: lambda: 200.0 Kp: [4.0, 4.0, 9.0, 3.0, 3.0, 7.0] tf: - base_frame: "base_link" - odom_frame: "map" + base_frame: "base_link_fsd" + odom_frame: "map_ned" hydrodynamics: mass: 13.5 weight: 114.80 diff --git a/blue_description/config/bluerov2_heavy/manager.yaml b/blue_description/config/bluerov2_heavy/manager.yaml deleted file mode 100644 index 82980f5d..00000000 --- a/blue_description/config/bluerov2_heavy/manager.yaml +++ /dev/null @@ -1,19 +0,0 @@ -blue_manager: - ros__parameters: - num_thrusters: 8 - mode_change: - timeout: 1.0 - retries: 3 - message_intervals: - ids: [31, 32] - rates: [200.0, 200.0] - request_interval: 30.0 - ekf_origin: - latitude: 44.65870 - longitude: -124.06556 - altitude: 0.0 - home_position: - latitude: 44.65870 - longitude: -124.06556 - altitude: 0.0 - request_interval: 30.0 diff --git a/blue_description/config/bluerov2_heavy_reach/controllers.yaml b/blue_description/config/bluerov2_heavy_reach/controllers.yaml index be9aa292..a8608511 100644 --- a/blue_description/config/bluerov2_heavy_reach/controllers.yaml +++ b/blue_description/config/bluerov2_heavy_reach/controllers.yaml @@ -1,26 +1,163 @@ -ismc: - ros__parameters: - mass: 13.5 - buoyancy: 112.80 - weight: 114.80 - inertia_tensor_coeff: [0.16, 0.16, 0.16] - added_mass_coeff: [-5.50, -12.70, -14.60, -0.12, -0.12, -0.12] - linear_damping_coeff: [-4.03, -6.22, -5.18, -0.07, -0.07, -0.07] - quadratic_damping_coeff: [-18.18, -21.66, -36.99, -1.55, -1.55, -1.55] - center_of_gravity: [0.0, 0.0, 0.0] - center_of_buoyancy: [0.0, 0.0, 0.0] - ocean_current: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - num_thrusters: 8 - tcm: [-0.707, -0.707, 0.707, 0.707, 0.0, 0.0, 0.0, 0.0, - 0.707, -0.707, 0.707, -0.707, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 1.0, 1.0, 1.0, 1.0, - 0.0, 0.0, 0.0, 0.0, -0.21805, 0.21805, -0.21805, 0.21805, - 0.0, 0.0, 0.0, 0.0, -0.12, -0.12, 0.12, 0.12, - 0.1888, -0.1888, -0.1888, 0.1888, 0.0, 0.0, 0.0 , 0.0] - control_rate: 100.0 - proportional_gain: [10.0, 10.0, 6.0, 3.0, 6.0, 10.0] - integral_gain: [0.01, 0.01, 0.01, 0.01, 0.01, 0.01] - derivative_gain: [0.1, 0.1, 0.1, 0.1, 0.1, 0.1] - sliding_gain: 20.0 - boundary_thickness: 200.0 - use_battery_state: false +controller_manager: + ros__parameters: + update_rate: 100 # Hz + + integral_sliding_mode_controller: + type: velocity_controllers/IntegralSlidingModeController + + thruster_allocation_matrix_controller: + type: thruster_allocation_matrix_controller/ThrusterAllocationMatrixController + + thruster1_controller: + type: thruster_controllers/PolynomialThrustCurveController + + thruster2_controller: + type: thruster_controllers/PolynomialThrustCurveController + + thruster3_controller: + type: thruster_controllers/PolynomialThrustCurveController + + thruster4_controller: + type: thruster_controllers/PolynomialThrustCurveController + + thruster5_controller: + type: thruster_controllers/PolynomialThrustCurveController + + thruster6_controller: + type: thruster_controllers/PolynomialThrustCurveController + + thruster7_controller: + type: thruster_controllers/PolynomialThrustCurveController + + thruster8_controller: + type: thruster_controllers/PolynomialThrustCurveController + +integral_sliding_mode_controller: + ros__parameters: + use_external_measured_states: true + reference_controller: thruster_allocation_matrix_controller + gains: + rho: 20.0 + lambda: 200.0 + Kp: [4.0, 4.0, 9.0, 3.0, 3.0, 7.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] + +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 + min_thrust: -40.0 + max_thrust: 60.0 + min_deadband: 1470 + max_deadband: 1530 + neutral_pwm: 1500 + thrust_curve_coefficients: [1498.0, 12.01, -0.04731, -0.002098, 0.00002251] + +thruster2_controller: + ros__parameters: + thruster: thruster2_joint + min_thrust: -40.0 + max_thrust: 60.0 + min_deadband: 1470 + max_deadband: 1530 + neutral_pwm: 1500 + thrust_curve_coefficients: [1498.0, 12.01, -0.04731, -0.002098, 0.00002251] + +thruster3_controller: + ros__parameters: + thruster: thruster3_joint + min_thrust: -40.0 + max_thrust: 60.0 + min_deadband: 1470 + max_deadband: 1530 + neutral_pwm: 1500 + thrust_curve_coefficients: [1498.0, 12.01, -0.04731, -0.002098, 0.00002251] + +thruster4_controller: + ros__parameters: + thruster: thruster4_joint + min_thrust: -40.0 + max_thrust: 60.0 + min_deadband: 1470 + max_deadband: 1530 + neutral_pwm: 1500 + thrust_curve_coefficients: [1498.0, 12.01, -0.04731, -0.002098, 0.00002251] + +thruster5_controller: + ros__parameters: + thruster: thruster5_joint + min_thrust: -40.0 + max_thrust: 60.0 + min_deadband: 1470 + max_deadband: 1530 + neutral_pwm: 1500 + thrust_curve_coefficients: [1498.0, 12.01, -0.04731, -0.002098, 0.00002251] + +thruster6_controller: + ros__parameters: + thruster: thruster6_joint + min_thrust: -40.0 + max_thrust: 60.0 + min_deadband: 1470 + max_deadband: 1530 + neutral_pwm: 1500 + thrust_curve_coefficients: [1498.0, 12.01, -0.04731, -0.002098, 0.00002251] + +thruster7_controller: + ros__parameters: + thruster: thruster7_joint + min_thrust: -40.0 + max_thrust: 60.0 + min_deadband: 1470 + max_deadband: 1530 + neutral_pwm: 1500 + thrust_curve_coefficients: [1498.0, 12.01, -0.04731, -0.002098, 0.00002251] + +thruster8_controller: + ros__parameters: + thruster: thruster8_joint + min_thrust: -40.0 + max_thrust: 60.0 + min_deadband: 1470 + max_deadband: 1530 + neutral_pwm: 1500 + thrust_curve_coefficients: [1498.0, 12.01, -0.04731, -0.002098, 0.00002251] diff --git a/blue_description/config/bluerov2_heavy_reach/manager.yaml b/blue_description/config/bluerov2_heavy_reach/manager.yaml deleted file mode 100644 index 82980f5d..00000000 --- a/blue_description/config/bluerov2_heavy_reach/manager.yaml +++ /dev/null @@ -1,19 +0,0 @@ -blue_manager: - ros__parameters: - num_thrusters: 8 - mode_change: - timeout: 1.0 - retries: 3 - message_intervals: - ids: [31, 32] - rates: [200.0, 200.0] - request_interval: 30.0 - ekf_origin: - latitude: 44.65870 - longitude: -124.06556 - altitude: 0.0 - home_position: - latitude: 44.65870 - longitude: -124.06556 - altitude: 0.0 - request_interval: 30.0 diff --git a/blue_description/xacro/bluerov2/config.xacro b/blue_description/xacro/bluerov2/config.xacro index 2309579f..20d6b071 100644 --- a/blue_description/xacro/bluerov2/config.xacro +++ b/blue_description/xacro/bluerov2/config.xacro @@ -5,16 +5,17 @@ - - + - + + + + + diff --git a/blue_description/xacro/bluerov2/ros2_control.xacro b/blue_description/xacro/bluerov2/ros2_control.xacro new file mode 100644 index 00000000..63e80692 --- /dev/null +++ b/blue_description/xacro/bluerov2/ros2_control.xacro @@ -0,0 +1,57 @@ + + + + + + + + thruster_hardware/ThrusterHardware + ${max_set_param_attempts} + + + + SERVO1_FUNCTION + 33 + 1 + + + + + SERVO2_FUNCTION + 34 + 2 + + + + + SERVO3_FUNCTION + 35 + 3 + + + + + SERVO4_FUNCTION + 36 + 4 + + + + + SERVO5_FUNCTION + 37 + 5 + + + + + SERVO6_FUNCTION + 38 + 6 + + + + + + + diff --git a/blue_description/xacro/bluerov2_heavy_reach/config.xacro b/blue_description/xacro/bluerov2_heavy_reach/config.xacro index 4db1a3be..cae3970b 100644 --- a/blue_description/xacro/bluerov2_heavy_reach/config.xacro +++ b/blue_description/xacro/bluerov2_heavy_reach/config.xacro @@ -5,9 +5,7 @@ - - + @@ -17,4 +15,8 @@ + + + + diff --git a/blue_description/xacro/bluerov2_heavy_reach/ros2_control.xacro b/blue_description/xacro/bluerov2_heavy_reach/ros2_control.xacro new file mode 100644 index 00000000..0ab9119a --- /dev/null +++ b/blue_description/xacro/bluerov2_heavy_reach/ros2_control.xacro @@ -0,0 +1,71 @@ + + + + + + + + thruster_hardware/ThrusterHardware + ${max_set_param_attempts} + + + + SERVO1_FUNCTION + 33 + 1 + + + + + SERVO2_FUNCTION + 34 + 2 + + + + + SERVO3_FUNCTION + 35 + 3 + + + + + SERVO4_FUNCTION + 36 + 4 + + + + + SERVO5_FUNCTION + 37 + 5 + + + + + SERVO6_FUNCTION + 38 + 6 + + + + + SERVO7_FUNCTION + 39 + 7 + + + + + SERVO8_FUNCTION + 40 + 8 + + + + + + + From 0c41a28c2aee2b4765dc8626a14d5c834e2a9ffa Mon Sep 17 00:00:00 2001 From: Evan Palmer Date: Tue, 26 Mar 2024 00:42:52 -0700 Subject: [PATCH 04/38] Removed old scripts and configuration files; continued ardusub manager integration --- .clang-format | 27 ----- .clang-tidy | 80 ------------- .markdownlint.json | 4 - .pre-commit-config.yaml | 16 +-- .ruff.toml | 1 + blue_bringup/CMakeLists.txt | 2 - .../launch/ardusub/ardusub.launch.yaml | 28 +++-- blue_bringup/launch/ardusub/base.launch.yaml | 108 ------------------ blue_bringup/launch/ardusub/sitl.launch.yaml | 8 +- .../launch/bluerov2/bluerov2.launch.yaml | 93 ++++++++++++--- .../bluerov2_heavy/bluerov2_heavy.launch.yaml | 93 ++++++++++++--- .../bluerov2_heavy_control.launch.py | 33 +----- .../bluerov2_heavy_reach.launch.yaml | 93 ++++++++++++--- blue_description/CMakeLists.txt | 2 +- .../config/ardusub/ardusub_manager.yaml | 6 +- requirements-dev.txt | 5 +- scripts/arm.sh | 7 -- scripts/disarm.sh | 7 -- scripts/send_cmd.sh | 5 - scripts/send_pwm.sh | 5 - scripts/start_keyboard_teleop.sh | 5 - setup.cfg | 18 --- 22 files changed, 263 insertions(+), 383 deletions(-) delete mode 100644 .clang-format delete mode 100644 .clang-tidy delete mode 100644 .markdownlint.json create mode 100644 .ruff.toml delete mode 100644 blue_bringup/launch/ardusub/base.launch.yaml delete mode 100755 scripts/arm.sh delete mode 100755 scripts/disarm.sh delete mode 100755 scripts/send_cmd.sh delete mode 100755 scripts/send_pwm.sh delete mode 100755 scripts/start_keyboard_teleop.sh delete mode 100644 setup.cfg diff --git a/.clang-format b/.clang-format deleted file mode 100644 index 5e0792e0..00000000 --- a/.clang-format +++ /dev/null @@ -1,27 +0,0 @@ ---- -Language: Cpp -BasedOnStyle: Google -ColumnLimit: 100 -MaxEmptyLinesToKeep: 1 - -IndentWidth: 2 -TabWidth: 2 -UseTab: Never -AccessModifierOffset: -2 -AlignAfterOpenBracket: AlwaysBreak -ConstructorInitializerIndentWidth: 0 -ContinuationIndentWidth: 2 -DerivePointerAlignment: false -PointerAlignment: Middle -PackConstructorInitializers: Never - -# Configure brace wrapping cases -BreakBeforeBraces: Custom -BraceWrapping: - AfterClass: true - AfterEnum: true - AfterFunction: true - AfterNamespace: true - AfterStruct: true - AfterUnion: true - BeforeCatch: true diff --git a/.clang-tidy b/.clang-tidy deleted file mode 100644 index d235ad0e..00000000 --- a/.clang-tidy +++ /dev/null @@ -1,80 +0,0 @@ ---- -Checks: > - -*, - abseil-*, - bugprone-*, - google-*, - misc-*, - modernize-*, - performance-*, - portability-*, - readability-*, - -google-readability-braces-around-statements, - -google-readability-namespace-comments, - -google-runtime-references, - -misc-non-private-member-variables-in-classes, - -modernize-return-braced-init-list, - -modernize-use-trailing-return-type, - -readability-braces-around-statements, - -readability-identifier-length, - -readability-magic-numbers, - -readability-named-parameter, - -readability-redundant-declaration, - -readability-function-cognitive-complexity, - -bugprone-narrowing-conversions, - -bugprone-easily-swappable-parameters, - -bugprone-implicit-widening-of-multiplication-result, - -clang-diagnostic-error, - -bugprone-exception-escape -WarningsAsErrors: "*" -CheckOptions: - - key: readability-braces-around-statements.ShortStatementLines - value: "2" - - key: readability-identifier-naming.NamespaceCase - value: lower_case - - key: readability-identifier-naming.ClassCase - value: CamelCase - - key: readability-identifier-naming.StructCase - value: CamelCase - - key: readability-identifier-naming.TemplateParameterCase, - value: CamelCase, - - key: readability-identifier-naming.FunctionCase - value: camelBack - - key: readability-identifier-naming.MethodCase - value: camelBack - - key: readability-identifier-naming.VariableCase - value: lower_case - - key: readability-identifier-naming.ClassMemberCase - value: lower_case - - key: readability-identifier-naming.ClassMemberSuffix - value: _ - - key: readability-identifier-naming.PrivateMemberSuffix - value: _ - - key: readability-identifier-naming.ProtectedMemberSuffix - value: _ - - key: readability-identifier-naming.EnumConstantCase - value: CamelCase - - key: readability-identifier-naming.EnumConstantPrefix - value: k - - key: readability-identifier-naming.ConstexprVariableCase - value: CamelCase - - key: readability-identifier-naming.ConstexprVariablePrefix - value: k - - key: readability-identifier-naming.GlobalConstantCase - value: CamelCase - - key: readability-identifier-naming.GlobalConstantPrefix - value: k - - key: readability-identifier-naming.MemberConstantCase - value: CamelCase - - key: readability-identifier-naming.MemberConstantPrefix - value: k - - key: readability-identifier-naming.StaticConstantCase - value: CamelCase - - key: readability-identifier-naming.StaticConstantPrefix - value: k - - key: readability-implicit-bool-conversion.AllowIntegerConditions - value: 1 - - key: readability-implicit-bool-conversion.AllowPointerConditions - value: 1 - - key: readability-function-cognitive-complexity.IgnoreMacros - value: 1 diff --git a/.markdownlint.json b/.markdownlint.json deleted file mode 100644 index 0e70762a..00000000 --- a/.markdownlint.json +++ /dev/null @@ -1,4 +0,0 @@ -{ - "MD026": false, - "MD013": false -} diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index 5abd3c3b..90e6240e 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -4,10 +4,11 @@ repos: hooks: - id: black - - repo: https://github.com/timothycrosley/isort - rev: 5.12.0 + - repo: https://github.com/astral-sh/ruff-pre-commit + rev: v0.0.291 hooks: - - id: isort + - id: ruff + args: ["--fix", "--exit-non-zero-on-fix"] - repo: https://github.com/codespell-project/codespell rev: v2.2.4 @@ -15,15 +16,6 @@ repos: - id: codespell args: ["--write-changes"] - - repo: local - hooks: - - id: clang-format - name: clang-format - entry: clang-format-14 - language: system - files: \.(c|cc|cxx|cpp|h|hpp|hxx|)$ - args: ["-fallback-style=Google", "-i"] - - repo: https://github.com/pre-commit/pre-commit-hooks rev: v4.4.0 hooks: diff --git a/.ruff.toml b/.ruff.toml new file mode 100644 index 00000000..fef81564 --- /dev/null +++ b/.ruff.toml @@ -0,0 +1 @@ +target-version = "py310" diff --git a/blue_bringup/CMakeLists.txt b/blue_bringup/CMakeLists.txt index df18d9e5..f7cbc54c 100644 --- a/blue_bringup/CMakeLists.txt +++ b/blue_bringup/CMakeLists.txt @@ -2,8 +2,6 @@ cmake_minimum_required(VERSION 3.8) project(blue_bringup) set(THIS_PACKAGE_INCLUDE_DEPENDS - rclcpp - rclpy ament_cmake ) diff --git a/blue_bringup/launch/ardusub/ardusub.launch.yaml b/blue_bringup/launch/ardusub/ardusub.launch.yaml index c720c84a..c25b95b8 100644 --- a/blue_bringup/launch/ardusub/ardusub.launch.yaml +++ b/blue_bringup/launch/ardusub/ardusub.launch.yaml @@ -1,24 +1,33 @@ launch: # Arguments - - arg: - name: description_package - default: blue_description - - arg: name: mavros_file - default: $(find-pkg-share $(var description_package))/config/ardusub/mavros.yaml - arg: name: gazebo_world_file + default: "" - arg: name: ardusub_params_file + default: "" + + - arg: + name: configuration_type + default: "" + + - arg: + name: manager_file + default: $(find-pkg-share blue_description)/config/ardusub/ardusub_manager.yaml - arg: name: use_sim default: "false" + - arg: + name: use_manager + default: "false" + # Nodes - node: pkg: mavros @@ -33,11 +42,16 @@ launch: file: $(find-pkg-share blue_bringup)/launch/ardusub/sitl.launch.yaml if: $(var use_sim) arg: - - name: description_package - value: $(var description_package) - name: configuration_type value: $(var configuration_type) - name: gazebo_world_file value: $(var gazebo_world_file) - name: ardusub_params_file value: $(var ardusub_params_file) + + - include: + file: $(find-pkg-share ardusub_manager)/launch/ardusub_manager.launch.py + if: $(var use_manager) + arg: + - name: ardusub_manager_file + value: $(var manager_file) diff --git a/blue_bringup/launch/ardusub/base.launch.yaml b/blue_bringup/launch/ardusub/base.launch.yaml deleted file mode 100644 index 8027aebe..00000000 --- a/blue_bringup/launch/ardusub/base.launch.yaml +++ /dev/null @@ -1,108 +0,0 @@ -launch: - - # Arguments - - arg: - name: description_package - default: blue_description - - - arg: - name: configuration_type - default: bluerov2_heavy - - - arg: - name: localization_file - default: $(find-pkg-share $(var description_package))/config/$(var configuration_type)/localization.yaml - - - arg: - name: mavros_file - default: $(find-pkg-share $(var description_package))/config/ardusub/mavros.yaml - - - arg: - name: joy_file - default: $(find-pkg-share $(var description_package))/config/joy_teleop.yaml - - - arg: - name: gazebo_world_file - default: $(find-pkg-share $(var description_package))/gazebo/worlds/$(var configuration_type)_underwater.world - - - arg: - name: rviz_config - default: $(find-pkg-share $(var description_package))/rviz/$(var configuration_type).rviz - - - arg: - name: description_file - default: $(find-pkg-share $(var description_package))/xacro/$(var configuration_type)/config.xacro - - - arg: - name: ardusub_params_file - default: $(find-pkg-share $(var description_package))/config/$(var configuration_type)/ardusub.parm - - - 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" - - - arg: - name: use_rviz - default: "false" - - - arg: - name: use_joy - default: "false" - - # Load the description file - - let: - name: robot_description - value: $(command 'xacro $(var description_file) use_sim:=$(var use_sim)') - - # Nodes - - node: - pkg: robot_state_publisher - exec: robot_state_publisher - param: - - name: robot_description - value: $(var robot_description) - - name: use_sim_time - value: $(var use_sim) - - - node: - pkg: rviz2 - exec: rviz2 - if: $(var use_rviz) - args: -d $(var rviz_config) - param: - - name: robot_description - value: $(var robot_description) - - # Includes - - 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/tf.launch.yaml diff --git a/blue_bringup/launch/ardusub/sitl.launch.yaml b/blue_bringup/launch/ardusub/sitl.launch.yaml index bacd79bf..54e5e370 100644 --- a/blue_bringup/launch/ardusub/sitl.launch.yaml +++ b/blue_bringup/launch/ardusub/sitl.launch.yaml @@ -1,20 +1,14 @@ launch: - # Arguments - - arg: - name: description_package - default: blue_description + # Arguments - arg: name: configuration_type - default: bluerov2_heavy - arg: name: gazebo_world_file - default: $(find-pkg-share $(var description_package))/gazebo/worlds/$(var configuration_type)_underwater.world - arg: name: ardusub_params_file - default: $(find-pkg-share $(var description_package))/config/$(var configuration_type)/ardusub.parm # Nodes - node: diff --git a/blue_bringup/launch/bluerov2/bluerov2.launch.yaml b/blue_bringup/launch/bluerov2/bluerov2.launch.yaml index d005e754..3db148c7 100644 --- a/blue_bringup/launch/bluerov2/bluerov2.launch.yaml +++ b/blue_bringup/launch/bluerov2/bluerov2.launch.yaml @@ -1,10 +1,10 @@ launch: - # Arguments - - arg: - name: localization_source - default: gazebo + - let: + name: configuration_type + value: bluerov2 + # Arguments - arg: name: use_camera default: "false" @@ -13,6 +13,14 @@ launch: name: use_mocap default: "false" + - arg: + name: localization_source + default: gazebo + choice: + - value: gazebo + - value: mocap + - value: camera + - arg: name: use_sim default: "false" @@ -25,34 +33,85 @@ launch: name: use_joy default: "false" + - arg: + name: use_manager + default: "false" + - arg: name: rviz_config - default: $(find-pkg-share blue_description)/rviz/bluerov2.rviz + default: $(find-pkg-share blue_description)/rviz/$(var configuration_type).rviz - arg: name: gazebo_world_file - default: $(find-pkg-share blue_description)/gazebo/worlds/bluerov2_underwater.world + default: $(find-pkg-share blue_description)/gazebo/worlds/$(var configuration_type)_underwater.world + + - arg: + name: mavros_file + default: $(find-pkg-share blue_description)/config/ardusub/mavros.yaml + + - arg: + name: ardusub_params_file + default: $(find-pkg-share blue_description)/config/$(var configuration_type)/ardusub.parm + + - arg: + name: localization_file + default: $(find-pkg-share blue_description)/config/$(var configuration_type)/localization.yaml + + # Load the description file + - let: + name: description_file + value: $(find-pkg-share blue_description)/xacro/$(var configuration_type)/config.xacro + + - let: + name: robot_description + value: $(command 'xacro $(var description_file) use_sim:=$(var use_sim)') + + # Nodes + - node: + pkg: robot_state_publisher + exec: robot_state_publisher + param: + - name: robot_description + value: $(var robot_description) + - name: use_sim_time + value: $(var use_sim) + + - node: + pkg: rviz2 + exec: rviz2 + if: $(var use_rviz) + args: -d $(var rviz_config) + param: + - name: robot_description + value: $(var robot_description) # Includes - include: - file: $(find-pkg-share blue_bringup)/launch/ardusub/base.launch.yaml + file: $(find-pkg-share blue_bringup)/launch/ardusub/ardusub.launch.yaml arg: - - name: configuration_type - value: bluerov2 + - name: mavros_file + value: $(var mavros_file) - name: gazebo_world_file value: $(var gazebo_world_file) + - name: ardusub_params_file + value: $(var ardusub_params_file) + - name: configuration_type + value: $(var configuration_type) + - name: use_sim + value: $(var use_sim) + - 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 + - name: use_sim_time value: $(var use_sim) - - name: use_rviz - value: $(var use_rviz) - - name: use_joy - value: $(var use_joy) - - - include: - file: $(find-pkg-share blue_bringup)/launch/bluerov2/thrusters.launch.yaml + - name: config_filepath + value: $(var localization_file) diff --git a/blue_bringup/launch/bluerov2_heavy/bluerov2_heavy.launch.yaml b/blue_bringup/launch/bluerov2_heavy/bluerov2_heavy.launch.yaml index c46e01f0..9ac6703a 100644 --- a/blue_bringup/launch/bluerov2_heavy/bluerov2_heavy.launch.yaml +++ b/blue_bringup/launch/bluerov2_heavy/bluerov2_heavy.launch.yaml @@ -1,10 +1,10 @@ launch: - # Arguments - - arg: - name: localization_source - default: gazebo + - let: + name: configuration_type + value: bluerov2_heavy + # Arguments - arg: name: use_camera default: "false" @@ -13,6 +13,14 @@ launch: name: use_mocap default: "false" + - arg: + name: localization_source + default: gazebo + choice: + - value: gazebo + - value: mocap + - value: camera + - arg: name: use_sim default: "false" @@ -25,34 +33,85 @@ launch: name: use_joy default: "false" + - arg: + name: use_manager + default: "false" + - arg: name: rviz_config - default: $(find-pkg-share blue_description)/rviz/bluerov2_heavy.rviz + default: $(find-pkg-share blue_description)/rviz/$(var configuration_type).rviz - arg: name: gazebo_world_file - default: $(find-pkg-share blue_description)/gazebo/worlds/bluerov2_heavy_underwater.world + default: $(find-pkg-share blue_description)/gazebo/worlds/$(var configuration_type)_underwater.world + + - arg: + name: mavros_file + default: $(find-pkg-share blue_description)/config/ardusub/mavros.yaml + + - arg: + name: ardusub_params_file + default: $(find-pkg-share blue_description)/config/$(var configuration_type)/ardusub.parm + + - arg: + name: localization_file + default: $(find-pkg-share blue_description)/config/$(var configuration_type)/localization.yaml + + # Load the description file + - let: + name: description_file + value: $(find-pkg-share blue_description)/xacro/$(var configuration_type)/config.xacro + + - let: + name: robot_description + value: $(command 'xacro $(var description_file) use_sim:=$(var use_sim)') + + # Nodes + - node: + pkg: robot_state_publisher + exec: robot_state_publisher + param: + - name: robot_description + value: $(var robot_description) + - name: use_sim_time + value: $(var use_sim) + + - node: + pkg: rviz2 + exec: rviz2 + if: $(var use_rviz) + args: -d $(var rviz_config) + param: + - name: robot_description + value: $(var robot_description) # Includes - include: - file: $(find-pkg-share blue_bringup)/launch/ardusub/base.launch.yaml + file: $(find-pkg-share blue_bringup)/launch/ardusub/ardusub.launch.yaml arg: - - name: configuration_type - value: bluerov2_heavy + - name: mavros_file + value: $(var mavros_file) - name: gazebo_world_file value: $(var gazebo_world_file) + - name: ardusub_params_file + value: $(var ardusub_params_file) + - name: configuration_type + value: $(var configuration_type) + - name: use_sim + value: $(var use_sim) + - 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 + - name: use_sim_time value: $(var use_sim) - - name: use_rviz - value: $(var use_rviz) - - name: use_joy - value: $(var use_joy) - - - include: - file: $(find-pkg-share blue_bringup)/launch/bluerov2_heavy/thrusters.launch.yaml + - name: config_filepath + value: $(var localization_file) diff --git a/blue_bringup/launch/bluerov2_heavy/bluerov2_heavy_control.launch.py b/blue_bringup/launch/bluerov2_heavy/bluerov2_heavy_control.launch.py index 748c2ad2..25e6f9e3 100644 --- a/blue_bringup/launch/bluerov2_heavy/bluerov2_heavy_control.launch.py +++ b/blue_bringup/launch/bluerov2_heavy/bluerov2_heavy_control.launch.py @@ -19,18 +19,16 @@ # THE SOFTWARE. from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription -from launch.launch_description_sources import PythonLaunchDescriptionSource -from launch_ros.actions import Node +from launch.actions import DeclareLaunchArgument, RegisterEventHandler +from launch.event_handlers import OnProcessExit from launch.substitutions import ( Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution, ) +from launch_ros.actions import Node from launch_ros.substitutions import FindPackageShare -from launch.actions import RegisterEventHandler -from launch.event_handlers import OnProcessExit def generate_launch_description() -> LaunchDescription: @@ -77,30 +75,6 @@ def generate_launch_description() -> LaunchDescription: ) robot_description = {"robot_description": robot_description_content} - ardusub_manager_launch = IncludeLaunchDescription( - PythonLaunchDescriptionSource( - [ - PathJoinSubstitution( - [ - FindPackageShare("ardusub_manager"), - "launch", - "ardusub_manager.launch.py", - ] - ) - ] - ), - launch_arguments={ - "ardusub_manager_file": PathJoinSubstitution( - [ - FindPackageShare("blue_description"), - "config", - "ardusub", - "ardusub_manager.yaml", - ] - ), - }.items(), - ) - controller_manager = Node( package="controller_manager", executable="ros2_control_node", @@ -185,7 +159,6 @@ def generate_launch_description() -> LaunchDescription: return LaunchDescription( [ *args, - ardusub_manager_launch, controller_manager, *delay_thruster_spawners, delay_tam_controller_spawner_after_thruster_controller_spawners, 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 b38ec559..85300deb 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 @@ -1,10 +1,10 @@ launch: - # Arguments - - arg: - name: localization_source - default: gazebo + - let: + name: configuration_type + value: bluerov2_heavy_reach + # Arguments - arg: name: use_camera default: "false" @@ -13,6 +13,14 @@ launch: name: use_mocap default: "false" + - arg: + name: localization_source + default: gazebo + choice: + - value: gazebo + - value: mocap + - value: camera + - arg: name: use_sim default: "false" @@ -25,34 +33,85 @@ launch: name: use_joy default: "false" + - arg: + name: use_manager + default: "false" + - arg: name: rviz_config - default: $(find-pkg-share blue_description)/rviz/bluerov2_heavy_reach.rviz + default: $(find-pkg-share blue_description)/rviz/$(var configuration_type).rviz - arg: name: gazebo_world_file - default: $(find-pkg-share blue_description)/gazebo/worlds/bluerov2_heavy_reach_underwater.world + default: $(find-pkg-share blue_description)/gazebo/worlds/$(var configuration_type)_underwater.world + + - arg: + name: mavros_file + default: $(find-pkg-share blue_description)/config/ardusub/mavros.yaml + + - arg: + name: ardusub_params_file + default: $(find-pkg-share blue_description)/config/$(var configuration_type)/ardusub.parm + + - arg: + name: localization_file + default: $(find-pkg-share blue_description)/config/$(var configuration_type)/localization.yaml + + # Load the description file + - let: + name: description_file + value: $(find-pkg-share blue_description)/xacro/$(var configuration_type)/config.xacro + + - let: + name: robot_description + value: $(command 'xacro $(var description_file) use_sim:=$(var use_sim)') + + # Nodes + - node: + pkg: robot_state_publisher + exec: robot_state_publisher + param: + - name: robot_description + value: $(var robot_description) + - name: use_sim_time + value: $(var use_sim) + + - node: + pkg: rviz2 + exec: rviz2 + if: $(var use_rviz) + args: -d $(var rviz_config) + param: + - name: robot_description + value: $(var robot_description) # Includes - include: - file: $(find-pkg-share blue_bringup)/launch/ardusub/base.launch.yaml + file: $(find-pkg-share blue_bringup)/launch/ardusub/ardusub.launch.yaml arg: - - name: configuration_type - value: bluerov2_heavy_reach + - name: mavros_file + value: $(var mavros_file) - name: gazebo_world_file value: $(var gazebo_world_file) + - name: ardusub_params_file + value: $(var ardusub_params_file) + - name: configuration_type + value: $(var configuration_type) + - name: use_sim + value: $(var use_sim) + - 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 + - name: use_sim_time value: $(var use_sim) - - name: use_rviz - value: $(var use_rviz) - - name: use_joy - value: $(var use_joy) - - - include: - file: $(find-pkg-share blue_bringup)/launch/bluerov2_heavy_reach/thrusters.launch.yaml + - name: config_filepath + value: $(var localization_file) diff --git a/blue_description/CMakeLists.txt b/blue_description/CMakeLists.txt index 6c8c048f..65d7de04 100644 --- a/blue_description/CMakeLists.txt +++ b/blue_description/CMakeLists.txt @@ -11,7 +11,7 @@ endforeach() install( DIRECTORY config gazebo meshes xacro rviz - DESTINATION share/${PROJECT_NAME} + DESTINATION share/blue_description ) ament_package() diff --git a/blue_description/config/ardusub/ardusub_manager.yaml b/blue_description/config/ardusub/ardusub_manager.yaml index 5af5b9ac..b1cae256 100644 --- a/blue_description/config/ardusub/ardusub_manager.yaml +++ b/blue_description/config/ardusub/ardusub_manager.yaml @@ -2,6 +2,6 @@ ardusub_manager: ros__parameters: message_intervals: ids: [31, 32] - rates: [100.0, 100.0] - set_ekf_origin: false - publish_tf: true + rates: [50.0, 50.0] + set_ekf_origin: true + publish_tf: false diff --git a/requirements-dev.txt b/requirements-dev.txt index 48e2641c..a3bd924d 100644 --- a/requirements-dev.txt +++ b/requirements-dev.txt @@ -1,6 +1,3 @@ -# Development tools that are not required for project installation +# Development tools not required for project installation pre-commit mypy -isort -flake8 -black diff --git a/scripts/arm.sh b/scripts/arm.sh deleted file mode 100755 index ace7be04..00000000 --- a/scripts/arm.sh +++ /dev/null @@ -1,7 +0,0 @@ -#!/usr/bin/env bash - -# This is a simple script used to make your life easier when arming a custom controller - -ros2 service call /blue/cmd/enable_passthrough std_srvs/srv/SetBool data:\ true\ - -ros2 service call /blue/cmd/arm std_srvs/srv/SetBool data:\ true\ diff --git a/scripts/disarm.sh b/scripts/disarm.sh deleted file mode 100755 index 778dd254..00000000 --- a/scripts/disarm.sh +++ /dev/null @@ -1,7 +0,0 @@ -#!/usr/bin/env bash - -# This is a simple script used to make your life easier when disarming a custom controller - -ros2 service call /blue/cmd/arm std_srvs/srv/SetBool data:\ false\ - -ros2 service call /blue/cmd/enable_passthrough std_srvs/srv/SetBool data:\ false\ diff --git a/scripts/send_cmd.sh b/scripts/send_cmd.sh deleted file mode 100755 index 643409fd..00000000 --- a/scripts/send_cmd.sh +++ /dev/null @@ -1,5 +0,0 @@ -#!/usr/bin/env bash - -# Send a velocity command to the BlueROV2 - -ros2 topic pub /blue/ismc/cmd_vel geometry_msgs/msg/Twist '{linear: {x: 0.0, y: 0.0, z: 0.0}, angular: {x: 0.0, y: 0.0, z: 0.0}}' diff --git a/scripts/send_pwm.sh b/scripts/send_pwm.sh deleted file mode 100755 index 39576ce2..00000000 --- a/scripts/send_pwm.sh +++ /dev/null @@ -1,5 +0,0 @@ -#!/usr/bin/env bash - -# Send a PWM command to the BlueROV2 - -ros2 topic pub /mavros/rc/override mavros_msgs/msg/OverrideRCIn "{channels: [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]}" diff --git a/scripts/start_keyboard_teleop.sh b/scripts/start_keyboard_teleop.sh deleted file mode 100755 index 0b83cd1f..00000000 --- a/scripts/start_keyboard_teleop.sh +++ /dev/null @@ -1,5 +0,0 @@ -#!/usr/bin/env bash - -# Start the keyboard teleop node for the ISMC controller - -ros2 run teleop_twist_keyboard teleop_twist_keyboard --ros-args -r /cmd_vel:=/blue/ismc/cmd_vel diff --git a/setup.cfg b/setup.cfg deleted file mode 100644 index 33bd47bc..00000000 --- a/setup.cfg +++ /dev/null @@ -1,18 +0,0 @@ -[flake8] -max-line-length = 88 -exclude = *.egg,build,install,temp,devel,.mypy_cache,__pycache__ -select = E,W,F -doctests = True -verbose = 2 -ignore = - E203 - W503 - -[isort] -profile=black - -[pydocstyle] -ignore = D203,D213,D215,D406,D407,D408,D409,D413,D100,D104 - -[codespell] -ignore-words-list = ardusub,mavros,nochange,rclpy,rclcpp,sitl,parm,srvs,ned From e190cd5c97d5dd59a2be6f608c7149f0f918af05 Mon Sep 17 00:00:00 2001 From: Evan Palmer Date: Tue, 26 Mar 2024 01:53:31 -0700 Subject: [PATCH 05/38] Fixed launch files and started debugging manager --- blue_bringup/launch/ardusub/ardusub.launch.yaml | 2 +- blue_bringup/launch/bluerov2/bluerov2.launch.yaml | 15 ++++++++++----- .../bluerov2_heavy/bluerov2_heavy.launch.yaml | 15 ++++++++++----- .../bluerov2_heavy_reach.launch.yaml | 15 ++++++++++----- 4 files changed, 31 insertions(+), 16 deletions(-) diff --git a/blue_bringup/launch/ardusub/ardusub.launch.yaml b/blue_bringup/launch/ardusub/ardusub.launch.yaml index c25b95b8..d1f6910f 100644 --- a/blue_bringup/launch/ardusub/ardusub.launch.yaml +++ b/blue_bringup/launch/ardusub/ardusub.launch.yaml @@ -18,7 +18,7 @@ launch: - arg: name: manager_file - default: $(find-pkg-share blue_description)/config/ardusub/ardusub_manager.yaml + default: "" - arg: name: use_sim diff --git a/blue_bringup/launch/bluerov2/bluerov2.launch.yaml b/blue_bringup/launch/bluerov2/bluerov2.launch.yaml index 3db148c7..13380ddd 100644 --- a/blue_bringup/launch/bluerov2/bluerov2.launch.yaml +++ b/blue_bringup/launch/bluerov2/bluerov2.launch.yaml @@ -29,10 +29,6 @@ launch: name: use_rviz default: "false" - - arg: - name: use_joy - default: "false" - - arg: name: use_manager default: "false" @@ -47,7 +43,11 @@ launch: - arg: name: mavros_file - default: $(find-pkg-share blue_description)/config/ardusub/mavros.yaml + default: $(find-pkg-share blue_description)/config/mavros.yaml + + - arg: + name: manager_file + default: $(find-pkg-share blue_description)/config/ardusub/ardusub_manager.yaml - arg: name: ardusub_params_file @@ -95,6 +95,8 @@ launch: value: $(var gazebo_world_file) - name: ardusub_params_file value: $(var ardusub_params_file) + - name: manager_file + value: $(var manager_file) - name: configuration_type value: $(var configuration_type) - name: use_sim @@ -115,3 +117,6 @@ launch: value: $(var use_sim) - name: config_filepath value: $(var localization_file) + + - include: + file: $(find-pkg-share blue_bringup)/launch/$(var configuration_type)/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 9ac6703a..30571549 100644 --- a/blue_bringup/launch/bluerov2_heavy/bluerov2_heavy.launch.yaml +++ b/blue_bringup/launch/bluerov2_heavy/bluerov2_heavy.launch.yaml @@ -29,10 +29,6 @@ launch: name: use_rviz default: "false" - - arg: - name: use_joy - default: "false" - - arg: name: use_manager default: "false" @@ -47,7 +43,11 @@ launch: - arg: name: mavros_file - default: $(find-pkg-share blue_description)/config/ardusub/mavros.yaml + default: $(find-pkg-share blue_description)/config/mavros.yaml + + - arg: + name: manager_file + default: $(find-pkg-share blue_description)/config/ardusub/ardusub_manager.yaml - arg: name: ardusub_params_file @@ -95,6 +95,8 @@ launch: value: $(var gazebo_world_file) - name: ardusub_params_file value: $(var ardusub_params_file) + - name: manager_file + value: $(var manager_file) - name: configuration_type value: $(var configuration_type) - name: use_sim @@ -115,3 +117,6 @@ launch: value: $(var use_sim) - name: config_filepath value: $(var localization_file) + + - include: + file: $(find-pkg-share blue_bringup)/launch/$(var configuration_type)/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 85300deb..47e53494 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 @@ -29,10 +29,6 @@ launch: name: use_rviz default: "false" - - arg: - name: use_joy - default: "false" - - arg: name: use_manager default: "false" @@ -47,7 +43,11 @@ launch: - arg: name: mavros_file - default: $(find-pkg-share blue_description)/config/ardusub/mavros.yaml + default: $(find-pkg-share blue_description)/config/mavros.yaml + + - arg: + name: manager_file + default: $(find-pkg-share blue_description)/config/ardusub/ardusub_manager.yaml - arg: name: ardusub_params_file @@ -95,6 +95,8 @@ launch: value: $(var gazebo_world_file) - name: ardusub_params_file value: $(var ardusub_params_file) + - name: manager_file + value: $(var manager_file) - name: configuration_type value: $(var configuration_type) - name: use_sim @@ -115,3 +117,6 @@ launch: value: $(var use_sim) - name: config_filepath value: $(var localization_file) + + - include: + file: $(find-pkg-share blue_bringup)/launch/$(var configuration_type)/thrusters.launch.yaml From 4845eaffa4fd11e7744a35373a9ff59bc73cfe4e Mon Sep 17 00:00:00 2001 From: Evan Palmer Date: Thu, 28 Mar 2024 17:14:19 -0700 Subject: [PATCH 06/38] Fixed mavros configuration file --- .../launch/ardusub/ardusub.launch.yaml | 57 ------------------- blue_bringup/launch/ardusub/sitl.launch.yaml | 43 -------------- .../launch/bluerov2/bluerov2.launch.yaml | 22 +++---- .../bluerov2_heavy/bluerov2_heavy.launch.yaml | 20 +++---- .../bluerov2_heavy_reach.launch.yaml | 20 +++---- blue_description/config/ardusub/mavros.yaml | 5 +- 6 files changed, 35 insertions(+), 132 deletions(-) delete mode 100644 blue_bringup/launch/ardusub/ardusub.launch.yaml delete mode 100644 blue_bringup/launch/ardusub/sitl.launch.yaml diff --git a/blue_bringup/launch/ardusub/ardusub.launch.yaml b/blue_bringup/launch/ardusub/ardusub.launch.yaml deleted file mode 100644 index d1f6910f..00000000 --- a/blue_bringup/launch/ardusub/ardusub.launch.yaml +++ /dev/null @@ -1,57 +0,0 @@ -launch: - - # Arguments - - arg: - name: mavros_file - - - arg: - name: gazebo_world_file - default: "" - - - arg: - name: ardusub_params_file - default: "" - - - arg: - name: configuration_type - default: "" - - - arg: - name: manager_file - default: "" - - - arg: - name: use_sim - default: "false" - - - arg: - name: use_manager - default: "false" - - # Nodes - - node: - pkg: mavros - exec: mavros_node - param: - - from: $(var mavros_file) - - name: use_sim_time - value: $(var use_sim) - - # Includes - - include: - file: $(find-pkg-share blue_bringup)/launch/ardusub/sitl.launch.yaml - if: $(var use_sim) - arg: - - name: configuration_type - value: $(var configuration_type) - - name: gazebo_world_file - value: $(var gazebo_world_file) - - name: ardusub_params_file - value: $(var ardusub_params_file) - - - include: - file: $(find-pkg-share ardusub_manager)/launch/ardusub_manager.launch.py - if: $(var use_manager) - arg: - - name: ardusub_manager_file - value: $(var manager_file) diff --git a/blue_bringup/launch/ardusub/sitl.launch.yaml b/blue_bringup/launch/ardusub/sitl.launch.yaml deleted file mode 100644 index 54e5e370..00000000 --- a/blue_bringup/launch/ardusub/sitl.launch.yaml +++ /dev/null @@ -1,43 +0,0 @@ -launch: - - # Arguments - - arg: - name: configuration_type - - - arg: - name: gazebo_world_file - - - arg: - name: ardusub_params_file - - # Nodes - - 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 configuration_type)/odometry@nav_msgs/msg/Odometry[gz.msgs.Odometry - - - node: - pkg: ros_gz_sim - exec: create - args: -name $(var configuration_type) -topic robot_description - - # Processes - - executable: - cmd: ardusub -S -w -M JSON --defaults $(var ardusub_params_file) -IO --home 44.65870,-124.06556,0.0,270.0 - - # Includes - - include: - file: $(find-pkg-share ros_gz_sim)/launch/gz_sim.launch.py - arg: - - name: gz_args - value: -v 4 -r $(var gazebo_world_file) diff --git a/blue_bringup/launch/bluerov2/bluerov2.launch.yaml b/blue_bringup/launch/bluerov2/bluerov2.launch.yaml index 13380ddd..6c25d35f 100644 --- a/blue_bringup/launch/bluerov2/bluerov2.launch.yaml +++ b/blue_bringup/launch/bluerov2/bluerov2.launch.yaml @@ -1,7 +1,7 @@ launch: - let: - name: configuration_type + name: model_name value: bluerov2 # Arguments @@ -35,15 +35,15 @@ launch: - arg: name: rviz_config - default: $(find-pkg-share blue_description)/rviz/$(var configuration_type).rviz + default: $(find-pkg-share blue_description)/rviz/$(var model_name).rviz - arg: name: gazebo_world_file - default: $(find-pkg-share blue_description)/gazebo/worlds/$(var configuration_type)_underwater.world + default: $(find-pkg-share blue_description)/gazebo/worlds/$(var model_name)_underwater.world - arg: name: mavros_file - default: $(find-pkg-share blue_description)/config/mavros.yaml + default: $(find-pkg-share blue_description)/config/ardusub/mavros.yaml - arg: name: manager_file @@ -51,16 +51,16 @@ launch: - arg: name: ardusub_params_file - default: $(find-pkg-share blue_description)/config/$(var configuration_type)/ardusub.parm + default: $(find-pkg-share blue_description)/config/$(var model_name)/ardusub.parm - arg: name: localization_file - default: $(find-pkg-share blue_description)/config/$(var configuration_type)/localization.yaml + default: $(find-pkg-share blue_description)/config/$(var model_name)/localization.yaml # Load the description file - let: name: description_file - value: $(find-pkg-share blue_description)/xacro/$(var configuration_type)/config.xacro + value: $(find-pkg-share blue_description)/xacro/$(var model_name)/config.xacro - let: name: robot_description @@ -87,7 +87,7 @@ launch: # Includes - include: - file: $(find-pkg-share blue_bringup)/launch/ardusub/ardusub.launch.yaml + file: $(find-pkg-share ardusub_bringup)/launch/ardusub.launch.yaml arg: - name: mavros_file value: $(var mavros_file) @@ -97,8 +97,8 @@ launch: value: $(var ardusub_params_file) - name: manager_file value: $(var manager_file) - - name: configuration_type - value: $(var configuration_type) + - name: model_name + value: $(var model_name) - name: use_sim value: $(var use_sim) - name: use_manager @@ -119,4 +119,4 @@ launch: value: $(var localization_file) - include: - file: $(find-pkg-share blue_bringup)/launch/$(var configuration_type)/thrusters.launch.yaml + 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 30571549..01e17015 100644 --- a/blue_bringup/launch/bluerov2_heavy/bluerov2_heavy.launch.yaml +++ b/blue_bringup/launch/bluerov2_heavy/bluerov2_heavy.launch.yaml @@ -1,7 +1,7 @@ launch: - let: - name: configuration_type + name: model_name value: bluerov2_heavy # Arguments @@ -35,11 +35,11 @@ launch: - arg: name: rviz_config - default: $(find-pkg-share blue_description)/rviz/$(var configuration_type).rviz + default: $(find-pkg-share blue_description)/rviz/$(var model_name).rviz - arg: name: gazebo_world_file - default: $(find-pkg-share blue_description)/gazebo/worlds/$(var configuration_type)_underwater.world + default: $(find-pkg-share blue_description)/gazebo/worlds/$(var model_name)_underwater.world - arg: name: mavros_file @@ -51,16 +51,16 @@ launch: - arg: name: ardusub_params_file - default: $(find-pkg-share blue_description)/config/$(var configuration_type)/ardusub.parm + default: $(find-pkg-share blue_description)/config/$(var model_name)/ardusub.parm - arg: name: localization_file - default: $(find-pkg-share blue_description)/config/$(var configuration_type)/localization.yaml + default: $(find-pkg-share blue_description)/config/$(var model_name)/localization.yaml # Load the description file - let: name: description_file - value: $(find-pkg-share blue_description)/xacro/$(var configuration_type)/config.xacro + value: $(find-pkg-share blue_description)/xacro/$(var model_name)/config.xacro - let: name: robot_description @@ -87,7 +87,7 @@ launch: # Includes - include: - file: $(find-pkg-share blue_bringup)/launch/ardusub/ardusub.launch.yaml + file: $(find-pkg-share ardusub_bringup)/launch/ardusub.launch.yaml arg: - name: mavros_file value: $(var mavros_file) @@ -97,8 +97,8 @@ launch: value: $(var ardusub_params_file) - name: manager_file value: $(var manager_file) - - name: configuration_type - value: $(var configuration_type) + - name: model_name + value: $(var model_name) - name: use_sim value: $(var use_sim) - name: use_manager @@ -119,4 +119,4 @@ launch: value: $(var localization_file) - include: - file: $(find-pkg-share blue_bringup)/launch/$(var configuration_type)/thrusters.launch.yaml + 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 47e53494..bdb5c1b2 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 @@ -1,7 +1,7 @@ launch: - let: - name: configuration_type + name: model_name value: bluerov2_heavy_reach # Arguments @@ -35,11 +35,11 @@ launch: - arg: name: rviz_config - default: $(find-pkg-share blue_description)/rviz/$(var configuration_type).rviz + default: $(find-pkg-share blue_description)/rviz/$(var model_name).rviz - arg: name: gazebo_world_file - default: $(find-pkg-share blue_description)/gazebo/worlds/$(var configuration_type)_underwater.world + default: $(find-pkg-share blue_description)/gazebo/worlds/$(var model_name)_underwater.world - arg: name: mavros_file @@ -51,16 +51,16 @@ launch: - arg: name: ardusub_params_file - default: $(find-pkg-share blue_description)/config/$(var configuration_type)/ardusub.parm + default: $(find-pkg-share blue_description)/config/$(var model_name)/ardusub.parm - arg: name: localization_file - default: $(find-pkg-share blue_description)/config/$(var configuration_type)/localization.yaml + default: $(find-pkg-share blue_description)/config/$(var model_name)/localization.yaml # Load the description file - let: name: description_file - value: $(find-pkg-share blue_description)/xacro/$(var configuration_type)/config.xacro + value: $(find-pkg-share blue_description)/xacro/$(var model_name)/config.xacro - let: name: robot_description @@ -87,7 +87,7 @@ launch: # Includes - include: - file: $(find-pkg-share blue_bringup)/launch/ardusub/ardusub.launch.yaml + file: $(find-pkg-share ardusub_bringup)/launch/ardusub.launch.yaml arg: - name: mavros_file value: $(var mavros_file) @@ -97,8 +97,8 @@ launch: value: $(var ardusub_params_file) - name: manager_file value: $(var manager_file) - - name: configuration_type - value: $(var configuration_type) + - name: model_name + value: $(var model_name) - name: use_sim value: $(var use_sim) - name: use_manager @@ -119,4 +119,4 @@ launch: value: $(var localization_file) - include: - file: $(find-pkg-share blue_bringup)/launch/$(var configuration_type)/thrusters.launch.yaml + file: $(find-pkg-share blue_bringup)/launch/$(var model_name)/thrusters.launch.yaml diff --git a/blue_description/config/ardusub/mavros.yaml b/blue_description/config/ardusub/mavros.yaml index 62994115..c9618c73 100644 --- a/blue_description/config/ardusub/mavros.yaml +++ b/blue_description/config/ardusub/mavros.yaml @@ -7,6 +7,7 @@ mavros: - imu - rc_io - param + - vision* - local* - global_position @@ -17,4 +18,6 @@ mavros_node: mavros/local_position: ros__parameters: - frame_id: map + frame_id: "map" + tf: + send: false From fbf3b54e3a188c3f6068a11b7efdca82f8fde802 Mon Sep 17 00:00:00 2001 From: Evan Palmer Date: Thu, 28 Mar 2024 21:36:46 -0700 Subject: [PATCH 07/38] Added vscode file --- .devcontainer/nouveau/devcontainer.json | 80 +------------------------ .devcontainer/nvidia/devcontainer.json | 80 +------------------------ .docker/Dockerfile | 30 +++------- .dockerignore | 4 -- .gitignore | 36 ----------- .vscode/c_cpp_properties.json | 19 ++++++ .vscode/settings.json | 77 ++++++++++++++++++++++++ .vscode/tasks.json | 70 ++++++++++++++++++++++ blue_bringup/package.xml | 7 +-- 9 files changed, 182 insertions(+), 221 deletions(-) create mode 100644 .vscode/c_cpp_properties.json create mode 100644 .vscode/settings.json create mode 100644 .vscode/tasks.json diff --git a/.devcontainer/nouveau/devcontainer.json b/.devcontainer/nouveau/devcontainer.json index b5842a2a..44b1e52a 100644 --- a/.devcontainer/nouveau/devcontainer.json +++ b/.devcontainer/nouveau/devcontainer.json @@ -22,94 +22,20 @@ }, "customizations": { "vscode": { - "settings": { - "files.associations": { - "*.repos": "yaml", - "*.world": "xml", - "*.xacro": "xml", - "*.srdf": "xml", - "*.rviz": "yaml", - "*.config": "xml", - "*.sdf": "xml" - }, - "terminal.integrated.defaultProfile.linux": "bash", - "files.insertFinalNewline": true, - "files.trimTrailingWhitespace": true, - "editor.formatOnSave": true, - "editor.tabSize": 2, - "xml.format.maxLineWidth": 100, - "json.format.enable": true, - "autoDocstring.startOnNewLine": false, - "autoDocstring.docstringFormat": "google-notypes", - "isort.args": ["--profile", "black"], - "isort.check": true, - "python.autoComplete.extraPaths": [ - "/opt/ros/rolling/lib/python3.10/site-packages/", - "/opt/ros/rolling/local/lib/python3.10/dist-packages/", - "${workspaceFolder}/install/" - ], - "python.analysis.extraPaths": [ - "/opt/ros/rolling/lib/python3.10/site-packages/", - "/opt/ros/rolling/local/lib/python3.10/dist-packages/", - "${workspaceFolder}/install/" - ], - "C_Cpp.default.intelliSenseMode": "linux-gcc-x86", - "C_Cpp.clang_format_fallbackStyle": "Google", - "C_Cpp.codeAnalysis.clangTidy.enabled": true, - "C_Cpp.codeAnalysis.clangTidy.runAutomatically": true, - "clang-format.executable": "/usr/bin/clang-format-14", - "[cpp]": { - "editor.rulers": [100], - "editor.tabSize": 2, - "editor.defaultFormatter": "xaver.clang-format" - }, - "[python]": { - "editor.tabSize": 4, - "editor.rulers": [90], - "editor.codeActionsOnSave": { - "source.organizeImports": true - }, - "editor.defaultFormatter": "ms-python.black-formatter" - }, - "[dockerfile]": { - "editor.quickSuggestions": { - "strings": true - }, - "editor.defaultFormatter": "ms-azuretools.vscode-docker", - "editor.tabSize": 4 - }, - "[json]": { - "editor.defaultFormatter": "esbenp.prettier-vscode" - }, - "[xml]": { - "editor.defaultFormatter": "redhat.vscode-xml" - }, - "[markdown]": { - "editor.rulers": [80], - "editor.defaultFormatter": "DavidAnson.vscode-markdownlint" - }, - "search.exclude": { - "**/build": true, - "**/install": true, - "**/log": true - } - }, "extensions": [ "ms-azuretools.vscode-docker", "ms-python.python", "njpwerner.autodocstring", - "cschlosser.doxdocgen", "ms-vscode.cpptools", "redhat.vscode-xml", "redhat.vscode-yaml", - "josetr.cmake-language-support-vscode", "smilerobotics.urdf", "DavidAnson.vscode-markdownlint", "esbenp.prettier-vscode", "xaver.clang-format", - "ms-python.isort", - "ms-python.flake8", - "ms-python.black-formatter" + "charliermarsh.ruff", + "ms-python.black-formatter", + "josetr.cmake-language-support-vscode" ] } } diff --git a/.devcontainer/nvidia/devcontainer.json b/.devcontainer/nvidia/devcontainer.json index e370e5f3..5493600d 100644 --- a/.devcontainer/nvidia/devcontainer.json +++ b/.devcontainer/nvidia/devcontainer.json @@ -26,94 +26,20 @@ }, "customizations": { "vscode": { - "settings": { - "files.associations": { - "*.repos": "yaml", - "*.world": "xml", - "*.xacro": "xml", - "*.srdf": "xml", - "*.rviz": "yaml", - "*.config": "xml", - "*.sdf": "xml" - }, - "terminal.integrated.defaultProfile.linux": "bash", - "files.insertFinalNewline": true, - "files.trimTrailingWhitespace": true, - "editor.formatOnSave": true, - "editor.tabSize": 2, - "xml.format.maxLineWidth": 100, - "json.format.enable": true, - "autoDocstring.startOnNewLine": false, - "autoDocstring.docstringFormat": "google-notypes", - "isort.args": ["--profile", "black"], - "isort.check": true, - "python.autoComplete.extraPaths": [ - "/opt/ros/rolling/lib/python3.10/site-packages/", - "/opt/ros/rolling/local/lib/python3.10/dist-packages/", - "${workspaceFolder}/install/" - ], - "python.analysis.extraPaths": [ - "/opt/ros/rolling/lib/python3.10/site-packages/", - "/opt/ros/rolling/local/lib/python3.10/dist-packages/", - "${workspaceFolder}/install/" - ], - "C_Cpp.default.intelliSenseMode": "linux-gcc-x86", - "C_Cpp.clang_format_fallbackStyle": "Google", - "C_Cpp.codeAnalysis.clangTidy.enabled": true, - "C_Cpp.codeAnalysis.clangTidy.runAutomatically": true, - "clang-format.executable": "/usr/bin/clang-format-14", - "[cpp]": { - "editor.rulers": [100], - "editor.tabSize": 2, - "editor.defaultFormatter": "xaver.clang-format" - }, - "[python]": { - "editor.tabSize": 4, - "editor.rulers": [90], - "editor.codeActionsOnSave": { - "source.organizeImports": true - }, - "editor.defaultFormatter": "ms-python.black-formatter" - }, - "[dockerfile]": { - "editor.quickSuggestions": { - "strings": true - }, - "editor.defaultFormatter": "ms-azuretools.vscode-docker", - "editor.tabSize": 4 - }, - "[json]": { - "editor.defaultFormatter": "esbenp.prettier-vscode" - }, - "[xml]": { - "editor.defaultFormatter": "redhat.vscode-xml" - }, - "[markdown]": { - "editor.rulers": [80], - "editor.defaultFormatter": "DavidAnson.vscode-markdownlint" - }, - "search.exclude": { - "**/build": true, - "**/install": true, - "**/log": true - } - }, "extensions": [ "ms-azuretools.vscode-docker", "ms-python.python", "njpwerner.autodocstring", - "cschlosser.doxdocgen", "ms-vscode.cpptools", "redhat.vscode-xml", "redhat.vscode-yaml", - "josetr.cmake-language-support-vscode", "smilerobotics.urdf", "DavidAnson.vscode-markdownlint", "esbenp.prettier-vscode", "xaver.clang-format", - "ms-python.isort", - "ms-python.flake8", - "ms-python.black-formatter" + "charliermarsh.ruff", + "ms-python.black-formatter", + "josetr.cmake-language-support-vscode" ] } } diff --git a/.docker/Dockerfile b/.docker/Dockerfile index ce156976..0361cb30 100644 --- a/.docker/Dockerfile +++ b/.docker/Dockerfile @@ -1,4 +1,4 @@ -ARG ROS_DISTRO=rolling +ARG ROS_DISTRO=iron FROM ros:$ROS_DISTRO-ros-base as ci ENV DEBIAN_FRONTEND=noninteractive @@ -18,6 +18,9 @@ RUN apt-get -q update \ clang-tools \ python3-pip \ python3-dev \ + lsb-release \ + wget \ + gnupg \ software-properties-common \ && apt-get autoremove -y \ && apt-get clean -y \ @@ -49,29 +52,16 @@ RUN groupadd --gid $USER_GID $USERNAME \ ENV DEBIAN_FRONTEND=noninteractive -# Switch to the non-root user for the rest of the installation. -# We do this to avoid any conflicts with user permissions +# Switch to the non-root user for the rest of the installation USER $USERNAME ENV USER=$USERNAME -# Install some core apt packages -RUN sudo apt-get -q update \ - && sudo apt-get -q -y upgrade \ - && sudo apt-get -q install --no-install-recommends -y \ - lsb-release \ - wget \ - gnupg \ - && sudo apt-get autoremove -y \ - && sudo apt-get clean -y \ - && sudo rm -rf /var/lib/apt/lists/* - # Install MAVROS dependencies WORKDIR /home/$USERNAME RUN wget https://raw.githubusercontent.com/mavlink/mavros/ros2/mavros/scripts/install_geographiclib_datasets.sh \ && chmod +x install_geographiclib_datasets.sh \ && sudo ./install_geographiclib_datasets.sh -# Copy the project over to the image ENV USER_WORKSPACE=/home/$USERNAME/ws_blue WORKDIR $USER_WORKSPACE COPY --chown=$USER_UID:$USER_GID . src/blue @@ -95,7 +85,6 @@ RUN sudo apt-get -q update \ && sudo apt-get clean -y \ && sudo rm -rf /var/lib/apt/lists/* -# Install all ROS dependencies RUN sudo apt-get -q update \ && sudo apt-get -q -y upgrade \ && rosdep update \ @@ -104,9 +93,8 @@ RUN sudo apt-get -q update \ && sudo apt-get clean -y \ && sudo rm -rf /var/lib/apt/lists/* -# Setup the environment variables RUN echo "source ${USER_WORKSPACE}/install/setup.bash" >> /home/$USERNAME/.bashrc \ - && echo "source /opt/ros/${ROS_DISTRO}/setup.bash" >> /home/$USERNAME/.bashr + && echo "source /opt/ros/${ROS_DISTRO}/setup.bash" >> /home/$USERNAME/.bashrc FROM robot as desktop @@ -116,7 +104,7 @@ ENV GZ_VERSION=garden # Install Gazebo Garden: https://gazebosim.org/docs/garden/install_ubuntu RUN sudo wget https://packages.osrfoundation.org/gazebo.gpg -O /usr/share/keyrings/pkgs-osrf-archive-keyring.gpg \ && echo "deb [arch=$(dpkg --print-architecture) signed-by=/usr/share/keyrings/pkgs-osrf-archive-keyring.gpg] http://packages.osrfoundation.org/gazebo/ubuntu-stable $(lsb_release -cs) main" | sudo tee /etc/apt/sources.list.d/gazebo-stable.list > /dev/null \ - && sudo apt-get update \ + && sudo apt-get -q update \ && sudo apt-get -y --quiet --no-install-recommends install \ gz-garden \ && sudo apt-get autoremove -y \ @@ -163,13 +151,13 @@ RUN [ "/bin/bash" , "-c" , " \ && cmake .. -DCMAKE_BUILD_TYPE=RelWithDebInfo \ && make -j4" ] -# Install Gazebo +# 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/blue.repos \ && rosdep update \ - && rosdep install -y --from-paths src --ignore-src --rosdistro ${ROS_DISTRO} --skip-keys="gz-transport12 gz-sim7 gz-math7 gz-msgs9 gz-plugin2" \ + && 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/* diff --git a/.dockerignore b/.dockerignore index 97eedc18..feb85c37 100644 --- a/.dockerignore +++ b/.dockerignore @@ -2,12 +2,8 @@ * # Except the following -!blue_dynamics -!blue_manager -!blue_control !blue_bringup !blue_localization -!blue_utils !blue_joy !blue_description !blue.repos diff --git a/.gitignore b/.gitignore index 5f181a1a..eb28dd58 100644 --- a/.gitignore +++ b/.gitignore @@ -1,44 +1,8 @@ -# Prerequisites -*.d - -# Compiled Object files -*.slo -*.lo -*.o -*.obj - -# Precompiled Headers -*.gch -*.pch - -# Compiled Dynamic libraries -*.so -*.dylib -*.dll - -# Fortran module files -*.mod -*.smod - -# Compiled Static libraries -*.lai -*.la -*.a -*.lib - -# Executables -*.exe -*.out -*.app - # ROS build/ install/ log/ -# VSCode -.vscode/ - # python .mypy_cache/ __pycache__ diff --git a/.vscode/c_cpp_properties.json b/.vscode/c_cpp_properties.json new file mode 100644 index 00000000..3c605782 --- /dev/null +++ b/.vscode/c_cpp_properties.json @@ -0,0 +1,19 @@ +{ + "configurations": [ + { + "name": "Linux", + "includePath": [ + "${workspaceFolder}/**", + "/opt/ros/iron/include/**", + "/usr/include/eigen3/**", + "/home/ros/ws_ros/**" + ], + "defines": [], + "compilerPath": "/usr/bin/gcc", + "cStandard": "c99", + "cppStandard": "c++20", + "intelliSenseMode": "linux-gcc-x64" + } + ], + "version": 4 +} diff --git a/.vscode/settings.json b/.vscode/settings.json new file mode 100644 index 00000000..ab0bdaf3 --- /dev/null +++ b/.vscode/settings.json @@ -0,0 +1,77 @@ +{ + "files.associations": { + "*.repos": "yaml", + "*.world": "xml", + "*.xacro": "xml", + "*.srdf": "xml", + "*.rviz": "yaml", + "*.config": "xml", + "*.sdf": "xml" + }, + "terminal.integrated.defaultProfile.linux": "bash", + "terminal.integrated.profiles.linux": { + "bash": { + "path": "bash", + "icon": "terminal-bash", + "args": ["-i"] + } + }, + "files.insertFinalNewline": true, + "files.trimTrailingWhitespace": true, + "editor.formatOnSave": true, + "editor.tabSize": 2, + "autoDocstring.startOnNewLine": false, + "autoDocstring.docstringFormat": "google-notypes", + "python.autoComplete.extraPaths": [ + "/opt/ros/iron/lib/python3.10/site-packages/", + "/opt/ros/iron/local/lib/python3.10/dist-packages/", + "${workspaceFolder}/install/" + ], + "python.analysis.extraPaths": [ + "/opt/ros/iron/lib/python3.10/site-packages/", + "/opt/ros/iron/local/lib/python3.10/dist-packages/", + "${workspaceFolder}/install/" + ], + "C_Cpp.default.intelliSenseMode": "linux-gcc-x86", + "C_Cpp.clang_format_fallbackStyle": "Google", + "C_Cpp.codeAnalysis.clangTidy.enabled": true, + "C_Cpp.codeAnalysis.clangTidy.codeAction.formatFixes": true, + "clang-format.executable": "/usr/bin/clang-format-14", + "[cpp]": { + "editor.rulers": [120], + "editor.tabSize": 2, + "editor.defaultFormatter": "xaver.clang-format" + }, + "[python]": { + "editor.tabSize": 4, + "editor.rulers": [90], + "editor.formatOnSave": true, + "editor.codeActionsOnSave": { + "source.fixAll": "explicit", + "source.organizeImports": "explicit" + }, + "editor.defaultFormatter": "ms-python.black-formatter" + }, + "[dockerfile]": { + "editor.quickSuggestions": { + "strings": true + }, + "editor.defaultFormatter": "ms-azuretools.vscode-docker", + "editor.tabSize": 4 + }, + "[json]": { + "editor.defaultFormatter": "esbenp.prettier-vscode" + }, + "[xml]": { + "editor.defaultFormatter": "redhat.vscode-xml" + }, + "[markdown]": { + "editor.rulers": [80], + "editor.defaultFormatter": "DavidAnson.vscode-markdownlint" + }, + "search.exclude": { + "**/build": true, + "**/install": true, + "**/log": true + } +} diff --git a/.vscode/tasks.json b/.vscode/tasks.json new file mode 100644 index 00000000..a1b4d034 --- /dev/null +++ b/.vscode/tasks.json @@ -0,0 +1,70 @@ +{ + "version": "2.0.0", + "tasks": [ + { + "label": "ROS 2: Build", + "detail": "Build the workspace using colcon", + "type": "shell", + "command": "colcon build --symlink-install", + "group": { + "kind": "build", + "isDefault": true + }, + "problemMatcher": ["$gcc"] + }, + { + "label": "ROS 2: Test", + "detail": "Run system tests using colcon", + "type": "shell", + "command": "colcon test && colcon test-result --all", + "group": { + "kind": "test", + "isDefault": true + } + }, + { + "label": "ROS 2: Source workspace", + "detail": "Source the ROS 2 workspace", + "type": "shell", + "command": "source install/setup.bash", + "problemMatcher": [] + }, + { + "label": "ROS 2: Create ament_cmake package", + "detail": "Create a new ROS 2 ament_cmake package", + "type": "shell", + "command": "ros2 pkg create --build-type ament_cmake --license ${input:license} ${input:packageName}", + "problemMatcher": [] + }, + { + "label": "ROS 2: Create ament_python package", + "detail": "Create a new ROS 2 ament_python package", + "type": "shell", + "command": "ros2 pkg create --build-type ament_python --license ${input:license} ${input:packageName}", + "problemMatcher": [] + } + ], + "inputs": [ + { + "id": "license", + "type": "pickString", + "description": "License", + "options": [ + "Apache-2.0", + "BSL-1.0", + "BSD-2.0", + "BSD-2-Clause", + "BSD-3-Clause", + "GPL-3.0-only", + "LGPL-3.0-only", + "MIT", + "MIT-0" + ] + }, + { + "id": "packageName", + "type": "promptString", + "description": "Package name" + } + ] +} diff --git a/blue_bringup/package.xml b/blue_bringup/package.xml index 2cd4bb08..a03bfb40 100644 --- a/blue_bringup/package.xml +++ b/blue_bringup/package.xml @@ -4,7 +4,7 @@ blue_bringup 0.0.1 - Entrypoints for the Blue project + Launch files for Blue Evan Palmer MIT @@ -18,11 +18,6 @@ mavros_extras ros2launch - ament_copyright - ament_flake8 - ament_pep257 - python3-pytest - ament_cmake From ef488c268217f0f62cfd9e8c51cd3ae9b5669a1c Mon Sep 17 00:00:00 2001 From: Evan Palmer Date: Mon, 1 Apr 2024 00:53:45 -0700 Subject: [PATCH 08/38] Working state - need to finish updates to joy package --- blue.repos | 5 +++++ .../launch/bluerov2/bluerov2.launch.yaml | 3 +++ .../bluerov2_heavy/bluerov2_heavy.launch.yaml | 3 +++ .../bluerov2_heavy_control.launch.py | 19 +++++++++++++++++++ .../bluerov2_heavy_reach.launch.yaml | 3 +++ blue_bringup/launch/tf.launch.yaml | 14 -------------- 6 files changed, 33 insertions(+), 14 deletions(-) delete mode 100644 blue_bringup/launch/tf.launch.yaml diff --git a/blue.repos b/blue.repos index a189f517..94868540 100644 --- a/blue.repos +++ b/blue.repos @@ -19,3 +19,8 @@ repositories: type: git url: https://github.com/Robotic-Decision-Making-Lab/ardusub_driver.git version: develop + + mobile_to_maritime: + type: git + url: https://github.com/Robotic-Decision-Making-Lab/mobile_to_maritime.git + version: main diff --git a/blue_bringup/launch/bluerov2/bluerov2.launch.yaml b/blue_bringup/launch/bluerov2/bluerov2.launch.yaml index 6c25d35f..0c064d78 100644 --- a/blue_bringup/launch/bluerov2/bluerov2.launch.yaml +++ b/blue_bringup/launch/bluerov2/bluerov2.launch.yaml @@ -120,3 +120,6 @@ launch: - include: file: $(find-pkg-share blue_bringup)/launch/$(var model_name)/thrusters.launch.yaml + + - include: + file: $(find-pkg-share mobile_to_maritime)/launch/tf.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 01e17015..b5a15809 100644 --- a/blue_bringup/launch/bluerov2_heavy/bluerov2_heavy.launch.yaml +++ b/blue_bringup/launch/bluerov2_heavy/bluerov2_heavy.launch.yaml @@ -120,3 +120,6 @@ launch: - include: file: $(find-pkg-share blue_bringup)/launch/$(var model_name)/thrusters.launch.yaml + + - include: + file: $(find-pkg-share mobile_to_maritime)/launch/tf.launch.yaml diff --git a/blue_bringup/launch/bluerov2_heavy/bluerov2_heavy_control.launch.py b/blue_bringup/launch/bluerov2_heavy/bluerov2_heavy_control.launch.py index 25e6f9e3..6cb30e17 100644 --- a/blue_bringup/launch/bluerov2_heavy/bluerov2_heavy_control.launch.py +++ b/blue_bringup/launch/bluerov2_heavy/bluerov2_heavy_control.launch.py @@ -75,6 +75,21 @@ def generate_launch_description() -> LaunchDescription: ) robot_description = {"robot_description": robot_description_content} + # The ISMC expects state information to be provided in the FSD frame + mobile_to_maritime_velocity_state = Node( + package="mobile_to_maritime", + executable="mobile_twist_stamped_to_maritime_twist", + name="velocity_state_transform", + parameters=[ + { + "in_topic": "/mavros/local_position/velocity_body", + "out_topic": "/integral_sliding_mode_controller/system_state", + "qos_reliability": "best_effort", + "qos_durability": "volatile", + } + ], + ) + controller_manager = Node( package="controller_manager", executable="ros2_control_node", @@ -101,6 +116,7 @@ def generate_launch_description() -> LaunchDescription: ["", "controller_manager"], ], ) + thruster_spawners = [ Node( package="controller_manager", @@ -139,6 +155,7 @@ def generate_launch_description() -> LaunchDescription: ["", "controller_manager"], ], ) + delay_tam_controller_spawner_after_thruster_controller_spawners = ( RegisterEventHandler( event_handler=OnProcessExit( @@ -147,6 +164,7 @@ def generate_launch_description() -> LaunchDescription: ) ) ) + delay_velocity_controller_spawner_after_tam_controller_spawner = ( RegisterEventHandler( event_handler=OnProcessExit( @@ -159,6 +177,7 @@ def generate_launch_description() -> LaunchDescription: return LaunchDescription( [ *args, + mobile_to_maritime_velocity_state, controller_manager, *delay_thruster_spawners, delay_tam_controller_spawner_after_thruster_controller_spawners, 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 bdb5c1b2..f928a4ba 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 @@ -120,3 +120,6 @@ launch: - include: file: $(find-pkg-share blue_bringup)/launch/$(var model_name)/thrusters.launch.yaml + + - include: + file: $(find-pkg-share mobile_to_maritime)/launch/tf.launch.yaml diff --git a/blue_bringup/launch/tf.launch.yaml b/blue_bringup/launch/tf.launch.yaml deleted file mode 100644 index 42f0af80..00000000 --- a/blue_bringup/launch/tf.launch.yaml +++ /dev/null @@ -1,14 +0,0 @@ -launch: - - # REP 156 Transforms - - node: - pkg: tf2_ros - exec: static_transform_publisher - name: base_link_to_base_link_fsd - args: --roll 3.142 --frame-id base_link --child-frame-id base_link_fsd - - - node: - pkg: tf2_ros - exec: static_transform_publisher - name: map_to_map_ned - args: --roll 3.142 --frame-id map --child-frame-id map_ned From ad7b6d4a5bd7e7c5c2ca7921d97efa1821736a93 Mon Sep 17 00:00:00 2001 From: Evan Palmer Date: Mon, 1 Apr 2024 13:35:39 -0700 Subject: [PATCH 09/38] Working state all vehicles updated --- .../launch/bluerov2/bluerov2.launch.yaml | 2 +- .../bluerov2/bluerov2_control.launch.py | 185 ++++++++++++++++++ .../bluerov2_heavy/bluerov2_heavy.launch.yaml | 2 +- .../bluerov2_heavy_control.launch.py | 5 +- .../bluerov2_heavy_reach.launch.yaml | 2 +- .../bluerov2_heavy_reach_control.launch.py | 185 ++++++++++++++++++ 6 files changed, 375 insertions(+), 6 deletions(-) create mode 100644 blue_bringup/launch/bluerov2/bluerov2_control.launch.py create mode 100644 blue_bringup/launch/bluerov2_heavy_reach/bluerov2_heavy_reach_control.launch.py diff --git a/blue_bringup/launch/bluerov2/bluerov2.launch.yaml b/blue_bringup/launch/bluerov2/bluerov2.launch.yaml index 0c064d78..d9daad64 100644 --- a/blue_bringup/launch/bluerov2/bluerov2.launch.yaml +++ b/blue_bringup/launch/bluerov2/bluerov2.launch.yaml @@ -31,7 +31,7 @@ launch: - arg: name: use_manager - default: "false" + default: "true" - arg: name: rviz_config diff --git a/blue_bringup/launch/bluerov2/bluerov2_control.launch.py b/blue_bringup/launch/bluerov2/bluerov2_control.launch.py new file mode 100644 index 00000000..7e914537 --- /dev/null +++ b/blue_bringup/launch/bluerov2/bluerov2_control.launch.py @@ -0,0 +1,185 @@ +# Copyright 2024, 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, RegisterEventHandler +from launch.event_handlers import OnProcessExit +from launch.substitutions import ( + Command, + FindExecutable, + LaunchConfiguration, + PathJoinSubstitution, +) +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare + + +def generate_launch_description() -> LaunchDescription: + """Generate a launch description for the BlueROV2 Heavy. + + This should be launched after MAVROS has fully loaded. + """ + args = [ + DeclareLaunchArgument( + "prefix", + default_value="", + description=( + "The prefix of the model. This is useful for multi-robot setups." + " Expected format '/'." + ), + ), + DeclareLaunchArgument( + "use_sim", + default_value="false", + description="Launch the Gazebo + ArduSub simulator.", + ), + ] + + robot_description_content = Command( + [ + PathJoinSubstitution([FindExecutable(name="xacro")]), + " ", + PathJoinSubstitution( + [ + FindPackageShare("blue_description"), + "xacro", + "bluerov2", + "config.xacro", + ] + ), + " ", + "prefix:=", + LaunchConfiguration("prefix"), + " ", + "use_sim:=", + LaunchConfiguration("use_sim"), + ] + ) + robot_description = {"robot_description": robot_description_content} + + # The ISMC expects state information to be provided in the FSD frame + mobile_to_maritime_velocity_state = Node( + package="mobile_to_maritime", + executable="mobile_twist_stamped_to_maritime_twist", + name="velocity_state_transform", + parameters=[ + { + "in_topic": "/mavros/local_position/velocity_body", + "out_topic": "/integral_sliding_mode_controller/system_state", + "qos_reliability": "best_effort", + "qos_durability": "volatile", + } + ], + ) + + controller_manager = Node( + package="controller_manager", + executable="ros2_control_node", + output="both", + parameters=[ + robot_description, + PathJoinSubstitution( + [ + FindPackageShare("blue_description"), + "config", + "bluerov2", + "controllers.yaml", + ] + ), + ], + ) + + velocity_controller_spawner = Node( + package="controller_manager", + executable="spawner", + arguments=[ + "integral_sliding_mode_controller", + "--controller-manager", + ["", "controller_manager"], + ], + ) + + thruster_spawners = [ + Node( + package="controller_manager", + executable="spawner", + arguments=[ + f"thruster{i + 1}_controller", + "--controller-manager", + ["", "controller_manager"], + ], + ) + for i in range(6) + ] + + delay_thruster_spawners = [] + for i, thruster_spawner in enumerate(thruster_spawners): + if not len(delay_thruster_spawners): + delay_thruster_spawners.append( + thruster_spawner, + ) + else: + delay_thruster_spawners.append( + RegisterEventHandler( + event_handler=OnProcessExit( + target_action=thruster_spawners[i - 1], + on_exit=[thruster_spawner], + ) + ) + ) + + tam_controller_spawner = Node( + package="controller_manager", + executable="spawner", + arguments=[ + "thruster_allocation_matrix_controller", + "--controller-manager", + ["", "controller_manager"], + ], + ) + + delay_tam_controller_spawner_after_thruster_controller_spawners = ( + RegisterEventHandler( + event_handler=OnProcessExit( + target_action=thruster_spawners[-1], + on_exit=[tam_controller_spawner], + ) + ) + ) + + delay_velocity_controller_spawner_after_tam_controller_spawner = ( + RegisterEventHandler( + event_handler=OnProcessExit( + target_action=tam_controller_spawner, + on_exit=[velocity_controller_spawner], + ) + ) + ) + + return LaunchDescription( + [ + *args, + mobile_to_maritime_velocity_state, + controller_manager, + *delay_thruster_spawners, + delay_tam_controller_spawner_after_thruster_controller_spawners, + delay_velocity_controller_spawner_after_tam_controller_spawner, + ] + ) diff --git a/blue_bringup/launch/bluerov2_heavy/bluerov2_heavy.launch.yaml b/blue_bringup/launch/bluerov2_heavy/bluerov2_heavy.launch.yaml index b5a15809..59df69f1 100644 --- a/blue_bringup/launch/bluerov2_heavy/bluerov2_heavy.launch.yaml +++ b/blue_bringup/launch/bluerov2_heavy/bluerov2_heavy.launch.yaml @@ -31,7 +31,7 @@ launch: - arg: name: use_manager - default: "false" + default: "true" - arg: name: rviz_config diff --git a/blue_bringup/launch/bluerov2_heavy/bluerov2_heavy_control.launch.py b/blue_bringup/launch/bluerov2_heavy/bluerov2_heavy_control.launch.py index 6cb30e17..b93aeb79 100644 --- a/blue_bringup/launch/bluerov2_heavy/bluerov2_heavy_control.launch.py +++ b/blue_bringup/launch/bluerov2_heavy/bluerov2_heavy_control.launch.py @@ -32,10 +32,9 @@ def generate_launch_description() -> LaunchDescription: - """Generate a launch description to run the system. + """Generate a launch description for the BlueROV2 Heavy. - Returns: - The launch description for the BlueROV2 base configuration. + This should be launched after MAVROS has fully loaded. """ args = [ DeclareLaunchArgument( 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 f928a4ba..458aa69b 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 @@ -31,7 +31,7 @@ launch: - arg: name: use_manager - default: "false" + default: "true" - arg: name: rviz_config diff --git a/blue_bringup/launch/bluerov2_heavy_reach/bluerov2_heavy_reach_control.launch.py b/blue_bringup/launch/bluerov2_heavy_reach/bluerov2_heavy_reach_control.launch.py new file mode 100644 index 00000000..183ac49d --- /dev/null +++ b/blue_bringup/launch/bluerov2_heavy_reach/bluerov2_heavy_reach_control.launch.py @@ -0,0 +1,185 @@ +# Copyright 2024, 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, RegisterEventHandler +from launch.event_handlers import OnProcessExit +from launch.substitutions import ( + Command, + FindExecutable, + LaunchConfiguration, + PathJoinSubstitution, +) +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare + + +def generate_launch_description() -> LaunchDescription: + """Generate a launch description for the BlueROV2 Heavy. + + This should be launched after MAVROS has fully loaded. + """ + args = [ + DeclareLaunchArgument( + "prefix", + default_value="", + description=( + "The prefix of the model. This is useful for multi-robot setups." + " Expected format '/'." + ), + ), + DeclareLaunchArgument( + "use_sim", + default_value="false", + description="Launch the Gazebo + ArduSub simulator.", + ), + ] + + robot_description_content = Command( + [ + PathJoinSubstitution([FindExecutable(name="xacro")]), + " ", + PathJoinSubstitution( + [ + FindPackageShare("blue_description"), + "xacro", + "bluerov2_heavy_reach", + "config.xacro", + ] + ), + " ", + "prefix:=", + LaunchConfiguration("prefix"), + " ", + "use_sim:=", + LaunchConfiguration("use_sim"), + ] + ) + robot_description = {"robot_description": robot_description_content} + + # The ISMC expects state information to be provided in the FSD frame + mobile_to_maritime_velocity_state = Node( + package="mobile_to_maritime", + executable="mobile_twist_stamped_to_maritime_twist", + name="velocity_state_transform", + parameters=[ + { + "in_topic": "/mavros/local_position/velocity_body", + "out_topic": "/integral_sliding_mode_controller/system_state", + "qos_reliability": "best_effort", + "qos_durability": "volatile", + } + ], + ) + + controller_manager = Node( + package="controller_manager", + executable="ros2_control_node", + output="both", + parameters=[ + robot_description, + PathJoinSubstitution( + [ + FindPackageShare("blue_description"), + "config", + "bluerov2_heavy_reach", + "controllers.yaml", + ] + ), + ], + ) + + velocity_controller_spawner = Node( + package="controller_manager", + executable="spawner", + arguments=[ + "integral_sliding_mode_controller", + "--controller-manager", + ["", "controller_manager"], + ], + ) + + thruster_spawners = [ + Node( + package="controller_manager", + executable="spawner", + arguments=[ + f"thruster{i + 1}_controller", + "--controller-manager", + ["", "controller_manager"], + ], + ) + for i in range(8) + ] + + delay_thruster_spawners = [] + for i, thruster_spawner in enumerate(thruster_spawners): + if not len(delay_thruster_spawners): + delay_thruster_spawners.append( + thruster_spawner, + ) + else: + delay_thruster_spawners.append( + RegisterEventHandler( + event_handler=OnProcessExit( + target_action=thruster_spawners[i - 1], + on_exit=[thruster_spawner], + ) + ) + ) + + tam_controller_spawner = Node( + package="controller_manager", + executable="spawner", + arguments=[ + "thruster_allocation_matrix_controller", + "--controller-manager", + ["", "controller_manager"], + ], + ) + + delay_tam_controller_spawner_after_thruster_controller_spawners = ( + RegisterEventHandler( + event_handler=OnProcessExit( + target_action=thruster_spawners[-1], + on_exit=[tam_controller_spawner], + ) + ) + ) + + delay_velocity_controller_spawner_after_tam_controller_spawner = ( + RegisterEventHandler( + event_handler=OnProcessExit( + target_action=tam_controller_spawner, + on_exit=[velocity_controller_spawner], + ) + ) + ) + + return LaunchDescription( + [ + *args, + mobile_to_maritime_velocity_state, + controller_manager, + *delay_thruster_spawners, + delay_tam_controller_spawner_after_thruster_controller_spawners, + delay_velocity_controller_spawner_after_tam_controller_spawner, + ] + ) From 45aec307cb8a3ab35ed7d935a2e1235b705df9f2 Mon Sep 17 00:00:00 2001 From: Evan Palmer Date: Mon, 1 Apr 2024 14:12:14 -0700 Subject: [PATCH 10/38] Save point before I break things --- .../bluerov2/bluerov2_control.launch.py | 2 +- blue_demos/CMakeLists.txt | 26 +++ blue_demos/LICENSE | 17 ++ blue_demos/control_examples/README.md | 0 .../config/bluerov2_controllers.yaml | 133 +++++++++++++ .../config/bluerov2_heavy_controllers.yaml | 163 +++++++++++++++ .../bluerov2_heavy_reach_controllers.yaml | 163 +++++++++++++++ .../launch/bluerov2_control.launch.py | 185 ++++++++++++++++++ .../launch}/bluerov2_heavy_control.launch.py | 0 .../bluerov2_heavy_reach_control.launch.py | 2 +- blue_demos/hardware_examples/README.md | 0 blue_demos/package.xml | 18 ++ blue_demos/teleop_examples/README.md | 0 13 files changed, 707 insertions(+), 2 deletions(-) create mode 100644 blue_demos/CMakeLists.txt create mode 100644 blue_demos/LICENSE create mode 100644 blue_demos/control_examples/README.md create mode 100644 blue_demos/control_examples/config/bluerov2_controllers.yaml create mode 100644 blue_demos/control_examples/config/bluerov2_heavy_controllers.yaml create mode 100644 blue_demos/control_examples/config/bluerov2_heavy_reach_controllers.yaml create mode 100644 blue_demos/control_examples/launch/bluerov2_control.launch.py rename {blue_bringup/launch/bluerov2_heavy => blue_demos/control_examples/launch}/bluerov2_heavy_control.launch.py (100%) rename {blue_bringup/launch/bluerov2_heavy_reach => blue_demos/control_examples/launch}/bluerov2_heavy_reach_control.launch.py (98%) create mode 100644 blue_demos/hardware_examples/README.md create mode 100644 blue_demos/package.xml create mode 100644 blue_demos/teleop_examples/README.md diff --git a/blue_bringup/launch/bluerov2/bluerov2_control.launch.py b/blue_bringup/launch/bluerov2/bluerov2_control.launch.py index 7e914537..fa8d87b1 100644 --- a/blue_bringup/launch/bluerov2/bluerov2_control.launch.py +++ b/blue_bringup/launch/bluerov2/bluerov2_control.launch.py @@ -32,7 +32,7 @@ def generate_launch_description() -> LaunchDescription: - """Generate a launch description for the BlueROV2 Heavy. + """Generate a launch description for the BlueROV2. This should be launched after MAVROS has fully loaded. """ diff --git a/blue_demos/CMakeLists.txt b/blue_demos/CMakeLists.txt new file mode 100644 index 00000000..cc5f0056 --- /dev/null +++ b/blue_demos/CMakeLists.txt @@ -0,0 +1,26 @@ +cmake_minimum_required(VERSION 3.8) +project(blue_demos) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +# uncomment the following section in order to fill in +# further dependencies manually. +# find_package( REQUIRED) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + # comment the line when a copyright and license is added to all source files + set(ament_cmake_copyright_FOUND TRUE) + # the following line skips cpplint (only works in a git repo) + # comment the line when this package is in a git repo and when + # a copyright and license is added to all source files + set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +ament_package() diff --git a/blue_demos/LICENSE b/blue_demos/LICENSE new file mode 100644 index 00000000..30e8e2ec --- /dev/null +++ b/blue_demos/LICENSE @@ -0,0 +1,17 @@ +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_demos/control_examples/README.md b/blue_demos/control_examples/README.md new file mode 100644 index 00000000..e69de29b diff --git a/blue_demos/control_examples/config/bluerov2_controllers.yaml b/blue_demos/control_examples/config/bluerov2_controllers.yaml new file mode 100644 index 00000000..f6076d1a --- /dev/null +++ b/blue_demos/control_examples/config/bluerov2_controllers.yaml @@ -0,0 +1,133 @@ +controller_manager: + ros__parameters: + update_rate: 100 # Hz + + integral_sliding_mode_controller: + type: velocity_controllers/IntegralSlidingModeController + + thruster_allocation_matrix_controller: + type: thruster_allocation_matrix_controller/ThrusterAllocationMatrixController + + thruster1_controller: + type: thruster_controllers/PolynomialThrustCurveController + + thruster2_controller: + type: thruster_controllers/PolynomialThrustCurveController + + thruster3_controller: + type: thruster_controllers/PolynomialThrustCurveController + + thruster4_controller: + type: thruster_controllers/PolynomialThrustCurveController + + thruster5_controller: + type: thruster_controllers/PolynomialThrustCurveController + + thruster6_controller: + type: thruster_controllers/PolynomialThrustCurveController + +integral_sliding_mode_controller: + ros__parameters: + use_external_measured_states: true + reference_controller: thruster_allocation_matrix_controller + gains: + rho: 20.0 + lambda: 200.0 + Kp: [4.0, 4.0, 9.0, 3.0, 3.0, 7.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] + +thruster_allocation_matrix_controller: + ros__parameters: + thrusters: + - thruster1_joint + - thruster2_joint + - thruster3_joint + - thruster4_joint + - thruster5_joint + - thruster6_joint + reference_controllers: + - thruster1_controller + - thruster2_controller + - thruster3_controller + - thruster4_controller + - thruster5_controller + - thruster6_controller + tam: + x: [ -0.707, -0.707, 0.707, 0.707, 0.0, 0.0] + y: [ 0.707, -0.707, 0.707, -0.707, 0.0, 0.0] + z: [ 0.0, 0.0, 0.0, 0.0, 1.0, 1.0] + rx: [ 0.0, 0.0, 0.0, 0.0, -0.21805, 0.21805] + ry: [ 0.0, 0.0, 0.0, 0.0, -0.12, -0.12] + rz: [0.1888, -0.1888, -0.1888, 0.1888, 0.0, 0.0] + +thruster1_controller: + ros__parameters: + thruster: thruster1_joint + min_thrust: -40.0 + max_thrust: 60.0 + min_deadband: 1470 + max_deadband: 1530 + neutral_pwm: 1500 + thrust_curve_coefficients: [1498.0, 12.01, -0.04731, -0.002098, 0.00002251] + +thruster2_controller: + ros__parameters: + thruster: thruster2_joint + min_thrust: -40.0 + max_thrust: 60.0 + min_deadband: 1470 + max_deadband: 1530 + neutral_pwm: 1500 + thrust_curve_coefficients: [1498.0, 12.01, -0.04731, -0.002098, 0.00002251] + +thruster3_controller: + ros__parameters: + thruster: thruster3_joint + min_thrust: -40.0 + max_thrust: 60.0 + min_deadband: 1470 + max_deadband: 1530 + neutral_pwm: 1500 + thrust_curve_coefficients: [1498.0, 12.01, -0.04731, -0.002098, 0.00002251] + +thruster4_controller: + ros__parameters: + thruster: thruster4_joint + min_thrust: -40.0 + max_thrust: 60.0 + min_deadband: 1470 + max_deadband: 1530 + neutral_pwm: 1500 + thrust_curve_coefficients: [1498.0, 12.01, -0.04731, -0.002098, 0.00002251] + +thruster5_controller: + ros__parameters: + thruster: thruster5_joint + min_thrust: -40.0 + max_thrust: 60.0 + min_deadband: 1470 + max_deadband: 1530 + neutral_pwm: 1500 + thrust_curve_coefficients: [1498.0, 12.01, -0.04731, -0.002098, 0.00002251] + +thruster6_controller: + ros__parameters: + thruster: thruster6_joint + min_thrust: -40.0 + max_thrust: 60.0 + min_deadband: 1470 + max_deadband: 1530 + neutral_pwm: 1500 + thrust_curve_coefficients: [1498.0, 12.01, -0.04731, -0.002098, 0.00002251] diff --git a/blue_demos/control_examples/config/bluerov2_heavy_controllers.yaml b/blue_demos/control_examples/config/bluerov2_heavy_controllers.yaml new file mode 100644 index 00000000..a8608511 --- /dev/null +++ b/blue_demos/control_examples/config/bluerov2_heavy_controllers.yaml @@ -0,0 +1,163 @@ +controller_manager: + ros__parameters: + update_rate: 100 # Hz + + integral_sliding_mode_controller: + type: velocity_controllers/IntegralSlidingModeController + + thruster_allocation_matrix_controller: + type: thruster_allocation_matrix_controller/ThrusterAllocationMatrixController + + thruster1_controller: + type: thruster_controllers/PolynomialThrustCurveController + + thruster2_controller: + type: thruster_controllers/PolynomialThrustCurveController + + thruster3_controller: + type: thruster_controllers/PolynomialThrustCurveController + + thruster4_controller: + type: thruster_controllers/PolynomialThrustCurveController + + thruster5_controller: + type: thruster_controllers/PolynomialThrustCurveController + + thruster6_controller: + type: thruster_controllers/PolynomialThrustCurveController + + thruster7_controller: + type: thruster_controllers/PolynomialThrustCurveController + + thruster8_controller: + type: thruster_controllers/PolynomialThrustCurveController + +integral_sliding_mode_controller: + ros__parameters: + use_external_measured_states: true + reference_controller: thruster_allocation_matrix_controller + gains: + rho: 20.0 + lambda: 200.0 + Kp: [4.0, 4.0, 9.0, 3.0, 3.0, 7.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] + +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 + min_thrust: -40.0 + max_thrust: 60.0 + min_deadband: 1470 + max_deadband: 1530 + neutral_pwm: 1500 + thrust_curve_coefficients: [1498.0, 12.01, -0.04731, -0.002098, 0.00002251] + +thruster2_controller: + ros__parameters: + thruster: thruster2_joint + min_thrust: -40.0 + max_thrust: 60.0 + min_deadband: 1470 + max_deadband: 1530 + neutral_pwm: 1500 + thrust_curve_coefficients: [1498.0, 12.01, -0.04731, -0.002098, 0.00002251] + +thruster3_controller: + ros__parameters: + thruster: thruster3_joint + min_thrust: -40.0 + max_thrust: 60.0 + min_deadband: 1470 + max_deadband: 1530 + neutral_pwm: 1500 + thrust_curve_coefficients: [1498.0, 12.01, -0.04731, -0.002098, 0.00002251] + +thruster4_controller: + ros__parameters: + thruster: thruster4_joint + min_thrust: -40.0 + max_thrust: 60.0 + min_deadband: 1470 + max_deadband: 1530 + neutral_pwm: 1500 + thrust_curve_coefficients: [1498.0, 12.01, -0.04731, -0.002098, 0.00002251] + +thruster5_controller: + ros__parameters: + thruster: thruster5_joint + min_thrust: -40.0 + max_thrust: 60.0 + min_deadband: 1470 + max_deadband: 1530 + neutral_pwm: 1500 + thrust_curve_coefficients: [1498.0, 12.01, -0.04731, -0.002098, 0.00002251] + +thruster6_controller: + ros__parameters: + thruster: thruster6_joint + min_thrust: -40.0 + max_thrust: 60.0 + min_deadband: 1470 + max_deadband: 1530 + neutral_pwm: 1500 + thrust_curve_coefficients: [1498.0, 12.01, -0.04731, -0.002098, 0.00002251] + +thruster7_controller: + ros__parameters: + thruster: thruster7_joint + min_thrust: -40.0 + max_thrust: 60.0 + min_deadband: 1470 + max_deadband: 1530 + neutral_pwm: 1500 + thrust_curve_coefficients: [1498.0, 12.01, -0.04731, -0.002098, 0.00002251] + +thruster8_controller: + ros__parameters: + thruster: thruster8_joint + min_thrust: -40.0 + max_thrust: 60.0 + min_deadband: 1470 + max_deadband: 1530 + neutral_pwm: 1500 + thrust_curve_coefficients: [1498.0, 12.01, -0.04731, -0.002098, 0.00002251] diff --git a/blue_demos/control_examples/config/bluerov2_heavy_reach_controllers.yaml b/blue_demos/control_examples/config/bluerov2_heavy_reach_controllers.yaml new file mode 100644 index 00000000..a8608511 --- /dev/null +++ b/blue_demos/control_examples/config/bluerov2_heavy_reach_controllers.yaml @@ -0,0 +1,163 @@ +controller_manager: + ros__parameters: + update_rate: 100 # Hz + + integral_sliding_mode_controller: + type: velocity_controllers/IntegralSlidingModeController + + thruster_allocation_matrix_controller: + type: thruster_allocation_matrix_controller/ThrusterAllocationMatrixController + + thruster1_controller: + type: thruster_controllers/PolynomialThrustCurveController + + thruster2_controller: + type: thruster_controllers/PolynomialThrustCurveController + + thruster3_controller: + type: thruster_controllers/PolynomialThrustCurveController + + thruster4_controller: + type: thruster_controllers/PolynomialThrustCurveController + + thruster5_controller: + type: thruster_controllers/PolynomialThrustCurveController + + thruster6_controller: + type: thruster_controllers/PolynomialThrustCurveController + + thruster7_controller: + type: thruster_controllers/PolynomialThrustCurveController + + thruster8_controller: + type: thruster_controllers/PolynomialThrustCurveController + +integral_sliding_mode_controller: + ros__parameters: + use_external_measured_states: true + reference_controller: thruster_allocation_matrix_controller + gains: + rho: 20.0 + lambda: 200.0 + Kp: [4.0, 4.0, 9.0, 3.0, 3.0, 7.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] + +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 + min_thrust: -40.0 + max_thrust: 60.0 + min_deadband: 1470 + max_deadband: 1530 + neutral_pwm: 1500 + thrust_curve_coefficients: [1498.0, 12.01, -0.04731, -0.002098, 0.00002251] + +thruster2_controller: + ros__parameters: + thruster: thruster2_joint + min_thrust: -40.0 + max_thrust: 60.0 + min_deadband: 1470 + max_deadband: 1530 + neutral_pwm: 1500 + thrust_curve_coefficients: [1498.0, 12.01, -0.04731, -0.002098, 0.00002251] + +thruster3_controller: + ros__parameters: + thruster: thruster3_joint + min_thrust: -40.0 + max_thrust: 60.0 + min_deadband: 1470 + max_deadband: 1530 + neutral_pwm: 1500 + thrust_curve_coefficients: [1498.0, 12.01, -0.04731, -0.002098, 0.00002251] + +thruster4_controller: + ros__parameters: + thruster: thruster4_joint + min_thrust: -40.0 + max_thrust: 60.0 + min_deadband: 1470 + max_deadband: 1530 + neutral_pwm: 1500 + thrust_curve_coefficients: [1498.0, 12.01, -0.04731, -0.002098, 0.00002251] + +thruster5_controller: + ros__parameters: + thruster: thruster5_joint + min_thrust: -40.0 + max_thrust: 60.0 + min_deadband: 1470 + max_deadband: 1530 + neutral_pwm: 1500 + thrust_curve_coefficients: [1498.0, 12.01, -0.04731, -0.002098, 0.00002251] + +thruster6_controller: + ros__parameters: + thruster: thruster6_joint + min_thrust: -40.0 + max_thrust: 60.0 + min_deadband: 1470 + max_deadband: 1530 + neutral_pwm: 1500 + thrust_curve_coefficients: [1498.0, 12.01, -0.04731, -0.002098, 0.00002251] + +thruster7_controller: + ros__parameters: + thruster: thruster7_joint + min_thrust: -40.0 + max_thrust: 60.0 + min_deadband: 1470 + max_deadband: 1530 + neutral_pwm: 1500 + thrust_curve_coefficients: [1498.0, 12.01, -0.04731, -0.002098, 0.00002251] + +thruster8_controller: + ros__parameters: + thruster: thruster8_joint + min_thrust: -40.0 + max_thrust: 60.0 + min_deadband: 1470 + max_deadband: 1530 + neutral_pwm: 1500 + thrust_curve_coefficients: [1498.0, 12.01, -0.04731, -0.002098, 0.00002251] diff --git a/blue_demos/control_examples/launch/bluerov2_control.launch.py b/blue_demos/control_examples/launch/bluerov2_control.launch.py new file mode 100644 index 00000000..fa8d87b1 --- /dev/null +++ b/blue_demos/control_examples/launch/bluerov2_control.launch.py @@ -0,0 +1,185 @@ +# Copyright 2024, 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, RegisterEventHandler +from launch.event_handlers import OnProcessExit +from launch.substitutions import ( + Command, + FindExecutable, + LaunchConfiguration, + PathJoinSubstitution, +) +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare + + +def generate_launch_description() -> LaunchDescription: + """Generate a launch description for the BlueROV2. + + This should be launched after MAVROS has fully loaded. + """ + args = [ + DeclareLaunchArgument( + "prefix", + default_value="", + description=( + "The prefix of the model. This is useful for multi-robot setups." + " Expected format '/'." + ), + ), + DeclareLaunchArgument( + "use_sim", + default_value="false", + description="Launch the Gazebo + ArduSub simulator.", + ), + ] + + robot_description_content = Command( + [ + PathJoinSubstitution([FindExecutable(name="xacro")]), + " ", + PathJoinSubstitution( + [ + FindPackageShare("blue_description"), + "xacro", + "bluerov2", + "config.xacro", + ] + ), + " ", + "prefix:=", + LaunchConfiguration("prefix"), + " ", + "use_sim:=", + LaunchConfiguration("use_sim"), + ] + ) + robot_description = {"robot_description": robot_description_content} + + # The ISMC expects state information to be provided in the FSD frame + mobile_to_maritime_velocity_state = Node( + package="mobile_to_maritime", + executable="mobile_twist_stamped_to_maritime_twist", + name="velocity_state_transform", + parameters=[ + { + "in_topic": "/mavros/local_position/velocity_body", + "out_topic": "/integral_sliding_mode_controller/system_state", + "qos_reliability": "best_effort", + "qos_durability": "volatile", + } + ], + ) + + controller_manager = Node( + package="controller_manager", + executable="ros2_control_node", + output="both", + parameters=[ + robot_description, + PathJoinSubstitution( + [ + FindPackageShare("blue_description"), + "config", + "bluerov2", + "controllers.yaml", + ] + ), + ], + ) + + velocity_controller_spawner = Node( + package="controller_manager", + executable="spawner", + arguments=[ + "integral_sliding_mode_controller", + "--controller-manager", + ["", "controller_manager"], + ], + ) + + thruster_spawners = [ + Node( + package="controller_manager", + executable="spawner", + arguments=[ + f"thruster{i + 1}_controller", + "--controller-manager", + ["", "controller_manager"], + ], + ) + for i in range(6) + ] + + delay_thruster_spawners = [] + for i, thruster_spawner in enumerate(thruster_spawners): + if not len(delay_thruster_spawners): + delay_thruster_spawners.append( + thruster_spawner, + ) + else: + delay_thruster_spawners.append( + RegisterEventHandler( + event_handler=OnProcessExit( + target_action=thruster_spawners[i - 1], + on_exit=[thruster_spawner], + ) + ) + ) + + tam_controller_spawner = Node( + package="controller_manager", + executable="spawner", + arguments=[ + "thruster_allocation_matrix_controller", + "--controller-manager", + ["", "controller_manager"], + ], + ) + + delay_tam_controller_spawner_after_thruster_controller_spawners = ( + RegisterEventHandler( + event_handler=OnProcessExit( + target_action=thruster_spawners[-1], + on_exit=[tam_controller_spawner], + ) + ) + ) + + delay_velocity_controller_spawner_after_tam_controller_spawner = ( + RegisterEventHandler( + event_handler=OnProcessExit( + target_action=tam_controller_spawner, + on_exit=[velocity_controller_spawner], + ) + ) + ) + + return LaunchDescription( + [ + *args, + mobile_to_maritime_velocity_state, + controller_manager, + *delay_thruster_spawners, + delay_tam_controller_spawner_after_thruster_controller_spawners, + delay_velocity_controller_spawner_after_tam_controller_spawner, + ] + ) diff --git a/blue_bringup/launch/bluerov2_heavy/bluerov2_heavy_control.launch.py b/blue_demos/control_examples/launch/bluerov2_heavy_control.launch.py similarity index 100% rename from blue_bringup/launch/bluerov2_heavy/bluerov2_heavy_control.launch.py rename to blue_demos/control_examples/launch/bluerov2_heavy_control.launch.py diff --git a/blue_bringup/launch/bluerov2_heavy_reach/bluerov2_heavy_reach_control.launch.py b/blue_demos/control_examples/launch/bluerov2_heavy_reach_control.launch.py similarity index 98% rename from blue_bringup/launch/bluerov2_heavy_reach/bluerov2_heavy_reach_control.launch.py rename to blue_demos/control_examples/launch/bluerov2_heavy_reach_control.launch.py index 183ac49d..7de5d602 100644 --- a/blue_bringup/launch/bluerov2_heavy_reach/bluerov2_heavy_reach_control.launch.py +++ b/blue_demos/control_examples/launch/bluerov2_heavy_reach_control.launch.py @@ -32,7 +32,7 @@ def generate_launch_description() -> LaunchDescription: - """Generate a launch description for the BlueROV2 Heavy. + """Generate a launch description for the BlueROV2 Heavy Reach configuration. This should be launched after MAVROS has fully loaded. """ diff --git a/blue_demos/hardware_examples/README.md b/blue_demos/hardware_examples/README.md new file mode 100644 index 00000000..e69de29b diff --git a/blue_demos/package.xml b/blue_demos/package.xml new file mode 100644 index 00000000..103316db --- /dev/null +++ b/blue_demos/package.xml @@ -0,0 +1,18 @@ + + + + blue_demos + 0.0.0 + TODO: Package description + blue + MIT + + ament_cmake + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/blue_demos/teleop_examples/README.md b/blue_demos/teleop_examples/README.md new file mode 100644 index 00000000..e69de29b From 2c2dce3b899e674a8be3e8c264a2bcd5b0941875 Mon Sep 17 00:00:00 2001 From: Evan Palmer Date: Mon, 1 Apr 2024 14:53:01 -0700 Subject: [PATCH 11/38] Waiting to add demos until i add documentation --- .../bluerov2/bluerov2_control.launch.py | 185 ------------------ blue_demos/CMakeLists.txt | 26 --- blue_demos/LICENSE | 17 -- blue_demos/control_examples/README.md | 0 .../config/bluerov2_controllers.yaml | 133 ------------- .../config/bluerov2_heavy_controllers.yaml | 163 --------------- .../bluerov2_heavy_reach_controllers.yaml | 163 --------------- .../launch/bluerov2_control.launch.py | 185 ------------------ .../launch/bluerov2_heavy_control.launch.py | 185 ------------------ .../bluerov2_heavy_reach_control.launch.py | 185 ------------------ blue_demos/hardware_examples/README.md | 0 blue_demos/package.xml | 18 -- blue_demos/teleop_examples/README.md | 0 .../config/bluerov2/controllers.yaml | 133 ------------- .../config/bluerov2_heavy/controllers.yaml | 163 --------------- .../bluerov2_heavy_reach/controllers.yaml | 163 --------------- blue_description/config/joy_teleop.yaml | 133 ------------- blue_joy/LICENSE | 17 -- blue_joy/blue_joy/__init__.py | 0 blue_joy/blue_joy/joy.py | 160 --------------- blue_joy/launch/joy.launch.py | 95 --------- blue_joy/package.xml | 29 --- blue_joy/resource/blue_joy | 0 blue_joy/setup.cfg | 4 - blue_joy/setup.py | 45 ----- blue_joy/test/test_copyright.py | 27 --- 26 files changed, 2229 deletions(-) delete mode 100644 blue_bringup/launch/bluerov2/bluerov2_control.launch.py delete mode 100644 blue_demos/CMakeLists.txt delete mode 100644 blue_demos/LICENSE delete mode 100644 blue_demos/control_examples/README.md delete mode 100644 blue_demos/control_examples/config/bluerov2_controllers.yaml delete mode 100644 blue_demos/control_examples/config/bluerov2_heavy_controllers.yaml delete mode 100644 blue_demos/control_examples/config/bluerov2_heavy_reach_controllers.yaml delete mode 100644 blue_demos/control_examples/launch/bluerov2_control.launch.py delete mode 100644 blue_demos/control_examples/launch/bluerov2_heavy_control.launch.py delete mode 100644 blue_demos/control_examples/launch/bluerov2_heavy_reach_control.launch.py delete mode 100644 blue_demos/hardware_examples/README.md delete mode 100644 blue_demos/package.xml delete mode 100644 blue_demos/teleop_examples/README.md delete mode 100644 blue_description/config/bluerov2/controllers.yaml delete mode 100644 blue_description/config/bluerov2_heavy/controllers.yaml delete mode 100644 blue_description/config/bluerov2_heavy_reach/controllers.yaml delete mode 100644 blue_description/config/joy_teleop.yaml delete mode 100644 blue_joy/LICENSE delete mode 100644 blue_joy/blue_joy/__init__.py delete mode 100644 blue_joy/blue_joy/joy.py delete mode 100644 blue_joy/launch/joy.launch.py delete mode 100644 blue_joy/package.xml delete mode 100644 blue_joy/resource/blue_joy delete mode 100644 blue_joy/setup.cfg delete mode 100644 blue_joy/setup.py delete mode 100644 blue_joy/test/test_copyright.py diff --git a/blue_bringup/launch/bluerov2/bluerov2_control.launch.py b/blue_bringup/launch/bluerov2/bluerov2_control.launch.py deleted file mode 100644 index fa8d87b1..00000000 --- a/blue_bringup/launch/bluerov2/bluerov2_control.launch.py +++ /dev/null @@ -1,185 +0,0 @@ -# Copyright 2024, 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, RegisterEventHandler -from launch.event_handlers import OnProcessExit -from launch.substitutions import ( - Command, - FindExecutable, - LaunchConfiguration, - PathJoinSubstitution, -) -from launch_ros.actions import Node -from launch_ros.substitutions import FindPackageShare - - -def generate_launch_description() -> LaunchDescription: - """Generate a launch description for the BlueROV2. - - This should be launched after MAVROS has fully loaded. - """ - args = [ - DeclareLaunchArgument( - "prefix", - default_value="", - description=( - "The prefix of the model. This is useful for multi-robot setups." - " Expected format '/'." - ), - ), - DeclareLaunchArgument( - "use_sim", - default_value="false", - description="Launch the Gazebo + ArduSub simulator.", - ), - ] - - robot_description_content = Command( - [ - PathJoinSubstitution([FindExecutable(name="xacro")]), - " ", - PathJoinSubstitution( - [ - FindPackageShare("blue_description"), - "xacro", - "bluerov2", - "config.xacro", - ] - ), - " ", - "prefix:=", - LaunchConfiguration("prefix"), - " ", - "use_sim:=", - LaunchConfiguration("use_sim"), - ] - ) - robot_description = {"robot_description": robot_description_content} - - # The ISMC expects state information to be provided in the FSD frame - mobile_to_maritime_velocity_state = Node( - package="mobile_to_maritime", - executable="mobile_twist_stamped_to_maritime_twist", - name="velocity_state_transform", - parameters=[ - { - "in_topic": "/mavros/local_position/velocity_body", - "out_topic": "/integral_sliding_mode_controller/system_state", - "qos_reliability": "best_effort", - "qos_durability": "volatile", - } - ], - ) - - controller_manager = Node( - package="controller_manager", - executable="ros2_control_node", - output="both", - parameters=[ - robot_description, - PathJoinSubstitution( - [ - FindPackageShare("blue_description"), - "config", - "bluerov2", - "controllers.yaml", - ] - ), - ], - ) - - velocity_controller_spawner = Node( - package="controller_manager", - executable="spawner", - arguments=[ - "integral_sliding_mode_controller", - "--controller-manager", - ["", "controller_manager"], - ], - ) - - thruster_spawners = [ - Node( - package="controller_manager", - executable="spawner", - arguments=[ - f"thruster{i + 1}_controller", - "--controller-manager", - ["", "controller_manager"], - ], - ) - for i in range(6) - ] - - delay_thruster_spawners = [] - for i, thruster_spawner in enumerate(thruster_spawners): - if not len(delay_thruster_spawners): - delay_thruster_spawners.append( - thruster_spawner, - ) - else: - delay_thruster_spawners.append( - RegisterEventHandler( - event_handler=OnProcessExit( - target_action=thruster_spawners[i - 1], - on_exit=[thruster_spawner], - ) - ) - ) - - tam_controller_spawner = Node( - package="controller_manager", - executable="spawner", - arguments=[ - "thruster_allocation_matrix_controller", - "--controller-manager", - ["", "controller_manager"], - ], - ) - - delay_tam_controller_spawner_after_thruster_controller_spawners = ( - RegisterEventHandler( - event_handler=OnProcessExit( - target_action=thruster_spawners[-1], - on_exit=[tam_controller_spawner], - ) - ) - ) - - delay_velocity_controller_spawner_after_tam_controller_spawner = ( - RegisterEventHandler( - event_handler=OnProcessExit( - target_action=tam_controller_spawner, - on_exit=[velocity_controller_spawner], - ) - ) - ) - - return LaunchDescription( - [ - *args, - mobile_to_maritime_velocity_state, - controller_manager, - *delay_thruster_spawners, - delay_tam_controller_spawner_after_thruster_controller_spawners, - delay_velocity_controller_spawner_after_tam_controller_spawner, - ] - ) diff --git a/blue_demos/CMakeLists.txt b/blue_demos/CMakeLists.txt deleted file mode 100644 index cc5f0056..00000000 --- a/blue_demos/CMakeLists.txt +++ /dev/null @@ -1,26 +0,0 @@ -cmake_minimum_required(VERSION 3.8) -project(blue_demos) - -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic) -endif() - -# find dependencies -find_package(ament_cmake REQUIRED) -# uncomment the following section in order to fill in -# further dependencies manually. -# find_package( REQUIRED) - -if(BUILD_TESTING) - find_package(ament_lint_auto REQUIRED) - # the following line skips the linter which checks for copyrights - # comment the line when a copyright and license is added to all source files - set(ament_cmake_copyright_FOUND TRUE) - # the following line skips cpplint (only works in a git repo) - # comment the line when this package is in a git repo and when - # a copyright and license is added to all source files - set(ament_cmake_cpplint_FOUND TRUE) - ament_lint_auto_find_test_dependencies() -endif() - -ament_package() diff --git a/blue_demos/LICENSE b/blue_demos/LICENSE deleted file mode 100644 index 30e8e2ec..00000000 --- a/blue_demos/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_demos/control_examples/README.md b/blue_demos/control_examples/README.md deleted file mode 100644 index e69de29b..00000000 diff --git a/blue_demos/control_examples/config/bluerov2_controllers.yaml b/blue_demos/control_examples/config/bluerov2_controllers.yaml deleted file mode 100644 index f6076d1a..00000000 --- a/blue_demos/control_examples/config/bluerov2_controllers.yaml +++ /dev/null @@ -1,133 +0,0 @@ -controller_manager: - ros__parameters: - update_rate: 100 # Hz - - integral_sliding_mode_controller: - type: velocity_controllers/IntegralSlidingModeController - - thruster_allocation_matrix_controller: - type: thruster_allocation_matrix_controller/ThrusterAllocationMatrixController - - thruster1_controller: - type: thruster_controllers/PolynomialThrustCurveController - - thruster2_controller: - type: thruster_controllers/PolynomialThrustCurveController - - thruster3_controller: - type: thruster_controllers/PolynomialThrustCurveController - - thruster4_controller: - type: thruster_controllers/PolynomialThrustCurveController - - thruster5_controller: - type: thruster_controllers/PolynomialThrustCurveController - - thruster6_controller: - type: thruster_controllers/PolynomialThrustCurveController - -integral_sliding_mode_controller: - ros__parameters: - use_external_measured_states: true - reference_controller: thruster_allocation_matrix_controller - gains: - rho: 20.0 - lambda: 200.0 - Kp: [4.0, 4.0, 9.0, 3.0, 3.0, 7.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] - -thruster_allocation_matrix_controller: - ros__parameters: - thrusters: - - thruster1_joint - - thruster2_joint - - thruster3_joint - - thruster4_joint - - thruster5_joint - - thruster6_joint - reference_controllers: - - thruster1_controller - - thruster2_controller - - thruster3_controller - - thruster4_controller - - thruster5_controller - - thruster6_controller - tam: - x: [ -0.707, -0.707, 0.707, 0.707, 0.0, 0.0] - y: [ 0.707, -0.707, 0.707, -0.707, 0.0, 0.0] - z: [ 0.0, 0.0, 0.0, 0.0, 1.0, 1.0] - rx: [ 0.0, 0.0, 0.0, 0.0, -0.21805, 0.21805] - ry: [ 0.0, 0.0, 0.0, 0.0, -0.12, -0.12] - rz: [0.1888, -0.1888, -0.1888, 0.1888, 0.0, 0.0] - -thruster1_controller: - ros__parameters: - thruster: thruster1_joint - min_thrust: -40.0 - max_thrust: 60.0 - min_deadband: 1470 - max_deadband: 1530 - neutral_pwm: 1500 - thrust_curve_coefficients: [1498.0, 12.01, -0.04731, -0.002098, 0.00002251] - -thruster2_controller: - ros__parameters: - thruster: thruster2_joint - min_thrust: -40.0 - max_thrust: 60.0 - min_deadband: 1470 - max_deadband: 1530 - neutral_pwm: 1500 - thrust_curve_coefficients: [1498.0, 12.01, -0.04731, -0.002098, 0.00002251] - -thruster3_controller: - ros__parameters: - thruster: thruster3_joint - min_thrust: -40.0 - max_thrust: 60.0 - min_deadband: 1470 - max_deadband: 1530 - neutral_pwm: 1500 - thrust_curve_coefficients: [1498.0, 12.01, -0.04731, -0.002098, 0.00002251] - -thruster4_controller: - ros__parameters: - thruster: thruster4_joint - min_thrust: -40.0 - max_thrust: 60.0 - min_deadband: 1470 - max_deadband: 1530 - neutral_pwm: 1500 - thrust_curve_coefficients: [1498.0, 12.01, -0.04731, -0.002098, 0.00002251] - -thruster5_controller: - ros__parameters: - thruster: thruster5_joint - min_thrust: -40.0 - max_thrust: 60.0 - min_deadband: 1470 - max_deadband: 1530 - neutral_pwm: 1500 - thrust_curve_coefficients: [1498.0, 12.01, -0.04731, -0.002098, 0.00002251] - -thruster6_controller: - ros__parameters: - thruster: thruster6_joint - min_thrust: -40.0 - max_thrust: 60.0 - min_deadband: 1470 - max_deadband: 1530 - neutral_pwm: 1500 - thrust_curve_coefficients: [1498.0, 12.01, -0.04731, -0.002098, 0.00002251] diff --git a/blue_demos/control_examples/config/bluerov2_heavy_controllers.yaml b/blue_demos/control_examples/config/bluerov2_heavy_controllers.yaml deleted file mode 100644 index a8608511..00000000 --- a/blue_demos/control_examples/config/bluerov2_heavy_controllers.yaml +++ /dev/null @@ -1,163 +0,0 @@ -controller_manager: - ros__parameters: - update_rate: 100 # Hz - - integral_sliding_mode_controller: - type: velocity_controllers/IntegralSlidingModeController - - thruster_allocation_matrix_controller: - type: thruster_allocation_matrix_controller/ThrusterAllocationMatrixController - - thruster1_controller: - type: thruster_controllers/PolynomialThrustCurveController - - thruster2_controller: - type: thruster_controllers/PolynomialThrustCurveController - - thruster3_controller: - type: thruster_controllers/PolynomialThrustCurveController - - thruster4_controller: - type: thruster_controllers/PolynomialThrustCurveController - - thruster5_controller: - type: thruster_controllers/PolynomialThrustCurveController - - thruster6_controller: - type: thruster_controllers/PolynomialThrustCurveController - - thruster7_controller: - type: thruster_controllers/PolynomialThrustCurveController - - thruster8_controller: - type: thruster_controllers/PolynomialThrustCurveController - -integral_sliding_mode_controller: - ros__parameters: - use_external_measured_states: true - reference_controller: thruster_allocation_matrix_controller - gains: - rho: 20.0 - lambda: 200.0 - Kp: [4.0, 4.0, 9.0, 3.0, 3.0, 7.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] - -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 - min_thrust: -40.0 - max_thrust: 60.0 - min_deadband: 1470 - max_deadband: 1530 - neutral_pwm: 1500 - thrust_curve_coefficients: [1498.0, 12.01, -0.04731, -0.002098, 0.00002251] - -thruster2_controller: - ros__parameters: - thruster: thruster2_joint - min_thrust: -40.0 - max_thrust: 60.0 - min_deadband: 1470 - max_deadband: 1530 - neutral_pwm: 1500 - thrust_curve_coefficients: [1498.0, 12.01, -0.04731, -0.002098, 0.00002251] - -thruster3_controller: - ros__parameters: - thruster: thruster3_joint - min_thrust: -40.0 - max_thrust: 60.0 - min_deadband: 1470 - max_deadband: 1530 - neutral_pwm: 1500 - thrust_curve_coefficients: [1498.0, 12.01, -0.04731, -0.002098, 0.00002251] - -thruster4_controller: - ros__parameters: - thruster: thruster4_joint - min_thrust: -40.0 - max_thrust: 60.0 - min_deadband: 1470 - max_deadband: 1530 - neutral_pwm: 1500 - thrust_curve_coefficients: [1498.0, 12.01, -0.04731, -0.002098, 0.00002251] - -thruster5_controller: - ros__parameters: - thruster: thruster5_joint - min_thrust: -40.0 - max_thrust: 60.0 - min_deadband: 1470 - max_deadband: 1530 - neutral_pwm: 1500 - thrust_curve_coefficients: [1498.0, 12.01, -0.04731, -0.002098, 0.00002251] - -thruster6_controller: - ros__parameters: - thruster: thruster6_joint - min_thrust: -40.0 - max_thrust: 60.0 - min_deadband: 1470 - max_deadband: 1530 - neutral_pwm: 1500 - thrust_curve_coefficients: [1498.0, 12.01, -0.04731, -0.002098, 0.00002251] - -thruster7_controller: - ros__parameters: - thruster: thruster7_joint - min_thrust: -40.0 - max_thrust: 60.0 - min_deadband: 1470 - max_deadband: 1530 - neutral_pwm: 1500 - thrust_curve_coefficients: [1498.0, 12.01, -0.04731, -0.002098, 0.00002251] - -thruster8_controller: - ros__parameters: - thruster: thruster8_joint - min_thrust: -40.0 - max_thrust: 60.0 - min_deadband: 1470 - max_deadband: 1530 - neutral_pwm: 1500 - thrust_curve_coefficients: [1498.0, 12.01, -0.04731, -0.002098, 0.00002251] diff --git a/blue_demos/control_examples/config/bluerov2_heavy_reach_controllers.yaml b/blue_demos/control_examples/config/bluerov2_heavy_reach_controllers.yaml deleted file mode 100644 index a8608511..00000000 --- a/blue_demos/control_examples/config/bluerov2_heavy_reach_controllers.yaml +++ /dev/null @@ -1,163 +0,0 @@ -controller_manager: - ros__parameters: - update_rate: 100 # Hz - - integral_sliding_mode_controller: - type: velocity_controllers/IntegralSlidingModeController - - thruster_allocation_matrix_controller: - type: thruster_allocation_matrix_controller/ThrusterAllocationMatrixController - - thruster1_controller: - type: thruster_controllers/PolynomialThrustCurveController - - thruster2_controller: - type: thruster_controllers/PolynomialThrustCurveController - - thruster3_controller: - type: thruster_controllers/PolynomialThrustCurveController - - thruster4_controller: - type: thruster_controllers/PolynomialThrustCurveController - - thruster5_controller: - type: thruster_controllers/PolynomialThrustCurveController - - thruster6_controller: - type: thruster_controllers/PolynomialThrustCurveController - - thruster7_controller: - type: thruster_controllers/PolynomialThrustCurveController - - thruster8_controller: - type: thruster_controllers/PolynomialThrustCurveController - -integral_sliding_mode_controller: - ros__parameters: - use_external_measured_states: true - reference_controller: thruster_allocation_matrix_controller - gains: - rho: 20.0 - lambda: 200.0 - Kp: [4.0, 4.0, 9.0, 3.0, 3.0, 7.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] - -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 - min_thrust: -40.0 - max_thrust: 60.0 - min_deadband: 1470 - max_deadband: 1530 - neutral_pwm: 1500 - thrust_curve_coefficients: [1498.0, 12.01, -0.04731, -0.002098, 0.00002251] - -thruster2_controller: - ros__parameters: - thruster: thruster2_joint - min_thrust: -40.0 - max_thrust: 60.0 - min_deadband: 1470 - max_deadband: 1530 - neutral_pwm: 1500 - thrust_curve_coefficients: [1498.0, 12.01, -0.04731, -0.002098, 0.00002251] - -thruster3_controller: - ros__parameters: - thruster: thruster3_joint - min_thrust: -40.0 - max_thrust: 60.0 - min_deadband: 1470 - max_deadband: 1530 - neutral_pwm: 1500 - thrust_curve_coefficients: [1498.0, 12.01, -0.04731, -0.002098, 0.00002251] - -thruster4_controller: - ros__parameters: - thruster: thruster4_joint - min_thrust: -40.0 - max_thrust: 60.0 - min_deadband: 1470 - max_deadband: 1530 - neutral_pwm: 1500 - thrust_curve_coefficients: [1498.0, 12.01, -0.04731, -0.002098, 0.00002251] - -thruster5_controller: - ros__parameters: - thruster: thruster5_joint - min_thrust: -40.0 - max_thrust: 60.0 - min_deadband: 1470 - max_deadband: 1530 - neutral_pwm: 1500 - thrust_curve_coefficients: [1498.0, 12.01, -0.04731, -0.002098, 0.00002251] - -thruster6_controller: - ros__parameters: - thruster: thruster6_joint - min_thrust: -40.0 - max_thrust: 60.0 - min_deadband: 1470 - max_deadband: 1530 - neutral_pwm: 1500 - thrust_curve_coefficients: [1498.0, 12.01, -0.04731, -0.002098, 0.00002251] - -thruster7_controller: - ros__parameters: - thruster: thruster7_joint - min_thrust: -40.0 - max_thrust: 60.0 - min_deadband: 1470 - max_deadband: 1530 - neutral_pwm: 1500 - thrust_curve_coefficients: [1498.0, 12.01, -0.04731, -0.002098, 0.00002251] - -thruster8_controller: - ros__parameters: - thruster: thruster8_joint - min_thrust: -40.0 - max_thrust: 60.0 - min_deadband: 1470 - max_deadband: 1530 - neutral_pwm: 1500 - thrust_curve_coefficients: [1498.0, 12.01, -0.04731, -0.002098, 0.00002251] diff --git a/blue_demos/control_examples/launch/bluerov2_control.launch.py b/blue_demos/control_examples/launch/bluerov2_control.launch.py deleted file mode 100644 index fa8d87b1..00000000 --- a/blue_demos/control_examples/launch/bluerov2_control.launch.py +++ /dev/null @@ -1,185 +0,0 @@ -# Copyright 2024, 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, RegisterEventHandler -from launch.event_handlers import OnProcessExit -from launch.substitutions import ( - Command, - FindExecutable, - LaunchConfiguration, - PathJoinSubstitution, -) -from launch_ros.actions import Node -from launch_ros.substitutions import FindPackageShare - - -def generate_launch_description() -> LaunchDescription: - """Generate a launch description for the BlueROV2. - - This should be launched after MAVROS has fully loaded. - """ - args = [ - DeclareLaunchArgument( - "prefix", - default_value="", - description=( - "The prefix of the model. This is useful for multi-robot setups." - " Expected format '/'." - ), - ), - DeclareLaunchArgument( - "use_sim", - default_value="false", - description="Launch the Gazebo + ArduSub simulator.", - ), - ] - - robot_description_content = Command( - [ - PathJoinSubstitution([FindExecutable(name="xacro")]), - " ", - PathJoinSubstitution( - [ - FindPackageShare("blue_description"), - "xacro", - "bluerov2", - "config.xacro", - ] - ), - " ", - "prefix:=", - LaunchConfiguration("prefix"), - " ", - "use_sim:=", - LaunchConfiguration("use_sim"), - ] - ) - robot_description = {"robot_description": robot_description_content} - - # The ISMC expects state information to be provided in the FSD frame - mobile_to_maritime_velocity_state = Node( - package="mobile_to_maritime", - executable="mobile_twist_stamped_to_maritime_twist", - name="velocity_state_transform", - parameters=[ - { - "in_topic": "/mavros/local_position/velocity_body", - "out_topic": "/integral_sliding_mode_controller/system_state", - "qos_reliability": "best_effort", - "qos_durability": "volatile", - } - ], - ) - - controller_manager = Node( - package="controller_manager", - executable="ros2_control_node", - output="both", - parameters=[ - robot_description, - PathJoinSubstitution( - [ - FindPackageShare("blue_description"), - "config", - "bluerov2", - "controllers.yaml", - ] - ), - ], - ) - - velocity_controller_spawner = Node( - package="controller_manager", - executable="spawner", - arguments=[ - "integral_sliding_mode_controller", - "--controller-manager", - ["", "controller_manager"], - ], - ) - - thruster_spawners = [ - Node( - package="controller_manager", - executable="spawner", - arguments=[ - f"thruster{i + 1}_controller", - "--controller-manager", - ["", "controller_manager"], - ], - ) - for i in range(6) - ] - - delay_thruster_spawners = [] - for i, thruster_spawner in enumerate(thruster_spawners): - if not len(delay_thruster_spawners): - delay_thruster_spawners.append( - thruster_spawner, - ) - else: - delay_thruster_spawners.append( - RegisterEventHandler( - event_handler=OnProcessExit( - target_action=thruster_spawners[i - 1], - on_exit=[thruster_spawner], - ) - ) - ) - - tam_controller_spawner = Node( - package="controller_manager", - executable="spawner", - arguments=[ - "thruster_allocation_matrix_controller", - "--controller-manager", - ["", "controller_manager"], - ], - ) - - delay_tam_controller_spawner_after_thruster_controller_spawners = ( - RegisterEventHandler( - event_handler=OnProcessExit( - target_action=thruster_spawners[-1], - on_exit=[tam_controller_spawner], - ) - ) - ) - - delay_velocity_controller_spawner_after_tam_controller_spawner = ( - RegisterEventHandler( - event_handler=OnProcessExit( - target_action=tam_controller_spawner, - on_exit=[velocity_controller_spawner], - ) - ) - ) - - return LaunchDescription( - [ - *args, - mobile_to_maritime_velocity_state, - controller_manager, - *delay_thruster_spawners, - delay_tam_controller_spawner_after_thruster_controller_spawners, - delay_velocity_controller_spawner_after_tam_controller_spawner, - ] - ) diff --git a/blue_demos/control_examples/launch/bluerov2_heavy_control.launch.py b/blue_demos/control_examples/launch/bluerov2_heavy_control.launch.py deleted file mode 100644 index b93aeb79..00000000 --- a/blue_demos/control_examples/launch/bluerov2_heavy_control.launch.py +++ /dev/null @@ -1,185 +0,0 @@ -# Copyright 2024, 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, RegisterEventHandler -from launch.event_handlers import OnProcessExit -from launch.substitutions import ( - Command, - FindExecutable, - LaunchConfiguration, - PathJoinSubstitution, -) -from launch_ros.actions import Node -from launch_ros.substitutions import FindPackageShare - - -def generate_launch_description() -> LaunchDescription: - """Generate a launch description for the BlueROV2 Heavy. - - This should be launched after MAVROS has fully loaded. - """ - args = [ - DeclareLaunchArgument( - "prefix", - default_value="", - description=( - "The prefix of the model. This is useful for multi-robot setups." - " Expected format '/'." - ), - ), - DeclareLaunchArgument( - "use_sim", - default_value="false", - description="Launch the Gazebo + ArduSub simulator.", - ), - ] - - robot_description_content = Command( - [ - PathJoinSubstitution([FindExecutable(name="xacro")]), - " ", - PathJoinSubstitution( - [ - FindPackageShare("blue_description"), - "xacro", - "bluerov2_heavy", - "config.xacro", - ] - ), - " ", - "prefix:=", - LaunchConfiguration("prefix"), - " ", - "use_sim:=", - LaunchConfiguration("use_sim"), - ] - ) - robot_description = {"robot_description": robot_description_content} - - # The ISMC expects state information to be provided in the FSD frame - mobile_to_maritime_velocity_state = Node( - package="mobile_to_maritime", - executable="mobile_twist_stamped_to_maritime_twist", - name="velocity_state_transform", - parameters=[ - { - "in_topic": "/mavros/local_position/velocity_body", - "out_topic": "/integral_sliding_mode_controller/system_state", - "qos_reliability": "best_effort", - "qos_durability": "volatile", - } - ], - ) - - controller_manager = Node( - package="controller_manager", - executable="ros2_control_node", - output="both", - parameters=[ - robot_description, - PathJoinSubstitution( - [ - FindPackageShare("blue_description"), - "config", - "bluerov2_heavy", - "controllers.yaml", - ] - ), - ], - ) - - velocity_controller_spawner = Node( - package="controller_manager", - executable="spawner", - arguments=[ - "integral_sliding_mode_controller", - "--controller-manager", - ["", "controller_manager"], - ], - ) - - thruster_spawners = [ - Node( - package="controller_manager", - executable="spawner", - arguments=[ - f"thruster{i + 1}_controller", - "--controller-manager", - ["", "controller_manager"], - ], - ) - for i in range(8) - ] - - delay_thruster_spawners = [] - for i, thruster_spawner in enumerate(thruster_spawners): - if not len(delay_thruster_spawners): - delay_thruster_spawners.append( - thruster_spawner, - ) - else: - delay_thruster_spawners.append( - RegisterEventHandler( - event_handler=OnProcessExit( - target_action=thruster_spawners[i - 1], - on_exit=[thruster_spawner], - ) - ) - ) - - tam_controller_spawner = Node( - package="controller_manager", - executable="spawner", - arguments=[ - "thruster_allocation_matrix_controller", - "--controller-manager", - ["", "controller_manager"], - ], - ) - - delay_tam_controller_spawner_after_thruster_controller_spawners = ( - RegisterEventHandler( - event_handler=OnProcessExit( - target_action=thruster_spawners[-1], - on_exit=[tam_controller_spawner], - ) - ) - ) - - delay_velocity_controller_spawner_after_tam_controller_spawner = ( - RegisterEventHandler( - event_handler=OnProcessExit( - target_action=tam_controller_spawner, - on_exit=[velocity_controller_spawner], - ) - ) - ) - - return LaunchDescription( - [ - *args, - mobile_to_maritime_velocity_state, - controller_manager, - *delay_thruster_spawners, - delay_tam_controller_spawner_after_thruster_controller_spawners, - delay_velocity_controller_spawner_after_tam_controller_spawner, - ] - ) diff --git a/blue_demos/control_examples/launch/bluerov2_heavy_reach_control.launch.py b/blue_demos/control_examples/launch/bluerov2_heavy_reach_control.launch.py deleted file mode 100644 index 7de5d602..00000000 --- a/blue_demos/control_examples/launch/bluerov2_heavy_reach_control.launch.py +++ /dev/null @@ -1,185 +0,0 @@ -# Copyright 2024, 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, RegisterEventHandler -from launch.event_handlers import OnProcessExit -from launch.substitutions import ( - Command, - FindExecutable, - LaunchConfiguration, - PathJoinSubstitution, -) -from launch_ros.actions import Node -from launch_ros.substitutions import FindPackageShare - - -def generate_launch_description() -> LaunchDescription: - """Generate a launch description for the BlueROV2 Heavy Reach configuration. - - This should be launched after MAVROS has fully loaded. - """ - args = [ - DeclareLaunchArgument( - "prefix", - default_value="", - description=( - "The prefix of the model. This is useful for multi-robot setups." - " Expected format '/'." - ), - ), - DeclareLaunchArgument( - "use_sim", - default_value="false", - description="Launch the Gazebo + ArduSub simulator.", - ), - ] - - robot_description_content = Command( - [ - PathJoinSubstitution([FindExecutable(name="xacro")]), - " ", - PathJoinSubstitution( - [ - FindPackageShare("blue_description"), - "xacro", - "bluerov2_heavy_reach", - "config.xacro", - ] - ), - " ", - "prefix:=", - LaunchConfiguration("prefix"), - " ", - "use_sim:=", - LaunchConfiguration("use_sim"), - ] - ) - robot_description = {"robot_description": robot_description_content} - - # The ISMC expects state information to be provided in the FSD frame - mobile_to_maritime_velocity_state = Node( - package="mobile_to_maritime", - executable="mobile_twist_stamped_to_maritime_twist", - name="velocity_state_transform", - parameters=[ - { - "in_topic": "/mavros/local_position/velocity_body", - "out_topic": "/integral_sliding_mode_controller/system_state", - "qos_reliability": "best_effort", - "qos_durability": "volatile", - } - ], - ) - - controller_manager = Node( - package="controller_manager", - executable="ros2_control_node", - output="both", - parameters=[ - robot_description, - PathJoinSubstitution( - [ - FindPackageShare("blue_description"), - "config", - "bluerov2_heavy_reach", - "controllers.yaml", - ] - ), - ], - ) - - velocity_controller_spawner = Node( - package="controller_manager", - executable="spawner", - arguments=[ - "integral_sliding_mode_controller", - "--controller-manager", - ["", "controller_manager"], - ], - ) - - thruster_spawners = [ - Node( - package="controller_manager", - executable="spawner", - arguments=[ - f"thruster{i + 1}_controller", - "--controller-manager", - ["", "controller_manager"], - ], - ) - for i in range(8) - ] - - delay_thruster_spawners = [] - for i, thruster_spawner in enumerate(thruster_spawners): - if not len(delay_thruster_spawners): - delay_thruster_spawners.append( - thruster_spawner, - ) - else: - delay_thruster_spawners.append( - RegisterEventHandler( - event_handler=OnProcessExit( - target_action=thruster_spawners[i - 1], - on_exit=[thruster_spawner], - ) - ) - ) - - tam_controller_spawner = Node( - package="controller_manager", - executable="spawner", - arguments=[ - "thruster_allocation_matrix_controller", - "--controller-manager", - ["", "controller_manager"], - ], - ) - - delay_tam_controller_spawner_after_thruster_controller_spawners = ( - RegisterEventHandler( - event_handler=OnProcessExit( - target_action=thruster_spawners[-1], - on_exit=[tam_controller_spawner], - ) - ) - ) - - delay_velocity_controller_spawner_after_tam_controller_spawner = ( - RegisterEventHandler( - event_handler=OnProcessExit( - target_action=tam_controller_spawner, - on_exit=[velocity_controller_spawner], - ) - ) - ) - - return LaunchDescription( - [ - *args, - mobile_to_maritime_velocity_state, - controller_manager, - *delay_thruster_spawners, - delay_tam_controller_spawner_after_thruster_controller_spawners, - delay_velocity_controller_spawner_after_tam_controller_spawner, - ] - ) diff --git a/blue_demos/hardware_examples/README.md b/blue_demos/hardware_examples/README.md deleted file mode 100644 index e69de29b..00000000 diff --git a/blue_demos/package.xml b/blue_demos/package.xml deleted file mode 100644 index 103316db..00000000 --- a/blue_demos/package.xml +++ /dev/null @@ -1,18 +0,0 @@ - - - - blue_demos - 0.0.0 - TODO: Package description - blue - MIT - - ament_cmake - - ament_lint_auto - ament_lint_common - - - ament_cmake - - diff --git a/blue_demos/teleop_examples/README.md b/blue_demos/teleop_examples/README.md deleted file mode 100644 index e69de29b..00000000 diff --git a/blue_description/config/bluerov2/controllers.yaml b/blue_description/config/bluerov2/controllers.yaml deleted file mode 100644 index f6076d1a..00000000 --- a/blue_description/config/bluerov2/controllers.yaml +++ /dev/null @@ -1,133 +0,0 @@ -controller_manager: - ros__parameters: - update_rate: 100 # Hz - - integral_sliding_mode_controller: - type: velocity_controllers/IntegralSlidingModeController - - thruster_allocation_matrix_controller: - type: thruster_allocation_matrix_controller/ThrusterAllocationMatrixController - - thruster1_controller: - type: thruster_controllers/PolynomialThrustCurveController - - thruster2_controller: - type: thruster_controllers/PolynomialThrustCurveController - - thruster3_controller: - type: thruster_controllers/PolynomialThrustCurveController - - thruster4_controller: - type: thruster_controllers/PolynomialThrustCurveController - - thruster5_controller: - type: thruster_controllers/PolynomialThrustCurveController - - thruster6_controller: - type: thruster_controllers/PolynomialThrustCurveController - -integral_sliding_mode_controller: - ros__parameters: - use_external_measured_states: true - reference_controller: thruster_allocation_matrix_controller - gains: - rho: 20.0 - lambda: 200.0 - Kp: [4.0, 4.0, 9.0, 3.0, 3.0, 7.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] - -thruster_allocation_matrix_controller: - ros__parameters: - thrusters: - - thruster1_joint - - thruster2_joint - - thruster3_joint - - thruster4_joint - - thruster5_joint - - thruster6_joint - reference_controllers: - - thruster1_controller - - thruster2_controller - - thruster3_controller - - thruster4_controller - - thruster5_controller - - thruster6_controller - tam: - x: [ -0.707, -0.707, 0.707, 0.707, 0.0, 0.0] - y: [ 0.707, -0.707, 0.707, -0.707, 0.0, 0.0] - z: [ 0.0, 0.0, 0.0, 0.0, 1.0, 1.0] - rx: [ 0.0, 0.0, 0.0, 0.0, -0.21805, 0.21805] - ry: [ 0.0, 0.0, 0.0, 0.0, -0.12, -0.12] - rz: [0.1888, -0.1888, -0.1888, 0.1888, 0.0, 0.0] - -thruster1_controller: - ros__parameters: - thruster: thruster1_joint - min_thrust: -40.0 - max_thrust: 60.0 - min_deadband: 1470 - max_deadband: 1530 - neutral_pwm: 1500 - thrust_curve_coefficients: [1498.0, 12.01, -0.04731, -0.002098, 0.00002251] - -thruster2_controller: - ros__parameters: - thruster: thruster2_joint - min_thrust: -40.0 - max_thrust: 60.0 - min_deadband: 1470 - max_deadband: 1530 - neutral_pwm: 1500 - thrust_curve_coefficients: [1498.0, 12.01, -0.04731, -0.002098, 0.00002251] - -thruster3_controller: - ros__parameters: - thruster: thruster3_joint - min_thrust: -40.0 - max_thrust: 60.0 - min_deadband: 1470 - max_deadband: 1530 - neutral_pwm: 1500 - thrust_curve_coefficients: [1498.0, 12.01, -0.04731, -0.002098, 0.00002251] - -thruster4_controller: - ros__parameters: - thruster: thruster4_joint - min_thrust: -40.0 - max_thrust: 60.0 - min_deadband: 1470 - max_deadband: 1530 - neutral_pwm: 1500 - thrust_curve_coefficients: [1498.0, 12.01, -0.04731, -0.002098, 0.00002251] - -thruster5_controller: - ros__parameters: - thruster: thruster5_joint - min_thrust: -40.0 - max_thrust: 60.0 - min_deadband: 1470 - max_deadband: 1530 - neutral_pwm: 1500 - thrust_curve_coefficients: [1498.0, 12.01, -0.04731, -0.002098, 0.00002251] - -thruster6_controller: - ros__parameters: - thruster: thruster6_joint - min_thrust: -40.0 - max_thrust: 60.0 - min_deadband: 1470 - max_deadband: 1530 - neutral_pwm: 1500 - thrust_curve_coefficients: [1498.0, 12.01, -0.04731, -0.002098, 0.00002251] diff --git a/blue_description/config/bluerov2_heavy/controllers.yaml b/blue_description/config/bluerov2_heavy/controllers.yaml deleted file mode 100644 index a8608511..00000000 --- a/blue_description/config/bluerov2_heavy/controllers.yaml +++ /dev/null @@ -1,163 +0,0 @@ -controller_manager: - ros__parameters: - update_rate: 100 # Hz - - integral_sliding_mode_controller: - type: velocity_controllers/IntegralSlidingModeController - - thruster_allocation_matrix_controller: - type: thruster_allocation_matrix_controller/ThrusterAllocationMatrixController - - thruster1_controller: - type: thruster_controllers/PolynomialThrustCurveController - - thruster2_controller: - type: thruster_controllers/PolynomialThrustCurveController - - thruster3_controller: - type: thruster_controllers/PolynomialThrustCurveController - - thruster4_controller: - type: thruster_controllers/PolynomialThrustCurveController - - thruster5_controller: - type: thruster_controllers/PolynomialThrustCurveController - - thruster6_controller: - type: thruster_controllers/PolynomialThrustCurveController - - thruster7_controller: - type: thruster_controllers/PolynomialThrustCurveController - - thruster8_controller: - type: thruster_controllers/PolynomialThrustCurveController - -integral_sliding_mode_controller: - ros__parameters: - use_external_measured_states: true - reference_controller: thruster_allocation_matrix_controller - gains: - rho: 20.0 - lambda: 200.0 - Kp: [4.0, 4.0, 9.0, 3.0, 3.0, 7.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] - -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 - min_thrust: -40.0 - max_thrust: 60.0 - min_deadband: 1470 - max_deadband: 1530 - neutral_pwm: 1500 - thrust_curve_coefficients: [1498.0, 12.01, -0.04731, -0.002098, 0.00002251] - -thruster2_controller: - ros__parameters: - thruster: thruster2_joint - min_thrust: -40.0 - max_thrust: 60.0 - min_deadband: 1470 - max_deadband: 1530 - neutral_pwm: 1500 - thrust_curve_coefficients: [1498.0, 12.01, -0.04731, -0.002098, 0.00002251] - -thruster3_controller: - ros__parameters: - thruster: thruster3_joint - min_thrust: -40.0 - max_thrust: 60.0 - min_deadband: 1470 - max_deadband: 1530 - neutral_pwm: 1500 - thrust_curve_coefficients: [1498.0, 12.01, -0.04731, -0.002098, 0.00002251] - -thruster4_controller: - ros__parameters: - thruster: thruster4_joint - min_thrust: -40.0 - max_thrust: 60.0 - min_deadband: 1470 - max_deadband: 1530 - neutral_pwm: 1500 - thrust_curve_coefficients: [1498.0, 12.01, -0.04731, -0.002098, 0.00002251] - -thruster5_controller: - ros__parameters: - thruster: thruster5_joint - min_thrust: -40.0 - max_thrust: 60.0 - min_deadband: 1470 - max_deadband: 1530 - neutral_pwm: 1500 - thrust_curve_coefficients: [1498.0, 12.01, -0.04731, -0.002098, 0.00002251] - -thruster6_controller: - ros__parameters: - thruster: thruster6_joint - min_thrust: -40.0 - max_thrust: 60.0 - min_deadband: 1470 - max_deadband: 1530 - neutral_pwm: 1500 - thrust_curve_coefficients: [1498.0, 12.01, -0.04731, -0.002098, 0.00002251] - -thruster7_controller: - ros__parameters: - thruster: thruster7_joint - min_thrust: -40.0 - max_thrust: 60.0 - min_deadband: 1470 - max_deadband: 1530 - neutral_pwm: 1500 - thrust_curve_coefficients: [1498.0, 12.01, -0.04731, -0.002098, 0.00002251] - -thruster8_controller: - ros__parameters: - thruster: thruster8_joint - min_thrust: -40.0 - max_thrust: 60.0 - min_deadband: 1470 - max_deadband: 1530 - neutral_pwm: 1500 - thrust_curve_coefficients: [1498.0, 12.01, -0.04731, -0.002098, 0.00002251] diff --git a/blue_description/config/bluerov2_heavy_reach/controllers.yaml b/blue_description/config/bluerov2_heavy_reach/controllers.yaml deleted file mode 100644 index a8608511..00000000 --- a/blue_description/config/bluerov2_heavy_reach/controllers.yaml +++ /dev/null @@ -1,163 +0,0 @@ -controller_manager: - ros__parameters: - update_rate: 100 # Hz - - integral_sliding_mode_controller: - type: velocity_controllers/IntegralSlidingModeController - - thruster_allocation_matrix_controller: - type: thruster_allocation_matrix_controller/ThrusterAllocationMatrixController - - thruster1_controller: - type: thruster_controllers/PolynomialThrustCurveController - - thruster2_controller: - type: thruster_controllers/PolynomialThrustCurveController - - thruster3_controller: - type: thruster_controllers/PolynomialThrustCurveController - - thruster4_controller: - type: thruster_controllers/PolynomialThrustCurveController - - thruster5_controller: - type: thruster_controllers/PolynomialThrustCurveController - - thruster6_controller: - type: thruster_controllers/PolynomialThrustCurveController - - thruster7_controller: - type: thruster_controllers/PolynomialThrustCurveController - - thruster8_controller: - type: thruster_controllers/PolynomialThrustCurveController - -integral_sliding_mode_controller: - ros__parameters: - use_external_measured_states: true - reference_controller: thruster_allocation_matrix_controller - gains: - rho: 20.0 - lambda: 200.0 - Kp: [4.0, 4.0, 9.0, 3.0, 3.0, 7.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] - -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 - min_thrust: -40.0 - max_thrust: 60.0 - min_deadband: 1470 - max_deadband: 1530 - neutral_pwm: 1500 - thrust_curve_coefficients: [1498.0, 12.01, -0.04731, -0.002098, 0.00002251] - -thruster2_controller: - ros__parameters: - thruster: thruster2_joint - min_thrust: -40.0 - max_thrust: 60.0 - min_deadband: 1470 - max_deadband: 1530 - neutral_pwm: 1500 - thrust_curve_coefficients: [1498.0, 12.01, -0.04731, -0.002098, 0.00002251] - -thruster3_controller: - ros__parameters: - thruster: thruster3_joint - min_thrust: -40.0 - max_thrust: 60.0 - min_deadband: 1470 - max_deadband: 1530 - neutral_pwm: 1500 - thrust_curve_coefficients: [1498.0, 12.01, -0.04731, -0.002098, 0.00002251] - -thruster4_controller: - ros__parameters: - thruster: thruster4_joint - min_thrust: -40.0 - max_thrust: 60.0 - min_deadband: 1470 - max_deadband: 1530 - neutral_pwm: 1500 - thrust_curve_coefficients: [1498.0, 12.01, -0.04731, -0.002098, 0.00002251] - -thruster5_controller: - ros__parameters: - thruster: thruster5_joint - min_thrust: -40.0 - max_thrust: 60.0 - min_deadband: 1470 - max_deadband: 1530 - neutral_pwm: 1500 - thrust_curve_coefficients: [1498.0, 12.01, -0.04731, -0.002098, 0.00002251] - -thruster6_controller: - ros__parameters: - thruster: thruster6_joint - min_thrust: -40.0 - max_thrust: 60.0 - min_deadband: 1470 - max_deadband: 1530 - neutral_pwm: 1500 - thrust_curve_coefficients: [1498.0, 12.01, -0.04731, -0.002098, 0.00002251] - -thruster7_controller: - ros__parameters: - thruster: thruster7_joint - min_thrust: -40.0 - max_thrust: 60.0 - min_deadband: 1470 - max_deadband: 1530 - neutral_pwm: 1500 - thrust_curve_coefficients: [1498.0, 12.01, -0.04731, -0.002098, 0.00002251] - -thruster8_controller: - ros__parameters: - thruster: thruster8_joint - min_thrust: -40.0 - max_thrust: 60.0 - min_deadband: 1470 - max_deadband: 1530 - neutral_pwm: 1500 - thrust_curve_coefficients: [1498.0, 12.01, -0.04731, -0.002098, 0.00002251] diff --git a/blue_description/config/joy_teleop.yaml b/blue_description/config/joy_teleop.yaml deleted file mode 100644 index e8255857..00000000 --- a/blue_description/config/joy_teleop.yaml +++ /dev/null @@ -1,133 +0,0 @@ -joy_node: - ros__parameters: - dev: /dev/input/js0 - -joy_interface: - ros__parameters: - trims: - min_pwm: 1300 - max_pwm: 1700 - -joy_teleop: - ros__parameters: - - arm_ardusub: - type: service - interface_type: mavros_msgs/srv/CommandBool - service_name: /mavros/cmd/arming - service_request: - value: True - buttons: [7] - - disarm_ardusub: - type: service - interface_type: mavros_msgs/srv/CommandBool - service_name: /mavros/cmd/arming - service_request: - value: False - buttons: [6] - - arm_custom_controller: - type: service - interface_type: std_srvs/srv/SetBool - service_name: /blue/cmd/arm - service_request: - data: True - buttons: [5] - - disarm_custom_controller: - type: service - interface_type: std_srvs/srv/SetBool - service_name: /blue/cmd/arm - service_request: - data: False - buttons: [4] - - vel_control: - type: topic - interface_type: std_msgs/msg/Bool - topic_name: /blue/joy_interface/start_vel_control - deadman_buttons: [5] - message_value: - data: - value: True - - pwm_control: - type: topic - interface_type: std_msgs/msg/Bool - topic_name: /blue/joy_interface/start_pwm_control - deadman_buttons: [4] - message_value: - data: - value: True - - enable_passthrough: - type: service - interface_type: std_srvs/srv/SetBool - service_name: /blue/cmd/enable_passthrough - service_request: - data: True - buttons: [9] - - disable_passthrough: - type: service - interface_type: std_srvs/srv/SetBool - service_name: /blue/cmd/enable_passthrough - service_request: - data: False - buttons: [10] - - manual_mode: - type: service - interface_type: mavros_msgs/srv/SetMode - service_name: /mavros/set_mode - service_request: - custom_mode: MANUAL - buttons: [0] - - depth_hold_mode: - type: service - interface_type: mavros_msgs/srv/SetMode - service_name: /mavros/set_mode - service_request: - custom_mode: ALT_HOLD - buttons: [1] - - guided_mode: - type: service - interface_type: mavros_msgs/srv/SetMode - service_name: /mavros/set_mode - service_request: - custom_mode: GUIDED - buttons: [2] - - auto_mode: - type: service - interface_type: mavros_msgs/srv/SetMode - service_name: /mavros/set_mode - service_request: - custom_mode: AUTO - buttons: [3] - - manual_control: - type: topic - interface_type: geometry_msgs/msg/Twist - topic_name: /blue/joy_interface/cmd_vel - deadman_axes: [2] - axis_mappings: - linear-x: - axis: 1 - scale: 0.5 - offset: 0 - linear-y: - axis: 0 - scale: 0.5 - offset: 0 - linear-z: - axis: 4 - scale: 0.5 - offset: 0 - angular-z: - axis: 3 - scale: 1.0 - offset: 0 diff --git a/blue_joy/LICENSE b/blue_joy/LICENSE deleted file mode 100644 index 30e8e2ec..00000000 --- a/blue_joy/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_joy/blue_joy/__init__.py b/blue_joy/blue_joy/__init__.py deleted file mode 100644 index e69de29b..00000000 diff --git a/blue_joy/blue_joy/joy.py b/blue_joy/blue_joy/joy.py deleted file mode 100644 index 9c6e48c6..00000000 --- a/blue_joy/blue_joy/joy.py +++ /dev/null @@ -1,160 +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 rclpy -from geometry_msgs.msg import Twist -from mavros_msgs.msg import OverrideRCIn -from rclpy.node import Node -from rclpy.qos import qos_profile_default -from std_msgs.msg import Bool - - -class JoyInterface(Node): - """Interface for sending manual control inputs to a controller.""" - - def __init__(self): - """Create a new joystick manual control interface.""" - super().__init__("joy_interface") - - self.declare_parameter("custom_controller_name", "ismc") - self.declare_parameters( - "trims", parameters=[("max_pwm", 1900), ("min_pwm", 1100)] # type: ignore - ) - - self.min_pwm = ( - self.get_parameter("trims.min_pwm").get_parameter_value().integer_value - ) - self.max_pwm = ( - self.get_parameter("trims.max_pwm").get_parameter_value().integer_value - ) - - # Keep track of whether or not we are controlling using PWMs or twists - self.vel_control = False - - # Subscribers - self.start_vel_cmd_sub = self.create_subscription( - Bool, - "/blue/joy_interface/start_vel_control", - self.start_vel_control_cb, - qos_profile_default, - ) - self.start_pwm_cmd_sub = self.create_subscription( - Bool, - "/blue/joy_interface/start_pwm_control", - self.start_pwm_control_cb, - qos_profile_default, - ) - self.cmd_vel_sub = self.create_subscription( - Twist, - "/blue/joy_interface/cmd_vel", - self.manual_control_cb, - qos_profile_default, - ) - - # Publishers - self.override_rc_in_pub = self.create_publisher( - OverrideRCIn, "/mavros/rc/override", qos_profile_default - ) - - controller_name = ( - self.get_parameter("custom_controller_name") - .get_parameter_value() - .string_value - ) - self.cmd_vel_pub = self.create_publisher( - Twist, f"/blue/{controller_name}/cmd_vel", 1 # only publish the latest data - ) - - @staticmethod - def map_range( - value: float, in_min: float, in_max: float, out_min: float, out_max: float - ): - """Map a value to the provided range. - - Args: - value: The value to map to the range. - in_min: The minimum value of the current range. - in_max: The maximum value of the current range. - out_min: The minimum value of the mapped range. - out_max: The maximum value of the mapped range. - - Returns: - A value mapped from an input range to and output range. - """ - return (((value - in_min) * (out_max - out_min)) / (in_max - in_min)) + out_min - - def start_vel_control_cb(self, _: Bool) -> None: - """Direct control inputs to the custom controller. - - Args: - _: Unused. - """ - self.vel_control = True - - def start_pwm_control_cb(self, _: Bool) -> None: - """Direct control inputs to the ArduSub controller. - - Args: - _: Unused. - """ - self.vel_control = False - - def manual_control_cb(self, cmd: Twist) -> None: - """Send the manual control inputs to the desired endpoint. - - Args: - cmd: The control input to send. - """ - if self.vel_control: - self.cmd_vel_pub.publish(cmd) - else: - rc = OverrideRCIn() - channels = [OverrideRCIn.CHAN_NOCHANGE] * 18 - - channels[4] = int( - self.map_range(cmd.linear.x, -1.0, 1.0, self.min_pwm, self.max_pwm) - ) - channels[5] = int( - self.map_range(-1 * cmd.linear.y, -1.0, 1.0, self.min_pwm, self.max_pwm) - ) - channels[2] = int( - self.map_range(cmd.linear.z, -1.0, 1.0, self.min_pwm, self.max_pwm) - ) - channels[3] = int( - self.map_range( - -1 * cmd.angular.z, -1.0, 1.0, self.min_pwm, self.max_pwm - ) - ) - - rc = OverrideRCIn() - rc.channels = channels - - self.override_rc_in_pub.publish(rc) - - -def main(args: list[str] | None = None): - """Run the joystick interface.""" - rclpy.init(args=args) - - node = JoyInterface() - rclpy.spin(node) - - node.destroy_node() - rclpy.shutdown() diff --git a/blue_joy/launch/joy.launch.py b/blue_joy/launch/joy.launch.py deleted file mode 100644 index bda74eec..00000000 --- a/blue_joy/launch/joy.launch.py +++ /dev/null @@ -1,95 +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 -from launch.conditions import IfCondition -from launch.substitutions import LaunchConfiguration -from launch_ros.actions import Node - - -def generate_launch_description() -> LaunchDescription: - """Generate a launch description for the Blue manager interface. - - Returns: - LaunchDescription: The Blue manager launch description. - """ - args = [ - DeclareLaunchArgument( - "config_filepath", - default_value=None, - description="The path to the joystick configuration YAML file.", - ), - DeclareLaunchArgument( - "controller", - default_value=None, - description="The name of the custom controller loaded.", - ), - DeclareLaunchArgument( - "use_sim_time", - default_value="false", - description=("Use the simulated Gazebo clock."), - ), - ] - - nodes = [ - Node( - package="joy_linux", - executable="joy_linux_node", - name="joy_node", - output="both", - parameters=[ - LaunchConfiguration("config_filepath"), - { - "use_sim_time": LaunchConfiguration("use_sim_time"), - }, - ], - condition=IfCondition(LaunchConfiguration("use_joy")), - ), - Node( - package="joy_teleop", - executable="joy_teleop", - name="joy_teleop", - output="both", - parameters=[ - LaunchConfiguration("config_filepath"), - { - "use_sim_time": LaunchConfiguration("use_sim_time"), - }, - ], - condition=IfCondition(LaunchConfiguration("use_joy")), - ), - Node( - package="blue_joy", - executable="joy_interface", - name="joy_interface", - output="both", - parameters=[ - LaunchConfiguration("config_filepath"), - { - "custom_controller_name": LaunchConfiguration("controller"), - "use_sim_time": LaunchConfiguration("use_sim_time"), - }, - ], - condition=IfCondition(LaunchConfiguration("use_joy")), - ), - ] - - return LaunchDescription(args + nodes) diff --git a/blue_joy/package.xml b/blue_joy/package.xml deleted file mode 100644 index b6c54f77..00000000 --- a/blue_joy/package.xml +++ /dev/null @@ -1,29 +0,0 @@ - - - - - blue_joy - 0.0.1 - An interface for manual control using a joystick. - - Evan Palmer - MIT - - https://github.com/Robotic-Decision-Making-Lab/blue.git - https://github.com/Robotic-Decision-Making-Lab/blue/issues - - Evan Palmer - - mavros_msgs - std_msgs - - joy_teleop - joy_linux - - ament_copyright - python3-pytest - - - ament_python - - diff --git a/blue_joy/resource/blue_joy b/blue_joy/resource/blue_joy deleted file mode 100644 index e69de29b..00000000 diff --git a/blue_joy/setup.cfg b/blue_joy/setup.cfg deleted file mode 100644 index 1bf321f7..00000000 --- a/blue_joy/setup.cfg +++ /dev/null @@ -1,4 +0,0 @@ -[develop] -script_dir=$base/lib/blue_joy -[install] -install_scripts=$base/lib/blue_joy diff --git a/blue_joy/setup.py b/blue_joy/setup.py deleted file mode 100644 index 5022d321..00000000 --- a/blue_joy/setup.py +++ /dev/null @@ -1,45 +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 setup - -package_name = "blue_joy" - -setup( - name=package_name, - version="0.0.1", - packages=[package_name], - 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"], - zip_safe=True, - maintainer="Evan Palmer", - maintainer_email="evanp922@gmail.com", - description=("An interface for manual control using a joystick."), - license="MIT", - tests_require=["pytest"], - entry_points={"console_scripts": ["joy_interface = blue_joy.joy:main"]}, -) diff --git a/blue_joy/test/test_copyright.py b/blue_joy/test/test_copyright.py deleted file mode 100644 index 8f18fa4b..00000000 --- a/blue_joy/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" From d3c8436309086dd0516482b87950d8c3a7492449 Mon Sep 17 00:00:00 2001 From: Evan Palmer Date: Sat, 13 Apr 2024 02:04:34 -0700 Subject: [PATCH 12/38] Added docs --- .devcontainer/documentation/Dockerfile | 12 + .devcontainer/documentation/devcontainer.json | 22 + .devcontainer/nouveau/devcontainer.json | 8 +- .devcontainer/nvidia/devcontainer.json | 8 +- blue_demos/CMakeLists.txt | 26 + blue_demos/LICENSE | 17 + blue_demos/package.xml | 28 + docs/.gitignore | 20 + docs/babel.config.js | 3 + docs/docs/installation.mdx | 72 + docs/docs/overview.mdx | 53 + docs/docs/tutorials/_category_.json | 4 + docs/docs/tutorials/control.mdx | 4 + docs/docs/tutorials/localization.mdx | 4 + docs/docs/tutorials/simulation.mdx | 4 + docs/docs/tutorials/teleop.mdx | 7 + docs/docs/welcome.mdx | 49 + docs/docusaurus.config.js | 76 + docs/package-lock.json | 17019 ++++++++++++++++ docs/package.json | 54 + docs/sidebars.js | 7 + docs/src/components/Topic.jsx | 25 + docs/src/components/Vehicle.jsx | 22 + docs/src/css/cards.scss | 13 + docs/src/css/custom.scss | 52 + docs/src/css/fonts-and-colors.scss | 115 + docs/src/css/header.scss | 39 + docs/src/css/typography.scss | 5 + docs/static/.nojekyll | 0 docs/static/img/docusaurus-social-card.jpg | Bin 0 -> 55746 bytes docs/static/img/docusaurus.png | Bin 0 -> 5142 bytes docs/static/img/favicon.ico | Bin 0 -> 3626 bytes docs/static/img/logo.svg | 1 + .../static/img/undraw_docusaurus_mountain.svg | 171 + docs/static/img/undraw_docusaurus_react.svg | 170 + docs/static/img/undraw_docusaurus_tree.svg | 40 + 36 files changed, 18140 insertions(+), 10 deletions(-) create mode 100644 .devcontainer/documentation/Dockerfile create mode 100644 .devcontainer/documentation/devcontainer.json create mode 100644 blue_demos/CMakeLists.txt create mode 100644 blue_demos/LICENSE create mode 100644 blue_demos/package.xml create mode 100644 docs/.gitignore create mode 100644 docs/babel.config.js create mode 100644 docs/docs/installation.mdx create mode 100644 docs/docs/overview.mdx create mode 100644 docs/docs/tutorials/_category_.json create mode 100644 docs/docs/tutorials/control.mdx create mode 100644 docs/docs/tutorials/localization.mdx create mode 100644 docs/docs/tutorials/simulation.mdx create mode 100644 docs/docs/tutorials/teleop.mdx create mode 100644 docs/docs/welcome.mdx create mode 100644 docs/docusaurus.config.js create mode 100644 docs/package-lock.json create mode 100644 docs/package.json create mode 100644 docs/sidebars.js create mode 100644 docs/src/components/Topic.jsx create mode 100644 docs/src/components/Vehicle.jsx create mode 100644 docs/src/css/cards.scss create mode 100644 docs/src/css/custom.scss create mode 100644 docs/src/css/fonts-and-colors.scss create mode 100644 docs/src/css/header.scss create mode 100644 docs/src/css/typography.scss create mode 100644 docs/static/.nojekyll create mode 100644 docs/static/img/docusaurus-social-card.jpg create mode 100644 docs/static/img/docusaurus.png create mode 100644 docs/static/img/favicon.ico create mode 100644 docs/static/img/logo.svg create mode 100644 docs/static/img/undraw_docusaurus_mountain.svg create mode 100644 docs/static/img/undraw_docusaurus_react.svg create mode 100644 docs/static/img/undraw_docusaurus_tree.svg diff --git a/.devcontainer/documentation/Dockerfile b/.devcontainer/documentation/Dockerfile new file mode 100644 index 00000000..dc13a7e4 --- /dev/null +++ b/.devcontainer/documentation/Dockerfile @@ -0,0 +1,12 @@ +FROM node:20 + +RUN apt-get -q update \ + && apt-get -q -y upgrade \ + && apt-get install sudo \ + && apt-get autoremove -y \ + && apt-get clean -y \ + && rm -rf /var/lib/apt/lists/* + +ARG USERNAME=node +RUN echo $USERNAME ALL=\(root\) NOPASSWD:ALL > /etc/sudoers.d/$USERNAME \ + && chmod 0440 /etc/sudoers.d/$USERNAME diff --git a/.devcontainer/documentation/devcontainer.json b/.devcontainer/documentation/devcontainer.json new file mode 100644 index 00000000..48a8b692 --- /dev/null +++ b/.devcontainer/documentation/devcontainer.json @@ -0,0 +1,22 @@ +{ + "name": "Documentation", + "build": { + "dockerfile": "Dockerfile" + }, + "remoteUser": "node", + "customizations": { + "vscode": { + "settings": { + "files.insertFinalNewline": true, + "files.trimTrailingWhitespace": true, + "editor.formatOnSave": true, + "editor.tabSize": 2, + "editor.rulers": [80] + }, + "extensions": [ + "esbenp.prettier-vscode", + "unifiedjs.vscode-mdx" + ] + } + } +} diff --git a/.devcontainer/nouveau/devcontainer.json b/.devcontainer/nouveau/devcontainer.json index 44b1e52a..13feab28 100644 --- a/.devcontainer/nouveau/devcontainer.json +++ b/.devcontainer/nouveau/devcontainer.json @@ -1,5 +1,5 @@ { - "name": "Blue Nouveau Dev Container", + "name": "Nouveau Dev Container", "dockerFile": "Dockerfile", "context": "../..", "workspaceMount": "source=${localWorkspaceFolder},target=/home/blue/ws_blue/src/blue,type=bind", @@ -26,16 +26,14 @@ "ms-azuretools.vscode-docker", "ms-python.python", "njpwerner.autodocstring", - "ms-vscode.cpptools", "redhat.vscode-xml", "redhat.vscode-yaml", "smilerobotics.urdf", - "DavidAnson.vscode-markdownlint", "esbenp.prettier-vscode", - "xaver.clang-format", "charliermarsh.ruff", "ms-python.black-formatter", - "josetr.cmake-language-support-vscode" + "josetr.cmake-language-support-vscode", + "unifiedjs.vscode-mdx" ] } } diff --git a/.devcontainer/nvidia/devcontainer.json b/.devcontainer/nvidia/devcontainer.json index 5493600d..03fecdb8 100644 --- a/.devcontainer/nvidia/devcontainer.json +++ b/.devcontainer/nvidia/devcontainer.json @@ -1,5 +1,5 @@ { - "name": "Blue NVIDIA Dev Container", + "name": "NVIDIA Dev Container", "dockerFile": "Dockerfile", "context": "../..", "workspaceMount": "source=${localWorkspaceFolder},target=/home/blue/ws_blue/src/blue,type=bind", @@ -30,16 +30,14 @@ "ms-azuretools.vscode-docker", "ms-python.python", "njpwerner.autodocstring", - "ms-vscode.cpptools", "redhat.vscode-xml", "redhat.vscode-yaml", "smilerobotics.urdf", - "DavidAnson.vscode-markdownlint", "esbenp.prettier-vscode", - "xaver.clang-format", "charliermarsh.ruff", "ms-python.black-formatter", - "josetr.cmake-language-support-vscode" + "josetr.cmake-language-support-vscode", + "unifiedjs.vscode-mdx" ] } } diff --git a/blue_demos/CMakeLists.txt b/blue_demos/CMakeLists.txt new file mode 100644 index 00000000..cc5f0056 --- /dev/null +++ b/blue_demos/CMakeLists.txt @@ -0,0 +1,26 @@ +cmake_minimum_required(VERSION 3.8) +project(blue_demos) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +# uncomment the following section in order to fill in +# further dependencies manually. +# find_package( REQUIRED) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + # comment the line when a copyright and license is added to all source files + set(ament_cmake_copyright_FOUND TRUE) + # the following line skips cpplint (only works in a git repo) + # comment the line when this package is in a git repo and when + # a copyright and license is added to all source files + set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +ament_package() diff --git a/blue_demos/LICENSE b/blue_demos/LICENSE new file mode 100644 index 00000000..30e8e2ec --- /dev/null +++ b/blue_demos/LICENSE @@ -0,0 +1,17 @@ +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_demos/package.xml b/blue_demos/package.xml new file mode 100644 index 00000000..18a74977 --- /dev/null +++ b/blue_demos/package.xml @@ -0,0 +1,28 @@ + + + + + blue_demos + 0.0.1 + A collection of demonstrations that show how to use Blue + + Evan Palmer + MIT + + https://github.com/Robotic-Decision-Making-Lab/blue.git + https://github.com/Robotic-Decision-Making-Lab/blue/issues + + Evan Palmer + + ament_cmake + + ament_lint_auto + + ament_cmake_copyright + ament_cmake_lint_cmake + ament_cmake_xmllint + + + ament_cmake + + diff --git a/docs/.gitignore b/docs/.gitignore new file mode 100644 index 00000000..b2d6de30 --- /dev/null +++ b/docs/.gitignore @@ -0,0 +1,20 @@ +# Dependencies +/node_modules + +# Production +/build + +# Generated files +.docusaurus +.cache-loader + +# Misc +.DS_Store +.env.local +.env.development.local +.env.test.local +.env.production.local + +npm-debug.log* +yarn-debug.log* +yarn-error.log* diff --git a/docs/babel.config.js b/docs/babel.config.js new file mode 100644 index 00000000..e00595da --- /dev/null +++ b/docs/babel.config.js @@ -0,0 +1,3 @@ +module.exports = { + presets: [require.resolve('@docusaurus/core/lib/babel/preset')], +}; diff --git a/docs/docs/installation.mdx b/docs/docs/installation.mdx new file mode 100644 index 00000000..a1ad5fec --- /dev/null +++ b/docs/docs/installation.mdx @@ -0,0 +1,72 @@ +--- +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 +a system with limited compute power, we recommend installing Blue from source. +For all other cases, we recommend installing Blue using Docker. + +## Docker installation + +Prior to installing Blue using Docker, make sure that Docker is installed on +your system and that you are logged into the GitHub Container Registery. If +Docker is not installed, you may follow the instructions provided +[here](https://docs.docker.com/get-docker/). If you are not already logged into +the GitHub Container Registery, please do so by following the instructions +provided [here](https://docs.github.com/en/packages/working-with-a-github-packages-registry/working-with-the-container-registry). +Once you have successfully installed Docker and logged into the GitHub Container +Registery, 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 distrubtion, e.g., `humble-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). | + +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, +first run the command: + +```bash +wget https://raw.githubusercontent.com/Robotic-Decision-Making-Lab/blue/iron/.docker/compose/nouveau-desktop.yaml && \ +docker compose -f nouveau-desktop.yaml up +``` + +then in a new tab, enter the container by running + +```bash +docker exec -it /bin/bash +``` + +where `` should be replaced with the name of the container that was +created. + +### Development container + +We recommend using one of the provided Visual Studio Code +[development containers](https://github.com/Robotic-Decision-Making-Lab/blue/tree/main/.devcontainer) +to implement and test your own packages. These containers use the `*-desktop` +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! + +## Source installation + +We recommend using the project [Dockerfile](https://github.com/Robotic-Decision-Making-Lab/blue/blob/main/.docker/Dockerfile) +(beginning at the `robot` stage) for complete source installation details. This +installation may be customized according to your specific needs. For example, +if you do not plan to use the simulation environment, you may forego installing +Gazebo and its dependencies. diff --git a/docs/docs/overview.mdx b/docs/docs/overview.mdx new file mode 100644 index 00000000..ab1d0d06 --- /dev/null +++ b/docs/docs/overview.mdx @@ -0,0 +1,53 @@ +--- +sidebar_position: 2 +--- + +import Vehicle from '@site/src/components/Vehicle'; + +# Overview + +Blue provides a set of tools to help you test and deploy software for underwater +vehicles. This is accomplished through the following features: + +1. **Simulation using Gazebo**: Vehicle models and configurations have been + created for Gazebo. We have found that Gazebo provides a good representation + of how the vehicle will behave in the real world. +2. **CI/CD pipelines**: Blue implements CI/CD pipelines to help you deploy your + software in the same environment that you performed development and testing + in. Docker images are built for the `amd64` and `arm64` CPU architectures so + that you can easily deploy your software to a variety of hardware platforms. +3. **Development environment**: Blue provides a development environment that + includes all of the tools you need to develop software for underwater + 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 [marine_localization](https://github.com/Robotic-Decision-Making-Lab/marine_localization). + +## Workflow + +Under construction! + +## Supported Vehicles + +Several vehicle models have been implemented and tested with Blue in +**simulation** and on **hardware**. These vehicles include: + +
+ + 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 Reach modifies the BlueROV2 Heavy by moving the + thrusters located on the upper chassis to the lower chassis. This was + proposed by [Reach Robotics](https://reachrobotics.com/) to enable + integration of a small manipulator for intervention tasks. + +
diff --git a/docs/docs/tutorials/_category_.json b/docs/docs/tutorials/_category_.json new file mode 100644 index 00000000..b9dada94 --- /dev/null +++ b/docs/docs/tutorials/_category_.json @@ -0,0 +1,4 @@ +{ + "label": "Tutorials", + "position": 4 +} diff --git a/docs/docs/tutorials/control.mdx b/docs/docs/tutorials/control.mdx new file mode 100644 index 00000000..649b1136 --- /dev/null +++ b/docs/docs/tutorials/control.mdx @@ -0,0 +1,4 @@ +--- +sidebar_position: 2 +title: Running Custom Controllers +--- diff --git a/docs/docs/tutorials/localization.mdx b/docs/docs/tutorials/localization.mdx new file mode 100644 index 00000000..9da50f5b --- /dev/null +++ b/docs/docs/tutorials/localization.mdx @@ -0,0 +1,4 @@ +--- +sidebar_position: 3 +title: Integrating Localization +--- diff --git a/docs/docs/tutorials/simulation.mdx b/docs/docs/tutorials/simulation.mdx new file mode 100644 index 00000000..1e8ab31d --- /dev/null +++ b/docs/docs/tutorials/simulation.mdx @@ -0,0 +1,4 @@ +--- +sidebar_position: 1 +title: Launching the Simulator +--- diff --git a/docs/docs/tutorials/teleop.mdx b/docs/docs/tutorials/teleop.mdx new file mode 100644 index 00000000..586eea00 --- /dev/null +++ b/docs/docs/tutorials/teleop.mdx @@ -0,0 +1,7 @@ +--- +sidebar_position: 4 +title: Teleoperation +--- + +# Teleoperation + diff --git a/docs/docs/welcome.mdx b/docs/docs/welcome.mdx new file mode 100644 index 00000000..bcb9453b --- /dev/null +++ b/docs/docs/welcome.mdx @@ -0,0 +1,49 @@ +--- +sidebar_position: 1 +title: Welcome! +hide_table_of_contents: true +slug: / +--- + +import Link from "@docusaurus/Link"; +import CodeBlock from '@theme/CodeBlock'; +import Topic from '@site/src/components/Topic'; + +# 🌊 Blue Documentation + +
+
+
+

+ [**Blue**](https://github.com/Robotic-Decision-Making-Lab/blue) is an + open source ROS 2 pipeline designed to support development, testing, and + sim-to-real deployment of underwater vehicles. Get started with Blue by + [installing the project](/installation) or following our + [first tutorial](/tutorials/simulation). If you have any questions + regarding usage of Blue, please ask a question on our [Discussions](https://github.com/Robotic-Decision-Making-Lab/blue/discussions) + board! + + If you find Blue to be helpful in your work, please consider citing our + paper: +

+
+ +{`@inproceedings{palmer2024angler, + author={Palmer, Evan and Holm, Christopher and Hollinger, Geoffrey}, + title={{Angler: An Autonomy Framework for Intervention Tasks with Lightweight Underwater Vehicle Manipulator Systems}}, + booktitle={IEEE International Conference on Robotics and Automation}, + year={2024} +}`} + +
+
+