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/.dockerignore b/.dockerignore index ba822352..fdcd7f9d 100644 --- a/.dockerignore +++ b/.dockerignore @@ -4,3 +4,5 @@ # Except the following !blue_dynamics !blue_manager +!blue_control +!blue_msgs diff --git a/blue_control/CMakeLists.txt b/blue_control/CMakeLists.txt new file mode 100644 index 00000000..679c2315 --- /dev/null +++ b/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() 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..ef19e6ed --- /dev/null +++ b/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 +#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" +#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`. + * + * @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 & 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 request, + std::shared_ptr response); + + bool armed_; + + // 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_; + + // 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 new file mode 100644 index 00000000..40b8838b --- /dev/null +++ b/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 + +#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::SharedPtr cmd_sub_; +}; + +} // namespace blue::control diff --git a/blue_control/package.xml b/blue_control/package.xml new file mode 100644 index 00000000..1c074f52 --- /dev/null +++ b/blue_control/package.xml @@ -0,0 +1,27 @@ + + + + blue_control + 0.0.1 + Control interface for the BlueROV2 + Evan Palmer + MIT + + nav_msgs + sensor_msgs + mavros_msgs + rclcpp + blue_dynamics + eigen + blue_msgs + std_srvs + + 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..6249caec --- /dev/null +++ b/blue_control/src/base_controller.cpp @@ -0,0 +1,150 @@ +// 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)), + armed_(false) +{ + // 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}); + 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 + 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}); + + // 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("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 + 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](sensor_msgs::msg::BatteryState::ConstSharedPtr msg) { battery_state_ = *msg; }); + + odom_sub_ = this->create_subscription( + "/mavros/local_position/odom", 1, + [this](nav_msgs::msg::Odometry::ConstSharedPtr msg) { odom_ = *msg; }); + + arm_srv_ = this->create_service( + "blue/control/arm", [this]( + const std::shared_ptr request, + std::shared_ptr response) { + armControllerCb(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]() { + if (armed_) { + rc_override_pub_->publish(update()); + } + }); +} + +void BaseController::armControllerCb( + 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 new file mode 100644 index 00000000..9c3f46d2 --- /dev/null +++ b/blue_control/src/ismc.cpp @@ -0,0 +1,132 @@ +// 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/ismc.hpp" + +#include +#include + +#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); + + // 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; + + // 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); + + // 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_); }); + + 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()) + + sliding_gain_ * surface; + + // Multiply the desired forces by the pseudoinverse of the thruster configuration matrix + const Eigen::VectorXd pwms = tcm_.completeOrthogonalDecomposition().pseudoInverse() * forces; + + // Convert the thruster forces to PWM values + pwms.unaryExpr([this](double x) { + return blue::dynamics::calculatePwmFromThrustSurface(x, battery_state_.voltage); + }); + + mavros_msgs::msg::OverrideRCIn msg; + + // 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; + } + + 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; + } + + // Clamp the PWM to the valid PWM range + msg.channels[i] = std::clamp(pwm, blue::dynamics::kMinPwm, blue::dynamics::kMaxPwm); + } + + 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 0f8b695b..c0674feb 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. * @@ -64,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 @@ -86,6 +90,11 @@ class Inertia class Coriolis { public: + /** + * @brief Default constructor for the Coriolis object. + */ + Coriolis() = default; + /** * @brief Construct a new Coriolis object. * @@ -101,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. @@ -112,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). @@ -131,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). @@ -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. * @@ -169,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. * @@ -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/include/blue_dynamics/thruster_dynamics.hpp b/blue_dynamics/include/blue_dynamics/thruster_dynamics.hpp index 4979ccb8..216fbd63 100644 --- a/blue_dynamics/include/blue_dynamics/thruster_dynamics.hpp +++ b/blue_dynamics/include/blue_dynamics/thruster_dynamics.hpp @@ -20,11 +20,30 @@ #pragma once +#include #include 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. + * + * @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_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)) { } 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) 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 + +