Feature tam control

thruster_allocation_matrix_controller/CMakeLists.txt

@@ -1,26 +1,87 @@
 cmake_minimum_required(VERSION 3.8)
 project(thruster_allocation_matrix_controller)
 
-if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)")
   add_compile_options(-Wall -Wextra -Wpedantic)
 endif()
 
-# find dependencies
+set(THIS_PACKAGE_INCLUDE_DEPENDS
+  controller_interface
+  generate_parameter_library
+  hardware_interface
+  pluginlib
+  rclcpp
+  rclcpp_lifecycle
+  realtime_tools
+  eigen3_cmake_module
+  Eigen3
+  geometry_msgs
+  control_msgs
+)
+
 find_package(ament_cmake REQUIRED)
-# uncomment the following section in order to fill in
-# further dependencies manually.
-# find_package(<dependency> REQUIRED)
+foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS})
+  find_package(${Dependency} REQUIRED)
+endforeach()
+
+generate_parameter_library(thruster_allocation_matrix_parameters
+  src/thruster_allocation_matrix_parameters.yaml
+)
+
+add_library(thruster_allocation_matrix_controller SHARED
+  src/thruster_allocation_matrix_controller.cpp
+)
+
+target_include_directories(thruster_allocation_matrix_controller
+  PUBLIC
+    $<INSTALL_INTERFACE:include/thruster_allocation_matrix_controller>
+    $<BUILD_INTERFACE:${PROJECT_SOURCE_DIR}/include>
+    ${EIGEN3_INCLUDE_DIR}
+  PRIVATE
+    ${PROJECT_SOURCE_DIR}/src
+)
+target_compile_features(thruster_allocation_matrix_controller PUBLIC cxx_std_17)
+target_link_libraries(thruster_allocation_matrix_controller
+  PUBLIC
+    ${rclcpp_LIBRARIES}
+    thruster_allocation_matrix_parameters
+)
+
+# Use dllexport instead of dllimport
+target_compile_definitions(thruster_allocation_matrix_controller PRIVATE "THRUSTER_ALLOCATION_MATRIX_CONTROLLER_BUILDING_DLL")
+ament_target_dependencies(thruster_allocation_matrix_controller
+  PUBLIC
+    ${THIS_PACKAGE_INCLUDE_DEPENDS}
+)
+
+pluginlib_export_plugin_description_file(controller_interface thruster_allocation_matrix_controller.xml)
 
 if(BUILD_TESTING)
   find_package(ament_lint_auto REQUIRED)
-  # the following line skips the linter which checks for copyrights
-  # comment the line when a copyright and license is added to all source files
-  set(ament_cmake_copyright_FOUND TRUE)
-  # the following line skips cpplint (only works in a git repo)
-  # comment the line when this package is in a git repo and when
-  # a copyright and license is added to all source files
-  set(ament_cmake_cpplint_FOUND TRUE)
+
+  set(ament_cmake_uncrustify_FOUND TRUE)
+
   ament_lint_auto_find_test_dependencies()
 endif()
 
+install(
+  DIRECTORY include/
+  DESTINATION include/thruster_allocation_matrix_controller
+)
+
+install(
+  TARGETS
+    thruster_allocation_matrix_controller
+    thruster_allocation_matrix_parameters
+  EXPORT
+    export_thruster_allocation_matrix_controller
+  RUNTIME DESTINATION bin
+  LIBRARY DESTINATION lib
+  ARCHIVE DESTINATION lib
+  INCLUDES DESTINATION include
+)
+
+ament_export_targets(export_thruster_allocation_matrix_controller HAS_LIBRARY_TARGET)
+ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS})
+
 ament_package()

thruster_allocation_matrix_controller/include/thruster_allocation_matrix_controller/thruster_allocation_matrix_controller.hpp

@@ -0,0 +1,140 @@
+// Copyright 2024, Evan Palmer
+//
+// Permission is hereby granted, free of charge, to any person obtaining a copy
+// of this software and associated documentation files (the "Software"), to deal
+// in the Software without restriction, including without limitation the rights
+// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+// copies of the Software, and to permit persons to whom the Software is
+// furnished to do so, subject to the following conditions:
+//
+// The above copyright notice and this permission notice shall be included in all
+// copies or substantial portions of the Software.
+//
+// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+// SOFTWARE.
+
+#pragma once
+
+#include <Eigen/Dense>
+#include <cstddef>
+#include <iostream>
+#include <memory>
+#include <stdexcept>
+#include <string>
+#include <vector>
+
+#include "control_msgs/msg/multi_dof_state_stamped.hpp"
+#include "controller_interface/chainable_controller_interface.hpp"
+#include "controller_interface/controller_interface.hpp"
+#include "geometry_msgs/msg/wrench.hpp"
+#include "rclcpp/rclcpp.hpp"
+#include "realtime_tools/realtime_buffer.h"
+#include "realtime_tools/realtime_publisher.h"
+#include "thruster_allocation_matrix_controller/visibility_control.h"
+
+// auto-generated by generate_parameter_library
+#include "thruster_allocation_matrix_parameters.hpp"
+
+namespace Eigen
+{
+
+// Extend the Eigen namespace to include commonly used matrix types
+using Matrix6d = Eigen::Matrix<double, 6, 6>;
+using Vector6d = Eigen::Matrix<double, 6, 1>;
+
+}  // namespace Eigen
+
+namespace thruster_allocation_matrix_controller
+{
+
+/**
+ * @brief Integral sliding mode controller (ISMC) for velocity control of an autonomous underwater vehicle.
+ */
+class ThrusterAllocationMatrixController : public controller_interface::ChainableControllerInterface
+{
+public:
+  THRUSTER_ALLOCATION_MATRIX_CONTROLLER_PUBLIC
+  ThrusterAllocationMatrixController() = default;
+
+  THRUSTER_ALLOCATION_MATRIX_CONTROLLER_PUBLIC controller_interface::CallbackReturn on_init() override;
+
+  THRUSTER_ALLOCATION_MATRIX_CONTROLLER_PUBLIC
+  controller_interface::InterfaceConfiguration command_interface_configuration() const override;
+
+  THRUSTER_ALLOCATION_MATRIX_CONTROLLER_PUBLIC
+  controller_interface::InterfaceConfiguration state_interface_configuration() const override;
+
+  THRUSTER_ALLOCATION_MATRIX_CONTROLLER_PUBLIC
+  controller_interface::CallbackReturn on_cleanup(const rclcpp_lifecycle::State & previous_state) override;
+
+  THRUSTER_ALLOCATION_MATRIX_CONTROLLER_PUBLIC
+  controller_interface::CallbackReturn on_configure(const rclcpp_lifecycle::State & previous_state) override;
+
+  THRUSTER_ALLOCATION_MATRIX_CONTROLLER_PUBLIC
+  controller_interface::CallbackReturn on_activate(const rclcpp_lifecycle::State & previous_state) override;
+
+  THRUSTER_ALLOCATION_MATRIX_CONTROLLER_PUBLIC
+  controller_interface::CallbackReturn on_deactivate(const rclcpp_lifecycle::State & previous_state) override;
+
+  THRUSTER_ALLOCATION_MATRIX_CONTROLLER_PUBLIC
+  controller_interface::return_type update_and_write_commands(
+    const rclcpp::Time & time, const rclcpp::Duration & period) override;
+
+  // TODO(evan): Might not need to override this
+  THRUSTER_ALLOCATION_MATRIX_CONTROLLER_PUBLIC
+  bool on_set_chained_mode(bool chained_mode) override;
+
+protected:
+  std::vector<hardware_interface::CommandInterface> on_export_reference_interfaces() override;
+
+  controller_interface::return_type update_reference_from_subscribers(
+    const rclcpp::Time & time, const rclcpp::Duration & period) override;
+
+  /**
+   * @brief Update the controller parameters, including the dynamic parameters.
+   */
+  void update_parameters();
+
+  /**
+   * @brief Configure the controller parameters.
+   *
+   * @note This can be called in a control loop to update the dynamic parameters for online controller tuning.
+   *
+   * @return controller_interface::CallbackReturn
+   */
+  controller_interface::CallbackReturn configure_parameters();
+
+  // Reference signal to track
+  realtime_tools::RealtimeBuffer<std::shared_ptr<geometry_msgs::msg::Wrench>> reference_;
+  std::shared_ptr<rclcpp::Subscription<geometry_msgs::msg::Wrench>> reference_sub_;
+
+  // Publish the controller state
+  std::shared_ptr<rclcpp::Publisher<control_msgs::msg::MultiDOFStateStamped>> controller_state_pub_;
+  std::unique_ptr<realtime_tools::RealtimePublisher<control_msgs::msg::MultiDOFStateStamped>> rt_controller_state_pub_;
+
+  // generate_parameter_library members
+  std::shared_ptr<thruster_allocation_matrix_controller::ParamListener> param_listener_;
+  thruster_allocation_matrix_controller::Params params_;
+
+  // DOF information
+  const std::array<std::string, 6> six_dof_names_{"x", "y", "z", "rx", "ry", "rz"};
+  std::vector<std::string> dof_names_;
+  size_t dof_;
+
+  // rows for tam matrix
+  Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> tam_;
+
+  std::int64_t num_thrusters_;
+
+private:
+  THRUSTER_ALLOCATION_MATRIX_CONTROLLER_LOCAL
+  // TODO: replace message type with something for the Wrench
+  void reference_callback(std::shared_ptr<geometry_msgs::msg::Wrench> msg);
+};
+
+}  // namespace thruster_allocation_matrix_controller

thruster_allocation_matrix_controller/package.xml

@@ -8,9 +8,27 @@
   <license>MIT</license>
 
   <buildtool_depend>ament_cmake</buildtool_depend>
+  <buildtool_depend>eigen3_cmake_module</buildtool_depend>
+
+  <depend>eigen</depend>
+  <depend>rclcpp</depend>
+  <depend>ros2_control</depend>
+  <depend>ros2_controllers</depend>
+  <depend>pluginlib</depend>
+  <depend>controller_interface</depend>
+  <depend>hardware_interface</depend>
+  <depend>rclcpp_lifecycle</depend>
+  <depend>generate_parameter_library</depend>
+  <depend>control_msgs</depend>
 
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_lint_common</test_depend>
+  <test_depend>ament_add_gmock</test_depend>
+  <test_depend>controller_manager</test_depend>
+  <test_depend>ros2_control_test_assets</test_depend>
+  <test_depend>hardware_interface</test_depend>
+
+  <buildtool_export_depend>eigen3_cmake_module</buildtool_export_depend>
 
   <export>
     <build_type>ament_cmake</build_type>