Tune ISMC

.gitignore

@@ -48,3 +48,7 @@ ros_gz/
 
 # ArduSub Sim
 eeprom.bin
+mav.parm
+mav.tlog
+mav.tlog.raw
+logs/

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
 

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()

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

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

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")),
         ),
     ]