From 6c076d4baa9445df4130c59f0bd0079805650f7c Mon Sep 17 00:00:00 2001 From: Evan Palmer Date: Thu, 11 May 2023 19:36:49 -0700 Subject: [PATCH 1/8] Implemented initial version of base controller --- .dockerignore | 1 + blue_control/CMakeLists.txt | 53 +++++++ blue_control/LICENSE | 17 +++ .../include/blue_control/base_controller.hpp | 73 ++++++++++ blue_control/include/blue_control/ismc.hpp | 19 +++ blue_control/package.xml | 25 ++++ blue_control/src/base_controller.cpp | 131 ++++++++++++++++++ blue_control/src/ismc.cpp | 19 +++ .../include/blue_dynamics/hydrodynamics.hpp | 36 ++++- blue_dynamics/src/hydrodynamics.cpp | 14 +- 10 files changed, 376 insertions(+), 12 deletions(-) create mode 100644 blue_control/CMakeLists.txt create mode 100644 blue_control/LICENSE create mode 100644 blue_control/include/blue_control/base_controller.hpp create mode 100644 blue_control/include/blue_control/ismc.hpp create mode 100644 blue_control/package.xml create mode 100644 blue_control/src/base_controller.cpp create mode 100644 blue_control/src/ismc.cpp diff --git a/.dockerignore b/.dockerignore index ba822352..1736e056 100644 --- a/.dockerignore +++ b/.dockerignore @@ -4,3 +4,4 @@ # Except the following !blue_dynamics !blue_manager +!blue_control diff --git a/blue_control/CMakeLists.txt b/blue_control/CMakeLists.txt new file mode 100644 index 00000000..69590d81 --- /dev/null +++ b/blue_control/CMakeLists.txt @@ -0,0 +1,53 @@ +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 + nav_msgs + sensor_msgs + mavros_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} SHARED + src/base_controller.cpp +) +target_include_directories(${PROJECT_NAME} PRIVATE include) +ament_target_dependencies(${PROJECT_NAME} ${THIS_PACKAGE_INCLUDE_DEPENDS}) + +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() diff --git a/blue_control/LICENSE b/blue_control/LICENSE new file mode 100644 index 00000000..30e8e2ec --- /dev/null +++ b/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. diff --git a/blue_control/include/blue_control/base_controller.hpp b/blue_control/include/blue_control/base_controller.hpp new file mode 100644 index 00000000..fdaec50e --- /dev/null +++ b/blue_control/include/blue_control/base_controller.hpp @@ -0,0 +1,73 @@ +// 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 +#include +#include + +#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" + +namespace blue::control +{ + +Eigen::VectorXd convertVectorToEigenVector(const std::vector & vec); +Eigen::MatrixXd convertVectorToEigenMatrix( + const std::vector & vec, size_t rows, size_t cols); + +class BaseController : public rclcpp::Node +{ +public: + explicit BaseController(const std::string & node_name); + +protected: + virtual mavros_msgs::msg::OverrideRCIn update() = 0; + + blue::dynamics::HydrodynamicParameters hydrodynamics_; + Eigen::MatrixXd tcm_; + sensor_msgs::msg::BatteryState::SharedPtr battery_state_; + + // 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::SharedPtr odom_; + +private: + void runControlLoopCb(); + void saveBatteryCb(const sensor_msgs::msg::BatteryState::SharedPtr msg); + void saveOdomCb(const nav_msgs::msg::Odometry::SharedPtr msg); + + // Publishers + rclcpp::Publisher::SharedPtr rc_override_pub_; + + // Subscribers + rclcpp::Subscription::SharedPtr battery_state_sub_; + rclcpp::Subscription::SharedPtr odom_sub_; + + // Timers + rclcpp::TimerBase::SharedPtr control_loop_timer_; +}; + +} // namespace blue::control diff --git a/blue_control/include/blue_control/ismc.hpp b/blue_control/include/blue_control/ismc.hpp new file mode 100644 index 00000000..0a25e9f7 --- /dev/null +++ b/blue_control/include/blue_control/ismc.hpp @@ -0,0 +1,19 @@ +// 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. diff --git a/blue_control/package.xml b/blue_control/package.xml new file mode 100644 index 00000000..0251a19d --- /dev/null +++ b/blue_control/package.xml @@ -0,0 +1,25 @@ + + + + blue_control + 0.0.1 + Control interface for the BlueROV2 + Evan Palmer + MIT + + nav_msgs + sensor_msgs + mavros_msgs + rclcpp + blue_dynamics + Eigen + + ament_cmake + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/blue_control/src/base_controller.cpp b/blue_control/src/base_controller.cpp new file mode 100644 index 00000000..4073a39f --- /dev/null +++ b/blue_control/src/base_controller.cpp @@ -0,0 +1,131 @@ +// 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 "blue_control/base_controller.hpp" + +namespace blue::control +{ + +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) +{ + Eigen::Map mat(vec.data(), rows, cols); + return mat; +} + +BaseController::BaseController(const std::string & node_name) +: Node(std::move(node_name)) +{ + // Declare ROS parameters + this->declare_parameter("mass", 11.5); + this->declare_parameter("buoyancy", 112.80); + this->declare_parameter("weight", 114.80); + 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}); + this->declare_parameter( + "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}); + + // I'm so sorry for this + // You can blame the ROS devs for not supporting nested arrays for parameters + 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}); + + // Default to the BlueROV2 Heavy configuration + this->declare_parameter("num_thrusters", 8); + + // Get the parameter values + 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()); + + // Get the thruster configuration matrix + std::vector tcm_vec = + this->get_parameter("thruster_configuration_matrix").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); + + // Initialize the hydrodynamic parameters + hydrodynamics_ = blue::dynamics::HydrodynamicParameters( + blue::dynamics::Inertia(mass, inertia_tensor_coeff, added_mass_coeff), + blue::dynamics::Coriolis(mass, inertia_tensor_coeff, added_mass_coeff), + blue::dynamics::Damping(linear_damping_coeff, quadratic_damping_coeff), + blue::dynamics::RestoringForces(weight, buoyancy, center_of_buoyancy, center_of_gravity), + blue::dynamics::CurrentEffects(ocean_current)); + + rc_override_pub_ = + this->create_publisher("mavros/rc/override", 1); + + battery_state_sub_ = this->create_subscription( + "/mavros/battery", 1, + [this](const sensor_msgs::msg::BatteryState::SharedPtr msg) { this->saveBatteryCb(msg); }); + + odom_sub_ = this->create_subscription( + "/mavros/local_position/odom", 1, + [this](const nav_msgs::msg::Odometry::SharedPtr msg) { this->saveOdomCb(msg); }); + + // Run at a control rate of 200 Hz + control_loop_timer_ = this->create_wall_timer( + std::chrono::milliseconds(5), std::bind(&BaseController::runControlLoopCb, this)); +} + +void BaseController::saveBatteryCb(const sensor_msgs::msg::BatteryState::SharedPtr msg) +{ + battery_state_ = msg; +} + +void BaseController::saveOdomCb(const nav_msgs::msg::Odometry::SharedPtr msg) { odom_ = msg; } + +void BaseController::runControlLoopCb() { rc_override_pub_->publish(update()); } + +} // namespace blue::control diff --git a/blue_control/src/ismc.cpp b/blue_control/src/ismc.cpp new file mode 100644 index 00000000..0a25e9f7 --- /dev/null +++ b/blue_control/src/ismc.cpp @@ -0,0 +1,19 @@ +// 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. diff --git a/blue_dynamics/include/blue_dynamics/hydrodynamics.hpp b/blue_dynamics/include/blue_dynamics/hydrodynamics.hpp index 0f8b695b..03fd23d8 100644 --- a/blue_dynamics/include/blue_dynamics/hydrodynamics.hpp +++ b/blue_dynamics/include/blue_dynamics/hydrodynamics.hpp @@ -49,6 +49,10 @@ namespace blue::dynamics class Inertia { public: + /** + * @brief Default constructor for the inertia object. + */ + Inertia() = default; /** * @brief Construct a new Inertia object. * @@ -86,6 +90,11 @@ class Inertia class Coriolis { public: + /** + * @brief Default constructor for the Coriolis object. + */ + Coriolis() = default; + /** * @brief Construct a new Coriolis object. * @@ -145,7 +154,7 @@ class Coriolis [[nodiscard]] Eigen::MatrixXd calculateAddedCoriolis(const Eigen::VectorXd & velocity) const; private: - double mass_; + double mass_{0.0}; Eigen::Matrix3d moments_; Eigen::VectorXd added_mass_coeff_; }; @@ -156,6 +165,11 @@ class Coriolis class Damping { public: + /** + * @brief Default constructor for the Damping object. + */ + Damping() = default; + /** * @brief Construct a new Damping object. * @@ -197,6 +211,11 @@ class Damping class RestoringForces { public: + /** + * @brief Default constructor for the RestoringForces object. + */ + RestoringForces() = default; + /** * @brief Construct a new RestoringForces object. * @@ -223,8 +242,8 @@ class RestoringForces [[nodiscard]] Eigen::VectorXd calculateRestoringForces(const Eigen::Matrix3d & rotation) const; private: - double weight_; - double buoyancy_; + double weight_{0.0}; + double buoyancy_{0.0}; Eigen::Vector3d center_of_buoyancy_; Eigen::Vector3d center_of_gravity_; }; @@ -235,6 +254,11 @@ class RestoringForces class CurrentEffects { public: + /** + * @brief Default constructor for the CurrentEffects object. + */ + CurrentEffects() = default; + /** * @brief Construct a new CurrentEffects object * @@ -270,9 +294,11 @@ struct HydrodynamicParameters RestoringForces restoring_forces; CurrentEffects current_effects; + HydrodynamicParameters() = default; + HydrodynamicParameters( - const Inertia & inertia, const Coriolis & coriolis, const Damping & damping, - const RestoringForces & restoring_forces, const CurrentEffects & current_effects); + Inertia inertia, Coriolis coriolis, Damping damping, RestoringForces restoring_forces, + CurrentEffects current_effects); }; } // namespace blue::dynamics diff --git a/blue_dynamics/src/hydrodynamics.cpp b/blue_dynamics/src/hydrodynamics.cpp index 6b7cab29..bd575165 100644 --- a/blue_dynamics/src/hydrodynamics.cpp +++ b/blue_dynamics/src/hydrodynamics.cpp @@ -164,13 +164,13 @@ CurrentEffects::CurrentEffects(const Eigen::VectorXd & current_velocity) } HydrodynamicParameters::HydrodynamicParameters( - const Inertia & inertia, const Coriolis & coriolis, const Damping & damping, - const RestoringForces & restoring_forces, const CurrentEffects & current_effects) -: inertia(inertia), - coriolis(coriolis), - damping(damping), - restoring_forces(restoring_forces), - current_effects(current_effects) + Inertia inertia, Coriolis coriolis, Damping damping, RestoringForces restoring_forces, + CurrentEffects current_effects) +: inertia(std::move(inertia)), + coriolis(std::move(coriolis)), + damping(std::move(damping)), + restoring_forces(std::move(restoring_forces)), + current_effects(std::move(current_effects)) { } From 8973d90c39fe403de639056bdde4249d941d3eed Mon Sep 17 00:00:00 2001 From: Evan Palmer Date: Thu, 11 May 2023 21:51:34 -0700 Subject: [PATCH 2/8] Fixed deps --- blue_control/package.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/blue_control/package.xml b/blue_control/package.xml index 0251a19d..00f69f42 100644 --- a/blue_control/package.xml +++ b/blue_control/package.xml @@ -12,7 +12,7 @@ mavros_msgs rclcpp blue_dynamics - Eigen + eigen ament_cmake From 37bcb1dd9d9c634a858edd19a0b949f8b9206fd6 Mon Sep 17 00:00:00 2001 From: Evan Palmer Date: Fri, 12 May 2023 12:56:02 -0700 Subject: [PATCH 3/8] Started implementation of ismc --- blue_control/CMakeLists.txt | 18 ++++++ blue_control/include/blue_control/ismc.hpp | 32 ++++++++++ blue_control/src/base_controller.cpp | 7 +-- blue_control/src/ismc.cpp | 68 ++++++++++++++++++++++ 4 files changed, 120 insertions(+), 5 deletions(-) diff --git a/blue_control/CMakeLists.txt b/blue_control/CMakeLists.txt index 69590d81..1cb1507b 100644 --- a/blue_control/CMakeLists.txt +++ b/blue_control/CMakeLists.txt @@ -36,10 +36,25 @@ include_directories( add_library(${PROJECT_NAME} SHARED src/base_controller.cpp + src/ismc.cpp ) target_include_directories(${PROJECT_NAME} PRIVATE include) ament_target_dependencies(${PROJECT_NAME} ${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) @@ -50,4 +65,7 @@ if(BUILD_TESTING) 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_control/include/blue_control/ismc.hpp b/blue_control/include/blue_control/ismc.hpp index 0a25e9f7..60d5084e 100644 --- a/blue_control/include/blue_control/ismc.hpp +++ b/blue_control/include/blue_control/ismc.hpp @@ -17,3 +17,35 @@ // 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_control/base_controller.hpp" +#include "mavros_msgs/msg/override_rc_in.hpp" + +namespace blue::control +{ + +class ISMC : public BaseController +{ +public: + ISMC(); + +protected: + mavros_msgs::msg::OverrideRCIn update() override; + +private: + Eigen::VectorXd total_velocity_error_; + Eigen::MatrixXd convergence_rate_; + double sliding_gain_; + double boundary_thickness_; + + static Eigen::VectorXd calculateError( + const Eigen::VectorXd & desired, const Eigen::VectorXd & actual); + static Eigen::VectorXd calculateSlidingSurface( + const Eigen::VectorXd & velocity_error, const Eigen::VectorXd & velocity_error_integral, + const Eigen::MatrixXd & convergence_rate); + static void applySignFunction(std::shared_ptr surface); +}; + +} // namespace blue::control diff --git a/blue_control/src/base_controller.cpp b/blue_control/src/base_controller.cpp index 4073a39f..2d0a9246 100644 --- a/blue_control/src/base_controller.cpp +++ b/blue_control/src/base_controller.cpp @@ -55,6 +55,7 @@ BaseController::BaseController(const std::string & node_name) 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); // I'm so sorry for this // You can blame the ROS devs for not supporting nested arrays for parameters @@ -66,9 +67,6 @@ BaseController::BaseController(const std::string & node_name) 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}); - // Default to the BlueROV2 Heavy configuration - this->declare_parameter("num_thrusters", 8); - // Get the parameter values const double mass = this->get_parameter("mass").as_double(); const double buoyancy = this->get_parameter("buoyancy").as_double(); @@ -89,8 +87,7 @@ BaseController::BaseController(const std::string & node_name) convertVectorToEigenVector(this->get_parameter("ocean_current").as_double_array()); // Get the thruster configuration matrix - std::vector tcm_vec = - this->get_parameter("thruster_configuration_matrix").as_double_array(); + 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); diff --git a/blue_control/src/ismc.cpp b/blue_control/src/ismc.cpp index 0a25e9f7..bcfb4901 100644 --- a/blue_control/src/ismc.cpp +++ b/blue_control/src/ismc.cpp @@ -17,3 +17,71 @@ // 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 "blue_control/ismc.hpp" + +#include "blue_dynamics/thruster_dynamics.hpp" + +namespace blue::control +{ + +ISMC::ISMC() +: BaseController("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{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); + + Eigen::VectorXd convergence_diag = + convertVectorToEigenVector(this->get_parameter("convergence_rate").as_double_array()); + convergence_rate_ = convergence_diag.asDiagonal().toDenseMatrix(); + sliding_gain_ = this->get_parameter("sliding_gain").as_double(); + boundary_thickness_ = this->get_parameter("boundary_thickness").as_double(); + total_velocity_error_ = Eigen::VectorXd::Zero(6); +} + +mavros_msgs::msg::OverrideRCIn ISMC::update() +{ + Eigen::VectorXd velocity(6); // NOLINT + velocity << odom_->twist.twist.linear.x, odom_->twist.twist.linear.y, odom_->twist.twist.linear.z, + odom_->twist.twist.angular.x, odom_->twist.twist.angular.y, odom_->twist.twist.angular.z; + + Eigen::Quaterniond orientation = Eigen::Quaterniond( + odom_->pose.pose.orientation.w, odom_->pose.pose.orientation.x, odom_->pose.pose.orientation.y, + odom_->pose.pose.orientation.z); + + // TODO(evan): Include inertia -> need desired acceleration and velocity error + // TODO(evan): Calculate s and apply the sign function + // TODO(evan): Include the current effects + const Eigen::VectorXd pwms = + hydrodynamics_.coriolis.calculateCoriolis(velocity) * velocity + + hydrodynamics_.damping.calculateDamping(velocity) * velocity + + hydrodynamics_.restoring_forces.calculateRestoringForces(orientation.toRotationMatrix()); + + // TODO(evan): convert the pwms to an std::array + // TODO(evan): convert to an OverrideRCIn message + + return mavros_msgs::msg::OverrideRCIn(); +} + +Eigen::VectorXd ISMC::calculateError( + const Eigen::VectorXd & desired, const Eigen::VectorXd & actual) +{ + return desired - actual; +} + +Eigen::VectorXd ISMC::calculateSlidingSurface( + const Eigen::VectorXd & velocity_error, const Eigen::VectorXd & velocity_error_integral, + const Eigen::MatrixXd & convergence_rate) +{ + return velocity_error + convergence_rate * velocity_error_integral; +} + +void ISMC::applySignFunction(std::shared_ptr surface) +{ /* data */ +} + +} // namespace blue::control From 965896401271e5e03208175e8e57301e4f1e6e7a Mon Sep 17 00:00:00 2001 From: Evan Palmer Date: Sun, 14 May 2023 14:12:34 -0700 Subject: [PATCH 4/8] Continued ismc implementation --- .dockerignore | 1 + blue_control/CMakeLists.txt | 1 + .../include/blue_control/base_controller.hpp | 8 +- blue_control/include/blue_control/ismc.hpp | 10 +-- blue_control/package.xml | 1 + blue_control/src/base_controller.cpp | 18 ++--- blue_control/src/ismc.cpp | 77 +++++++++++++------ blue_msgs/CMakeLists.txt | 29 +++++++ blue_msgs/LICENSE | 17 ++++ blue_msgs/msg/Reference.msg | 4 + blue_msgs/package.xml | 25 ++++++ 11 files changed, 141 insertions(+), 50 deletions(-) create mode 100644 blue_msgs/CMakeLists.txt create mode 100644 blue_msgs/LICENSE create mode 100644 blue_msgs/msg/Reference.msg create mode 100644 blue_msgs/package.xml diff --git a/.dockerignore b/.dockerignore index 1736e056..fdcd7f9d 100644 --- a/.dockerignore +++ b/.dockerignore @@ -5,3 +5,4 @@ !blue_dynamics !blue_manager !blue_control +!blue_msgs diff --git a/blue_control/CMakeLists.txt b/blue_control/CMakeLists.txt index 1cb1507b..d899df68 100644 --- a/blue_control/CMakeLists.txt +++ b/blue_control/CMakeLists.txt @@ -19,6 +19,7 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS rclcpp ament_cmake blue_dynamics + blue_msgs nav_msgs sensor_msgs mavros_msgs diff --git a/blue_control/include/blue_control/base_controller.hpp b/blue_control/include/blue_control/base_controller.hpp index fdaec50e..b8d934ff 100644 --- a/blue_control/include/blue_control/base_controller.hpp +++ b/blue_control/include/blue_control/base_controller.hpp @@ -47,18 +47,14 @@ class BaseController : public rclcpp::Node blue::dynamics::HydrodynamicParameters hydrodynamics_; Eigen::MatrixXd tcm_; - sensor_msgs::msg::BatteryState::SharedPtr battery_state_; + sensor_msgs::msg::BatteryState battery_state_; // 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::SharedPtr odom_; + nav_msgs::msg::Odometry odom_; private: - void runControlLoopCb(); - void saveBatteryCb(const sensor_msgs::msg::BatteryState::SharedPtr msg); - void saveOdomCb(const nav_msgs::msg::Odometry::SharedPtr msg); - // Publishers rclcpp::Publisher::SharedPtr rc_override_pub_; diff --git a/blue_control/include/blue_control/ismc.hpp b/blue_control/include/blue_control/ismc.hpp index 60d5084e..149edc21 100644 --- a/blue_control/include/blue_control/ismc.hpp +++ b/blue_control/include/blue_control/ismc.hpp @@ -21,6 +21,7 @@ #include #include "blue_control/base_controller.hpp" +#include "blue_msgs/msg/reference.hpp" #include "mavros_msgs/msg/override_rc_in.hpp" namespace blue::control @@ -35,17 +36,14 @@ class ISMC : public BaseController 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_; - static Eigen::VectorXd calculateError( - const Eigen::VectorXd & desired, const Eigen::VectorXd & actual); - static Eigen::VectorXd calculateSlidingSurface( - const Eigen::VectorXd & velocity_error, const Eigen::VectorXd & velocity_error_integral, - const Eigen::MatrixXd & convergence_rate); - static void applySignFunction(std::shared_ptr surface); + blue_msgs::msg::Reference cmd_; + rclcpp::Subscription::SharedPtr cmd_sub_; }; } // namespace blue::control diff --git a/blue_control/package.xml b/blue_control/package.xml index 00f69f42..3f44437f 100644 --- a/blue_control/package.xml +++ b/blue_control/package.xml @@ -13,6 +13,7 @@ rclcpp blue_dynamics eigen + blue_msgs ament_cmake diff --git a/blue_control/src/base_controller.cpp b/blue_control/src/base_controller.cpp index 2d0a9246..6c74f8a1 100644 --- a/blue_control/src/base_controller.cpp +++ b/blue_control/src/base_controller.cpp @@ -89,7 +89,6 @@ BaseController::BaseController(const std::string & node_name) // 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); // Initialize the hydrodynamic parameters @@ -105,24 +104,17 @@ BaseController::BaseController(const std::string & node_name) battery_state_sub_ = this->create_subscription( "/mavros/battery", 1, - [this](const sensor_msgs::msg::BatteryState::SharedPtr msg) { this->saveBatteryCb(msg); }); + [this](sensor_msgs::msg::BatteryState::ConstSharedPtr msg) { battery_state_ = *msg; }); odom_sub_ = this->create_subscription( "/mavros/local_position/odom", 1, - [this](const nav_msgs::msg::Odometry::SharedPtr msg) { this->saveOdomCb(msg); }); + [this](nav_msgs::msg::Odometry::ConstSharedPtr msg) { odom_ = *msg; }); // Run at a control 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), std::bind(&BaseController::runControlLoopCb, this)); -} - -void BaseController::saveBatteryCb(const sensor_msgs::msg::BatteryState::SharedPtr msg) -{ - battery_state_ = msg; + std::chrono::milliseconds(5), [this]() { rc_override_pub_->publish(update()); }); } -void BaseController::saveOdomCb(const nav_msgs::msg::Odometry::SharedPtr msg) { odom_ = msg; } - -void BaseController::runControlLoopCb() { rc_override_pub_->publish(update()); } - } // namespace blue::control diff --git a/blue_control/src/ismc.cpp b/blue_control/src/ismc.cpp index bcfb4901..b9daef90 100644 --- a/blue_control/src/ismc.cpp +++ b/blue_control/src/ismc.cpp @@ -20,6 +20,8 @@ #include "blue_control/ismc.hpp" +#include + #include "blue_dynamics/thruster_dynamics.hpp" namespace blue::control @@ -41,47 +43,72 @@ ISMC::ISMC() sliding_gain_ = this->get_parameter("sliding_gain").as_double(); boundary_thickness_ = this->get_parameter("boundary_thickness").as_double(); total_velocity_error_ = Eigen::VectorXd::Zero(6); + + // 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; }); } mavros_msgs::msg::OverrideRCIn ISMC::update() { Eigen::VectorXd velocity(6); // NOLINT - velocity << odom_->twist.twist.linear.x, odom_->twist.twist.linear.y, odom_->twist.twist.linear.z, - odom_->twist.twist.angular.x, odom_->twist.twist.angular.y, odom_->twist.twist.angular.z; + velocity << odom_.twist.twist.linear.x, odom_.twist.twist.linear.y, odom_.twist.twist.linear.z, + odom_.twist.twist.angular.x, odom_.twist.twist.angular.y, odom_.twist.twist.angular.z; + + // Calculate the velocity error + Eigen::VectorXd velocity_error(6); + velocity_error << cmd_.twist.linear.x, cmd_.twist.linear.y, cmd_.twist.linear.z, + cmd_.twist.angular.x, cmd_.twist.angular.y, cmd_.twist.angular.z; + velocity_error -= velocity; + + Eigen::VectorXd desired_accel(6); // 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; + // Get the current rotation of the vehicle in the inertial frame Eigen::Quaterniond orientation = Eigen::Quaterniond( - odom_->pose.pose.orientation.w, odom_->pose.pose.orientation.x, odom_->pose.pose.orientation.y, - odom_->pose.pose.orientation.z); + odom_.pose.pose.orientation.w, odom_.pose.pose.orientation.x, odom_.pose.pose.orientation.y, + odom_.pose.pose.orientation.z); + + // 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; + + // Calculate the sliding surface + Eigen::VectorXd surface = velocity_error + convergence_rate_ * total_velocity_error_; // NOLINT + + // Apply the sign function to the surface + surface.unaryExpr([this](double x) { return tanh(x / boundary_thickness_); }); - // TODO(evan): Include inertia -> need desired acceleration and velocity error - // TODO(evan): Calculate s and apply the sign function // TODO(evan): Include the current effects - const Eigen::VectorXd pwms = + const Eigen::VectorXd 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()); + hydrodynamics_.restoring_forces.calculateRestoringForces(orientation.toRotationMatrix()) + + sliding_gain_ * surface; - // TODO(evan): convert the pwms to an std::array - // TODO(evan): convert to an OverrideRCIn message + // Multiply the desired forces by the pseudoinverse of the thruster configuration matrix + const Eigen::VectorXd pwms = tcm_.completeOrthogonalDecomposition().pseudoInverse() * forces; - return mavros_msgs::msg::OverrideRCIn(); -} + // Convert the thruster forces to PWM values + pwms.unaryExpr([this](double x) { + return blue::dynamics::calculatePwmFromThrustSurface(x, battery_state_.voltage); + }); -Eigen::VectorXd ISMC::calculateError( - const Eigen::VectorXd & desired, const Eigen::VectorXd & actual) -{ - return desired - actual; -} + mavros_msgs::msg::OverrideRCIn msg; -Eigen::VectorXd ISMC::calculateSlidingSurface( - const Eigen::VectorXd & velocity_error, const Eigen::VectorXd & velocity_error_integral, - const Eigen::MatrixXd & convergence_rate) -{ - return velocity_error + convergence_rate * velocity_error_integral; -} + // 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; + } + + // TODO(evan): clamp the PWM values to 1500 if they fall in the deadband zone + for (uint16_t i = 0; i < pwms.size(); i++) { + msg.channels[i] = static_cast(pwms[i]); + } -void ISMC::applySignFunction(std::shared_ptr surface) -{ /* data */ + return msg; } } // namespace blue::control diff --git a/blue_msgs/CMakeLists.txt b/blue_msgs/CMakeLists.txt new file mode 100644 index 00000000..efa6dc4e --- /dev/null +++ b/blue_msgs/CMakeLists.txt @@ -0,0 +1,29 @@ +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/LICENSE b/blue_msgs/LICENSE new file mode 100644 index 00000000..30e8e2ec --- /dev/null +++ b/blue_msgs/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. diff --git a/blue_msgs/msg/Reference.msg b/blue_msgs/msg/Reference.msg new file mode 100644 index 00000000..abf872b6 --- /dev/null +++ b/blue_msgs/msg/Reference.msg @@ -0,0 +1,4 @@ +# Defines the desired reference values for a controller. +std_msgs/Header header +geometry_msgs/Twist twist +geometry_msgs/Accel accel diff --git a/blue_msgs/package.xml b/blue_msgs/package.xml new file mode 100644 index 00000000..93fa7b69 --- /dev/null +++ b/blue_msgs/package.xml @@ -0,0 +1,25 @@ + + + + blue_msgs + 0.0.1 + Custom messages available for use by the controllers. + Evan Palmer + MIT + + ament_cmake + + ament_lint_auto + ament_lint_common + + std_msgs + geometry_msgs + + rosidl_default_generators + rosidl_default_runtime + rosidl_interface_packages + + + ament_cmake + + From f1a82d748a6c40dfb56877e80c3b295f022e8cc9 Mon Sep 17 00:00:00 2001 From: Evan Palmer Date: Mon, 15 May 2023 10:45:08 -0700 Subject: [PATCH 5/8] Added service to arm and disarm controller --- blue_control/CMakeLists.txt | 1 + .../include/blue_control/base_controller.hpp | 58 +++++++++++++++++++ blue_control/include/blue_control/ismc.hpp | 8 +++ blue_control/package.xml | 1 + blue_control/src/base_controller.cpp | 38 ++++++++++-- blue_control/src/ismc.cpp | 24 ++++++-- .../include/blue_dynamics/hydrodynamics.hpp | 20 +++---- .../blue_dynamics/thruster_dynamics.hpp | 9 +++ blue_manager/blue_manager/manager.py | 33 +++++++++-- 9 files changed, 169 insertions(+), 23 deletions(-) diff --git a/blue_control/CMakeLists.txt b/blue_control/CMakeLists.txt index d899df68..2c2f8bad 100644 --- a/blue_control/CMakeLists.txt +++ b/blue_control/CMakeLists.txt @@ -23,6 +23,7 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS nav_msgs sensor_msgs mavros_msgs + std_srvs eigen3_cmake_module Eigen3 ) diff --git a/blue_control/include/blue_control/base_controller.hpp b/blue_control/include/blue_control/base_controller.hpp index b8d934ff..02bf2c20 100644 --- a/blue_control/include/blue_control/base_controller.hpp +++ b/blue_control/include/blue_control/base_controller.hpp @@ -18,6 +18,8 @@ // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN // THE SOFTWARE. +#pragma once + #include #include #include @@ -29,24 +31,71 @@ #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 & vec); + +/** + * @brief Convert an `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. + */ 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_; // It is important to note here that the pose information is provided in the inertial frame @@ -55,6 +104,12 @@ class BaseController : public rclcpp::Node nav_msgs::msg::Odometry odom_; private: + void armController( + std::shared_ptr request, + std::shared_ptr response); + + bool armed_; + // Publishers rclcpp::Publisher::SharedPtr rc_override_pub_; @@ -64,6 +119,9 @@ class BaseController : public rclcpp::Node // Timers rclcpp::TimerBase::SharedPtr control_loop_timer_; + + // Services + rclcpp::Service::SharedPtr arm_srv_; }; } // namespace blue::control diff --git a/blue_control/include/blue_control/ismc.hpp b/blue_control/include/blue_control/ismc.hpp index 149edc21..40b8838b 100644 --- a/blue_control/include/blue_control/ismc.hpp +++ b/blue_control/include/blue_control/ismc.hpp @@ -18,6 +18,8 @@ // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN // THE SOFTWARE. +#pragma once + #include #include "blue_control/base_controller.hpp" @@ -27,9 +29,15 @@ namespace blue::control { +/** + * @brief Integral sliding mode controller for the BlueROV2. + */ class ISMC : public BaseController { public: + /** + * @brief Construct a new ISMC object. + */ ISMC(); protected: diff --git a/blue_control/package.xml b/blue_control/package.xml index 3f44437f..1c074f52 100644 --- a/blue_control/package.xml +++ b/blue_control/package.xml @@ -14,6 +14,7 @@ blue_dynamics eigen blue_msgs + std_srvs ament_cmake diff --git a/blue_control/src/base_controller.cpp b/blue_control/src/base_controller.cpp index 6c74f8a1..388c1c9b 100644 --- a/blue_control/src/base_controller.cpp +++ b/blue_control/src/base_controller.cpp @@ -39,7 +39,8 @@ Eigen::MatrixXd convertVectorToEigenMatrix( } BaseController::BaseController(const std::string & node_name) -: Node(std::move(node_name)) +: Node(std::move(node_name)), + armed_(false) { // Declare ROS parameters this->declare_parameter("mass", 11.5); @@ -110,11 +111,40 @@ BaseController::BaseController(const std::string & node_name) "/mavros/local_position/odom", 1, [this](nav_msgs::msg::Odometry::ConstSharedPtr msg) { odom_ = *msg; }); - // Run at a control rate of 200 Hz + arm_srv_ = this->create_service( + "blue/control/arm", [this]( + const std::shared_ptr request, + std::shared_ptr response) { + armController(request, response); + }); + + // 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]() { rc_override_pub_->publish(update()); }); + control_loop_timer_ = this->create_wall_timer(std::chrono::milliseconds(5), [this]() { + if (armed_) { + rc_override_pub_->publish(update()); + } + }); +} + +void BaseController::armController( + const std::shared_ptr request, + std::shared_ptr response) +{ + if (request->data) { + // Arm the controller + armed_ = true; + response->success = true; + response->message = "Controller armed."; + RCLCPP_WARN(this->get_logger(), "Custom BlueROV2 controller armed."); + } else { + // Disarm the controller + armed_ = false; + response->success = true; + response->message = "Controller disarmed."; + RCLCPP_WARN(this->get_logger(), "Custom BlueROV2 controller disarmed."); + } } } // namespace blue::control diff --git a/blue_control/src/ismc.cpp b/blue_control/src/ismc.cpp index b9daef90..5f9b2567 100644 --- a/blue_control/src/ismc.cpp +++ b/blue_control/src/ismc.cpp @@ -71,7 +71,7 @@ mavros_msgs::msg::OverrideRCIn ISMC::update() odom_.pose.pose.orientation.z); // Make sure to update the velocity error integral term BEFORE calculating the sliding surface - // The integral is up to time "t" + // (the integral is up to time "t") total_velocity_error_ += velocity_error; // Calculate the sliding surface @@ -80,7 +80,6 @@ mavros_msgs::msg::OverrideRCIn ISMC::update() // Apply the sign function to the surface surface.unaryExpr([this](double x) { return tanh(x / boundary_thickness_); }); - // TODO(evan): Include the current effects const Eigen::VectorXd forces = hydrodynamics_.inertia.getInertia() * (desired_accel + convergence_rate_ * velocity_error) + hydrodynamics_.coriolis.calculateCoriolis(velocity) * velocity + @@ -103,12 +102,29 @@ mavros_msgs::msg::OverrideRCIn ISMC::update() channel = mavros_msgs::msg::OverrideRCIn::CHAN_NOCHANGE; } - // TODO(evan): clamp the PWM values to 1500 if they fall in the deadband zone + // Apply the deadband to the PWM values + const std::tuple deadband = blue::dynamics::calculateDeadZone(battery_state_.voltage); + for (uint16_t i = 0; i < pwms.size(); i++) { - msg.channels[i] = static_cast(pwms[i]); + uint16_t pwm = static_cast(pwms[i]); + + if (pwm > std::get<0>(deadband) && pwm < std::get<1>(deadband)) { + pwm = blue::dynamics::kNoThrustPwm; + } + + msg.channels[i] = pwm; } return msg; } } // namespace blue::control + +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + auto node = std::make_shared(); + rclcpp::spin(node); + rclcpp::shutdown(); + return 0; +} diff --git a/blue_dynamics/include/blue_dynamics/hydrodynamics.hpp b/blue_dynamics/include/blue_dynamics/hydrodynamics.hpp index 03fd23d8..c0674feb 100644 --- a/blue_dynamics/include/blue_dynamics/hydrodynamics.hpp +++ b/blue_dynamics/include/blue_dynamics/hydrodynamics.hpp @@ -68,9 +68,9 @@ class Inertia /** * @brief Get the vehicle's inertia matrix. * - * @note The inertia matrix `M` is the sum of the rigid body mass `M_RB` and the added mass `M_A` - * such that `M = M_RB + M_A`. The definition used for the rigid body inertia matrix `M_RB` is - * provided by Thor I. Fossen's textbook "Handbook of Marine Craft Hydrodynamics and Motion + * @details The inertia matrix `M` is the sum of the rigid body mass `M_RB` and the added mass + * `M_A` such that `M = M_RB + M_A`. The definition used for the rigid body inertia matrix `M_RB` + * is provided by Thor I. Fossen's textbook "Handbook of Marine Craft Hydrodynamics and Motion * Control" in Equation 3.44. Note that, in this model, we define the body frame to be coincident * with the center of mass, such that r_g^b = 0. The result is that `M_RB` is a diagonal matrix. * The definition used for the added mass inertia matrix `M_A` is provided by Gianluca Antonelli's @@ -110,7 +110,7 @@ class Coriolis /** * @brief Calculate the Coriolis and centripetal forces for the vehicle. * - * @note The Coriolis and centripetal force matrix `C` is the sum of the rigid body Coriolis + * @details The Coriolis and centripetal force matrix `C` is the sum of the rigid body Coriolis * forces `C_RB` and the added Coriolis forces `C_A` such that `C = C_RB + C_A`. * * @param velocity The current velocity of the vehicle in the body frame. @@ -121,12 +121,12 @@ class Coriolis /** * @brief Calculate the rigid body Coriolis matrix. * - * @note The definition of the rigid body Coriolis-centripetal matrix `C_RB` used in this work is - * provided by Thor I. Fossen's textbook "Handbook of Marine Craft Hydrodynamics and Motion + * @details The definition of the rigid body Coriolis-centripetal matrix `C_RB` used in this work + * is provided by Thor I. Fossen's textbook "Handbook of Marine Craft Hydrodynamics and Motion * Control" in Equation 3.57. Note that, in this model, we define the body frame to be coincident * with the center of mass, such that r_g^b = 0. * - * @remark The preferred entrypoint for calculating the Coriolis matrix is through the + * @note The preferred entrypoint for calculating the Coriolis matrix is through the * `calculateCoriolis` method; however, this method is made accessible for advanced users who wish * to calculate the rigid body Coriolis matrix directly (which may be necessary when calculating * the dynamics using the relative velocity). @@ -140,10 +140,10 @@ class Coriolis /** * @brief Calculate the added Coriolis matrix. * - * @note The definition of the added Coriolis-centripetal matrix `C_A` used in this work is + * @details The definition of the added Coriolis-centripetal matrix `C_A` used in this work is * provided by Gianluca Antonelli's "Underwater Robots" in Section 2.4.1. * - * @remark The preferred entrypoint for calculating the Coriolis matrix is through the + * @note The preferred entrypoint for calculating the Coriolis matrix is through the * `calculateCoriolis` method; however, this method is made accessible for advanced users who wish * to calculate the added Coriolis matrix directly (which may be necessary when calculating * the dynamics using the relative velocity). @@ -183,7 +183,7 @@ class Damping /** * @brief Calculate the damping forces for the vehicle. * - * @note The rigid body damping matrix `D` is defined as the sum of the linear and nonlinear + * @details The rigid body damping matrix `D` is defined as the sum of the linear and nonlinear * damping coefficients. The definition of the linear and nonlinear damping matrices used in this * work is provided by Gianluca Antonelli's "Underwater Robots" in Section 2.4.2. * diff --git a/blue_dynamics/include/blue_dynamics/thruster_dynamics.hpp b/blue_dynamics/include/blue_dynamics/thruster_dynamics.hpp index 4979ccb8..d14b1d77 100644 --- a/blue_dynamics/include/blue_dynamics/thruster_dynamics.hpp +++ b/blue_dynamics/include/blue_dynamics/thruster_dynamics.hpp @@ -20,11 +20,20 @@ #pragma once +#include #include namespace blue::dynamics { +/** + * @brief A standard PWM value for no thrust. + * + * @note This can be used when the thruster is not being used, or when the desired thrust falls + * within the deadband zone. + */ +const uint16_t kNoThrustPwm = 1500; + /** * @brief Calculates the T200 deadzone band for a given voltage. * diff --git a/blue_manager/blue_manager/manager.py b/blue_manager/blue_manager/manager.py index 580dcbfd..a7bc2b17 100644 --- a/blue_manager/blue_manager/manager.py +++ b/blue_manager/blue_manager/manager.py @@ -74,14 +74,21 @@ def __init__(self) -> None: ) # Service clients + def wait_for_client(client) -> None: + while not client.wait_for_service(timeout_sec=1.0): + ... + self.set_param_srv_client = self.create_client( SetParameters, "/mavros/param/set_parameters", callback_group=reentrant_callback_group, ) + wait_for_client(self.set_param_srv_client) - while not self.set_param_srv_client.wait_for_service(timeout_sec=1.0): - ... + self.arm_controller_srv_client = self.create_client( + SetBool, "blue/control/arm", callback_group=reentrant_callback_group + ) + wait_for_client(self.arm_controller_srv_client) def _get_ros_params(self) -> None: """Get the ROS parameters from the parameter server.""" @@ -180,6 +187,20 @@ def set_thruster_params(self, params: list[Parameter]) -> bool: ) return all([result.successful for result in response.results]) + def arm_controller(self, arm: bool) -> bool: + """Arm/disarm the custom BlueROV2 controller. + + Args: + arm: Arm/disarm the controller. + + Returns: + True if the controller was successfully armed/disarmed, False otherwise. + """ + response: SetBool.Response = self.arm_controller_srv_client.call( + SetBool.Request(data=arm) + ) + return response.success + def set_rc_passthrough_mode_cb( self, request: SetBool.Request, response: SetBool.Response ) -> SetBool.Response: @@ -227,7 +248,8 @@ def set_rc_passthrough_mode_cb( for _ in range(self.retries): self.passthrough_enabled = self.set_thruster_params( list(passthrough_params.values()) - ) + ) and self.arm_controller(True) + response.success = self.passthrough_enabled if response.success: @@ -260,7 +282,8 @@ def set_rc_passthrough_mode_cb( for _ in range(self.retries): self.passthrough_enabled = not self.set_thruster_params( list(self.thruster_params_backup.values()) - ) + ) and self.arm_controller(False) + response.success = not self.passthrough_enabled if response.success: @@ -275,7 +298,7 @@ def set_rc_passthrough_mode_cb( f" QGC: {self.thruster_params_backup}" ) response.message = ( - "Failed to leave RC Passthrough mode. Good luck soldier." + "Failed to disable RC Passthrough mode. Good luck soldier." ) self.get_logger().info(response.message) From 3675497aae478945d635ba1afbe1f8051ca4d5f7 Mon Sep 17 00:00:00 2001 From: Evan Palmer Date: Mon, 15 May 2023 11:18:37 -0700 Subject: [PATCH 6/8] Fixed custom controllers build --- blue_control/CMakeLists.txt | 46 ++++++++++++++++++++++--------------- 1 file changed, 28 insertions(+), 18 deletions(-) diff --git a/blue_control/CMakeLists.txt b/blue_control/CMakeLists.txt index 2c2f8bad..679c2315 100644 --- a/blue_control/CMakeLists.txt +++ b/blue_control/CMakeLists.txt @@ -32,31 +32,44 @@ foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) find_package(${Dependency} REQUIRED) endforeach() -include_directories( - include +set(CUSTOM_CONTROLLER_HEADERS + include/blue_control/ismc.hpp ) -add_library(${PROJECT_NAME} SHARED - src/base_controller.cpp +set(CUSTOM_CONTROLLER_SOURCES src/ismc.cpp ) -target_include_directories(${PROJECT_NAME} PRIVATE include) -ament_target_dependencies(${PROJECT_NAME} ${THIS_PACKAGE_INCLUDE_DEPENDS}) -install( - DIRECTORY include/ - DESTINATION include +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 ${PROJECT_NAME} - EXPORT ${PROJECT_NAME} - RUNTIME DESTINATION bin - LIBRARY DESTINATION lib - ARCHIVE DESTINATION lib - INCLUDES DESTINATION include + TARGETS base_controller custom_controllers ismc + DESTINATION lib/${PROJECT_NAME} ) +install(DIRECTORY + include/ + DESTINATION include +) + + if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) @@ -67,7 +80,4 @@ if(BUILD_TESTING) ament_lint_auto_find_test_dependencies() endif() -ament_export_targets(${PROJECT_NAME} HAS_LIBRARY_TARGET) -ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS}) - ament_package() From 75397873a19dec64049bd19b5936ffe685b14fd8 Mon Sep 17 00:00:00 2001 From: Evan Palmer Date: Mon, 15 May 2023 11:51:22 -0700 Subject: [PATCH 7/8] use modernize bind --- .clang-tidy | 2 -- blue_control/include/blue_control/base_controller.hpp | 1 + 2 files changed, 1 insertion(+), 2 deletions(-) diff --git a/.clang-tidy b/.clang-tidy index a6bfad84..e221a2e2 100644 --- a/.clang-tidy +++ b/.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 diff --git a/blue_control/include/blue_control/base_controller.hpp b/blue_control/include/blue_control/base_controller.hpp index 02bf2c20..879a8e4c 100644 --- a/blue_control/include/blue_control/base_controller.hpp +++ b/blue_control/include/blue_control/base_controller.hpp @@ -22,6 +22,7 @@ #include #include +#include #include #include From 12db4b53de185c0d1a0d3d65c912ccfdc6c1d0da Mon Sep 17 00:00:00 2001 From: Evan Palmer Date: Mon, 15 May 2023 12:28:59 -0700 Subject: [PATCH 8/8] Resolved PR comments --- .../include/blue_control/base_controller.hpp | 25 +++++++++++++++---- blue_control/src/base_controller.cpp | 4 +-- blue_control/src/ismc.cpp | 6 +++-- .../blue_dynamics/thruster_dynamics.hpp | 10 ++++++++ 4 files changed, 36 insertions(+), 9 deletions(-) diff --git a/blue_control/include/blue_control/base_controller.hpp b/blue_control/include/blue_control/base_controller.hpp index 879a8e4c..ef19e6ed 100644 --- a/blue_control/include/blue_control/base_controller.hpp +++ b/blue_control/include/blue_control/base_controller.hpp @@ -51,6 +51,9 @@ 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. @@ -95,17 +98,29 @@ class BaseController : public rclcpp::Node * * @note This can be used to approximate the thrust curve according to the current battery * voltage. - * */ sensor_msgs::msg::BatteryState battery_state_; - // 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 + // + /** + * @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: - void armController( + /** + * @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 request, std::shared_ptr response); diff --git a/blue_control/src/base_controller.cpp b/blue_control/src/base_controller.cpp index 388c1c9b..6249caec 100644 --- a/blue_control/src/base_controller.cpp +++ b/blue_control/src/base_controller.cpp @@ -115,7 +115,7 @@ BaseController::BaseController(const std::string & node_name) "blue/control/arm", [this]( const std::shared_ptr request, std::shared_ptr response) { - armController(request, response); + armControllerCb(request, response); }); // Run the controller at a rate of 200 Hz @@ -128,7 +128,7 @@ BaseController::BaseController(const std::string & node_name) }); } -void BaseController::armController( +void BaseController::armControllerCb( const std::shared_ptr request, std::shared_ptr response) { diff --git a/blue_control/src/ismc.cpp b/blue_control/src/ismc.cpp index 5f9b2567..9c3f46d2 100644 --- a/blue_control/src/ismc.cpp +++ b/blue_control/src/ismc.cpp @@ -20,6 +20,7 @@ #include "blue_control/ismc.hpp" +#include #include #include "blue_dynamics/thruster_dynamics.hpp" @@ -102,17 +103,18 @@ mavros_msgs::msg::OverrideRCIn ISMC::update() channel = mavros_msgs::msg::OverrideRCIn::CHAN_NOCHANGE; } - // Apply the deadband to the PWM values const std::tuple deadband = blue::dynamics::calculateDeadZone(battery_state_.voltage); for (uint16_t i = 0; i < pwms.size(); i++) { uint16_t pwm = static_cast(pwms[i]); + // Apply the deadband to the PWM values if (pwm > std::get<0>(deadband) && pwm < std::get<1>(deadband)) { pwm = blue::dynamics::kNoThrustPwm; } - msg.channels[i] = pwm; + // Clamp the PWM to the valid PWM range + msg.channels[i] = std::clamp(pwm, blue::dynamics::kMinPwm, blue::dynamics::kMaxPwm); } return msg; diff --git a/blue_dynamics/include/blue_dynamics/thruster_dynamics.hpp b/blue_dynamics/include/blue_dynamics/thruster_dynamics.hpp index d14b1d77..216fbd63 100644 --- a/blue_dynamics/include/blue_dynamics/thruster_dynamics.hpp +++ b/blue_dynamics/include/blue_dynamics/thruster_dynamics.hpp @@ -26,6 +26,16 @@ namespace blue::dynamics { +/** + * @brief The minimum PWM value that can be sent to the T200 thrusters. + */ +const uint16_t kMinPwm = 1100; + +/** + * @brief The maximum PWM value that can be sent to the T200 thrusters. + */ +const uint16_t kMaxPwm = 1900; + /** * @brief A standard PWM value for no thrust. *