Port angler_localization to blue_localization

.docker/Dockerfile

@@ -21,11 +21,32 @@ RUN apt-get -q update \
     clang-format-14 \
     clang-tidy \
     clang-tools \
+    python3-pip \
+    python3-dev \
     software-properties-common \
     && apt-get autoremove -y \
     && apt-get clean -y \
     && rm -rf /var/lib/apt/lists/*
 
+# Install Qualisys Python SDK
+RUN python3 -m pip install qtm
+
+# Install gstreamer
+RUN apt-get -q update \
+    && apt-get -q -y upgrade \
+    && apt-get -q install --no-install-recommends -y \
+    python3-gi \
+    gstreamer1.0-tools \
+    gir1.2-gstreamer-1.0 \
+    gir1.2-gst-plugins-base-1.0 \
+    gstreamer1.0-plugins-good \
+    gstreamer1.0-plugins-ugly \
+    gstreamer1.0-plugins-bad \
+    gstreamer1.0-libav \
+    && apt-get autoremove -y \
+    && apt-get clean -y \
+    && rm -rf /var/lib/apt/lists/*
+
 # Install all ROS dependencies
 RUN apt-get -q update \
     && apt-get -q -y upgrade \
@@ -110,8 +131,6 @@ RUN apt-get -q update \
 RUN apt-get -q update \
     && apt-get -q -y upgrade \
     && apt-get -q install --no-install-recommends -y \
-    python3-dev \
-    python3-pip \
     iputils-ping \
     net-tools \
     gdb \
@@ -120,7 +139,7 @@ RUN apt-get -q update \
     && rm -rf /var/lib/apt/lists/*
 
 # Install debugging/linting Python packages
-RUN pip3 install \
+RUN python3 -m pip install \
     pre-commit \
     mypy \
     isort \

.dockerignore

@@ -7,3 +7,4 @@
 !blue_control
 !blue_msgs
 !blue_bringup
+!blue_localization

blue_bringup/config/blue.yaml

@@ -25,6 +25,35 @@ ismc:
           -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
+
+aruco_marker_localizer:
+  ros__parameters:
+    camera_matrix:
+      [1078.17559, 0.0,        1010.57086,
+       0.0,        1076.46176, 463.06243,
+       0.0,        0.0,        1.0]
+    distortion_coefficients:
+      [0.019645, 0.007271, -0.004324, -0.001628, 0.000000]
+    projection_matrix:
+      [1108.25366, 0.0,        1003.75555, 0.0,
+       0.0,        1108.39001, 456.92861,  0.0,
+       0.0,        0.0,        1.0,        0.0]
+
+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
 
 mavros:
   ros__parameters:

blue_bringup/launch/blue.launch.py

@@ -48,6 +48,28 @@ def generate_launch_description() -> LaunchDescription:
             ),
             choices=["ismc"],
         ),
+        DeclareLaunchArgument(
+            "localization_source",
+            default_value="mocap",
+            choices=["mocap", "camera"],
+            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."
+            ),
+        ),
     ]
 
     config_filepath = PathJoinSubstitution(
@@ -88,6 +110,19 @@ def generate_launch_description() -> LaunchDescription:
                 "controller": LaunchConfiguration("controller"),
             }.items(),
         ),
+        IncludeLaunchDescription(
+            PythonLaunchDescriptionSource(
+                PathJoinSubstitution(
+                    [FindPackageShare("blue_localization"), "localization.launch.py"]
+                )
+            ),
+            launch_arguments={
+                "config_filepath": config_filepath,
+                "localization_source": LaunchConfiguration("localization_source"),
+                "use_mocap": LaunchConfiguration("use_mocap"),
+                "use_camera": LaunchConfiguration("use_camera"),
+            }.items(),
+        ),
     ]
 
     return LaunchDescription(args + nodes + includes)

blue_control/CMakeLists.txt

@@ -45,7 +45,7 @@ include_directories(
 )
 
 # Create a library for the base_controller class
-add_library(base_controller include/blue_control/base_controller.hpp src/base_controller.cpp)
+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

blue_control/include/blue_control/base_controller.hpp → blue_control/include/blue_control/controller.hpp

@@ -66,15 +66,15 @@ Eigen::MatrixXd convertVectorToEigenMatrix(
 /**
  * @brief A base class for custom BlueROV2 controllers.
  */
-class BaseController : public rclcpp::Node
+class Controller : public rclcpp::Node
 {
 public:
   /**
-   * @brief Construct a new BaseController object.
+   * @brief Construct a new Controller object.
    *
    * @param node_name The name of the ROS node.
    */
-  explicit BaseController(const std::string & node_name);
+  explicit Controller(const std::string & node_name);
 
 protected:
   /**
@@ -102,7 +102,6 @@ class BaseController : public rclcpp::Node
    */
   sensor_msgs::msg::BatteryState battery_state_;
 
-  //
   /**
    * @brief The current pose and twist of the BlueROV2.
    *
@@ -112,6 +111,13 @@ class BaseController : public rclcpp::Node
    */
   nav_msgs::msg::Odometry odom_;
 
+  /**
+   * @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};
+
 private:
   /**
    * @brief Enable the controller.

blue_control/include/blue_control/ismc.hpp

@@ -22,7 +22,7 @@
 
 #include <Eigen/Dense>
 
-#include "blue_control/base_controller.hpp"
+#include "blue_control/controller.hpp"
 #include "blue_msgs/msg/reference.hpp"
 #include "mavros_msgs/msg/override_rc_in.hpp"
 
@@ -32,7 +32,7 @@ namespace blue::control
 /**
  * @brief Integral sliding mode controller for the BlueROV2.
  */
-class ISMC : public BaseController
+class ISMC : public Controller
 {
 public:
   /**

blue_control/src/base_controller.cpp → blue_control/src/controller.cpp

@@ -18,7 +18,7 @@
 // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
 // THE SOFTWARE.
 
-#include "blue_control/base_controller.hpp"
+#include "blue_control/controller.hpp"
 
 namespace blue::control
 {
@@ -38,7 +38,7 @@ Eigen::MatrixXd convertVectorToEigenMatrix(
   return mat;
 }
 
-BaseController::BaseController(const std::string & node_name)
+Controller::Controller(const std::string & node_name)
 : Node(std::move(node_name)),
   armed_(false)
 {
@@ -59,6 +59,7 @@ BaseController::BaseController(const std::string & node_name)
   this->declare_parameter("num_thrusters", 8);
   this->declare_parameter("msg_ids", std::vector<int>({31, 32}));
   this->declare_parameter("msg_rates", std::vector<double>({100, 100}));
+  this->declare_parameter("control_loop_freq", 200.0);
 
   // I'm so sorry for this
   // You can blame the ROS devs for not supporting nested arrays for parameters
@@ -149,17 +150,18 @@ BaseController::BaseController(const std::string & node_name)
     std::chrono::seconds(10),
     [this, msg_ids, msg_rates]() -> void { setMessageRates(msg_ids, msg_rates); });
 
-  // Run the controller at a rate of 200 Hz
-  // ArduSub only runs at a rate of 100 Hz, but we want to make sure to run the controller at
-  // a faster rate than the autopilot
-  control_loop_timer_ = this->create_wall_timer(std::chrono::milliseconds(5), [this]() -> void {
-    if (armed_) {
-      rc_override_pub_->publish(update());
-    }
-  });
+  // Convert the control loop frequency to seconds
+  dt_ = 1 / this->get_parameter("control_loop_freq").as_double();
+
+  control_loop_timer_ =
+    this->create_wall_timer(std::chrono::duration<double>(dt_), [this]() -> void {
+      if (armed_) {
+        rc_override_pub_->publish(update());
+      }
+    });
 }
 
-void BaseController::armControllerCb(
+void Controller::armControllerCb(
   const std::shared_ptr<std_srvs::srv::SetBool::Request> request,
   std::shared_ptr<std_srvs::srv::SetBool::Response> response)
 {
@@ -178,7 +180,7 @@ void BaseController::armControllerCb(
   }
 }
 
-void BaseController::setMessageRates(
+void Controller::setMessageRates(
   const std::vector<int64_t> & msg_ids, const std::vector<float> & rates)
 {
   // Check that the message IDs and rates are the same length
@@ -196,7 +198,7 @@ void BaseController::setMessageRates(
   }
 }
 
-void BaseController::setMessageRate(int64_t msg_id, float rate)
+void Controller::setMessageRate(int64_t msg_id, float rate)
 {
   auto request = std::make_shared<mavros_msgs::srv::MessageInterval::Request>();
 

blue_control/src/ismc.cpp

@@ -29,12 +29,12 @@ namespace blue::control
 {
 
 ISMC::ISMC()
-: BaseController("ismc")
+: Controller("ismc")
 {
   // 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<double>{100.0, 100.0, 100.0, 100.0, 100.0, 100.0});
+    "convergence_rate", std::vector<double>({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);
 
@@ -73,7 +73,7 @@ mavros_msgs::msg::OverrideRCIn ISMC::update()
 
   // 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;
+  total_velocity_error_ += velocity_error * dt_;
 
   // Calculate the sliding surface
   Eigen::VectorXd surface = velocity_error + convergence_rate_ * total_velocity_error_;  // NOLINT

blue_localization/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.