Implement a base controller and integral sliding mode controller

.clang-tidy

@@ -15,7 +15,6 @@ Checks: >
   -misc-non-private-member-variables-in-classes,
   -modernize-return-braced-init-list,
   -modernize-use-trailing-return-type,
-  -performance-move-const-arg,
   -readability-braces-around-statements,
   -readability-identifier-length,
   -readability-magic-numbers,
@@ -25,7 +24,6 @@ Checks: >
   -bugprone-narrowing-conversions,
   -bugprone-easily-swappable-parameters,
   -bugprone-implicit-widening-of-multiplication-result,
-  -modernize-avoid-bind
 WarningsAsErrors: "*"
 CheckOptions:
   - key: readability-braces-around-statements.ShortStatementLines

.dockerignore

@@ -4,3 +4,5 @@
 # Except the following
 !blue_dynamics
 !blue_manager
+!blue_control
+!blue_msgs

blue_control/CMakeLists.txt

@@ -0,0 +1,83 @@
+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_msgs
+  nav_msgs
+  sensor_msgs
+  mavros_msgs
+  std_srvs
+  eigen3_cmake_module
+  Eigen3
+)
+
+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/base_controller.hpp src/base_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
+)
+
+
+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_package()

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

blue_control/include/blue_control/base_controller.hpp

@@ -0,0 +1,143 @@
+// 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 <Eigen/Dense>
+#include <atomic>
+#include <memory>
+#include <string>
+#include <vector>
+
+#include "blue_dynamics/hydrodynamics.hpp"
+#include "blue_dynamics/thruster_dynamics.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"
+
+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<double> & 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<double> & vec, size_t rows, size_t cols);
+
+/**
+ * @brief A base class for custom BlueROV2 controllers.
+ */
+class BaseController : public rclcpp::Node
+{
+public:
+  /**
+   * @brief Construct a new BaseController object.
+   *
+   * @param node_name The name of the ROS node.
+   */
+  explicit BaseController(const std::string & node_name);
+
+protected:
+  /**
+   * @brief Update the current control inputs to the thrusters
+   *
+   * @return mavros_msgs::msg::OverrideRCIn
+   */
+  virtual mavros_msgs::msg::OverrideRCIn update() = 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.
+   *
+   * @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_;
+
+private:
+  /**
+   * @brief Enable 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<std_srvs::srv::SetBool::Request> request,
+    std::shared_ptr<std_srvs::srv::SetBool::Response> response);
+
+  bool armed_;
+
+  // Publishers
+  rclcpp::Publisher<mavros_msgs::msg::OverrideRCIn>::SharedPtr rc_override_pub_;
+
+  // Subscribers
+  rclcpp::Subscription<sensor_msgs::msg::BatteryState>::SharedPtr battery_state_sub_;
+  rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr odom_sub_;
+
+  // Timers
+  rclcpp::TimerBase::SharedPtr control_loop_timer_;
+
+  // Services
+  rclcpp::Service<std_srvs::srv::SetBool>::SharedPtr arm_srv_;
+};
+
+}  // namespace blue::control

blue_control/include/blue_control/ismc.hpp

@@ -0,0 +1,57 @@
+// 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 <Eigen/Dense>
+
+#include "blue_control/base_controller.hpp"
+#include "blue_msgs/msg/reference.hpp"
+#include "mavros_msgs/msg/override_rc_in.hpp"
+
+namespace blue::control
+{
+
+/**
+ * @brief Integral sliding mode controller for the BlueROV2.
+ */
+class ISMC : public BaseController
+{
+public:
+  /**
+   * @brief Construct a new ISMC object.
+   */
+  ISMC();
+
+protected:
+  mavros_msgs::msg::OverrideRCIn update() override;
+
+private:
+  // Hyperparameters used by the ISMC
+  Eigen::VectorXd total_velocity_error_;
+  Eigen::MatrixXd convergence_rate_;
+  double sliding_gain_;
+  double boundary_thickness_;
+
+  blue_msgs::msg::Reference cmd_;
+  rclcpp::Subscription<blue_msgs::msg::Reference>::SharedPtr cmd_sub_;
+};
+
+}  // namespace blue::control

blue_control/package.xml

@@ -0,0 +1,27 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>blue_control</name>
+  <version>0.0.1</version>
+  <description>Control interface for the BlueROV2</description>
+  <maintainer email="evanp922@gmail.com">Evan Palmer</maintainer>
+  <license>MIT</license>
+
+  <depend>nav_msgs</depend>
+  <depend>sensor_msgs</depend>
+  <depend>mavros_msgs</depend>
+  <depend>rclcpp</depend>
+  <depend>blue_dynamics</depend>
+  <depend>eigen</depend>
+  <depend>blue_msgs</depend>
+  <depend>std_srvs</depend>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>