diff --git a/.gitignore b/.gitignore
index 53a92ef5..5f181a1a 100644
--- a/.gitignore
+++ b/.gitignore
@@ -48,3 +48,7 @@ ros_gz/
# ArduSub Sim
eeprom.bin
+mav.parm
+mav.tlog
+mav.tlog.raw
+logs/
diff --git a/README.md b/README.md
index d51d1a8d..223b381e 100644
--- a/README.md
+++ b/README.md
@@ -1,8 +1,60 @@
# BlueROV2 ROS 2 driver :ocean:
The BlueROV2 driver is a collection of ROS 2 packages designed
-to support ROS 2 integration with the [BlueROV2 Heavy](https://bluerobotics.com/)
-and [ArduSub](https://www.ardusub.com/).
+to support ROS 2 integration with the [BlueROV2](https://bluerobotics.com/)
+(both base and heavy configurations) and [ArduSub](https://www.ardusub.com/).
+
+## Main Features
+
+The main features of the project include:
+
+- Support for developing custom controllers for the BlueROV2
+- RC Passthrough mode: a custom mode which enables sending thrust commands to individual thrusters
+- External localization support to enable developing custom SLAM algorithms and interfaces for localization sensors
+- ArduSub + Gazebo SITL integration for evaluating the performance of your algorithms in a simulation environment
+- Interfaces for adjusting hydrodynamic parameters
+- Visualization support using Foxglove Studio
+
+## Installation
+
+The BlueROV2 driver is currently supported on Linux, and is available for the
+ROS distributions Humble, Iron, and Rolling. To install the BlueROV2 driver,
+first clone this project to the `src` directory of your ROS workspace, replacing
+`$ROS_DISTRO` with the desired ROS distribution or `main` for Rolling:
+
+```bash
+git clone -b $ROS_DISTRO git@github.com:evan-palmer/blue.git
+```
+
+After cloning the project, install all external dependencies using `vcs`:
+
+```bash
+vcs import src < src/blue/blue.repos
+```
+
+Finally, install the ROS dependencies using `rosdep`, again, replacing
+`$ROS_DISTRO` with the desired ROS distribution:
+
+```bash
+rosdep update && \
+rosdep install -y --from-paths src --ignore-src --rosdistro $ROS_DISTRO
+```
+
+## Quick start
+
+ROS 2 launch files have been provided to start the BlueROV2 driver for the base
+and heavy configurations. To launch the system for the BlueROV2 Heavy, run
+
+```bash
+ros2 launch blue_bringup bluerov2_heavy.launch.py
+```
+
+A full description of the launch arguments and their respective default values
+can be obtained by running the following command:
+
+```bash
+ros2 launch blue_bringup bluerov2_heavy.launch.py --show-args
+```
## Getting help
diff --git a/blue_bringup/CMakeLists.txt b/blue_bringup/CMakeLists.txt
new file mode 100644
index 00000000..727bc1f1
--- /dev/null
+++ b/blue_bringup/CMakeLists.txt
@@ -0,0 +1,19 @@
+cmake_minimum_required(VERSION 3.8)
+project(blue_bringup)
+
+set(THIS_PACKAGE_INCLUDE_DEPENDS
+ rclcpp
+ rclpy
+ ament_cmake
+)
+
+foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS})
+ find_package(${Dependency} REQUIRED)
+endforeach()
+
+install(
+ DIRECTORY launch config
+ DESTINATION share/${PROJECT_NAME}
+)
+
+ament_package()
diff --git a/blue_bringup/blue_bringup/__init__.py b/blue_bringup/blue_bringup/__init__.py
deleted file mode 100644
index e69de29b..00000000
diff --git a/blue_bringup/config/bluerov2.yaml b/blue_bringup/config/bluerov2.yaml
index 168cadee..c1c371c9 100644
--- a/blue_bringup/config/bluerov2.yaml
+++ b/blue_bringup/config/bluerov2.yaml
@@ -4,6 +4,7 @@ blue_manager:
mode_change_timeout: 1.0
mode_change_retries: 3
+# Controller
ismc:
ros__parameters:
mass: 10.0
@@ -17,17 +18,35 @@ ismc:
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.051, -0.051, 0.051, -0.051, 0.111, -0.111,
- 0.051, 0.051, -0.051, -0.051, 0.002, -0.002,
- -0.167, 0.167, 0.175, -0.175, 0.0, 0.0 ]
+ 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]
msg_ids: [31, 32]
msg_rates: [100.0, 100.0]
- control_loop_freq: 200.0
+ control_rate: 200.0
+ proportional_gain: [10.0, 10.0, 6.0, 6.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
+# Localization sources
+camera:
+ ros__parameters:
+ port: 5600
+
+qualisys_mocap:
+ ros__parameters:
+ ip: "192.168.0.0"
+ port: 22223
+ version: "1.22"
+ body: "bluerov"
+
+# Localizers
aruco_marker_localizer:
ros__parameters:
camera_matrix:
@@ -41,21 +60,15 @@ aruco_marker_localizer:
0.0, 1108.39001, 456.92861, 0.0,
0.0, 0.0, 1.0, 0.0]
-camera:
- ros__parameters:
- port: 5600
-
-qualisys_mocap:
- ros__parameters:
- ip: "192.168.0.0"
- port: 22223
- version: "1.22"
- body: "bluerov"
-
qualisys_localizer:
ros__parameters:
body: "bluerov" # This should be the same as the body parameter setting for the qualisys_mocap node
+gazebo_localizer:
+ ros__parameters:
+ gazebo_odom_topic: "/model/bluerov2/odometry"
+
+# MAVROS & MAVROS plugins
mavros:
ros__parameters:
system_id: 255
diff --git a/blue_bringup/config/bluerov2_heavy.yaml b/blue_bringup/config/bluerov2_heavy.yaml
index 8ce1deca..4a10cd78 100644
--- a/blue_bringup/config/bluerov2_heavy.yaml
+++ b/blue_bringup/config/bluerov2_heavy.yaml
@@ -6,7 +6,7 @@ blue_manager:
ismc:
ros__parameters:
- mass: 11.5
+ mass: 13.5
buoyancy: 112.80
weight: 114.80
inertia_tensor_coeff: [0.16, 0.16, 0.16]
@@ -17,15 +17,20 @@ ismc:
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.06, -0.06, 0.06, -0.06, -0.218, -0.218, 0.218, 0.218,
- 0.06, 0.06, -0.06, -0.06, 0.120, -0.120, 0.120, -0.120,
- -0.1888, 0.1888, 0.1888, -0.1888, 0.0, 0.0, 0.0, 0.0]
+ 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]
msg_ids: [31, 32]
msg_rates: [100.0, 100.0]
- control_loop_freq: 200.0
+ control_rate: 200.0
+ proportional_gain: [10.0, 10.0, 6.0, 6.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
aruco_marker_localizer:
@@ -56,6 +61,10 @@ qualisys_localizer:
ros__parameters:
body: "bluerov" # This should be the same as the body parameter setting for the qualisys_mocap node
+gazebo_localizer:
+ ros__parameters:
+ gazebo_odom_topic: "/model/bluerov2_heavy/odometry"
+
mavros:
ros__parameters:
system_id: 255
@@ -78,3 +87,6 @@ mavros/local_position:
frame_id: "map"
tf:
send: false
+ frame_id: "map"
+ child_frame_id: "base_link"
+ send_fcu: false
diff --git a/blue_bringup/launch/bluerov2.launch.py b/blue_bringup/launch/bluerov2.launch.py
index 7c896456..f390e19c 100644
--- a/blue_bringup/launch/bluerov2.launch.py
+++ b/blue_bringup/launch/bluerov2.launch.py
@@ -24,18 +24,19 @@
ExecuteProcess,
IncludeLaunchDescription,
)
-from launch.conditions import IfCondition, UnlessCondition
+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
+from launch_xml.launch_description_sources import XMLLaunchDescriptionSource
def generate_launch_description() -> LaunchDescription:
"""Generate a launch description to run the system.
Returns:
- The Blue ROS 2 launch description.
+ The launch description for the BlueROV2 base configuration.
"""
args = [
DeclareLaunchArgument(
@@ -54,8 +55,8 @@ def generate_launch_description() -> LaunchDescription:
),
DeclareLaunchArgument(
"localization_source",
- default_value="mocap",
- choices=["mocap", "camera"],
+ default_value="gazebo",
+ choices=["mocap", "camera", "gazebo"],
description="The localization source to stream from.",
),
DeclareLaunchArgument(
@@ -77,7 +78,12 @@ def generate_launch_description() -> LaunchDescription:
DeclareLaunchArgument(
"use_sim",
default_value="false",
- description="Automatically start Gazebo.",
+ description="Launch the Gazebo + ArduSub simulator.",
+ ),
+ DeclareLaunchArgument(
+ "use_foxglove",
+ default_value="false",
+ description="Start the Foxglove bridge.",
),
DeclareLaunchArgument(
"description_package",
@@ -101,11 +107,19 @@ def generate_launch_description() -> LaunchDescription:
" simulation."
),
),
+ DeclareLaunchArgument(
+ "foxglove_bridge_address",
+ default_value="127.0.0.1",
+ description="The Foxglove Studio datasource address.",
+ ),
+ DeclareLaunchArgument(
+ "foxglove_bridge_port",
+ default_value="8765",
+ description="The Foxglove Studio datasource port.",
+ ),
]
- description_package = LaunchConfiguration("description_package")
use_sim = LaunchConfiguration("use_sim")
- gazebo_world_file = LaunchConfiguration("gazebo_world_file")
config_filepath = PathJoinSubstitution(
[
@@ -118,7 +132,7 @@ def generate_launch_description() -> LaunchDescription:
ardusub_params_filepath = PathJoinSubstitution(
[
FindPackageShare("blue_description"),
- "config",
+ "params",
LaunchConfiguration("ardusub_params_file"),
]
)
@@ -151,10 +165,10 @@ def generate_launch_description() -> LaunchDescription:
"-r",
PathJoinSubstitution(
[
- FindPackageShare(description_package),
+ FindPackageShare(LaunchConfiguration("description_package")),
"gazebo",
"worlds",
- gazebo_world_file,
+ LaunchConfiguration("gazebo_world_file"),
]
),
],
@@ -212,7 +226,21 @@ def generate_launch_description() -> LaunchDescription:
"use_mocap": LaunchConfiguration("use_mocap"),
"use_camera": LaunchConfiguration("use_camera"),
}.items(),
- condition=UnlessCondition(use_sim),
+ ),
+ IncludeLaunchDescription(
+ XMLLaunchDescriptionSource(
+ PathJoinSubstitution(
+ [
+ FindPackageShare("foxglove_bridge"),
+ "foxglove_bridge_launch.xml",
+ ]
+ ),
+ ),
+ launch_arguments={
+ "address": LaunchConfiguration("foxglove_bridge_address"),
+ "port": LaunchConfiguration("foxglove_bridge_port"),
+ }.items(),
+ condition=IfCondition(LaunchConfiguration("use_foxglove")),
),
]
diff --git a/blue_bringup/launch/bluerov2_heavy.launch.py b/blue_bringup/launch/bluerov2_heavy.launch.py
index 036cfb28..1b7e06d8 100644
--- a/blue_bringup/launch/bluerov2_heavy.launch.py
+++ b/blue_bringup/launch/bluerov2_heavy.launch.py
@@ -24,18 +24,19 @@
ExecuteProcess,
IncludeLaunchDescription,
)
-from launch.conditions import IfCondition, UnlessCondition
+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
+from launch_xml.launch_description_sources import XMLLaunchDescriptionSource
def generate_launch_description() -> LaunchDescription:
"""Generate a launch description to run the system.
Returns:
- The Blue ROS 2 launch description.
+ The launch description for the BlueROV2 heavy configuration.
"""
args = [
DeclareLaunchArgument(
@@ -54,8 +55,8 @@ def generate_launch_description() -> LaunchDescription:
),
DeclareLaunchArgument(
"localization_source",
- default_value="mocap",
- choices=["mocap", "camera"],
+ default_value="gazebo",
+ choices=["mocap", "camera", "gazebo"],
description="The localization source to stream from.",
),
DeclareLaunchArgument(
@@ -77,7 +78,12 @@ def generate_launch_description() -> LaunchDescription:
DeclareLaunchArgument(
"use_sim",
default_value="false",
- description="Automatically start Gazebo.",
+ description="Launch the Gazebo + ArduSub simulator.",
+ ),
+ DeclareLaunchArgument(
+ "use_foxglove",
+ default_value="false",
+ description="Start the Foxglove bridge.",
),
DeclareLaunchArgument(
"description_package",
@@ -101,11 +107,19 @@ def generate_launch_description() -> LaunchDescription:
" simulation."
),
),
+ DeclareLaunchArgument(
+ "foxglove_bridge_address",
+ default_value="127.0.0.1",
+ description="The Foxglove Studio datasource address.",
+ ),
+ DeclareLaunchArgument(
+ "foxglove_bridge_port",
+ default_value="8765",
+ description="The Foxglove Studio datasource port.",
+ ),
]
- description_package = LaunchConfiguration("description_package")
use_sim = LaunchConfiguration("use_sim")
- gazebo_world_file = LaunchConfiguration("gazebo_world_file")
config_filepath = PathJoinSubstitution(
[
@@ -118,7 +132,7 @@ def generate_launch_description() -> LaunchDescription:
ardusub_params_filepath = PathJoinSubstitution(
[
FindPackageShare("blue_description"),
- "config",
+ "params",
LaunchConfiguration("ardusub_params_file"),
]
)
@@ -151,10 +165,10 @@ def generate_launch_description() -> LaunchDescription:
"-r",
PathJoinSubstitution(
[
- FindPackageShare(description_package),
+ FindPackageShare(LaunchConfiguration("description_package")),
"gazebo",
"worlds",
- gazebo_world_file,
+ LaunchConfiguration("gazebo_world_file"),
]
),
],
@@ -212,7 +226,21 @@ def generate_launch_description() -> LaunchDescription:
"use_mocap": LaunchConfiguration("use_mocap"),
"use_camera": LaunchConfiguration("use_camera"),
}.items(),
- condition=UnlessCondition(use_sim),
+ ),
+ IncludeLaunchDescription(
+ XMLLaunchDescriptionSource(
+ PathJoinSubstitution(
+ [
+ FindPackageShare("foxglove_bridge"),
+ "foxglove_bridge_launch.xml",
+ ]
+ ),
+ ),
+ launch_arguments={
+ "address": LaunchConfiguration("foxglove_bridge_address"),
+ "port": LaunchConfiguration("foxglove_bridge_port"),
+ }.items(),
+ condition=IfCondition(LaunchConfiguration("use_foxglove")),
),
]
diff --git a/blue_bringup/package.xml b/blue_bringup/package.xml
index 8fff7072..c68ab0b2 100644
--- a/blue_bringup/package.xml
+++ b/blue_bringup/package.xml
@@ -17,6 +17,8 @@
mavros
mavros_extras
python3-matplotlib
+ foxglove_bridge
+ teleop_twist_keyboard
ament_copyright
ament_flake8
@@ -24,6 +26,6 @@
python3-pytest
- ament_python
+ ament_cmake
diff --git a/blue_bringup/resource/blue_bringup b/blue_bringup/resource/blue_bringup
deleted file mode 100644
index e69de29b..00000000
diff --git a/blue_bringup/setup.cfg b/blue_bringup/setup.cfg
deleted file mode 100644
index a03d75ab..00000000
--- a/blue_bringup/setup.cfg
+++ /dev/null
@@ -1,4 +0,0 @@
-[develop]
-script_dir=$base/lib/blue_bringup
-[install]
-install_scripts=$base/lib/blue_bringup
diff --git a/blue_bringup/setup.py b/blue_bringup/setup.py
deleted file mode 100644
index f4702d36..00000000
--- a/blue_bringup/setup.py
+++ /dev/null
@@ -1,48 +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_bringup"
-
-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")),
- (os.path.join("share", package_name, "config"), glob("config/*.yaml")),
- ],
- install_requires=["setuptools"],
- zip_safe=True,
- maintainer="Evan Palmer",
- maintainer_email="evanp922@gmail.com",
- description="Entrypoints for the Blue project.",
- license="MIT",
- tests_require=["pytest"],
- entry_points={
- "console_scripts": [],
- },
-)
diff --git a/blue_bringup/test/test_copyright.py b/blue_bringup/test/test_copyright.py
deleted file mode 100644
index 8f18fa4b..00000000
--- a/blue_bringup/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_bringup/test/test_flake8.py b/blue_bringup/test/test_flake8.py
deleted file mode 100644
index f494570f..00000000
--- a/blue_bringup/test/test_flake8.py
+++ /dev/null
@@ -1,25 +0,0 @@
-# Copyright 2017 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_flake8.main import main_with_errors
-
-
-@pytest.mark.flake8
-@pytest.mark.linter
-def test_flake8():
- rc, errors = main_with_errors(argv=[])
- assert rc == 0, "Found %d code style errors / warnings:\n" % len(
- errors
- ) + "\n".join(errors)
diff --git a/blue_control/CMakeLists.txt b/blue_control/CMakeLists.txt
index e5b363d6..eef384b8 100644
--- a/blue_control/CMakeLists.txt
+++ b/blue_control/CMakeLists.txt
@@ -19,14 +19,16 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS
rclcpp
ament_cmake
blue_dynamics
- blue_msgs
+ 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})
diff --git a/blue_control/include/blue_control/controller.hpp b/blue_control/include/blue_control/controller.hpp
index 4f684507..a2fbc5bc 100644
--- a/blue_control/include/blue_control/controller.hpp
+++ b/blue_control/include/blue_control/controller.hpp
@@ -27,42 +27,24 @@
#include
#include "blue_dynamics/hydrodynamics.hpp"
-#include "blue_dynamics/thruster_dynamics.hpp"
+#include "blue_utils/frames.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 "mavros_msgs/srv/message_interval.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 Convert an `std::vector` to an `Eigen::VectorXd`.
- *
- * @note This method is useful when converting a ROS parameter that has been read as a `std::vector`
- * to an `Eigen::VectorXd`.
- *
- * @param vec The `std::vector` to convert.
- * @return An `Eigen::VectorXd` with the same values as `vec`.
- */
-Eigen::VectorXd convertVectorToEigenVector(const std::vector & vec);
-
-/**
- * @brief Convert an `std::vector` to an `Eigen::MatrixXd`.
- *
- * @note This method is useful when converting a ROS parameter that has been read as a `std::vector`
- * to an `Eigen::MatrixXd`.
- *
- * @param vec The `std::vector` to convert.
- * @param rows The total number of rows in the resulting matrix.
- * @param cols The total number of columns in the resulting matrix.
- * @return An `Eigen::MatrixXd` with the same values as `vec`.
- */
-Eigen::MatrixXd convertVectorToEigenMatrix(
- const std::vector & vec, size_t rows, size_t cols);
-
/**
* @brief A base class for custom BlueROV2 controllers.
*/
@@ -77,12 +59,22 @@ class Controller : public rclcpp::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 update() = 0;
+ virtual mavros_msgs::msg::OverrideRCIn calculateControlInput() = 0;
/**
* @brief A collection of the hydrodynamic parameters for the BlueROV2.
@@ -104,13 +96,16 @@ class Controller : public rclcpp::Node
/**
* @brief The current pose and twist of the BlueROV2.
- *
- * @note It is important to note here that the pose information is provided in the inertial frame
- * and the twist is provided in the body frame. For more information on this see:
- * https://github.com/mavlink/mavros/issues/1251
*/
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
*
@@ -118,9 +113,13 @@ class Controller : public rclcpp::Node
*/
double dt_{0.0};
+ // TF2
+ std::shared_ptr tf_buffer_;
+ std::unique_ptr tf_broadcaster_;
+
private:
/**
- * @brief Enable the controller.
+ * @brief Enable/disable the controller.
*
* @details This method enables/disables sending RC Override messages to the BlueROV2.
*
@@ -131,6 +130,15 @@ class Controller : public rclcpp::Node
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);
+
/**
* @brief Set custom MAVLink message rates.
*
@@ -150,14 +158,23 @@ class Controller : public rclcpp::Node
*/
void setMessageRate(int64_t msg_id, float rate);
+ // Manages whether or not control inputs are sent to ArduSub
bool armed_;
+ // 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_;
diff --git a/blue_control/include/blue_control/ismc.hpp b/blue_control/include/blue_control/ismc.hpp
index 071ec3bb..3e545ac6 100644
--- a/blue_control/include/blue_control/ismc.hpp
+++ b/blue_control/include/blue_control/ismc.hpp
@@ -23,7 +23,10 @@
#include
#include "blue_control/controller.hpp"
-#include "blue_msgs/msg/reference.hpp"
+#include "blue_dynamics/thruster_dynamics.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
@@ -41,18 +44,35 @@ class ISMC : public Controller
ISMC();
protected:
- mavros_msgs::msg::OverrideRCIn update() override;
+ void onArm() override;
+ void onDisarm() override;
+ mavros_msgs::msg::OverrideRCIn calculateControlInput() override;
private:
- // Hyperparameters used by the ISMC
- blue::dynamics::Vector6d total_velocity_error_;
- blue::dynamics::Matrix6d convergence_rate_;
+ // ISMC gains
+ blue::dynamics::Matrix6d integral_gain_;
+ blue::dynamics::Matrix6d proportional_gain_;
+ blue::dynamics::Matrix6d derivative_gain_;
double sliding_gain_;
double boundary_thickness_;
+ // Error terms
+ blue::dynamics::Vector6d initial_velocity_error_;
+ blue::dynamics::Vector6d initial_acceleration_error_;
+ blue::dynamics::Vector6d total_velocity_error_;
+
+ // Control whether or not the battery state is used when converting thrust to PWM
bool use_battery_state_;
- blue_msgs::msg::Reference cmd_;
- rclcpp::Subscription::SharedPtr cmd_sub_;
+
+ // 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/package.xml b/blue_control/package.xml
index a1f264a3..931e2144 100644
--- a/blue_control/package.xml
+++ b/blue_control/package.xml
@@ -1,21 +1,29 @@
+
blue_control
0.0.1
Control interface for the BlueROV2
+
Evan Palmer
MIT
+ https://github.com/evan-palmer/blue.git
+ https://github.com/evan-palmer/blue/issues
+
+ Evan Palmer
+
nav_msgs
sensor_msgs
mavros_msgs
rclcpp
- blue_dynamics
eigen
- blue_msgs
std_srvs
tf2_eigen
+ tf2
+ tf2_geometry_msgs
+ tf2_ros
ament_cmake
diff --git a/blue_control/src/controller.cpp b/blue_control/src/controller.cpp
index 8667616b..1fc31aa8 100644
--- a/blue_control/src/controller.cpp
+++ b/blue_control/src/controller.cpp
@@ -20,32 +20,26 @@
#include "blue_control/controller.hpp"
-namespace blue::control
-{
+#include "blue_utils/utils.hpp"
-Eigen::VectorXd convertVectorToEigenVector(const std::vector & vec)
-{
- Eigen::VectorXd eigen_vec(vec.size());
- eigen_vec = Eigen::Map(vec.data(), vec.size());
-
- return eigen_vec;
-}
-
-Eigen::MatrixXd convertVectorToEigenMatrix(
- const std::vector & vec, size_t rows, size_t cols)
+namespace blue::control
{
- Eigen::Map mat(vec.data(), rows, cols);
- return mat;
-}
Controller::Controller(const std::string & node_name)
: Node(std::move(node_name)),
armed_(false)
{
// Declare ROS parameters
- this->declare_parameter("mass", 11.5);
+ 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("msg_ids", std::vector({31, 32}));
+ this->declare_parameter("msg_rates", std::vector({100, 100}));
+ 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}));
@@ -53,47 +47,45 @@ Controller::Controller(const std::string & node_name)
"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("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("msg_ids", std::vector({31, 32}));
- this->declare_parameter("msg_rates", std::vector({100, 100}));
- this->declare_parameter("control_loop_freq", 200.0);
+ this->declare_parameter(
+ "frame", std::vector({true, true, false, false, true, false, false, true}));
- // I'm so sorry for this
- // You can blame the ROS devs for not supporting nested arrays for parameters
+ // 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.06, -0.06, 0.06, -0.06, -0.218, -0.218, 0.218, 0.218,
- 0.06, 0.06, -0.06, -0.06, 0.120, -0.120, 0.120, -0.120,
- -0.1888, 0.1888, 0.1888, -0.1888, 0.0, 0.0, 0.0, 0.0}));
-
- // Get the parameter values
+ "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
+
+ using blue::utils::convertVectorToEigenMatrix;
+
+ // 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 =
- convertVectorToEigenVector(this->get_parameter("inertia_tensor_coeff").as_double_array());
- const Eigen::VectorXd added_mass_coeff =
- convertVectorToEigenVector(this->get_parameter("added_mass_coeff").as_double_array());
- const Eigen::VectorXd linear_damping_coeff =
- convertVectorToEigenVector(this->get_parameter("linear_damping_coeff").as_double_array());
- const Eigen::VectorXd quadratic_damping_coeff =
- convertVectorToEigenVector(this->get_parameter("quadratic_damping_coeff").as_double_array());
- const Eigen::Vector3d center_of_gravity =
- convertVectorToEigenVector(this->get_parameter("center_of_gravity").as_double_array());
- const Eigen::Vector3d center_of_buoyancy =
- convertVectorToEigenVector(this->get_parameter("center_of_buoyancy").as_double_array());
- const Eigen::VectorXd ocean_current =
- convertVectorToEigenVector(this->get_parameter("ocean_current").as_double_array());
+ const Eigen::Vector3d inertia_tensor_coeff = convertVectorToEigenMatrix(
+ this->get_parameter("inertia_tensor_coeff").as_double_array(), 3, 1);
+ const Eigen::Matrix added_mass_coeff = convertVectorToEigenMatrix(
+ this->get_parameter("added_mass_coeff").as_double_array(), 6, 1);
+ const Eigen::Matrix linear_damping_coeff = convertVectorToEigenMatrix(
+ this->get_parameter("linear_damping_coeff").as_double_array(), 6, 1);
+ const Eigen::Matrix quadratic_damping_coeff = convertVectorToEigenMatrix(
+ this->get_parameter("quadratic_damping_coeff").as_double_array(), 6, 1);
+ const Eigen::Vector3d center_of_gravity = convertVectorToEigenMatrix(
+ this->get_parameter("center_of_gravity").as_double_array(), 3, 1);
+ const Eigen::Vector3d center_of_buoyancy = convertVectorToEigenMatrix(
+ this->get_parameter("center_of_buoyancy").as_double_array(), 3, 1);
+ const Eigen::Matrix ocean_current = convertVectorToEigenMatrix(
+ 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();
- size_t num_thrusters = this->get_parameter("num_thrusters").as_int();
- tcm_ = convertVectorToEigenMatrix(tcm_vec, tcm_vec.size() / num_thrusters, num_thrusters);
+ const int num_thrusters = this->get_parameter("num_thrusters").as_int();
+ tcm_ = convertVectorToEigenMatrix(
+ tcm_vec, static_cast(tcm_vec.size() / num_thrusters), num_thrusters);
// Initialize the hydrodynamic parameters
hydrodynamics_ = blue::dynamics::HydrodynamicParameters(
@@ -104,21 +96,26 @@ Controller::Controller(const std::string & node_name)
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", 1);
rc_override_pub_ =
this->create_publisher("mavros/rc/override", 1);
battery_state_sub_ = this->create_subscription(
- "/mavros/battery", 1,
+ "/mavros/battery", rclcpp::SensorDataQoS(),
[this](sensor_msgs::msg::BatteryState::ConstSharedPtr msg) { battery_state_ = *msg; });
odom_sub_ = this->create_subscription(
- "/mavros/local_position/odom", 1,
- [this](nav_msgs::msg::Odometry::ConstSharedPtr msg) { odom_ = *msg; });
+ "/mavros/local_position/odom", rclcpp::SensorDataQoS(),
+ [this](nav_msgs::msg::Odometry::ConstSharedPtr msg) { updateOdomCb(msg); });
arm_srv_ = this->create_service(
- "blue/control/arm", [this](
- const std::shared_ptr request,
- std::shared_ptr response) {
+ "blue/cmd/arm", [this](
+ const std::shared_ptr request,
+ std::shared_ptr response) {
armControllerCb(request, response);
});
@@ -151,14 +148,18 @@ Controller::Controller(const std::string & node_name)
[this, msg_ids, msg_rates]() -> void { setMessageRates(msg_ids, msg_rates); });
// Convert the control loop frequency to seconds
- dt_ = 1 / this->get_parameter("control_loop_freq").as_double();
+ dt_ = 1 / this->get_parameter("control_rate").as_double();
- control_loop_timer_ =
- this->create_wall_timer(std::chrono::duration(dt_), [this]() -> void {
+ // 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(update());
+ rc_override_pub_->publish(calculateControlInput());
}
- });
+ },
+ control_loop_cb_group_);
}
void Controller::armControllerCb(
@@ -166,6 +167,11 @@ void Controller::armControllerCb(
std::shared_ptr response)
{
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;
@@ -177,9 +183,41 @@ void Controller::armControllerCb(
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)
+{
+ // 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);
+
+ // Update the current Odometry reading
+ odom_ = *msg;
+}
+
void Controller::setMessageRates(
const std::vector & msg_ids, const std::vector & rates)
{
diff --git a/blue_control/src/ismc.cpp b/blue_control/src/ismc.cpp
index 26482468..e0882e2c 100644
--- a/blue_control/src/ismc.cpp
+++ b/blue_control/src/ismc.cpp
@@ -22,53 +22,117 @@
#include
#include
-#include
#include "blue_dynamics/thruster_dynamics.hpp"
+#include "blue_utils/utils.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")
+: Controller("ismc"),
+ initial_velocity_error_(blue::dynamics::Vector6d::Zero()),
+ initial_acceleration_error_(blue::dynamics::Vector6d::Zero()),
+ total_velocity_error_(blue::dynamics::Vector6d::Zero())
{
// Declare the ROS parameters specific to this controller
- // There are additional parameters defined by the base controller as well
- this->declare_parameter(
- "convergence_rate", std::vector({100.0, 100.0, 100.0, 100.0, 100.0, 100.0}));
- this->declare_parameter("sliding_gain", 0.0);
- this->declare_parameter("boundary_thickness", 0.0);
+ 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);
- Eigen::VectorXd convergence_diag =
- convertVectorToEigenVector(this->get_parameter("convergence_rate").as_double_array());
- convergence_rate_ = convergence_diag.asDiagonal().toDenseMatrix();
+ // Get the gain matrices
+ Eigen::VectorXd integral_gain_coeff = blue::utils::convertVectorToEigenMatrix(
+ this->get_parameter("integral_gain").as_double_array(), 6, 1);
+ Eigen::VectorXd proportional_gain_coeff = blue::utils::convertVectorToEigenMatrix(
+ this->get_parameter("proportional_gain").as_double_array(), 6, 1);
+ Eigen::VectorXd derivative_gain_coeff = blue::utils::convertVectorToEigenMatrix(
+ 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();
- total_velocity_error_ = blue::dynamics::Vector6d::Zero();
use_battery_state_ = this->get_parameter("use_battery_state").as_bool();
+ // Publish the desired wrench and errors to help with tuning and visualization
+ desired_wrench_pub_ =
+ this->create_publisher("/blue/ismc/desired_wrench", 1);
+ velocity_error_pub_ =
+ this->create_publisher("/blue/ismc/velocity_error", 1);
+
// Update the reference signal when a new command is received
- cmd_sub_ = this->create_subscription(
- "/blue/ismc/cmd", 1, [this](blue_msgs::msg::Reference::ConstSharedPtr msg) { cmd_ = *msg; });
+ cmd_sub_ = this->create_subscription(
+ "/blue/ismc/cmd_vel", 1,
+ [this](geometry_msgs::msg::Twist::ConstSharedPtr msg) { cmd_ = *msg; });
}
-mavros_msgs::msg::OverrideRCIn ISMC::update()
+void ISMC::onArm()
+{
+ // Reset the total velocity error
+ total_velocity_error_ = blue::dynamics::Vector6d::Zero();
+
+ // Reset the initial conditions
+ initial_velocity_error_ = blue::dynamics::Vector6d::Zero();
+ initial_acceleration_error_ = blue::dynamics::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.
+ blue::dynamics::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
+ blue::dynamics::Vector6d accel;
+ blue::utils::fromMsg(accel_, accel);
+ initial_acceleration_error_ -= accel;
+};
+
+void ISMC::onDisarm()
+{
+ // Reset the total velocity error on disarm just to be safe
+ total_velocity_error_ = blue::dynamics::Vector6d::Zero();
+
+ // Reset the initial conditions too
+ initial_velocity_error_ = blue::dynamics::Vector6d::Zero();
+ initial_acceleration_error_ = blue::dynamics::Vector6d::Zero();
+};
+
+mavros_msgs::msg::OverrideRCIn ISMC::calculateControlInput()
{
blue::dynamics::Vector6d velocity;
tf2::fromMsg(odom_.twist.twist, velocity);
// Calculate the velocity error
blue::dynamics::Vector6d velocity_error;
- tf2::fromMsg(cmd_.twist, velocity_error);
+ tf2::fromMsg(cmd_, velocity_error);
velocity_error -= velocity;
- // There is no suitable tf2_eigen function for Accel types :(
- blue::dynamics::Vector6d desired_accel; // NOLINT
- desired_accel << cmd_.accel.linear.x, cmd_.accel.linear.y, cmd_.accel.linear.z,
- cmd_.accel.angular.x, cmd_.accel.angular.y, cmd_.accel.angular.z;
+ // Calculate the acceleration error; assume that the desired acceleration is 0
+ blue::dynamics::Vector6d accel_error;
+ blue::utils::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 frame
+ // 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);
@@ -78,37 +142,80 @@ mavros_msgs::msg::OverrideRCIn ISMC::update()
// Calculate the sliding surface
blue::dynamics::Vector6d surface =
- velocity_error + convergence_rate_ * total_velocity_error_; // NOLINT
+ 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
- surface.unaryExpr([this](double x) { return tanh(x / boundary_thickness_); });
+ // 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
+ blue::dynamics::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
+ blue::dynamics::Vector6d tau1 = -sliding_gain_ * surface;
- const blue::dynamics::Vector6d forces =
- hydrodynamics_.inertia.getInertia() * (desired_accel + convergence_rate_ * velocity_error) +
- hydrodynamics_.coriolis.calculateCoriolis(velocity) * velocity +
- hydrodynamics_.damping.calculateDamping(velocity) * velocity +
- hydrodynamics_.restoring_forces.calculateRestoringForces(orientation.toRotationMatrix()) +
- sliding_gain_ * surface;
+ blue::dynamics::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 = blue::utils::toMsg(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.c_str(), blue::transforms::kBaseLinkFrdFrameId.c_str(),
+ e.what());
+ return msg;
+ }
+
+ geometry_msgs::msg::WrenchStamped wrench_frd;
+ tf2::doTransform(wrench, wrench_frd, transform);
+ blue::utils::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
- const Eigen::VectorXd pwms = tcm_.completeOrthogonalDecomposition().pseudoInverse() * forces;
+ Eigen::VectorXd thruster_forces = tcm_.completeOrthogonalDecomposition().pseudoInverse() * forces;
// Convert the thruster forces to PWM values
+ Eigen::VectorXi pwms;
+
if (use_battery_state_) {
- pwms.unaryExpr([this](double x) {
+ pwms = thruster_forces.unaryExpr([this](double x) {
return blue::dynamics::calculatePwmFromThrustSurface(x, battery_state_.voltage);
});
} else {
- pwms.unaryExpr([this](double x) { return blue::dynamics::calculatePwmFromThrustCurve(x); });
- }
-
- mavros_msgs::msg::OverrideRCIn msg;
-
- // We only modify the first "n" channels where "n" is the total number of thrusters
- for (uint16_t & channel : msg.channels) {
- channel = mavros_msgs::msg::OverrideRCIn::CHAN_NOCHANGE;
+ pwms = thruster_forces.unaryExpr(
+ [this](double x) { return blue::dynamics::calculatePwmFromThrustCurve(x); });
}
// Calculate the deadzone band
@@ -142,7 +249,9 @@ int main(int argc, char ** argv)
{
rclcpp::init(argc, argv);
auto node = std::make_shared();
- rclcpp::spin(node);
+ rclcpp::executors::MultiThreadedExecutor executor;
+ executor.add_node(node);
+ executor.spin();
rclcpp::shutdown();
return 0;
}
diff --git a/blue_description/CMakeLists.txt b/blue_description/CMakeLists.txt
index 2def9d00..30091493 100644
--- a/blue_description/CMakeLists.txt
+++ b/blue_description/CMakeLists.txt
@@ -29,7 +29,7 @@ if(BUILD_TESTING)
endif()
install(
- DIRECTORY gazebo config
+ DIRECTORY gazebo params
DESTINATION share/${PROJECT_NAME}
)
diff --git a/blue_description/gazebo/models/bluerov2/model.sdf b/blue_description/gazebo/models/bluerov2/model.sdf
index 5bb2bebb..ecdbf16a 100644
--- a/blue_description/gazebo/models/bluerov2/model.sdf
+++ b/blue_description/gazebo/models/bluerov2/model.sdf
@@ -415,5 +415,15 @@
+
+
+ map
+ base_link
+ 3
+ 100
+
+
diff --git a/blue_description/gazebo/models/bluerov2_heavy/model.sdf b/blue_description/gazebo/models/bluerov2_heavy/model.sdf
index 9525634b..8fac7cdd 100644
--- a/blue_description/gazebo/models/bluerov2_heavy/model.sdf
+++ b/blue_description/gazebo/models/bluerov2_heavy/model.sdf
@@ -8,7 +8,7 @@
0.0 0.0 0.011 0 0 0
- 13.5
+ 13.0
0.26
0
@@ -33,7 +33,7 @@
0.0 0.0 0.06 0 0 0
- 0.457 0.575 0.052
+ 0.457 0.575 0.05
@@ -520,5 +520,15 @@
+
+
+ map
+ base_link
+ 3
+ 100
+
+
diff --git a/blue_description/config/bluerov2.parm b/blue_description/params/bluerov2.parm
similarity index 77%
rename from blue_description/config/bluerov2.parm
rename to blue_description/params/bluerov2.parm
index 8c1f0012..936c093d 100644
--- a/blue_description/config/bluerov2.parm
+++ b/blue_description/params/bluerov2.parm
@@ -88,3 +88,16 @@ BARO_EXT_BUS 1
EK2_ENABLE 0
EK3_ENABLE 1
AHRS_EKF_TYPE 3
+
+# Enable external nav
+# From https://ardupilot.org/copter/docs/common-vio-tracking-camera.html
+GPS_TYPE 0 # Disable GPS
+VISO_TYPE 1 # External vision
+EK3_SRC1_POSXY 6 # External nav
+EK3_SRC1_VELXY 6 # External nav
+EK3_SRC1_POSZ 1 # Baro is the primary z source
+EK3_SRC1_VELZ 6 # External nav z velocity influences EKF
+COMPASS_USE 0 # Disable compass 1
+COMPASS_USE2 0 # Disable compass 2
+COMPASS_USE3 0 # Disable compass 3
+EK3_SRC1_YAW 6 # External nav
diff --git a/blue_description/config/bluerov2_heavy.parm b/blue_description/params/bluerov2_heavy.parm
similarity index 72%
rename from blue_description/config/bluerov2_heavy.parm
rename to blue_description/params/bluerov2_heavy.parm
index 09630bad..4f220f34 100644
--- a/blue_description/config/bluerov2_heavy.parm
+++ b/blue_description/params/bluerov2_heavy.parm
@@ -66,3 +66,16 @@ FRAME_CONFIG 2.000
EK2_ENABLE 0
EK3_ENABLE 1
AHRS_EKF_TYPE 3
+
+# Enable external nav
+# From https://ardupilot.org/copter/docs/common-vio-tracking-camera.html
+GPS_TYPE 0 # Disable GPS
+VISO_TYPE 1 # External vision
+EK3_SRC1_POSXY 6 # External nav
+EK3_SRC1_VELXY 6 # External nav
+EK3_SRC1_POSZ 1 # Baro is the primary z source
+EK3_SRC1_VELZ 6 # External nav z velocity influences EKF
+COMPASS_USE 0 # Disable compass 1
+COMPASS_USE2 0 # Disable compass 2
+COMPASS_USE3 0 # Disable compass 3
+EK3_SRC1_YAW 6 # External nav
diff --git a/blue_localization/blue_localization/localizer.py b/blue_localization/blue_localization/localizer.py
index ceb4bc49..1eb4b412 100644
--- a/blue_localization/blue_localization/localizer.py
+++ b/blue_localization/blue_localization/localizer.py
@@ -27,17 +27,24 @@
import tf2_geometry_msgs # noqa
import tf_transformations as tf
from cv_bridge import CvBridge
-from geometry_msgs.msg import PoseStamped
+from geometry_msgs.msg import Pose, PoseStamped, TransformStamped
+from nav_msgs.msg import Odometry
from rclpy.node import Node
from sensor_msgs.msg import Image
from tf2_ros import TransformException
from tf2_ros.buffer import Buffer
+from tf2_ros.transform_broadcaster import TransformBroadcaster
from tf2_ros.transform_listener import TransformListener
class Localizer(Node, ABC):
"""Base class for implementing a visual localization interface."""
+ MAP_FRAME = "map"
+ MAP_NED_FRAME = "map_ned"
+ BASE_LINK_FRAME = "base_link"
+ BASE_LINK_FRD_FRAME = "base_link_frd"
+
def __init__(self, node_name: str) -> None:
"""Create a new localizer.
@@ -50,11 +57,45 @@ def __init__(self, node_name: str) -> None:
# Provide access to TF2
self.tf_buffer = Buffer()
self.tf_listener = TransformListener(self.tf_buffer, self)
+ self.tf_broadcaster = TransformBroadcaster(self, 1)
# Poses are sent to the ArduPilot EKF
self.localization_pub = self.create_publisher(
PoseStamped, "/mavros/vision_pose/pose", 1
)
+ self.odometry_pub = self.create_publisher(Odometry, "/mavros/odometry/out", 1)
+
+ @staticmethod
+ def convert_pose_to_transform(
+ node: Node, pose: Pose, reference_frame: str, child_frame: str
+ ) -> TransformStamped:
+ """Convert a Pose message into a TransformStamped.
+
+ Args:
+ node: The ROS 2 node that is calling the method.
+ pose: The Pose message to convert into a TransformStamped message.
+ reference_frame: The frame that the TransformStamped transforms from.
+ child_frame: The frame that the TrasnformStamped transforms to.
+
+ Returns:
+ The converted TransformStamped message.
+ """
+ tf = TransformStamped()
+
+ tf.header.stamp = node.get_clock().now().to_msg()
+ tf.header.frame_id = reference_frame
+ tf.child_frame_id = child_frame
+
+ tf.transform.translation.x = pose.position.x
+ tf.transform.translation.y = pose.position.y
+ tf.transform.translation.z = pose.position.z
+
+ tf.transform.rotation.x = pose.orientation.x
+ tf.transform.rotation.y = pose.orientation.y
+ tf.transform.rotation.z = pose.orientation.z
+ tf.transform.rotation.w = pose.orientation.w
+
+ return tf
class ArucoMarkerLocalizer(Localizer):
@@ -278,6 +319,49 @@ def proxy_pose_cb(self, pose: PoseStamped) -> None:
self.localization_pub.publish(pose)
+class GazeboLocalizer(Localizer):
+ """Localize the BlueROV2 using the Gazebo ground-truth data."""
+
+ def __init__(self) -> None:
+ """Create a new Gazebo localizer."""
+ super().__init__("gazebo_localizer")
+
+ # We need to know the topic to stream from
+ self.declare_parameter("gazebo_odom_topic", "")
+
+ # Subscribe to that topic so that we can proxy messages to the ArduSub EKF
+ self.create_subscription(
+ Odometry,
+ self.get_parameter("gazebo_odom_topic").get_parameter_value().string_value,
+ self.proxy_odom_cb,
+ 1,
+ )
+
+ def proxy_odom_cb(self, msg: Odometry) -> None:
+ """Proxy the pose data from the Gazebo odometry ground-truth data.
+
+ Args:
+ msg: The Gazebo ground-truth odometry for the BlueROV2.
+ """
+ # Use the Odometry message to publish the transform from the map frame to the
+ # base_link frame
+ tf_map_base = Localizer.convert_pose_to_transform(
+ self, msg.pose.pose, self.MAP_FRAME, self.BASE_LINK_FRAME
+ )
+ self.tf_broadcaster.sendTransform(tf_map_base)
+
+ pose = PoseStamped()
+
+ # Pose is provided in the parent header frame
+ pose.header.frame_id = msg.header.frame_id
+ pose.header.stamp = msg.header.stamp
+
+ # We only need the pose; we don't need the covariance
+ pose.pose = msg.pose.pose
+
+ self.localization_pub.publish(pose)
+
+
def main_aruco(args: list[str] | None = None):
"""Run the ArUco marker detector."""
rclpy.init(args=args)
@@ -298,3 +382,14 @@ def main_qualisys(args: list[str] | None = None):
node.destroy_node()
rclpy.shutdown()
+
+
+def main_gazebo(args: list[str] | None = None):
+ """Run the Gazebo localizer."""
+ rclpy.init(args=args)
+
+ node = GazeboLocalizer()
+ rclpy.spin(node)
+
+ node.destroy_node()
+ rclpy.shutdown()
diff --git a/blue_localization/launch/localization.launch.py b/blue_localization/launch/localization.launch.py
index 461117ff..a77f2cff 100644
--- a/blue_localization/launch/localization.launch.py
+++ b/blue_localization/launch/localization.launch.py
@@ -46,7 +46,7 @@ def generate_launch_description() -> LaunchDescription:
DeclareLaunchArgument(
"localization_source",
default_value="mocap",
- choices=["mocap", "camera"],
+ choices=["mocap", "camera", "gazebo"],
description="The localization source to stream from.",
),
DeclareLaunchArgument(
@@ -128,6 +128,16 @@ def generate_launch_description() -> LaunchDescription:
PythonExpression(["'", localization_source, "' == 'mocap'"])
),
),
+ Node(
+ package="blue_localization",
+ executable="gazebo_localizer",
+ name="gazebo_localizer",
+ output="screen",
+ parameters=[LaunchConfiguration("config_filepath")],
+ condition=IfCondition(
+ PythonExpression(["'", localization_source, "' == 'gazebo'"])
+ ),
+ ),
]
includes = [
diff --git a/blue_localization/setup.py b/blue_localization/setup.py
index 7c45d1dd..638ab6df 100644
--- a/blue_localization/setup.py
+++ b/blue_localization/setup.py
@@ -50,6 +50,7 @@
"qualisys_mocap = blue_localization.source:main_qualisys_mocap",
"aruco_marker_localizer = blue_localization.localizer:main_aruco",
"qualisys_localizer = blue_localization.localizer:main_qualisys",
+ "gazebo_localizer = blue_localization.localizer:main_gazebo",
],
},
)
diff --git a/blue_manager/blue_manager/manager.py b/blue_manager/blue_manager/manager.py
index 06024034..2275ae31 100644
--- a/blue_manager/blue_manager/manager.py
+++ b/blue_manager/blue_manager/manager.py
@@ -68,7 +68,7 @@ def __init__(self) -> None:
# Services
self.set_pwm_passthrough_srv = self.create_service(
SetBool,
- "/blue/manager/enable_passthrough",
+ "/blue/cmd/enable_passthrough",
self.set_rc_passthrough_mode_cb,
callback_group=reentrant_callback_group,
)
@@ -223,8 +223,17 @@ def set_rc_passthrough_mode_cb(
# 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
- for param in passthrough_params.values():
- param.value.integer_value = 1 # type: ignore
+ 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(
diff --git a/blue_msgs/CMakeLists.txt b/blue_msgs/CMakeLists.txt
deleted file mode 100644
index efa6dc4e..00000000
--- a/blue_msgs/CMakeLists.txt
+++ /dev/null
@@ -1,29 +0,0 @@
-cmake_minimum_required(VERSION 3.8)
-project(blue_msgs)
-
-if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
- add_compile_options(-Wall -Wextra -Wpedantic)
-endif()
-
-find_package(ament_cmake REQUIRED)
-find_package(std_msgs REQUIRED)
-find_package(geometry_msgs REQUIRED)
-find_package(rosidl_default_generators REQUIRED)
-
-set(msg_files
- "msg/Reference.msg"
-)
-
-rosidl_generate_interfaces(${PROJECT_NAME}
- ${msg_files}
- DEPENDENCIES builtin_interfaces std_msgs geometry_msgs
-)
-
-if(BUILD_TESTING)
- find_package(ament_lint_auto REQUIRED)
- ament_lint_auto_find_test_dependencies()
-endif()
-
-ament_export_dependencies(rosidl_default_runtime)
-
-ament_package()
diff --git a/blue_msgs/msg/Reference.msg b/blue_msgs/msg/Reference.msg
deleted file mode 100644
index abf872b6..00000000
--- a/blue_msgs/msg/Reference.msg
+++ /dev/null
@@ -1,4 +0,0 @@
-# Defines the desired reference values for a controller.
-std_msgs/Header header
-geometry_msgs/Twist twist
-geometry_msgs/Accel accel
diff --git a/blue_utils/CMakeLists.txt b/blue_utils/CMakeLists.txt
new file mode 100644
index 00000000..6ab6b4b1
--- /dev/null
+++ b/blue_utils/CMakeLists.txt
@@ -0,0 +1,68 @@
+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
+ blue_dynamics
+ geometry_msgs
+ eigen3_cmake_module
+ Eigen3
+)
+
+foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS})
+ find_package(${Dependency} REQUIRED)
+endforeach()
+
+include_directories(
+ include
+)
+
+add_library(${PROJECT_NAME} INTERFACE)
+
+target_link_libraries(${PROJECT_NAME} INTERFACE ${rclcpp_LIBRARIES})
+target_include_directories(${PROJECT_NAME} INTERFACE)
+ament_target_dependencies(${PROJECT_NAME} INTERFACE ${THIS_PACKAGE_INCLUDE_DEPENDS})
+
+install(
+ DIRECTORY include/
+ DESTINATION include
+)
+
+install(
+ TARGETS ${PROJECT_NAME}
+ 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 the two below
+ set(ament_cmake_copyright_FOUND TRUE)
+ set(ament_cmake_uncrustify_FOUND TRUE)
+
+ ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_export_targets(${PROJECT_NAME} HAS_LIBRARY_TARGET)
+ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS})
+
+ament_package()
diff --git a/blue_msgs/LICENSE b/blue_utils/LICENSE
similarity index 100%
rename from blue_msgs/LICENSE
rename to blue_utils/LICENSE
diff --git a/blue_utils/include/blue_utils/frames.hpp b/blue_utils/include/blue_utils/frames.hpp
new file mode 100644
index 00000000..120f5eb4
--- /dev/null
+++ b/blue_utils/include/blue_utils/frames.hpp
@@ -0,0 +1,32 @@
+// 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
+
+namespace blue::transforms
+{
+
+// Coordinate frame IDs
+const std::string kMapFrameId{"map"};
+const std::string kMapNedFrameId{"map_ned"};
+const std::string kBaseLinkFrameId{"base_link"};
+const std::string kBaseLinkFrdFrameId{"base_link_frd"};
+
+} // namespace blue::transforms
diff --git a/blue_utils/include/blue_utils/utils.hpp b/blue_utils/include/blue_utils/utils.hpp
new file mode 100644
index 00000000..de957878
--- /dev/null
+++ b/blue_utils/include/blue_utils/utils.hpp
@@ -0,0 +1,98 @@
+// 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/hydrodynamics.hpp"
+#include "geometry_msgs/msg/accel.hpp"
+#include "geometry_msgs/msg/wrench.hpp"
+
+namespace blue::utils
+{
+
+/**
+ * @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 convertVectorToEigenMatrix(
+ 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;
+}
+
+/**
+ * @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, blue::dynamics::Vector6d & out)
+{
+ blue::dynamics::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, blue::dynamics::Vector6d & out)
+{
+ blue::dynamics::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.
+ *
+ * @param in The Eigen vector to convert into a Wrench message.
+ * @return geometry_msgs::msg::Wrench
+ */
+inline geometry_msgs::msg::Wrench toMsg(const blue::dynamics::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 blue::utils
diff --git a/blue_msgs/package.xml b/blue_utils/package.xml
similarity index 66%
rename from blue_msgs/package.xml
rename to blue_utils/package.xml
index e3baa42a..f598fb6f 100644
--- a/blue_msgs/package.xml
+++ b/blue_utils/package.xml
@@ -2,9 +2,9 @@
- blue_msgs
+ blue_utils
0.0.1
- Custom messages available for use by the controllers
+ Common helper functions and constants
Evan Palmer
MIT
@@ -14,18 +14,14 @@
Evan Palmer
+ blue_dynamics
+ eigen
+
ament_cmake
ament_lint_auto
ament_lint_common
- std_msgs
- geometry_msgs
-
- rosidl_default_generators
- rosidl_default_runtime
- rosidl_interface_packages
-
ament_cmake
diff --git a/blue_utils/scripts/arm.sh b/blue_utils/scripts/arm.sh
new file mode 100755
index 00000000..ace7be04
--- /dev/null
+++ b/blue_utils/scripts/arm.sh
@@ -0,0 +1,7 @@
+#!/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/blue_utils/scripts/disarm.sh b/blue_utils/scripts/disarm.sh
new file mode 100755
index 00000000..778dd254
--- /dev/null
+++ b/blue_utils/scripts/disarm.sh
@@ -0,0 +1,7 @@
+#!/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/blue_utils/scripts/send_cmd.sh b/blue_utils/scripts/send_cmd.sh
new file mode 100755
index 00000000..643409fd
--- /dev/null
+++ b/blue_utils/scripts/send_cmd.sh
@@ -0,0 +1,5 @@
+#!/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/blue_utils/scripts/send_pwm.sh b/blue_utils/scripts/send_pwm.sh
new file mode 100755
index 00000000..39576ce2
--- /dev/null
+++ b/blue_utils/scripts/send_pwm.sh
@@ -0,0 +1,5 @@
+#!/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]}"