From e97bea87265348b3d63fcfeef1fe45949a330cff Mon Sep 17 00:00:00 2001 From: Evan Palmer Date: Tue, 20 Jun 2023 20:47:35 -0700 Subject: [PATCH 01/16] Started debugging controller --- blue_bringup/config/bluerov2.yaml | 15 ++-- blue_bringup/config/bluerov2_heavy.yaml | 3 + blue_bringup/scripts/arm.sh | 7 ++ blue_bringup/scripts/disarm.sh | 8 +++ blue_bringup/scripts/test_command.sh | 0 blue_control/CMakeLists.txt | 8 +++ .../include/blue_control/controller.hpp | 63 ++++++++++++----- blue_control/package.xml | 2 + blue_control/src/controller.cpp | 52 +++++--------- blue_control/src/ismc.cpp | 25 +++++-- blue_control/test/test_vector_to_eigen.cpp | 69 +++++++++++++++++++ 11 files changed, 189 insertions(+), 63 deletions(-) create mode 100755 blue_bringup/scripts/arm.sh create mode 100755 blue_bringup/scripts/disarm.sh create mode 100644 blue_bringup/scripts/test_command.sh create mode 100644 blue_control/test/test_vector_to_eigen.cpp diff --git a/blue_bringup/config/bluerov2.yaml b/blue_bringup/config/bluerov2.yaml index 168cadee..52e1f91d 100644 --- a/blue_bringup/config/bluerov2.yaml +++ b/blue_bringup/config/bluerov2.yaml @@ -17,15 +17,18 @@ ismc: center_of_buoyancy: [0.0, 0.0, 0.2] ocean_current: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] num_thrusters: 6 - tcm: [ 0.707, 0.707, -0.707, -0.707, 0.0, 0.0, - -0.707, 0.707, -0.707, 0.707, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 1.0, 1.0, - 0.051, -0.051, 0.051, -0.051, 0.111, -0.111, - 0.051, 0.051, -0.051, -0.051, 0.002, -0.002, - -0.167, 0.167, 0.175, -0.175, 0.0, 0.0 ] + tcm: [ 0.707, 0.707, -0.707, -0.707, 0.0, 0.0, + -0.707, 0.707, -0.707, 0.707, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, -1.0, 1.0, + 0.0, 0.0, 0.0, 0.0, 0.218, 0.218, + 0.0, 0.0, 0.0, 0.0, 0.12, -0.12, + -0.1888, 0.1888, 0.1888, -0.1888, 0.0, 0.0] msg_ids: [31, 32] msg_rates: [100.0, 100.0] control_loop_freq: 200.0 + convergence_rate: [2.0, 2.0, 2.0, 2.0, 2.0, 2.0] + sliding_gain: 100.0 + boundary_thickness: 100.0 use_battery_state: false aruco_marker_localizer: diff --git a/blue_bringup/config/bluerov2_heavy.yaml b/blue_bringup/config/bluerov2_heavy.yaml index 8ce1deca..9f197a72 100644 --- a/blue_bringup/config/bluerov2_heavy.yaml +++ b/blue_bringup/config/bluerov2_heavy.yaml @@ -26,6 +26,9 @@ ismc: msg_ids: [31, 32] msg_rates: [100.0, 100.0] control_loop_freq: 200.0 + convergence_rate: [100.0, 100.0, 100.0, 100.0, 100.0, 100.0] + sliding_gain: 0.0 + boundary_thickness: 0.0 use_battery_state: false aruco_marker_localizer: diff --git a/blue_bringup/scripts/arm.sh b/blue_bringup/scripts/arm.sh new file mode 100755 index 00000000..5b032278 --- /dev/null +++ b/blue_bringup/scripts/arm.sh @@ -0,0 +1,7 @@ +#!/usr/bin/env bash + +# This is a simple script used to make your life easier when arming a custom controller + +ros2 service call /blue/manager/enable_passthrough std_srvs/srv/SetBool data:\ true\ + +ros2 service call /blue/control/arm std_srvs/srv/SetBool data:\ true\ diff --git a/blue_bringup/scripts/disarm.sh b/blue_bringup/scripts/disarm.sh new file mode 100755 index 00000000..1fefbb73 --- /dev/null +++ b/blue_bringup/scripts/disarm.sh @@ -0,0 +1,8 @@ +#!/usr/bin/env bash + +# This is a simple script used to make your life easier when disarming a custom controller + +ros2 service call /blue/control/arm std_srvs/srv/SetBool data:\ false\ + +ros2 service call /blue/manager/enable_passthrough std_srvs/srv/SetBool data:\ false\ + diff --git a/blue_bringup/scripts/test_command.sh b/blue_bringup/scripts/test_command.sh new file mode 100644 index 00000000..e69de29b diff --git a/blue_control/CMakeLists.txt b/blue_control/CMakeLists.txt index e5b363d6..24b9759d 100644 --- a/blue_control/CMakeLists.txt +++ b/blue_control/CMakeLists.txt @@ -76,6 +76,7 @@ install(DIRECTORY ) if(BUILD_TESTING) + find_package(ament_cmake_gtest REQUIRED) find_package(ament_lint_auto REQUIRED) # Run linters found in package.xml except the two below @@ -83,6 +84,13 @@ if(BUILD_TESTING) set(ament_cmake_uncrustify_FOUND TRUE) ament_lint_auto_find_test_dependencies() + + # Setup unit tests + ament_add_gtest( + test_vector_to_eigen + test/test_vector_to_eigen.cpp + ) + target_link_libraries(test_vector_to_eigen custom_controllers) endif() ament_package() diff --git a/blue_control/include/blue_control/controller.hpp b/blue_control/include/blue_control/controller.hpp index 4f684507..5791e3b7 100644 --- a/blue_control/include/blue_control/controller.hpp +++ b/blue_control/include/blue_control/controller.hpp @@ -28,6 +28,7 @@ #include "blue_dynamics/hydrodynamics.hpp" #include "blue_dynamics/thruster_dynamics.hpp" +#include "geometry_msgs/msg/pose_stamped.hpp" #include "mavros_msgs/msg/override_rc_in.hpp" #include "mavros_msgs/srv/message_interval.hpp" #include "nav_msgs/msg/odometry.hpp" @@ -39,29 +40,55 @@ namespace blue::control { /** - * @brief Convert an `std::vector` to an `Eigen::VectorXd`. + * @brief Convert an std::vector into an Eigen::Matrix. * - * @note This method is useful when converting a ROS parameter that has been read as a `std::vector` - * to an `Eigen::VectorXd`. + * @note The elements are copied over column-wise because Eigen defaults to column major. * - * @param vec The `std::vector` to convert. - * @return An `Eigen::VectorXd` with the same values as `vec`. + * @note While it would be preferable to define the rows and cols as template parameters, the + * primary use-case for this method within the scope of this implementation is to use it with ROS 2 + * parameters which are not known at compile time. Therefore, the rows and cols are made to be + * function parameters. + * + * @tparam T The type of values held by the vector. + * @param rows The number of rows in the resulting matrix. + * @param cols The number of columns in the resulting matrix. + * @param vec The vector to convert into a matrix. + * @return The converted Eigen matrix. */ -Eigen::VectorXd convertVectorToEigenVector(const std::vector & vec); +template +inline Eigen::Matrix convertVectorToEigenMatrix( + const std::vector & vec, int rows, int cols) +{ + typedef const Eigen::Matrix M; + Eigen::Map mat(vec.data(), rows, cols); + + return mat; +} /** - * @brief Convert an `std::vector` to an `Eigen::MatrixXd`. + * @brief Convert an std::vector into an Eigen::Matrix * - * @note This method is useful when converting a ROS parameter that has been read as a `std::vector` - * to an `Eigen::MatrixXd`. + * @note While it would be preferable to define the rows and cols as template parameters, the + * primary use-case for this method within the scope of this implementation is to use it with ROS 2 + * parameters which are not known at compile time. Therefore, the rows and cols are made to be + * function parameters. * - * @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`. + * @tparam T The type of values held by the vector. + * @tparam major The order to copy over the elements in (e.g., ``Eigen::RowMajor``) + * @param rows The number of rows in the resulting matrix. + * @param cols The number of columns in the resulting matrix. + * @param vec The vector to convert into a matrix. + * @return The converted Eigen matrix. */ -Eigen::MatrixXd convertVectorToEigenMatrix( - const std::vector & vec, size_t rows, size_t cols); +template +inline Eigen::Matrix convertVectorToEigenMatrix( + const std::vector & vec, int rows, int cols) +{ + typedef const Eigen::Matrix M; + Eigen::Map mat(vec.data(), rows, cols); + + return mat; +} /** * @brief A base class for custom BlueROV2 controllers. @@ -105,8 +132,8 @@ class Controller : public rclcpp::Node /** * @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: + * @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_; @@ -158,8 +185,10 @@ class Controller : public rclcpp::Node // Subscribers rclcpp::Subscription::SharedPtr battery_state_sub_; rclcpp::Subscription::SharedPtr odom_sub_; + rclcpp::Subscription::SharedPtr ardu_pose_sub_; // Timers + rclcpp::CallbackGroup::SharedPtr timer_cb_group_; rclcpp::TimerBase::SharedPtr control_loop_timer_; rclcpp::TimerBase::SharedPtr set_message_rate_timer_; diff --git a/blue_control/package.xml b/blue_control/package.xml index a1f264a3..4e47276f 100644 --- a/blue_control/package.xml +++ b/blue_control/package.xml @@ -19,6 +19,8 @@ ament_cmake + ament_cmake_gtest + ament_cmake_gmock ament_lint_auto ament_lint_common diff --git a/blue_control/src/controller.cpp b/blue_control/src/controller.cpp index 8667616b..fa310c6d 100644 --- a/blue_control/src/controller.cpp +++ b/blue_control/src/controller.cpp @@ -23,21 +23,6 @@ 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; -} - Controller::Controller(const std::string & node_name) : Node(std::move(node_name)), armed_(false) @@ -75,25 +60,26 @@ Controller::Controller(const std::string & node_name) 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()); + const Eigen::Vector3d inertia_tensor_coeff = convertVectorToEigenMatrix( + this->get_parameter("inertia_tensor_coeff").as_double_array(), 3, 1); + const Eigen::Matrix added_mass_coeff = convertVectorToEigenMatrix( + this->get_parameter("added_mass_coeff").as_double_array(), 6, 1); + const Eigen::Matrix linear_damping_coeff = convertVectorToEigenMatrix( + this->get_parameter("linear_damping_coeff").as_double_array(), 6, 1); + const Eigen::Matrix quadratic_damping_coeff = convertVectorToEigenMatrix( + this->get_parameter("quadratic_damping_coeff").as_double_array(), 6, 1); + const Eigen::Vector3d center_of_gravity = convertVectorToEigenMatrix( + this->get_parameter("center_of_gravity").as_double_array(), 3, 1); + const Eigen::Vector3d center_of_buoyancy = convertVectorToEigenMatrix( + this->get_parameter("center_of_buoyancy").as_double_array(), 3, 1); + const Eigen::Matrix ocean_current = convertVectorToEigenMatrix( + this->get_parameter("ocean_current").as_double_array(), 6, 1); // 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); + const int num_thrusters = this->get_parameter("num_thrusters").as_int(); + tcm_ = convertVectorToEigenMatrix( + tcm_vec, static_cast(tcm_vec.size() / num_thrusters), num_thrusters); // Initialize the hydrodynamic parameters hydrodynamics_ = blue::dynamics::HydrodynamicParameters( @@ -108,11 +94,11 @@ Controller::Controller(const std::string & node_name) this->create_publisher("mavros/rc/override", 1); battery_state_sub_ = this->create_subscription( - "/mavros/battery", 1, + "/mavros/battery", rclcpp::SensorDataQoS(), [this](sensor_msgs::msg::BatteryState::ConstSharedPtr msg) { battery_state_ = *msg; }); odom_sub_ = this->create_subscription( - "/mavros/local_position/odom", 1, + "/mavros/local_position/odom", rclcpp::SensorDataQoS(), [this](nav_msgs::msg::Odometry::ConstSharedPtr msg) { odom_ = *msg; }); arm_srv_ = this->create_service( diff --git a/blue_control/src/ismc.cpp b/blue_control/src/ismc.cpp index 26482468..9424bf63 100644 --- a/blue_control/src/ismc.cpp +++ b/blue_control/src/ismc.cpp @@ -40,8 +40,8 @@ ISMC::ISMC() this->declare_parameter("boundary_thickness", 0.0); this->declare_parameter("use_battery_state", false); - Eigen::VectorXd convergence_diag = - convertVectorToEigenVector(this->get_parameter("convergence_rate").as_double_array()); + Eigen::VectorXd convergence_diag = convertVectorToEigenMatrix( + this->get_parameter("convergence_rate").as_double_array(), 6, 1); convergence_rate_ = convergence_diag.asDiagonal().toDenseMatrix(); sliding_gain_ = this->get_parameter("sliding_gain").as_double(); boundary_thickness_ = this->get_parameter("boundary_thickness").as_double(); @@ -81,7 +81,7 @@ mavros_msgs::msg::OverrideRCIn ISMC::update() 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_); }); + surface = surface.unaryExpr([this](double x) { return tanh(x / boundary_thickness_); }); const blue::dynamics::Vector6d forces = hydrodynamics_.inertia.getInertia() * (desired_accel + convergence_rate_ * velocity_error) + @@ -93,17 +93,26 @@ mavros_msgs::msg::OverrideRCIn ISMC::update() // Multiply the desired forces by the pseudoinverse of the thruster configuration matrix // The size of this vector will depend on the number of thrusters so we don't assign it to a // fixed-size matrix - const Eigen::VectorXd pwms = tcm_.completeOrthogonalDecomposition().pseudoInverse() * forces; + Eigen::VectorXd thruster_forces = tcm_.completeOrthogonalDecomposition().pseudoInverse() * forces; // Convert the thruster forces to PWM values + Eigen::VectorXi pwms; + if (use_battery_state_) { - pwms.unaryExpr([this](double x) { + pwms = thruster_forces.unaryExpr([this](double x) { return blue::dynamics::calculatePwmFromThrustSurface(x, battery_state_.voltage); }); } else { - pwms.unaryExpr([this](double x) { return blue::dynamics::calculatePwmFromThrustCurve(x); }); + pwms = thruster_forces.unaryExpr( + [this](double x) { return blue::dynamics::calculatePwmFromThrustCurve(x); }); } + std::string sep = "\n----------------------------------------\n"; + std::stringstream ss; + ss << std::endl << velocity_error << sep << forces << sep << pwms << sep; + std::string sad = ss.str(); + RCLCPP_WARN(this->get_logger(), "pwms: %s", sad.c_str()); + mavros_msgs::msg::OverrideRCIn msg; // We only modify the first "n" channels where "n" is the total number of thrusters @@ -142,7 +151,9 @@ int main(int argc, char ** argv) { rclcpp::init(argc, argv); auto node = std::make_shared(); - rclcpp::spin(node); + rclcpp::executors::MultiThreadedExecutor executor; + executor.add_node(node); + executor.spin(); rclcpp::shutdown(); return 0; } diff --git a/blue_control/test/test_vector_to_eigen.cpp b/blue_control/test/test_vector_to_eigen.cpp new file mode 100644 index 00000000..f7fb541a --- /dev/null +++ b/blue_control/test/test_vector_to_eigen.cpp @@ -0,0 +1,69 @@ +// 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_control/controller.hpp" + +namespace blue::controller::test +{ + +using blue::control::convertVectorToEigenMatrix; + +TEST(ControllerTest, TestVectorToEigenVector) +{ + const std::vector coeff = {1, 2, 3, 4}; + + Eigen::Matrix expected; // NOLINT + expected << 1, 2, 3, 4; + + const Eigen::Matrix actual = convertVectorToEigenMatrix(coeff, 4, 1); + + ASSERT_TRUE(actual.isApprox(expected)); +} + +TEST(ControllerTest, TestVectorToEigenMatrix) +{ + const std::vector coeff = {1, 2, 3, 4}; + + Eigen::Matrix expected; // NOLINT + expected << 1, 2, 3, 4; + + const Eigen::Matrix actual = + convertVectorToEigenMatrix(coeff, 2, 2); + + std::cout << expected << std::endl << actual << std::endl; + + ASSERT_TRUE(actual.isApprox(expected)); +} + +} // namespace blue::controller::test + +int main(int argc, char ** argv) +{ + ::testing::InitGoogleTest(&argc, argv); + const int result = RUN_ALL_TESTS(); + + return result; +} From 0476435a32a96453f42ca84ede62c54e13903057 Mon Sep 17 00:00:00 2001 From: Evan Palmer Date: Wed, 21 Jun 2023 23:33:16 -0700 Subject: [PATCH 02/16] Added proportional gain and fixed missing initial conditions --- blue_bringup/config/bluerov2.yaml | 7 +- blue_bringup/config/bluerov2_heavy.yaml | 5 +- .../include/blue_control/controller.hpp | 46 +++++------- blue_control/include/blue_control/ismc.hpp | 7 +- blue_control/src/controller.cpp | 15 +++- blue_control/src/ismc.cpp | 73 ++++++++++++++++--- 6 files changed, 106 insertions(+), 47 deletions(-) diff --git a/blue_bringup/config/bluerov2.yaml b/blue_bringup/config/bluerov2.yaml index 52e1f91d..1249ef93 100644 --- a/blue_bringup/config/bluerov2.yaml +++ b/blue_bringup/config/bluerov2.yaml @@ -25,9 +25,10 @@ ismc: -0.1888, 0.1888, 0.1888, -0.1888, 0.0, 0.0] msg_ids: [31, 32] msg_rates: [100.0, 100.0] - control_loop_freq: 200.0 - convergence_rate: [2.0, 2.0, 2.0, 2.0, 2.0, 2.0] - sliding_gain: 100.0 + control_rate: 200.0 + proportional_gain: [5.0, 5.0, 5.0, 5.0, 5.0, 5.0] + integral_gain: [0.1, 0.1, 0.1, 0.1, 0.1, 0.1] + sliding_gain: 5.0 boundary_thickness: 100.0 use_battery_state: false diff --git a/blue_bringup/config/bluerov2_heavy.yaml b/blue_bringup/config/bluerov2_heavy.yaml index 9f197a72..05048c9a 100644 --- a/blue_bringup/config/bluerov2_heavy.yaml +++ b/blue_bringup/config/bluerov2_heavy.yaml @@ -25,8 +25,9 @@ ismc: -0.1888, 0.1888, 0.1888, -0.1888, 0.0, 0.0, 0.0, 0.0] msg_ids: [31, 32] msg_rates: [100.0, 100.0] - control_loop_freq: 200.0 - convergence_rate: [100.0, 100.0, 100.0, 100.0, 100.0, 100.0] + control_rate: 200.0 + proportional_gain: [5.0, 5.0, 5.0, 5.0, 5.0, 5.0] + integral_gain: [0.1, 0.1, 0.1, 0.1, 0.1, 0.1] sliding_gain: 0.0 boundary_thickness: 0.0 use_battery_state: false diff --git a/blue_control/include/blue_control/controller.hpp b/blue_control/include/blue_control/controller.hpp index 5791e3b7..76c409d9 100644 --- a/blue_control/include/blue_control/controller.hpp +++ b/blue_control/include/blue_control/controller.hpp @@ -28,6 +28,7 @@ #include "blue_dynamics/hydrodynamics.hpp" #include "blue_dynamics/thruster_dynamics.hpp" +#include "geometry_msgs/msg/accel.hpp" #include "geometry_msgs/msg/pose_stamped.hpp" #include "mavros_msgs/msg/override_rc_in.hpp" #include "mavros_msgs/srv/message_interval.hpp" @@ -39,32 +40,6 @@ namespace blue::control { -/** - * @brief Convert an std::vector into an Eigen::Matrix. - * - * @note The elements are copied over column-wise because Eigen defaults to column major. - * - * @note While it would be preferable to define the rows and cols as template parameters, the - * primary use-case for this method within the scope of this implementation is to use it with ROS 2 - * parameters which are not known at compile time. Therefore, the rows and cols are made to be - * function parameters. - * - * @tparam T The type of values held by the vector. - * @param rows The number of rows in the resulting matrix. - * @param cols The number of columns in the resulting matrix. - * @param vec The vector to convert into a matrix. - * @return The converted Eigen matrix. - */ -template -inline Eigen::Matrix convertVectorToEigenMatrix( - const std::vector & vec, int rows, int cols) -{ - typedef const Eigen::Matrix M; - Eigen::Map mat(vec.data(), rows, cols); - - return mat; -} - /** * @brief Convert an std::vector into an Eigen::Matrix * @@ -80,7 +55,7 @@ inline Eigen::Matrix convertVectorToEigenMatr * @param vec The vector to convert into a matrix. * @return The converted Eigen matrix. */ -template +template inline Eigen::Matrix convertVectorToEigenMatrix( const std::vector & vec, int rows, int cols) { @@ -104,6 +79,16 @@ class Controller : public rclcpp::Node explicit Controller(const std::string & node_name); protected: + /** + * @brief Function executed when the controller is armed. + */ + virtual void onArm() = 0; + + /** + * @brief Function executed when the controller is disarmed. + */ + virtual void onDisarm() = 0; + /** * @brief Update the current control inputs to the thrusters * @@ -138,6 +123,13 @@ class Controller : public rclcpp::Node */ nav_msgs::msg::Odometry odom_; + /** + * @brief The current acceleration of the BlueROV2. + * + * @note This is not provided by the system directly and is calculated using finite differencing. + */ + geometry_msgs::msg::Accel accel_; + /** * @brief The total time (s) between control loop iterations * diff --git a/blue_control/include/blue_control/ismc.hpp b/blue_control/include/blue_control/ismc.hpp index 071ec3bb..1249c5ac 100644 --- a/blue_control/include/blue_control/ismc.hpp +++ b/blue_control/include/blue_control/ismc.hpp @@ -41,12 +41,17 @@ class ISMC : public Controller ISMC(); protected: + void onArm() override; + void onDisarm() override; mavros_msgs::msg::OverrideRCIn update() override; private: // Hyperparameters used by the ISMC + blue::dynamics::Vector6d initial_velocity_error_; + blue::dynamics::Vector6d initial_acceleration_error_; blue::dynamics::Vector6d total_velocity_error_; - blue::dynamics::Matrix6d convergence_rate_; + blue::dynamics::Matrix6d integral_gain_; + blue::dynamics::Matrix6d proportional_gain_; double sliding_gain_; double boundary_thickness_; diff --git a/blue_control/src/controller.cpp b/blue_control/src/controller.cpp index fa310c6d..fa0955fa 100644 --- a/blue_control/src/controller.cpp +++ b/blue_control/src/controller.cpp @@ -44,7 +44,7 @@ Controller::Controller(const std::string & node_name) this->declare_parameter("num_thrusters", 8); this->declare_parameter("msg_ids", std::vector({31, 32})); this->declare_parameter("msg_rates", std::vector({100, 100})); - this->declare_parameter("control_loop_freq", 200.0); + this->declare_parameter("control_rate", 200.0); // I'm so sorry for this // You can blame the ROS devs for not supporting nested arrays for parameters @@ -99,7 +99,10 @@ Controller::Controller(const std::string & node_name) odom_sub_ = this->create_subscription( "/mavros/local_position/odom", rclcpp::SensorDataQoS(), - [this](nav_msgs::msg::Odometry::ConstSharedPtr msg) { odom_ = *msg; }); + [this](nav_msgs::msg::Odometry::ConstSharedPtr msg) { + // TODO(evan): Calculate the current acceleration here + odom_ = *msg; + }); arm_srv_ = this->create_service( "blue/control/arm", [this]( @@ -137,7 +140,7 @@ Controller::Controller(const std::string & node_name) [this, msg_ids, msg_rates]() -> void { setMessageRates(msg_ids, msg_rates); }); // Convert the control loop frequency to seconds - dt_ = 1 / this->get_parameter("control_loop_freq").as_double(); + dt_ = 1 / this->get_parameter("control_rate").as_double(); control_loop_timer_ = this->create_wall_timer(std::chrono::duration(dt_), [this]() -> void { @@ -152,6 +155,9 @@ void Controller::armControllerCb( std::shared_ptr response) { if (request->data) { + // Run the controller arming function prior to actually arming the controller + onArm(); + // Arm the controller armed_ = true; response->success = true; @@ -163,6 +169,9 @@ void Controller::armControllerCb( response->success = true; response->message = "Controller disarmed."; RCLCPP_WARN(this->get_logger(), "Custom BlueROV2 controller disarmed."); + + // Run the controller disarming function after the controller has been fully disarmed + onDisarm(); } } diff --git a/blue_control/src/ismc.cpp b/blue_control/src/ismc.cpp index 9424bf63..fa14e6fd 100644 --- a/blue_control/src/ismc.cpp +++ b/blue_control/src/ismc.cpp @@ -30,22 +30,29 @@ namespace blue::control { ISMC::ISMC() -: Controller("ismc") +: Controller("ismc"), + initial_velocity_error_(blue::dynamics::Vector6d::Zero()), + initial_acceleration_error_(blue::dynamics::Vector6d::Zero()), + total_velocity_error_(blue::dynamics::Vector6d::Zero()) { // 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("integral_gain", std::vector({1.0, 1.0, 1.0, 1.0, 1.0, 1.0})); + this->declare_parameter("proportional_gain", std::vector({1.0, 1.0, 1.0, 1.0, 1.0, 1.0})); this->declare_parameter("sliding_gain", 0.0); this->declare_parameter("boundary_thickness", 0.0); this->declare_parameter("use_battery_state", false); - Eigen::VectorXd convergence_diag = convertVectorToEigenMatrix( - this->get_parameter("convergence_rate").as_double_array(), 6, 1); - convergence_rate_ = convergence_diag.asDiagonal().toDenseMatrix(); + // Get the gain matrices + Eigen::VectorXd integral_gain_coeff = convertVectorToEigenMatrix( + this->get_parameter("integral_gain").as_double_array(), 6, 1); + Eigen::VectorXd proportional_gain_coeff = convertVectorToEigenMatrix( + this->get_parameter("integral_gain").as_double_array(), 6, 1); + + integral_gain_ = integral_gain_coeff.asDiagonal().toDenseMatrix(); + proportional_gain_ = proportional_gain_coeff.asDiagonal().toDenseMatrix(); + sliding_gain_ = this->get_parameter("sliding_gain").as_double(); boundary_thickness_ = this->get_parameter("boundary_thickness").as_double(); - total_velocity_error_ = blue::dynamics::Vector6d::Zero(); use_battery_state_ = this->get_parameter("use_battery_state").as_bool(); // Update the reference signal when a new command is received @@ -53,6 +60,48 @@ ISMC::ISMC() "/blue/ismc/cmd", 1, [this](blue_msgs::msg::Reference::ConstSharedPtr msg) { cmd_ = *msg; }); } +void ISMC::onArm() +{ + // Reset the total velocity error + total_velocity_error_ = blue::dynamics::Vector6d::Zero(); + + // Reset the initial conditions + initial_velocity_error_ = blue::dynamics::Vector6d::Zero(); + initial_acceleration_error_ = blue::dynamics::Vector6d::Zero(); + + // We need to calculate the initial conditions for the controller now. This includes the + // initial velocity and acceleration errors + + // Start by calculating the velocity error i.c. + blue::dynamics::Vector6d velocity; + tf2::fromMsg(odom_.twist.twist, velocity); + + tf2::fromMsg(cmd_.twist, initial_velocity_error_); + initial_velocity_error_ -= velocity; + + // Now calculate the accleration error i.c. + blue::dynamics::Vector6d accel; + accel << accel_.linear.x, accel_.linear.y, accel_.linear.z, accel_.angular.x, accel_.angular.y, + accel_.angular.z; + + // There is no suitable tf2_eigen function for Accel types :( + blue::dynamics::Vector6d accel_error; + accel_error << cmd_.accel.linear.x, cmd_.accel.linear.y, cmd_.accel.linear.z, + cmd_.accel.angular.x, cmd_.accel.angular.y, cmd_.accel.angular.z; + initial_acceleration_error_ = accel_error; + initial_acceleration_error_ -= accel; +}; + +void ISMC::onDisarm() +{ + // Reset the total velocity error on disarm just to be safe + total_velocity_error_ = blue::dynamics::Vector6d::Zero(); + + // Reset the intial conditions too + initial_velocity_error_ = blue::dynamics::Vector6d::Zero(); + initial_acceleration_error_ = blue::dynamics::Vector6d::Zero(); +}; + mavros_msgs::msg::OverrideRCIn ISMC::update() { blue::dynamics::Vector6d velocity; @@ -63,7 +112,7 @@ mavros_msgs::msg::OverrideRCIn ISMC::update() tf2::fromMsg(cmd_.twist, velocity_error); velocity_error -= velocity; - // There is no suitable tf2_eigen function for Accel types :( + // :( blue::dynamics::Vector6d desired_accel; // 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; @@ -78,13 +127,15 @@ mavros_msgs::msg::OverrideRCIn ISMC::update() // Calculate the sliding surface blue::dynamics::Vector6d surface = - velocity_error + convergence_rate_ * total_velocity_error_; // NOLINT + proportional_gain_ * velocity_error + integral_gain_ * total_velocity_error_ - + proportional_gain_ * initial_velocity_error_ - initial_acceleration_error_; // NOLINT // Apply the sign function to the surface surface = surface.unaryExpr([this](double x) { return tanh(x / boundary_thickness_); }); const blue::dynamics::Vector6d forces = - hydrodynamics_.inertia.getInertia() * (desired_accel + convergence_rate_ * velocity_error) + + hydrodynamics_.inertia.getInertia() * + (desired_accel + proportional_gain_.inverse() * integral_gain_ * velocity_error) + hydrodynamics_.coriolis.calculateCoriolis(velocity) * velocity + hydrodynamics_.damping.calculateDamping(velocity) * velocity + hydrodynamics_.restoring_forces.calculateRestoringForces(orientation.toRotationMatrix()) + From d9d80c33ea6e8e5d85ffa0a1229c9c1ceb493cdd Mon Sep 17 00:00:00 2001 From: Evan Palmer Date: Thu, 22 Jun 2023 17:27:30 -0700 Subject: [PATCH 03/16] Added gazebo localizer and fixed some more bugs --- blue_bringup/config/bluerov2.yaml | 36 +-- blue_bringup/config/bluerov2_heavy.yaml | 4 + blue_bringup/launch/bluerov2.launch.py | 9 +- blue_bringup/launch/bluerov2_heavy.launch.py | 219 ------------------ blue_bringup/scripts/test_command.sh | 0 .../include/blue_control/controller.hpp | 29 ++- blue_control/package.xml | 3 + blue_control/src/controller.cpp | 54 ++++- blue_description/CMakeLists.txt | 2 +- .../gazebo/models/bluerov2/model.sdf | 10 + .../gazebo/models/bluerov2_heavy/model.sdf | 10 + .../{config => params}/bluerov2.parm | 13 ++ .../{config => params}/bluerov2_heavy.parm | 13 ++ .../blue_localization/localizer.py | 48 ++++ .../launch/localization.launch.py | 12 +- blue_localization/setup.py | 1 + blue_manager/blue_manager/manager.py | 13 +- 17 files changed, 223 insertions(+), 253 deletions(-) delete mode 100644 blue_bringup/launch/bluerov2_heavy.launch.py delete mode 100644 blue_bringup/scripts/test_command.sh rename blue_description/{config => params}/bluerov2.parm (77%) rename blue_description/{config => params}/bluerov2_heavy.parm (72%) diff --git a/blue_bringup/config/bluerov2.yaml b/blue_bringup/config/bluerov2.yaml index 1249ef93..07212380 100644 --- a/blue_bringup/config/bluerov2.yaml +++ b/blue_bringup/config/bluerov2.yaml @@ -4,6 +4,7 @@ blue_manager: mode_change_timeout: 1.0 mode_change_retries: 3 +# Controller ismc: ros__parameters: mass: 10.0 @@ -26,12 +27,25 @@ ismc: msg_ids: [31, 32] msg_rates: [100.0, 100.0] control_rate: 200.0 - proportional_gain: [5.0, 5.0, 5.0, 5.0, 5.0, 5.0] - integral_gain: [0.1, 0.1, 0.1, 0.1, 0.1, 0.1] + proportional_gain: [10.0, 10.0, 10.0, 10.0, 10.0, 10.0] + integral_gain: [3.0, 3.0, 3.0, 3.0, 3.0, 3.0] sliding_gain: 5.0 - boundary_thickness: 100.0 + boundary_thickness: 1.0 use_battery_state: false +# Localization sources +camera: + ros__parameters: + port: 5600 + +qualisys_mocap: + ros__parameters: + ip: "192.168.0.0" + port: 22223 + version: "1.22" + body: "bluerov" + +# Localizers aruco_marker_localizer: ros__parameters: camera_matrix: @@ -45,21 +59,15 @@ aruco_marker_localizer: 0.0, 1108.39001, 456.92861, 0.0, 0.0, 0.0, 1.0, 0.0] -camera: - ros__parameters: - port: 5600 - -qualisys_mocap: - ros__parameters: - ip: "192.168.0.0" - port: 22223 - version: "1.22" - body: "bluerov" - qualisys_localizer: ros__parameters: body: "bluerov" # This should be the same as the body parameter setting for the qualisys_mocap node +gazebo_localizer: + ros__parameters: + gazebo_odom_topic: "/model/bluerov2/odometry" + +# MAVROS & MAVROS plugins mavros: ros__parameters: system_id: 255 diff --git a/blue_bringup/config/bluerov2_heavy.yaml b/blue_bringup/config/bluerov2_heavy.yaml index 05048c9a..4135cffc 100644 --- a/blue_bringup/config/bluerov2_heavy.yaml +++ b/blue_bringup/config/bluerov2_heavy.yaml @@ -60,6 +60,10 @@ qualisys_localizer: ros__parameters: body: "bluerov" # This should be the same as the body parameter setting for the qualisys_mocap node +gazebo_localizer: + ros__parameters: + gazebo_odom_topic: "/model/bluerov2_heavy/odometry" + mavros: ros__parameters: system_id: 255 diff --git a/blue_bringup/launch/bluerov2.launch.py b/blue_bringup/launch/bluerov2.launch.py index 7c896456..0b40abc9 100644 --- a/blue_bringup/launch/bluerov2.launch.py +++ b/blue_bringup/launch/bluerov2.launch.py @@ -24,7 +24,7 @@ ExecuteProcess, IncludeLaunchDescription, ) -from launch.conditions import IfCondition, UnlessCondition +from launch.conditions import IfCondition from launch.launch_description_sources import PythonLaunchDescriptionSource from launch.substitutions import LaunchConfiguration, PathJoinSubstitution from launch_ros.actions import Node @@ -54,8 +54,8 @@ def generate_launch_description() -> LaunchDescription: ), DeclareLaunchArgument( "localization_source", - default_value="mocap", - choices=["mocap", "camera"], + default_value="gazebo", + choices=["mocap", "camera", "gazebo"], description="The localization source to stream from.", ), DeclareLaunchArgument( @@ -118,7 +118,7 @@ def generate_launch_description() -> LaunchDescription: ardusub_params_filepath = PathJoinSubstitution( [ FindPackageShare("blue_description"), - "config", + "params", LaunchConfiguration("ardusub_params_file"), ] ) @@ -212,7 +212,6 @@ def generate_launch_description() -> LaunchDescription: "use_mocap": LaunchConfiguration("use_mocap"), "use_camera": LaunchConfiguration("use_camera"), }.items(), - condition=UnlessCondition(use_sim), ), ] diff --git a/blue_bringup/launch/bluerov2_heavy.launch.py b/blue_bringup/launch/bluerov2_heavy.launch.py deleted file mode 100644 index 036cfb28..00000000 --- a/blue_bringup/launch/bluerov2_heavy.launch.py +++ /dev/null @@ -1,219 +0,0 @@ -# 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. - -from launch import LaunchDescription -from launch.actions import ( - DeclareLaunchArgument, - ExecuteProcess, - IncludeLaunchDescription, -) -from launch.conditions import IfCondition, UnlessCondition -from launch.launch_description_sources import PythonLaunchDescriptionSource -from launch.substitutions import LaunchConfiguration, PathJoinSubstitution -from launch_ros.actions import Node -from launch_ros.substitutions import FindPackageShare - - -def generate_launch_description() -> LaunchDescription: - """Generate a launch description to run the system. - - Returns: - The Blue ROS 2 launch description. - """ - args = [ - DeclareLaunchArgument( - "config", - default_value="bluerov2_heavy.yaml", - description="The ROS 2 parameters configuration file.", - ), - DeclareLaunchArgument( - "controller", - default_value="ismc", - description=( - "The controller to use; this should be the same name as the" - " controller's executable." - ), - choices=["ismc"], - ), - DeclareLaunchArgument( - "localization_source", - default_value="mocap", - choices=["mocap", "camera"], - description="The localization source to stream from.", - ), - DeclareLaunchArgument( - "use_camera", - default_value="false", - description=( - "Launch the BlueROV2 camera stream. This is automatically set to true" - " when using the camera for localization." - ), - ), - DeclareLaunchArgument( - "use_mocap", - default_value="false", - description=( - "Launch the Qualisys motion capture stream. This is automatically" - " set to true when using the motion capture system for localization." - ), - ), - DeclareLaunchArgument( - "use_sim", - default_value="false", - description="Automatically start Gazebo.", - ), - DeclareLaunchArgument( - "description_package", - default_value="blue_description", - description=( - "The description package with the BlueROV2 models. This is typically" - " not set, but is available in case another description package has" - " been defined." - ), - ), - DeclareLaunchArgument( - "gazebo_world_file", - default_value="bluerov2_heavy_underwater.world", - description="The world configuration to load if using Gazebo.", - ), - DeclareLaunchArgument( - "ardusub_params_file", - default_value="bluerov2_heavy.parm", - description=( - "The ArduSub parameters that the BlueROV2 should use if running in" - " simulation." - ), - ), - ] - - description_package = LaunchConfiguration("description_package") - use_sim = LaunchConfiguration("use_sim") - gazebo_world_file = LaunchConfiguration("gazebo_world_file") - - config_filepath = PathJoinSubstitution( - [ - FindPackageShare("blue_bringup"), - "config", - LaunchConfiguration("config"), - ] - ) - - ardusub_params_filepath = PathJoinSubstitution( - [ - FindPackageShare("blue_description"), - "config", - LaunchConfiguration("ardusub_params_file"), - ] - ) - - nodes = [ - Node( - package="mavros", - executable="mavros_node", - output="screen", - parameters=[config_filepath], - ), - Node( - package="ros_gz_bridge", - executable="parameter_bridge", - arguments=[ - "/model/bluerov2_heavy/odometry@nav_msgs/msg/Odometry[gz.msgs.Odometry", - ], - output="screen", - ), - ] - - processes = [ - # Launch Ignition Gazebo - this launches Garden (what ardupilot_gazebo uses) - ExecuteProcess( - cmd=[ - "gz", - "sim", - "-v", - "3", - "-r", - PathJoinSubstitution( - [ - FindPackageShare(description_package), - "gazebo", - "worlds", - gazebo_world_file, - ] - ), - ], - output="screen", - condition=IfCondition(use_sim), - ), - # Launch ArduSub for simulation purposes - ExecuteProcess( - cmd=[ - "ardusub", - "-S", - "-w", - "-M", - "JSON", - "--defaults", - ardusub_params_filepath, - "-I0", - "--home", - "44.65870,-124.06556,0.0,270.0", # my not-so-secrect surf spot - ], - output="screen", - condition=IfCondition(use_sim), - ), - ] - - includes = [ - IncludeLaunchDescription( - PythonLaunchDescriptionSource( - PathJoinSubstitution( - [FindPackageShare("blue_manager"), "manager.launch.py"] - ) - ), - launch_arguments={"config_filepath": config_filepath}.items(), - ), - IncludeLaunchDescription( - PythonLaunchDescriptionSource( - PathJoinSubstitution( - [FindPackageShare("blue_control"), "launch", "control.launch.py"] - ) - ), - launch_arguments={ - "config_filepath": config_filepath, - "controller": LaunchConfiguration("controller"), - }.items(), - ), - IncludeLaunchDescription( - PythonLaunchDescriptionSource( - PathJoinSubstitution( - [FindPackageShare("blue_localization"), "localization.launch.py"] - ) - ), - launch_arguments={ - "config_filepath": config_filepath, - "localization_source": LaunchConfiguration("localization_source"), - "use_mocap": LaunchConfiguration("use_mocap"), - "use_camera": LaunchConfiguration("use_camera"), - }.items(), - condition=UnlessCondition(use_sim), - ), - ] - - return LaunchDescription(args + nodes + processes + includes) diff --git a/blue_bringup/scripts/test_command.sh b/blue_bringup/scripts/test_command.sh deleted file mode 100644 index e69de29b..00000000 diff --git a/blue_control/include/blue_control/controller.hpp b/blue_control/include/blue_control/controller.hpp index 76c409d9..d693d3a3 100644 --- a/blue_control/include/blue_control/controller.hpp +++ b/blue_control/include/blue_control/controller.hpp @@ -29,13 +29,16 @@ #include "blue_dynamics/hydrodynamics.hpp" #include "blue_dynamics/thruster_dynamics.hpp" #include "geometry_msgs/msg/accel.hpp" +#include "geometry_msgs/msg/accel_stamped.hpp" #include "geometry_msgs/msg/pose_stamped.hpp" +#include "geometry_msgs/msg/transform_stamped.hpp" #include "mavros_msgs/msg/override_rc_in.hpp" #include "mavros_msgs/srv/message_interval.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" +#include "tf2_ros/transform_broadcaster.h" namespace blue::control { @@ -116,10 +119,6 @@ class Controller : public rclcpp::Node /** * @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_; @@ -138,6 +137,10 @@ class Controller : public rclcpp::Node double dt_{0.0}; private: + // Transform IDs + const std::string kMapFrameId{"map"}; + const std::string kBaseFrameId{"base_link"}; + /** * @brief Enable the controller. * @@ -150,6 +153,15 @@ class Controller : public rclcpp::Node std::shared_ptr request, std::shared_ptr response); + /** + * @brief Handle the incoming odometry messages. + * + * @note This message will be published AFTER mavros and all of it's plugins are loaded. + * + * @param msg The current odometry message. + */ + void updateOdomCb(nav_msgs::msg::Odometry::ConstSharedPtr msg); + /** * @brief Set custom MAVLink message rates. * @@ -171,9 +183,18 @@ class Controller : public rclcpp::Node bool armed_; + // Dynamic transforms + geometry_msgs::msg::TransformStamped tf_map_base_; + + // TF2 + std::unique_ptr tf_broadcaster_; + // Publishers rclcpp::Publisher::SharedPtr rc_override_pub_; + // Visualizion Publishers + rclcpp::Publisher::SharedPtr accel_vis_pub_; + // Subscribers rclcpp::Subscription::SharedPtr battery_state_sub_; rclcpp::Subscription::SharedPtr odom_sub_; diff --git a/blue_control/package.xml b/blue_control/package.xml index 4e47276f..b4e6a39d 100644 --- a/blue_control/package.xml +++ b/blue_control/package.xml @@ -16,6 +16,9 @@ blue_msgs std_srvs tf2_eigen + tf2 + tf2_geometry_msgs + tf2_ros ament_cmake diff --git a/blue_control/src/controller.cpp b/blue_control/src/controller.cpp index fa0955fa..33265213 100644 --- a/blue_control/src/controller.cpp +++ b/blue_control/src/controller.cpp @@ -20,6 +20,8 @@ #include "blue_control/controller.hpp" +#include "tf2/transform_datatypes.h" + namespace blue::control { @@ -45,9 +47,6 @@ Controller::Controller(const std::string & node_name) this->declare_parameter("msg_ids", std::vector({31, 32})); this->declare_parameter("msg_rates", std::vector({100, 100})); this->declare_parameter("control_rate", 200.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, @@ -90,6 +89,8 @@ Controller::Controller(const std::string & node_name) blue::dynamics::CurrentEffects(ocean_current)); // Setup the ROS things + tf_broadcaster_ = std::make_unique(*this); + rc_override_pub_ = this->create_publisher("mavros/rc/override", 1); @@ -99,10 +100,7 @@ Controller::Controller(const std::string & node_name) odom_sub_ = this->create_subscription( "/mavros/local_position/odom", rclcpp::SensorDataQoS(), - [this](nav_msgs::msg::Odometry::ConstSharedPtr msg) { - // TODO(evan): Calculate the current acceleration here - odom_ = *msg; - }); + [this](nav_msgs::msg::Odometry::ConstSharedPtr msg) { updateOdomCb(msg); }); arm_srv_ = this->create_service( "blue/control/arm", [this]( @@ -175,6 +173,48 @@ void Controller::armControllerCb( } } +void Controller::updateOdomCb(nav_msgs::msg::Odometry::ConstSharedPtr msg) +{ + // Get the duration between the readings + rclcpp::Time prev_stamp(odom_.header.stamp.sec, odom_.header.stamp.nanosec); + rclcpp::Time current_stamp(msg->header.stamp.sec, msg->header.stamp.nanosec); + + // Get the duration in seconds + const double dt = (current_stamp - prev_stamp).seconds(); + + // Calculate the current acceleration using finite differencing + geometry_msgs::msg::Accel accel; + accel.linear.x = (msg->twist.twist.linear.x - odom_.twist.twist.linear.x) / dt; + accel.linear.y = (msg->twist.twist.linear.y - odom_.twist.twist.linear.y) / dt; + accel.linear.z = (msg->twist.twist.linear.z - odom_.twist.twist.linear.z) / dt; + accel.angular.x = (msg->twist.twist.angular.x - odom_.twist.twist.angular.x) / dt; + accel.angular.y = (msg->twist.twist.angular.y - odom_.twist.twist.angular.y) / dt; + accel.angular.z = (msg->twist.twist.angular.z - odom_.twist.twist.angular.z) / dt; + + // Update the current acceleration + accel_ = accel; + + // Update the odom + odom_ = *msg; + + // Publish the map -> base_link transform + tf_map_base_.header.stamp = this->get_clock()->now(); + tf_map_base_.header.frame_id = kMapFrameId; + tf_map_base_.child_frame_id = kBaseFrameId; + + tf_map_base_.transform.translation.x = odom_.pose.pose.position.x; + tf_map_base_.transform.translation.y = odom_.pose.pose.position.y; + tf_map_base_.transform.translation.z = odom_.pose.pose.position.z; + + tf_map_base_.transform.rotation.x = odom_.pose.pose.orientation.x; + tf_map_base_.transform.rotation.y = odom_.pose.pose.orientation.y; + tf_map_base_.transform.rotation.z = odom_.pose.pose.orientation.z; + tf_map_base_.transform.rotation.w = odom_.pose.pose.orientation.w; + + // Publish the transform + tf_broadcaster_->sendTransform(tf_map_base_); +} + void Controller::setMessageRates( const std::vector & msg_ids, const std::vector & rates) { diff --git a/blue_description/CMakeLists.txt b/blue_description/CMakeLists.txt index 2def9d00..30091493 100644 --- a/blue_description/CMakeLists.txt +++ b/blue_description/CMakeLists.txt @@ -29,7 +29,7 @@ if(BUILD_TESTING) endif() install( - DIRECTORY gazebo config + DIRECTORY gazebo params DESTINATION share/${PROJECT_NAME} ) diff --git a/blue_description/gazebo/models/bluerov2/model.sdf b/blue_description/gazebo/models/bluerov2/model.sdf index 5bb2bebb..ecdbf16a 100644 --- a/blue_description/gazebo/models/bluerov2/model.sdf +++ b/blue_description/gazebo/models/bluerov2/model.sdf @@ -415,5 +415,15 @@ + + + map + base_link + 3 + 100 + + diff --git a/blue_description/gazebo/models/bluerov2_heavy/model.sdf b/blue_description/gazebo/models/bluerov2_heavy/model.sdf index 9525634b..b42040ba 100644 --- a/blue_description/gazebo/models/bluerov2_heavy/model.sdf +++ b/blue_description/gazebo/models/bluerov2_heavy/model.sdf @@ -520,5 +520,15 @@ + + + map + base_link + 3 + 100 + + diff --git a/blue_description/config/bluerov2.parm b/blue_description/params/bluerov2.parm similarity index 77% rename from blue_description/config/bluerov2.parm rename to blue_description/params/bluerov2.parm index 8c1f0012..936c093d 100644 --- a/blue_description/config/bluerov2.parm +++ b/blue_description/params/bluerov2.parm @@ -88,3 +88,16 @@ BARO_EXT_BUS 1 EK2_ENABLE 0 EK3_ENABLE 1 AHRS_EKF_TYPE 3 + +# Enable external nav +# From https://ardupilot.org/copter/docs/common-vio-tracking-camera.html +GPS_TYPE 0 # Disable GPS +VISO_TYPE 1 # External vision +EK3_SRC1_POSXY 6 # External nav +EK3_SRC1_VELXY 6 # External nav +EK3_SRC1_POSZ 1 # Baro is the primary z source +EK3_SRC1_VELZ 6 # External nav z velocity influences EKF +COMPASS_USE 0 # Disable compass 1 +COMPASS_USE2 0 # Disable compass 2 +COMPASS_USE3 0 # Disable compass 3 +EK3_SRC1_YAW 6 # External nav diff --git a/blue_description/config/bluerov2_heavy.parm b/blue_description/params/bluerov2_heavy.parm similarity index 72% rename from blue_description/config/bluerov2_heavy.parm rename to blue_description/params/bluerov2_heavy.parm index 09630bad..4f220f34 100644 --- a/blue_description/config/bluerov2_heavy.parm +++ b/blue_description/params/bluerov2_heavy.parm @@ -66,3 +66,16 @@ FRAME_CONFIG 2.000 EK2_ENABLE 0 EK3_ENABLE 1 AHRS_EKF_TYPE 3 + +# Enable external nav +# From https://ardupilot.org/copter/docs/common-vio-tracking-camera.html +GPS_TYPE 0 # Disable GPS +VISO_TYPE 1 # External vision +EK3_SRC1_POSXY 6 # External nav +EK3_SRC1_VELXY 6 # External nav +EK3_SRC1_POSZ 1 # Baro is the primary z source +EK3_SRC1_VELZ 6 # External nav z velocity influences EKF +COMPASS_USE 0 # Disable compass 1 +COMPASS_USE2 0 # Disable compass 2 +COMPASS_USE3 0 # Disable compass 3 +EK3_SRC1_YAW 6 # External nav diff --git a/blue_localization/blue_localization/localizer.py b/blue_localization/blue_localization/localizer.py index ceb4bc49..7b99f0f1 100644 --- a/blue_localization/blue_localization/localizer.py +++ b/blue_localization/blue_localization/localizer.py @@ -33,6 +33,7 @@ from tf2_ros import TransformException from tf2_ros.buffer import Buffer from tf2_ros.transform_listener import TransformListener +from nav_msgs.msg import Odometry class Localizer(Node, ABC): @@ -278,6 +279,42 @@ def proxy_pose_cb(self, pose: PoseStamped) -> None: self.localization_pub.publish(pose) +class GazeboLocalizer(Localizer): + """Localize the BlueROV2 using the Gazebo ground-truth data.""" + + def __init__(self) -> None: + """Create a new Gazebo localizer.""" + super().__init__("gazebo_localizer") + + # We need to know the topic to stream from + self.declare_parameter("gazebo_odom_topic", "") + + # Subscribe to that topic so that we can proxy messages to the ArduSub EKF + self.create_subscription( + Odometry, + self.get_parameter("gazebo_odom_topic").get_parameter_value().string_value, + self.proxy_odom_cb, + 1, + ) + + def proxy_odom_cb(self, msg: Odometry) -> None: + """Proxy the pose data from the Gazebo odometry ground-truth data. + + Args: + msg: The Gazebo ground-truth odometry for the BlueROV2. + """ + pose = PoseStamped() + + # Pose is provided in the parent header frame + pose.header.frame_id = msg.header.frame_id + pose.header.stamp = msg.header.stamp + + # We only need the pose; we don't need the covariance + pose.pose = msg.pose.pose + + self.localization_pub.publish(pose) + + def main_aruco(args: list[str] | None = None): """Run the ArUco marker detector.""" rclpy.init(args=args) @@ -298,3 +335,14 @@ def main_qualisys(args: list[str] | None = None): node.destroy_node() rclpy.shutdown() + + +def main_gazebo(args: list[str] | None = None): + """Run the Gazebo localizer.""" + rclpy.init(args=args) + + node = GazeboLocalizer() + rclpy.spin(node) + + node.destroy_node() + rclpy.shutdown() diff --git a/blue_localization/launch/localization.launch.py b/blue_localization/launch/localization.launch.py index 461117ff..a77f2cff 100644 --- a/blue_localization/launch/localization.launch.py +++ b/blue_localization/launch/localization.launch.py @@ -46,7 +46,7 @@ def generate_launch_description() -> LaunchDescription: DeclareLaunchArgument( "localization_source", default_value="mocap", - choices=["mocap", "camera"], + choices=["mocap", "camera", "gazebo"], description="The localization source to stream from.", ), DeclareLaunchArgument( @@ -128,6 +128,16 @@ def generate_launch_description() -> LaunchDescription: PythonExpression(["'", localization_source, "' == 'mocap'"]) ), ), + Node( + package="blue_localization", + executable="gazebo_localizer", + name="gazebo_localizer", + output="screen", + parameters=[LaunchConfiguration("config_filepath")], + condition=IfCondition( + PythonExpression(["'", localization_source, "' == 'gazebo'"]) + ), + ), ] includes = [ diff --git a/blue_localization/setup.py b/blue_localization/setup.py index 7c45d1dd..638ab6df 100644 --- a/blue_localization/setup.py +++ b/blue_localization/setup.py @@ -50,6 +50,7 @@ "qualisys_mocap = blue_localization.source:main_qualisys_mocap", "aruco_marker_localizer = blue_localization.localizer:main_aruco", "qualisys_localizer = blue_localization.localizer:main_qualisys", + "gazebo_localizer = blue_localization.localizer:main_gazebo", ], }, ) diff --git a/blue_manager/blue_manager/manager.py b/blue_manager/blue_manager/manager.py index 06024034..909750a5 100644 --- a/blue_manager/blue_manager/manager.py +++ b/blue_manager/blue_manager/manager.py @@ -223,8 +223,17 @@ def set_rc_passthrough_mode_cb( # Set the servo mode to "RC Passthrough" # This disables the arming and failsafe features, but now lets us send PWM # values to the thrusters without any mixing - for param in passthrough_params.values(): - param.value.integer_value = 1 # type: ignore + try: + for param in passthrough_params.values(): + param.value.integer_value = 1 # type: ignore + except AttributeError: + response.success = False + response.message = ( + "Failed to switch to RC Passthrough mode. Please ensure that all" + " ArduSub parameters have been loaded prior to attempting to" + " switch modes." + ) + return response for _ in range(self.retries): self.passthrough_enabled = self.set_thruster_params( From 6381b132728a41ac9c720f9f6e3d5b6531f22237 Mon Sep 17 00:00:00 2001 From: Evan Palmer Date: Thu, 22 Jun 2023 19:37:11 -0700 Subject: [PATCH 04/16] IT WORKS --- .docker/Dockerfile | 1 + blue_bringup/config/bluerov2.yaml | 2 +- .../include/blue_control/controller.hpp | 12 ++++----- blue_control/include/blue_control/ismc.hpp | 6 +++++ blue_control/src/controller.cpp | 11 +++++++- blue_control/src/ismc.cpp | 26 +++++++++++++++---- 6 files changed, 44 insertions(+), 14 deletions(-) diff --git a/.docker/Dockerfile b/.docker/Dockerfile index 58641746..bbec1c20 100644 --- a/.docker/Dockerfile +++ b/.docker/Dockerfile @@ -200,6 +200,7 @@ RUN apt-get -q update \ iputils-ping \ net-tools \ gdb \ + ros-$ROS_DISTRO-foxglove-bridge \ && apt-get autoremove -y \ && apt-get clean -y \ && rm -rf /var/lib/apt/lists/* diff --git a/blue_bringup/config/bluerov2.yaml b/blue_bringup/config/bluerov2.yaml index 07212380..560306ec 100644 --- a/blue_bringup/config/bluerov2.yaml +++ b/blue_bringup/config/bluerov2.yaml @@ -30,7 +30,7 @@ ismc: proportional_gain: [10.0, 10.0, 10.0, 10.0, 10.0, 10.0] integral_gain: [3.0, 3.0, 3.0, 3.0, 3.0, 3.0] sliding_gain: 5.0 - boundary_thickness: 1.0 + boundary_thickness: 1000.0 use_battery_state: false # Localization sources diff --git a/blue_control/include/blue_control/controller.hpp b/blue_control/include/blue_control/controller.hpp index d693d3a3..726fe4a1 100644 --- a/blue_control/include/blue_control/controller.hpp +++ b/blue_control/include/blue_control/controller.hpp @@ -82,6 +82,10 @@ class Controller : public rclcpp::Node explicit Controller(const std::string & node_name); protected: + // Transform IDs + const std::string kMapFrameId{"map"}; + const std::string kBaseFrameId{"base_link"}; + /** * @brief Function executed when the controller is armed. */ @@ -137,10 +141,6 @@ class Controller : public rclcpp::Node double dt_{0.0}; private: - // Transform IDs - const std::string kMapFrameId{"map"}; - const std::string kBaseFrameId{"base_link"}; - /** * @brief Enable the controller. * @@ -191,9 +191,7 @@ class Controller : public rclcpp::Node // Publishers rclcpp::Publisher::SharedPtr rc_override_pub_; - - // Visualizion Publishers - rclcpp::Publisher::SharedPtr accel_vis_pub_; + rclcpp::Publisher::SharedPtr accel_pub_; // Subscribers rclcpp::Subscription::SharedPtr battery_state_sub_; diff --git a/blue_control/include/blue_control/ismc.hpp b/blue_control/include/blue_control/ismc.hpp index 1249c5ac..e4711a66 100644 --- a/blue_control/include/blue_control/ismc.hpp +++ b/blue_control/include/blue_control/ismc.hpp @@ -24,6 +24,7 @@ #include "blue_control/controller.hpp" #include "blue_msgs/msg/reference.hpp" +#include "geometry_msgs/msg/wrench_stamped.hpp" #include "mavros_msgs/msg/override_rc_in.hpp" namespace blue::control @@ -57,6 +58,11 @@ class ISMC : public Controller bool use_battery_state_; blue_msgs::msg::Reference cmd_; + + // Publishers + rclcpp::Publisher::SharedPtr desired_wrench_pub_; + + // Subscribers rclcpp::Subscription::SharedPtr cmd_sub_; }; diff --git a/blue_control/src/controller.cpp b/blue_control/src/controller.cpp index 33265213..baca209a 100644 --- a/blue_control/src/controller.cpp +++ b/blue_control/src/controller.cpp @@ -94,6 +94,8 @@ Controller::Controller(const std::string & node_name) rc_override_pub_ = this->create_publisher("mavros/rc/override", 1); + accel_pub_ = this->create_publisher("/blue/state/accel", 1); + battery_state_sub_ = this->create_subscription( "/mavros/battery", rclcpp::SensorDataQoS(), [this](sensor_msgs::msg::BatteryState::ConstSharedPtr msg) { battery_state_ = *msg; }); @@ -191,9 +193,16 @@ void Controller::updateOdomCb(nav_msgs::msg::Odometry::ConstSharedPtr msg) accel.angular.y = (msg->twist.twist.angular.y - odom_.twist.twist.angular.y) / dt; accel.angular.z = (msg->twist.twist.angular.z - odom_.twist.twist.angular.z) / dt; - // Update the current acceleration + // Update the current acceleration and publish it accel_ = accel; + geometry_msgs::msg::AccelStamped accel_stamped; + accel_stamped.header.frame_id = kBaseFrameId; + accel_stamped.header.stamp = this->get_clock()->now(); + accel_stamped.accel = accel_; + + accel_pub_->publish(accel_stamped); + // Update the odom odom_ = *msg; diff --git a/blue_control/src/ismc.cpp b/blue_control/src/ismc.cpp index fa14e6fd..355232aa 100644 --- a/blue_control/src/ismc.cpp +++ b/blue_control/src/ismc.cpp @@ -55,6 +55,10 @@ ISMC::ISMC() boundary_thickness_ = this->get_parameter("boundary_thickness").as_double(); use_battery_state_ = this->get_parameter("use_battery_state").as_bool(); + // Publish the desired wrench to help with tuning and visualization + desired_wrench_pub_ = + this->create_publisher("/blue/ismc/desired_wrench", 1); + // 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; }); @@ -141,6 +145,18 @@ mavros_msgs::msg::OverrideRCIn ISMC::update() hydrodynamics_.restoring_forces.calculateRestoringForces(orientation.toRotationMatrix()) + sliding_gain_ * surface; + geometry_msgs::msg::WrenchStamped wrench; + wrench.header.frame_id = kBaseFrameId; + wrench.header.stamp = this->get_clock()->now(); + wrench.wrench.force.x = forces[0]; + wrench.wrench.force.y = forces[1]; + wrench.wrench.force.z = forces[2]; + wrench.wrench.torque.x = forces[3]; + wrench.wrench.torque.y = forces[4]; + wrench.wrench.torque.z = forces[5]; + + desired_wrench_pub_->publish(wrench); + // Multiply the desired forces by the pseudoinverse of the thruster configuration matrix // The size of this vector will depend on the number of thrusters so we don't assign it to a // fixed-size matrix @@ -158,11 +174,11 @@ mavros_msgs::msg::OverrideRCIn ISMC::update() [this](double x) { return blue::dynamics::calculatePwmFromThrustCurve(x); }); } - std::string sep = "\n----------------------------------------\n"; - std::stringstream ss; - ss << std::endl << velocity_error << sep << forces << sep << pwms << sep; - std::string sad = ss.str(); - RCLCPP_WARN(this->get_logger(), "pwms: %s", sad.c_str()); + // std::string sep = "\n----------------------------------------\n"; + // std::stringstream ss; + // ss << std::endl << velocity_error << sep << forces << sep << pwms << sep; + // std::string sad = ss.str(); + // RCLCPP_WARN(this->get_logger(), "pwms: %s", sad.c_str()); mavros_msgs::msg::OverrideRCIn msg; From 7987396457db5b8c67e167c3949796c1bd86d677 Mon Sep 17 00:00:00 2001 From: Evan Palmer Date: Thu, 22 Jun 2023 22:19:28 -0700 Subject: [PATCH 05/16] Cleaned up comments and re-added heavy launch file --- blue_bringup/launch/bluerov2.launch.py | 2 +- blue_bringup/launch/bluerov2_heavy.launch.py | 218 ++++++++++++++++++ .../include/blue_control/controller.hpp | 8 +- blue_control/include/blue_control/ismc.hpp | 10 +- blue_control/src/controller.cpp | 21 +- blue_control/src/ismc.cpp | 11 +- 6 files changed, 244 insertions(+), 26 deletions(-) create mode 100644 blue_bringup/launch/bluerov2_heavy.launch.py diff --git a/blue_bringup/launch/bluerov2.launch.py b/blue_bringup/launch/bluerov2.launch.py index 0b40abc9..860e879a 100644 --- a/blue_bringup/launch/bluerov2.launch.py +++ b/blue_bringup/launch/bluerov2.launch.py @@ -35,7 +35,7 @@ def generate_launch_description() -> LaunchDescription: """Generate a launch description to run the system. Returns: - The Blue ROS 2 launch description. + The launch description for the BlueROV2 base configuration. """ args = [ DeclareLaunchArgument( diff --git a/blue_bringup/launch/bluerov2_heavy.launch.py b/blue_bringup/launch/bluerov2_heavy.launch.py new file mode 100644 index 00000000..13bf6535 --- /dev/null +++ b/blue_bringup/launch/bluerov2_heavy.launch.py @@ -0,0 +1,218 @@ +# 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. + +from launch import LaunchDescription +from launch.actions import ( + DeclareLaunchArgument, + ExecuteProcess, + IncludeLaunchDescription, +) +from launch.conditions import IfCondition +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration, PathJoinSubstitution +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare + + +def generate_launch_description() -> LaunchDescription: + """Generate a launch description to run the system. + + Returns: + The launch description for the BlueROV2 heavy configuration. + """ + args = [ + DeclareLaunchArgument( + "config", + default_value="bluerov2_heavy.yaml", + description="The ROS 2 parameters configuration file.", + ), + DeclareLaunchArgument( + "controller", + default_value="ismc", + description=( + "The controller to use; this should be the same name as the" + " controller's executable." + ), + choices=["ismc"], + ), + DeclareLaunchArgument( + "localization_source", + default_value="gazebo", + choices=["mocap", "camera", "gazebo"], + description="The localization source to stream from.", + ), + DeclareLaunchArgument( + "use_camera", + default_value="false", + description=( + "Launch the BlueROV2 camera stream. This is automatically set to true" + " when using the camera for localization." + ), + ), + DeclareLaunchArgument( + "use_mocap", + default_value="false", + description=( + "Launch the Qualisys motion capture stream. This is automatically" + " set to true when using the motion capture system for localization." + ), + ), + DeclareLaunchArgument( + "use_sim", + default_value="false", + description="Automatically start Gazebo.", + ), + DeclareLaunchArgument( + "description_package", + default_value="blue_description", + description=( + "The description package with the BlueROV2 models. This is typically" + " not set, but is available in case another description package has" + " been defined." + ), + ), + DeclareLaunchArgument( + "gazebo_world_file", + default_value="bluerov2_heavy_underwater.world", + description="The world configuration to load if using Gazebo.", + ), + DeclareLaunchArgument( + "ardusub_params_file", + default_value="bluerov2_heavy.parm", + description=( + "The ArduSub parameters that the BlueROV2 should use if running in" + " simulation." + ), + ), + ] + + description_package = LaunchConfiguration("description_package") + use_sim = LaunchConfiguration("use_sim") + gazebo_world_file = LaunchConfiguration("gazebo_world_file") + + config_filepath = PathJoinSubstitution( + [ + FindPackageShare("blue_bringup"), + "config", + LaunchConfiguration("config"), + ] + ) + + ardusub_params_filepath = PathJoinSubstitution( + [ + FindPackageShare("blue_description"), + "params", + LaunchConfiguration("ardusub_params_file"), + ] + ) + + nodes = [ + Node( + package="mavros", + executable="mavros_node", + output="screen", + parameters=[config_filepath], + ), + Node( + package="ros_gz_bridge", + executable="parameter_bridge", + arguments=[ + "/model/bluerov2_heavy/odometry@nav_msgs/msg/Odometry[gz.msgs.Odometry", + ], + output="screen", + ), + ] + + processes = [ + # Launch Ignition Gazebo - this launches Garden (what ardupilot_gazebo uses) + ExecuteProcess( + cmd=[ + "gz", + "sim", + "-v", + "3", + "-r", + PathJoinSubstitution( + [ + FindPackageShare(description_package), + "gazebo", + "worlds", + gazebo_world_file, + ] + ), + ], + output="screen", + condition=IfCondition(use_sim), + ), + # Launch ArduSub for simulation purposes + ExecuteProcess( + cmd=[ + "ardusub", + "-S", + "-w", + "-M", + "JSON", + "--defaults", + ardusub_params_filepath, + "-I0", + "--home", + "44.65870,-124.06556,0.0,270.0", # my not-so-secrect surf spot + ], + output="screen", + condition=IfCondition(use_sim), + ), + ] + + includes = [ + IncludeLaunchDescription( + PythonLaunchDescriptionSource( + PathJoinSubstitution( + [FindPackageShare("blue_manager"), "manager.launch.py"] + ) + ), + launch_arguments={"config_filepath": config_filepath}.items(), + ), + IncludeLaunchDescription( + PythonLaunchDescriptionSource( + PathJoinSubstitution( + [FindPackageShare("blue_control"), "launch", "control.launch.py"] + ) + ), + launch_arguments={ + "config_filepath": config_filepath, + "controller": LaunchConfiguration("controller"), + }.items(), + ), + IncludeLaunchDescription( + PythonLaunchDescriptionSource( + PathJoinSubstitution( + [FindPackageShare("blue_localization"), "localization.launch.py"] + ) + ), + launch_arguments={ + "config_filepath": config_filepath, + "localization_source": LaunchConfiguration("localization_source"), + "use_mocap": LaunchConfiguration("use_mocap"), + "use_camera": LaunchConfiguration("use_camera"), + }.items(), + ), + ] + + return LaunchDescription(args + nodes + processes + includes) diff --git a/blue_control/include/blue_control/controller.hpp b/blue_control/include/blue_control/controller.hpp index 726fe4a1..8a18c720 100644 --- a/blue_control/include/blue_control/controller.hpp +++ b/blue_control/include/blue_control/controller.hpp @@ -129,7 +129,7 @@ class Controller : public rclcpp::Node /** * @brief The current acceleration of the BlueROV2. * - * @note This is not provided by the system directly and is calculated using finite differencing. + * @note This is not provided by ArduSub directly and is calculated using finite differencing. */ geometry_msgs::msg::Accel accel_; @@ -142,7 +142,7 @@ class Controller : public rclcpp::Node private: /** - * @brief Enable the controller. + * @brief Enable/disable the controller. * * @details This method enables/disables sending RC Override messages to the BlueROV2. * @@ -198,8 +198,10 @@ class Controller : public rclcpp::Node rclcpp::Subscription::SharedPtr odom_sub_; rclcpp::Subscription::SharedPtr ardu_pose_sub_; + // Callback groups + rclcpp::CallbackGroup::SharedPtr control_loop_cb_group_; + // Timers - rclcpp::CallbackGroup::SharedPtr timer_cb_group_; rclcpp::TimerBase::SharedPtr control_loop_timer_; rclcpp::TimerBase::SharedPtr set_message_rate_timer_; diff --git a/blue_control/include/blue_control/ismc.hpp b/blue_control/include/blue_control/ismc.hpp index e4711a66..8809a911 100644 --- a/blue_control/include/blue_control/ismc.hpp +++ b/blue_control/include/blue_control/ismc.hpp @@ -47,15 +47,17 @@ class ISMC : public Controller mavros_msgs::msg::OverrideRCIn update() override; private: - // Hyperparameters used by the ISMC - blue::dynamics::Vector6d initial_velocity_error_; - blue::dynamics::Vector6d initial_acceleration_error_; - blue::dynamics::Vector6d total_velocity_error_; + // ISMC gains blue::dynamics::Matrix6d integral_gain_; blue::dynamics::Matrix6d proportional_gain_; double sliding_gain_; double boundary_thickness_; + // Error terms + blue::dynamics::Vector6d initial_velocity_error_; + blue::dynamics::Vector6d initial_acceleration_error_; + blue::dynamics::Vector6d total_velocity_error_; + bool use_battery_state_; blue_msgs::msg::Reference cmd_; diff --git a/blue_control/src/controller.cpp b/blue_control/src/controller.cpp index baca209a..6d32c1a4 100644 --- a/blue_control/src/controller.cpp +++ b/blue_control/src/controller.cpp @@ -55,7 +55,7 @@ Controller::Controller(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})); - // Get the parameter values + // Get hydrodynamic parameters 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(); @@ -142,12 +142,16 @@ Controller::Controller(const std::string & node_name) // Convert the control loop frequency to seconds dt_ = 1 / this->get_parameter("control_rate").as_double(); - control_loop_timer_ = - this->create_wall_timer(std::chrono::duration(dt_), [this]() -> void { + // Give the control loop its own callback group to avoid issues with long callbacks in the + // default callback group + control_loop_timer_ = this->create_wall_timer( + std::chrono::duration(dt_), + [this]() -> void { if (armed_) { rc_override_pub_->publish(update()); } - }); + }, + control_loop_cb_group_); } void Controller::armControllerCb( @@ -156,6 +160,8 @@ void Controller::armControllerCb( { if (request->data) { // Run the controller arming function prior to actually arming the controller + // This makes sure that any processing that needs to happen before the controller is *actually* + // armed can occur onArm(); // Arm the controller @@ -180,11 +186,9 @@ void Controller::updateOdomCb(nav_msgs::msg::Odometry::ConstSharedPtr msg) // Get the duration between the readings rclcpp::Time prev_stamp(odom_.header.stamp.sec, odom_.header.stamp.nanosec); rclcpp::Time current_stamp(msg->header.stamp.sec, msg->header.stamp.nanosec); - - // Get the duration in seconds const double dt = (current_stamp - prev_stamp).seconds(); - // Calculate the current acceleration using finite differencing + // Calculate the current acceleration using finite differencing and publish it for debugging geometry_msgs::msg::Accel accel; accel.linear.x = (msg->twist.twist.linear.x - odom_.twist.twist.linear.x) / dt; accel.linear.y = (msg->twist.twist.linear.y - odom_.twist.twist.linear.y) / dt; @@ -193,7 +197,6 @@ void Controller::updateOdomCb(nav_msgs::msg::Odometry::ConstSharedPtr msg) accel.angular.y = (msg->twist.twist.angular.y - odom_.twist.twist.angular.y) / dt; accel.angular.z = (msg->twist.twist.angular.z - odom_.twist.twist.angular.z) / dt; - // Update the current acceleration and publish it accel_ = accel; geometry_msgs::msg::AccelStamped accel_stamped; @@ -203,7 +206,6 @@ void Controller::updateOdomCb(nav_msgs::msg::Odometry::ConstSharedPtr msg) accel_pub_->publish(accel_stamped); - // Update the odom odom_ = *msg; // Publish the map -> base_link transform @@ -220,7 +222,6 @@ void Controller::updateOdomCb(nav_msgs::msg::Odometry::ConstSharedPtr msg) tf_map_base_.transform.rotation.z = odom_.pose.pose.orientation.z; tf_map_base_.transform.rotation.w = odom_.pose.pose.orientation.w; - // Publish the transform tf_broadcaster_->sendTransform(tf_map_base_); } diff --git a/blue_control/src/ismc.cpp b/blue_control/src/ismc.cpp index 355232aa..eafe6dee 100644 --- a/blue_control/src/ismc.cpp +++ b/blue_control/src/ismc.cpp @@ -50,7 +50,6 @@ ISMC::ISMC() integral_gain_ = integral_gain_coeff.asDiagonal().toDenseMatrix(); proportional_gain_ = proportional_gain_coeff.asDiagonal().toDenseMatrix(); - sliding_gain_ = this->get_parameter("sliding_gain").as_double(); boundary_thickness_ = this->get_parameter("boundary_thickness").as_double(); use_battery_state_ = this->get_parameter("use_battery_state").as_bool(); @@ -121,7 +120,7 @@ mavros_msgs::msg::OverrideRCIn ISMC::update() 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 + // Get the current rotation of the vehicle in the inertial (map) frame Eigen::Quaterniond orientation; tf2::fromMsg(odom_.pose.pose.orientation, orientation); @@ -137,6 +136,7 @@ mavros_msgs::msg::OverrideRCIn ISMC::update() // Apply the sign function to the surface surface = surface.unaryExpr([this](double x) { return tanh(x / boundary_thickness_); }); + // Calculate the desired torques const blue::dynamics::Vector6d forces = hydrodynamics_.inertia.getInertia() * (desired_accel + proportional_gain_.inverse() * integral_gain_ * velocity_error) + @@ -145,6 +145,7 @@ mavros_msgs::msg::OverrideRCIn ISMC::update() hydrodynamics_.restoring_forces.calculateRestoringForces(orientation.toRotationMatrix()) + sliding_gain_ * surface; + // Publish the desired torques to help with debugging and visualization geometry_msgs::msg::WrenchStamped wrench; wrench.header.frame_id = kBaseFrameId; wrench.header.stamp = this->get_clock()->now(); @@ -174,12 +175,6 @@ mavros_msgs::msg::OverrideRCIn ISMC::update() [this](double x) { return blue::dynamics::calculatePwmFromThrustCurve(x); }); } - // std::string sep = "\n----------------------------------------\n"; - // std::stringstream ss; - // ss << std::endl << velocity_error << sep << forces << sep << pwms << sep; - // std::string sad = ss.str(); - // RCLCPP_WARN(this->get_logger(), "pwms: %s", sad.c_str()); - mavros_msgs::msg::OverrideRCIn msg; // We only modify the first "n" channels where "n" is the total number of thrusters From fa468892da0a6bc2e781c3d10cbd8760209912d8 Mon Sep 17 00:00:00 2001 From: Evan Palmer Date: Fri, 23 Jun 2023 01:40:55 -0700 Subject: [PATCH 06/16] More bug fixes --- blue_bringup/config/bluerov2.yaml | 4 ++-- blue_bringup/config/bluerov2_heavy.yaml | 20 +++++++++---------- blue_control/CMakeLists.txt | 1 + .../include/blue_control/controller.hpp | 16 ++++++++++++++- blue_control/src/controller.cpp | 4 +++- blue_control/src/ismc.cpp | 19 +++++++++--------- .../gazebo/models/bluerov2_heavy/model.sdf | 14 ++----------- 7 files changed, 43 insertions(+), 35 deletions(-) diff --git a/blue_bringup/config/bluerov2.yaml b/blue_bringup/config/bluerov2.yaml index 560306ec..8391eacf 100644 --- a/blue_bringup/config/bluerov2.yaml +++ b/blue_bringup/config/bluerov2.yaml @@ -29,8 +29,8 @@ ismc: control_rate: 200.0 proportional_gain: [10.0, 10.0, 10.0, 10.0, 10.0, 10.0] integral_gain: [3.0, 3.0, 3.0, 3.0, 3.0, 3.0] - sliding_gain: 5.0 - boundary_thickness: 1000.0 + sliding_gain: 50.0 + boundary_thickness: 100.0 use_battery_state: false # Localization sources diff --git a/blue_bringup/config/bluerov2_heavy.yaml b/blue_bringup/config/bluerov2_heavy.yaml index 4135cffc..cd0504a4 100644 --- a/blue_bringup/config/bluerov2_heavy.yaml +++ b/blue_bringup/config/bluerov2_heavy.yaml @@ -17,19 +17,19 @@ ismc: center_of_buoyancy: [0.0, 0.0, 0.0] ocean_current: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] num_thrusters: 8 - tcm: [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] + tcm: [ 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.0, 0.0, 0.0, 0.0, 0.218, 0.218, -0.218, -0.218, + 0.0, 0.0, 0.0, 0.0, 0.12, -0.12, 0.12, -0.12, + -0.1888, 0.1888, 0.1888, -0.1888, 0.0, 0.0, 0.0, 0.0] msg_ids: [31, 32] msg_rates: [100.0, 100.0] control_rate: 200.0 - proportional_gain: [5.0, 5.0, 5.0, 5.0, 5.0, 5.0] - integral_gain: [0.1, 0.1, 0.1, 0.1, 0.1, 0.1] - sliding_gain: 0.0 - boundary_thickness: 0.0 + proportional_gain: [10.0, 10.0, 10.0, 10.0, 10.0, 10.0] + integral_gain: [3.0, 3.0, 3.0, 3.0, 3.0, 3.0] + sliding_gain: 10.0 + boundary_thickness: 100.0 use_battery_state: false aruco_marker_localizer: diff --git a/blue_control/CMakeLists.txt b/blue_control/CMakeLists.txt index 24b9759d..87980df1 100644 --- a/blue_control/CMakeLists.txt +++ b/blue_control/CMakeLists.txt @@ -27,6 +27,7 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS eigen3_cmake_module Eigen3 tf2_eigen + tf2_geometry_msgs ) foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) diff --git a/blue_control/include/blue_control/controller.hpp b/blue_control/include/blue_control/controller.hpp index 8a18c720..bccb653a 100644 --- a/blue_control/include/blue_control/controller.hpp +++ b/blue_control/include/blue_control/controller.hpp @@ -38,7 +38,9 @@ #include "rclcpp/rclcpp.hpp" #include "sensor_msgs/msg/battery_state.hpp" #include "std_srvs/srv/set_bool.hpp" +#include "tf2_ros/buffer.h" #include "tf2_ros/transform_broadcaster.h" +#include "tf2_ros/transform_listener.h" namespace blue::control { @@ -84,7 +86,9 @@ class Controller : public rclcpp::Node protected: // Transform IDs const std::string kMapFrameId{"map"}; + const std::string kMapNedFrameId{"map_ned"}; const std::string kBaseFrameId{"base_link"}; + const std::string kBaseNedFrameId{"base_link_frd"}; /** * @brief Function executed when the controller is armed. @@ -140,6 +144,16 @@ class Controller : public rclcpp::Node */ double dt_{0.0}; + /** + * @brief A transform buffer to use for looking up transformations. + */ + std::shared_ptr tf_buffer_; + + /** + * @brief Transform broadcaster which can be used to broadcast transformations. + */ + std::unique_ptr tf_broadcaster_; + private: /** * @brief Enable/disable the controller. @@ -187,7 +201,7 @@ class Controller : public rclcpp::Node geometry_msgs::msg::TransformStamped tf_map_base_; // TF2 - std::unique_ptr tf_broadcaster_; + std::unique_ptr tf_listener_; // Publishers rclcpp::Publisher::SharedPtr rc_override_pub_; diff --git a/blue_control/src/controller.cpp b/blue_control/src/controller.cpp index 6d32c1a4..82e73bcb 100644 --- a/blue_control/src/controller.cpp +++ b/blue_control/src/controller.cpp @@ -89,7 +89,9 @@ Controller::Controller(const std::string & node_name) blue::dynamics::CurrentEffects(ocean_current)); // Setup the ROS things - tf_broadcaster_ = std::make_unique(*this); + tf_buffer_ = std::make_shared(get_clock()); + tf_listener_ = std::make_unique(*tf_buffer_); + tf_broadcaster_ = std::make_unique(this); rc_override_pub_ = this->create_publisher("mavros/rc/override", 1); diff --git a/blue_control/src/ismc.cpp b/blue_control/src/ismc.cpp index eafe6dee..78bd4094 100644 --- a/blue_control/src/ismc.cpp +++ b/blue_control/src/ismc.cpp @@ -22,9 +22,10 @@ #include #include -#include #include "blue_dynamics/thruster_dynamics.hpp" +#include "tf2_eigen/tf2_eigen.hpp" +#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" namespace blue::control { @@ -137,7 +138,7 @@ mavros_msgs::msg::OverrideRCIn ISMC::update() surface = surface.unaryExpr([this](double x) { return tanh(x / boundary_thickness_); }); // Calculate the desired torques - const blue::dynamics::Vector6d forces = + blue::dynamics::Vector6d forces = hydrodynamics_.inertia.getInertia() * (desired_accel + proportional_gain_.inverse() * integral_gain_ * velocity_error) + hydrodynamics_.coriolis.calculateCoriolis(velocity) * velocity + @@ -158,6 +159,13 @@ mavros_msgs::msg::OverrideRCIn ISMC::update() desired_wrench_pub_->publish(wrench); + mavros_msgs::msg::OverrideRCIn msg; + + // Set all channels to "NOCHANGE" by default + for (uint16_t & channel : msg.channels) { + channel = mavros_msgs::msg::OverrideRCIn::CHAN_NOCHANGE; + } + // Multiply the desired forces by the pseudoinverse of the thruster configuration matrix // The size of this vector will depend on the number of thrusters so we don't assign it to a // fixed-size matrix @@ -175,13 +183,6 @@ mavros_msgs::msg::OverrideRCIn ISMC::update() [this](double x) { return blue::dynamics::calculatePwmFromThrustCurve(x); }); } - 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; - } - // Calculate the deadzone band std::tuple deadband; diff --git a/blue_description/gazebo/models/bluerov2_heavy/model.sdf b/blue_description/gazebo/models/bluerov2_heavy/model.sdf index b42040ba..ac335771 100644 --- a/blue_description/gazebo/models/bluerov2_heavy/model.sdf +++ b/blue_description/gazebo/models/bluerov2_heavy/model.sdf @@ -8,7 +8,7 @@ 0.0 0.0 0.011 0 0 0 - 13.5 + 13.0 0.26 0 @@ -33,7 +33,7 @@ 0.0 0.0 0.06 0 0 0 - 0.457 0.575 0.052 + 0.457 0.575 0.05 @@ -520,15 +520,5 @@ - - - map - base_link - 3 - 100 - - From ec960cb823b7df264b5fea14c869d76eae09316a Mon Sep 17 00:00:00 2001 From: Evan Palmer Date: Fri, 23 Jun 2023 13:43:04 -0700 Subject: [PATCH 07/16] Started cleanup --- blue_bringup/config/bluerov2.yaml | 6 +- blue_control/CMakeLists.txt | 8 --- .../include/blue_control/controller.hpp | 68 ++++++++----------- .../blue_control/frames.hpp} | 53 +++------------ blue_control/include/blue_control/ismc.hpp | 6 +- blue_control/src/controller.cpp | 42 ++++++------ blue_control/src/ismc.cpp | 8 ++- .../gazebo/models/bluerov2_heavy/model.sdf | 10 +++ 8 files changed, 82 insertions(+), 119 deletions(-) rename blue_control/{test/test_vector_to_eigen.cpp => include/blue_control/frames.hpp} (50%) diff --git a/blue_bringup/config/bluerov2.yaml b/blue_bringup/config/bluerov2.yaml index 8391eacf..979b6c29 100644 --- a/blue_bringup/config/bluerov2.yaml +++ b/blue_bringup/config/bluerov2.yaml @@ -27,9 +27,9 @@ ismc: msg_ids: [31, 32] msg_rates: [100.0, 100.0] control_rate: 200.0 - proportional_gain: [10.0, 10.0, 10.0, 10.0, 10.0, 10.0] - integral_gain: [3.0, 3.0, 3.0, 3.0, 3.0, 3.0] - sliding_gain: 50.0 + proportional_gain: [10.0, 10.0, 10.0, 0.001, 0.001, 10.0] + integral_gain: [3.0, 3.0, 3.0, 0.001, 0.001, 3.0] + sliding_gain: 10.0 boundary_thickness: 100.0 use_battery_state: false diff --git a/blue_control/CMakeLists.txt b/blue_control/CMakeLists.txt index 87980df1..2d6b1fc3 100644 --- a/blue_control/CMakeLists.txt +++ b/blue_control/CMakeLists.txt @@ -77,7 +77,6 @@ install(DIRECTORY ) if(BUILD_TESTING) - find_package(ament_cmake_gtest REQUIRED) find_package(ament_lint_auto REQUIRED) # Run linters found in package.xml except the two below @@ -85,13 +84,6 @@ if(BUILD_TESTING) set(ament_cmake_uncrustify_FOUND TRUE) ament_lint_auto_find_test_dependencies() - - # Setup unit tests - ament_add_gtest( - test_vector_to_eigen - test/test_vector_to_eigen.cpp - ) - target_link_libraries(test_vector_to_eigen custom_controllers) endif() ament_package() diff --git a/blue_control/include/blue_control/controller.hpp b/blue_control/include/blue_control/controller.hpp index bccb653a..7100abc8 100644 --- a/blue_control/include/blue_control/controller.hpp +++ b/blue_control/include/blue_control/controller.hpp @@ -26,8 +26,8 @@ #include #include +#include "blue_control/frames.hpp" #include "blue_dynamics/hydrodynamics.hpp" -#include "blue_dynamics/thruster_dynamics.hpp" #include "geometry_msgs/msg/accel.hpp" #include "geometry_msgs/msg/accel_stamped.hpp" #include "geometry_msgs/msg/pose_stamped.hpp" @@ -45,31 +45,6 @@ namespace blue::control { -/** - * @brief Convert an std::vector into an Eigen::Matrix - * - * @note While it would be preferable to define the rows and cols as template parameters, the - * primary use-case for this method within the scope of this implementation is to use it with ROS 2 - * parameters which are not known at compile time. Therefore, the rows and cols are made to be - * function parameters. - * - * @tparam T The type of values held by the vector. - * @tparam major The order to copy over the elements in (e.g., ``Eigen::RowMajor``) - * @param rows The number of rows in the resulting matrix. - * @param cols The number of columns in the resulting matrix. - * @param vec The vector to convert into a matrix. - * @return The converted Eigen matrix. - */ -template -inline Eigen::Matrix convertVectorToEigenMatrix( - const std::vector & vec, int rows, int cols) -{ - typedef const Eigen::Matrix M; - Eigen::Map mat(vec.data(), rows, cols); - - return mat; -} - /** * @brief A base class for custom BlueROV2 controllers. */ @@ -84,11 +59,29 @@ class Controller : public rclcpp::Node explicit Controller(const std::string & node_name); protected: - // Transform IDs - const std::string kMapFrameId{"map"}; - const std::string kMapNedFrameId{"map_ned"}; - const std::string kBaseFrameId{"base_link"}; - const std::string kBaseNedFrameId{"base_link_frd"}; + /** + * @brief Convert an std::vector into an Eigen::Matrix + * + * @tparam T The type of values held by the vector. + * @tparam major The order to copy over the elements in (e.g., ``Eigen::RowMajor``) + * @param rows The number of rows in the resulting matrix. + * @param cols The number of columns in the resulting matrix. + * @param vec The vector to convert into a matrix. + * @return The converted Eigen matrix. + */ + template + static inline Eigen::Matrix convertVectorToEigenMatrix( + const std::vector & vec, int rows, int cols) + { + // While it would be preferable to define the rows and cols as template parameters, the + // primary use-case for this method within the scope of this implementation is to use it with + // ROS 2 parameters which are not always known at compile time (e.g., TCM). Therefore, the rows + // and cols are made to be function parameters. + typedef const Eigen::Matrix M; + Eigen::Map mat(vec.data(), rows, cols); + + return mat; + } /** * @brief Function executed when the controller is armed. @@ -105,7 +98,7 @@ class Controller : public rclcpp::Node * * @return mavros_msgs::msg::OverrideRCIn */ - virtual mavros_msgs::msg::OverrideRCIn update() = 0; + virtual mavros_msgs::msg::OverrideRCIn calculateControlInput() = 0; /** * @brief A collection of the hydrodynamic parameters for the BlueROV2. @@ -144,14 +137,8 @@ class Controller : public rclcpp::Node */ double dt_{0.0}; - /** - * @brief A transform buffer to use for looking up transformations. - */ + // TF2 std::shared_ptr tf_buffer_; - - /** - * @brief Transform broadcaster which can be used to broadcast transformations. - */ std::unique_ptr tf_broadcaster_; private: @@ -170,7 +157,7 @@ class Controller : public rclcpp::Node /** * @brief Handle the incoming odometry messages. * - * @note This message will be published AFTER mavros and all of it's plugins are loaded. + * @note This message will be published after MAVROS and all of it's plugins are loaded. * * @param msg The current odometry message. */ @@ -195,6 +182,7 @@ class Controller : public rclcpp::Node */ void setMessageRate(int64_t msg_id, float rate); + // Manages whether or not control inputs are sent to ArduSub bool armed_; // Dynamic transforms diff --git a/blue_control/test/test_vector_to_eigen.cpp b/blue_control/include/blue_control/frames.hpp similarity index 50% rename from blue_control/test/test_vector_to_eigen.cpp rename to blue_control/include/blue_control/frames.hpp index f7fb541a..1514901d 100644 --- a/blue_control/test/test_vector_to_eigen.cpp +++ b/blue_control/include/blue_control/frames.hpp @@ -18,52 +18,15 @@ // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN // THE SOFTWARE. -#include +#include -#include -#include -#include - -#include "blue_control/controller.hpp" - -namespace blue::controller::test -{ - -using blue::control::convertVectorToEigenMatrix; - -TEST(ControllerTest, TestVectorToEigenVector) -{ - const std::vector coeff = {1, 2, 3, 4}; - - Eigen::Matrix expected; // NOLINT - expected << 1, 2, 3, 4; - - const Eigen::Matrix actual = convertVectorToEigenMatrix(coeff, 4, 1); - - ASSERT_TRUE(actual.isApprox(expected)); -} - -TEST(ControllerTest, TestVectorToEigenMatrix) +namespace blue::transforms { - const std::vector coeff = {1, 2, 3, 4}; - - Eigen::Matrix expected; // NOLINT - expected << 1, 2, 3, 4; - - const Eigen::Matrix actual = - convertVectorToEigenMatrix(coeff, 2, 2); - std::cout << expected << std::endl << actual << std::endl; - - ASSERT_TRUE(actual.isApprox(expected)); -} - -} // namespace blue::controller::test - -int main(int argc, char ** argv) -{ - ::testing::InitGoogleTest(&argc, argv); - const int result = RUN_ALL_TESTS(); +// Coordinate frame IDs +const std::string kMapFrameId{"map"}; +const std::string kMapNedFrameId{"map_ned"}; +const std::string kBaseFrameId{"base_link"}; +const std::string kBaseNedFrameId{"base_link_frd"}; - return result; -} +} // namespace blue::transforms diff --git a/blue_control/include/blue_control/ismc.hpp b/blue_control/include/blue_control/ismc.hpp index 8809a911..781a8aee 100644 --- a/blue_control/include/blue_control/ismc.hpp +++ b/blue_control/include/blue_control/ismc.hpp @@ -23,6 +23,7 @@ #include #include "blue_control/controller.hpp" +#include "blue_dynamics/thruster_dynamics.hpp" #include "blue_msgs/msg/reference.hpp" #include "geometry_msgs/msg/wrench_stamped.hpp" #include "mavros_msgs/msg/override_rc_in.hpp" @@ -44,7 +45,7 @@ class ISMC : public Controller protected: void onArm() override; void onDisarm() override; - mavros_msgs::msg::OverrideRCIn update() override; + mavros_msgs::msg::OverrideRCIn calculateControlInput() override; private: // ISMC gains @@ -58,7 +59,10 @@ class ISMC : public Controller blue::dynamics::Vector6d initial_acceleration_error_; blue::dynamics::Vector6d total_velocity_error_; + // Control whether or not the battery state is used when converting thrust to PWM bool use_battery_state_; + + // Reference signal blue_msgs::msg::Reference cmd_; // Publishers diff --git a/blue_control/src/controller.cpp b/blue_control/src/controller.cpp index 82e73bcb..fe7614e6 100644 --- a/blue_control/src/controller.cpp +++ b/blue_control/src/controller.cpp @@ -30,16 +30,9 @@ Controller::Controller(const std::string & node_name) armed_(false) { // Declare ROS parameters - this->declare_parameter("mass", 11.5); + this->declare_parameter("mass", 13.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})); @@ -47,13 +40,23 @@ Controller::Controller(const std::string & node_name) this->declare_parameter("msg_ids", std::vector({31, 32})); this->declare_parameter("msg_rates", std::vector({100, 100})); this->declare_parameter("control_rate", 200.0); + 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( - "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})); + "quadratic_damping_coeff", std::vector({-18.18, -21.66, -36.99, -1.55, -1.55, -1.55})); + + // clang-format off + 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.0, 0.0, 0.0, 0.0, 0.218, 0.218, -0.218, -0.218, + 0.0, 0.0, 0.0, 0.0, 0.12, -0.12, 0.12, -0.12, + -0.1888, 0.1888, 0.1888, -0.1888, 0.0, 0.0, 0.0, 0.0})); + // clang-format on // Get hydrodynamic parameters const double mass = this->get_parameter("mass").as_double(); @@ -93,11 +96,10 @@ Controller::Controller(const std::string & node_name) tf_listener_ = std::make_unique(*tf_buffer_); tf_broadcaster_ = std::make_unique(this); + accel_pub_ = this->create_publisher("/blue/state/accel", 1); rc_override_pub_ = this->create_publisher("mavros/rc/override", 1); - accel_pub_ = this->create_publisher("/blue/state/accel", 1); - battery_state_sub_ = this->create_subscription( "/mavros/battery", rclcpp::SensorDataQoS(), [this](sensor_msgs::msg::BatteryState::ConstSharedPtr msg) { battery_state_ = *msg; }); @@ -150,7 +152,7 @@ Controller::Controller(const std::string & node_name) std::chrono::duration(dt_), [this]() -> void { if (armed_) { - rc_override_pub_->publish(update()); + rc_override_pub_->publish(calculateControlInput()); } }, control_loop_cb_group_); @@ -202,7 +204,7 @@ void Controller::updateOdomCb(nav_msgs::msg::Odometry::ConstSharedPtr msg) accel_ = accel; geometry_msgs::msg::AccelStamped accel_stamped; - accel_stamped.header.frame_id = kBaseFrameId; + accel_stamped.header.frame_id = blue::transforms::kBaseFrameId; accel_stamped.header.stamp = this->get_clock()->now(); accel_stamped.accel = accel_; @@ -212,8 +214,8 @@ void Controller::updateOdomCb(nav_msgs::msg::Odometry::ConstSharedPtr msg) // Publish the map -> base_link transform tf_map_base_.header.stamp = this->get_clock()->now(); - tf_map_base_.header.frame_id = kMapFrameId; - tf_map_base_.child_frame_id = kBaseFrameId; + tf_map_base_.header.frame_id = blue::transforms::kMapFrameId; + tf_map_base_.child_frame_id = blue::transforms::kBaseFrameId; tf_map_base_.transform.translation.x = odom_.pose.pose.position.x; tf_map_base_.transform.translation.y = odom_.pose.pose.position.y; diff --git a/blue_control/src/ismc.cpp b/blue_control/src/ismc.cpp index 78bd4094..1552f56c 100644 --- a/blue_control/src/ismc.cpp +++ b/blue_control/src/ismc.cpp @@ -94,6 +94,10 @@ void ISMC::onArm() cmd_.accel.angular.x, cmd_.accel.angular.y, cmd_.accel.angular.z; initial_acceleration_error_ = accel_error; initial_acceleration_error_ -= accel; + + // !TESTING PURPOSES ONLY + cmd_.twist.linear.x = -10.0; + cmd_.twist.linear.z = 10.0; }; void ISMC::onDisarm() @@ -106,7 +110,7 @@ void ISMC::onDisarm() initial_acceleration_error_ = blue::dynamics::Vector6d::Zero(); }; -mavros_msgs::msg::OverrideRCIn ISMC::update() +mavros_msgs::msg::OverrideRCIn ISMC::calculateControlInput() { blue::dynamics::Vector6d velocity; tf2::fromMsg(odom_.twist.twist, velocity); @@ -148,7 +152,7 @@ mavros_msgs::msg::OverrideRCIn ISMC::update() // Publish the desired torques to help with debugging and visualization geometry_msgs::msg::WrenchStamped wrench; - wrench.header.frame_id = kBaseFrameId; + wrench.header.frame_id = blue::transforms::kBaseFrameId; wrench.header.stamp = this->get_clock()->now(); wrench.wrench.force.x = forces[0]; wrench.wrench.force.y = forces[1]; diff --git a/blue_description/gazebo/models/bluerov2_heavy/model.sdf b/blue_description/gazebo/models/bluerov2_heavy/model.sdf index ac335771..8fac7cdd 100644 --- a/blue_description/gazebo/models/bluerov2_heavy/model.sdf +++ b/blue_description/gazebo/models/bluerov2_heavy/model.sdf @@ -520,5 +520,15 @@ + + + map + base_link + 3 + 100 + + From 7a78be5bcd35d6939903f95bf607d3c78975ab39 Mon Sep 17 00:00:00 2001 From: Evan Palmer Date: Fri, 23 Jun 2023 14:34:50 -0700 Subject: [PATCH 08/16] Continued cleanup and updated readme --- README.md | 56 +++++++++++++++++++- blue_bringup/CMakeLists.txt | 19 +++++++ blue_bringup/blue_bringup/__init__.py | 0 blue_bringup/launch/bluerov2.launch.py | 14 +++++ blue_bringup/launch/bluerov2_heavy.launch.py | 14 +++++ blue_bringup/package.xml | 3 +- blue_bringup/resource/blue_bringup | 0 blue_bringup/setup.cfg | 4 -- blue_bringup/setup.py | 48 ----------------- blue_bringup/test/test_copyright.py | 27 ---------- blue_bringup/test/test_flake8.py | 25 --------- blue_control/src/ismc.cpp | 4 -- 12 files changed, 103 insertions(+), 111 deletions(-) create mode 100644 blue_bringup/CMakeLists.txt delete mode 100644 blue_bringup/blue_bringup/__init__.py delete mode 100644 blue_bringup/resource/blue_bringup delete mode 100644 blue_bringup/setup.cfg delete mode 100644 blue_bringup/setup.py delete mode 100644 blue_bringup/test/test_copyright.py delete mode 100644 blue_bringup/test/test_flake8.py diff --git a/README.md b/README.md index d51d1a8d..223b381e 100644 --- a/README.md +++ b/README.md @@ -1,8 +1,60 @@ # BlueROV2 ROS 2 driver :ocean: The BlueROV2 driver is a collection of ROS 2 packages designed -to support ROS 2 integration with the [BlueROV2 Heavy](https://bluerobotics.com/) -and [ArduSub](https://www.ardusub.com/). +to support ROS 2 integration with the [BlueROV2](https://bluerobotics.com/) +(both base and heavy configurations) and [ArduSub](https://www.ardusub.com/). + +## Main Features + +The main features of the project include: + +- Support for developing custom controllers for the BlueROV2 +- RC Passthrough mode: a custom mode which enables sending thrust commands to individual thrusters +- External localization support to enable developing custom SLAM algorithms and interfaces for localization sensors +- ArduSub + Gazebo SITL integration for evaluating the performance of your algorithms in a simulation environment +- Interfaces for adjusting hydrodynamic parameters +- Visualization support using Foxglove Studio + +## Installation + +The BlueROV2 driver is currently supported on Linux, and is available for the +ROS distributions Humble, Iron, and Rolling. To install the BlueROV2 driver, +first clone this project to the `src` directory of your ROS workspace, replacing +`$ROS_DISTRO` with the desired ROS distribution or `main` for Rolling: + +```bash +git clone -b $ROS_DISTRO git@github.com:evan-palmer/blue.git +``` + +After cloning the project, install all external dependencies using `vcs`: + +```bash +vcs import src < src/blue/blue.repos +``` + +Finally, install the ROS dependencies using `rosdep`, again, replacing +`$ROS_DISTRO` with the desired ROS distribution: + +```bash +rosdep update && \ +rosdep install -y --from-paths src --ignore-src --rosdistro $ROS_DISTRO +``` + +## Quick start + +ROS 2 launch files have been provided to start the BlueROV2 driver for the base +and heavy configurations. To launch the system for the BlueROV2 Heavy, run + +```bash +ros2 launch blue_bringup bluerov2_heavy.launch.py +``` + +A full description of the launch arguments and their respective default values +can be obtained by running the following command: + +```bash +ros2 launch blue_bringup bluerov2_heavy.launch.py --show-args +``` ## Getting help diff --git a/blue_bringup/CMakeLists.txt b/blue_bringup/CMakeLists.txt new file mode 100644 index 00000000..727bc1f1 --- /dev/null +++ b/blue_bringup/CMakeLists.txt @@ -0,0 +1,19 @@ +cmake_minimum_required(VERSION 3.8) +project(blue_bringup) + +set(THIS_PACKAGE_INCLUDE_DEPENDS + rclcpp + rclpy + ament_cmake +) + +foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) + find_package(${Dependency} REQUIRED) +endforeach() + +install( + DIRECTORY launch config + DESTINATION share/${PROJECT_NAME} +) + +ament_package() diff --git a/blue_bringup/blue_bringup/__init__.py b/blue_bringup/blue_bringup/__init__.py deleted file mode 100644 index e69de29b..00000000 diff --git a/blue_bringup/launch/bluerov2.launch.py b/blue_bringup/launch/bluerov2.launch.py index 860e879a..d92ab0b7 100644 --- a/blue_bringup/launch/bluerov2.launch.py +++ b/blue_bringup/launch/bluerov2.launch.py @@ -79,6 +79,11 @@ def generate_launch_description() -> LaunchDescription: default_value="false", description="Automatically start Gazebo.", ), + DeclareLaunchArgument( + "use_foxglove", + default_value="false", + description="Start the Foxglove bridge.", + ), DeclareLaunchArgument( "description_package", default_value="blue_description", @@ -105,6 +110,7 @@ def generate_launch_description() -> LaunchDescription: description_package = LaunchConfiguration("description_package") use_sim = LaunchConfiguration("use_sim") + use_foxglove = LaunchConfiguration("use_foxglove") gazebo_world_file = LaunchConfiguration("gazebo_world_file") config_filepath = PathJoinSubstitution( @@ -213,6 +219,14 @@ def generate_launch_description() -> LaunchDescription: "use_camera": LaunchConfiguration("use_camera"), }.items(), ), + IncludeLaunchDescription( + PythonLaunchDescriptionSource( + PathJoinSubstitution( + [FindPackageShare("foxglove_bridge"), "foxglove_bridge_launch.xml"] + ) + ), + condition=IfCondition(use_foxglove), + ), ] return LaunchDescription(args + nodes + processes + includes) diff --git a/blue_bringup/launch/bluerov2_heavy.launch.py b/blue_bringup/launch/bluerov2_heavy.launch.py index 13bf6535..9e53e694 100644 --- a/blue_bringup/launch/bluerov2_heavy.launch.py +++ b/blue_bringup/launch/bluerov2_heavy.launch.py @@ -79,6 +79,11 @@ def generate_launch_description() -> LaunchDescription: default_value="false", description="Automatically start Gazebo.", ), + DeclareLaunchArgument( + "use_foxglove", + default_value="false", + description="Start the Foxglove bridge.", + ), DeclareLaunchArgument( "description_package", default_value="blue_description", @@ -105,6 +110,7 @@ def generate_launch_description() -> LaunchDescription: description_package = LaunchConfiguration("description_package") use_sim = LaunchConfiguration("use_sim") + use_foxglove = LaunchConfiguration("use_foxglove") gazebo_world_file = LaunchConfiguration("gazebo_world_file") config_filepath = PathJoinSubstitution( @@ -213,6 +219,14 @@ def generate_launch_description() -> LaunchDescription: "use_camera": LaunchConfiguration("use_camera"), }.items(), ), + IncludeLaunchDescription( + PythonLaunchDescriptionSource( + PathJoinSubstitution( + [FindPackageShare("foxglove_bridge"), "foxglove_bridge_launch.xml"] + ) + ), + condition=IfCondition(use_foxglove), + ), ] return LaunchDescription(args + nodes + processes + includes) diff --git a/blue_bringup/package.xml b/blue_bringup/package.xml index 8fff7072..a619e712 100644 --- a/blue_bringup/package.xml +++ b/blue_bringup/package.xml @@ -17,6 +17,7 @@ mavros mavros_extras python3-matplotlib + foxglove-bridge ament_copyright ament_flake8 @@ -24,6 +25,6 @@ python3-pytest - ament_python + ament_cmake diff --git a/blue_bringup/resource/blue_bringup b/blue_bringup/resource/blue_bringup deleted file mode 100644 index e69de29b..00000000 diff --git a/blue_bringup/setup.cfg b/blue_bringup/setup.cfg deleted file mode 100644 index a03d75ab..00000000 --- a/blue_bringup/setup.cfg +++ /dev/null @@ -1,4 +0,0 @@ -[develop] -script_dir=$base/lib/blue_bringup -[install] -install_scripts=$base/lib/blue_bringup diff --git a/blue_bringup/setup.py b/blue_bringup/setup.py deleted file mode 100644 index f4702d36..00000000 --- a/blue_bringup/setup.py +++ /dev/null @@ -1,48 +0,0 @@ -# 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. - -import os -from glob import glob - -from setuptools import setup - -package_name = "blue_bringup" - -setup( - name=package_name, - version="0.0.1", - packages=[package_name], - data_files=[ - ("share/ament_index/resource_index/packages", ["resource/" + package_name]), - ("share/" + package_name, ["package.xml"]), - (os.path.join("share", package_name), glob("launch/*.launch.py")), - (os.path.join("share", package_name, "config"), glob("config/*.yaml")), - ], - install_requires=["setuptools"], - zip_safe=True, - maintainer="Evan Palmer", - maintainer_email="evanp922@gmail.com", - description="Entrypoints for the Blue project.", - license="MIT", - tests_require=["pytest"], - entry_points={ - "console_scripts": [], - }, -) diff --git a/blue_bringup/test/test_copyright.py b/blue_bringup/test/test_copyright.py deleted file mode 100644 index 8f18fa4b..00000000 --- a/blue_bringup/test/test_copyright.py +++ /dev/null @@ -1,27 +0,0 @@ -# Copyright 2015 Open Source Robotics Foundation, Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -import pytest -from ament_copyright.main import main - - -# Remove the `skip` decorator once the source file(s) have a copyright header -@pytest.mark.skip( - reason="No copyright header has been placed in the generated source file." -) -@pytest.mark.copyright -@pytest.mark.linter -def test_copyright(): - rc = main(argv=[".", "test"]) - assert rc == 0, "Found errors" diff --git a/blue_bringup/test/test_flake8.py b/blue_bringup/test/test_flake8.py deleted file mode 100644 index f494570f..00000000 --- a/blue_bringup/test/test_flake8.py +++ /dev/null @@ -1,25 +0,0 @@ -# Copyright 2017 Open Source Robotics Foundation, Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -import pytest -from ament_flake8.main import main_with_errors - - -@pytest.mark.flake8 -@pytest.mark.linter -def test_flake8(): - rc, errors = main_with_errors(argv=[]) - assert rc == 0, "Found %d code style errors / warnings:\n" % len( - errors - ) + "\n".join(errors) diff --git a/blue_control/src/ismc.cpp b/blue_control/src/ismc.cpp index 1552f56c..15edac35 100644 --- a/blue_control/src/ismc.cpp +++ b/blue_control/src/ismc.cpp @@ -94,10 +94,6 @@ void ISMC::onArm() cmd_.accel.angular.x, cmd_.accel.angular.y, cmd_.accel.angular.z; initial_acceleration_error_ = accel_error; initial_acceleration_error_ -= accel; - - // !TESTING PURPOSES ONLY - cmd_.twist.linear.x = -10.0; - cmd_.twist.linear.z = 10.0; }; void ISMC::onDisarm() From 7fd81027a25787a80dff601d0cd807a2126a06ef Mon Sep 17 00:00:00 2001 From: Evan Palmer Date: Fri, 23 Jun 2023 17:03:20 -0700 Subject: [PATCH 09/16] Added support to run foxglove from launch --- .docker/Dockerfile | 1 - blue_bringup/launch/bluerov2.launch.py | 33 ++++++++++++++------ blue_bringup/launch/bluerov2_heavy.launch.py | 33 ++++++++++++++------ blue_bringup/package.xml | 2 +- blue_msgs/msg/Reference.msg | 1 - 5 files changed, 49 insertions(+), 21 deletions(-) diff --git a/.docker/Dockerfile b/.docker/Dockerfile index bbec1c20..58641746 100644 --- a/.docker/Dockerfile +++ b/.docker/Dockerfile @@ -200,7 +200,6 @@ RUN apt-get -q update \ iputils-ping \ net-tools \ gdb \ - ros-$ROS_DISTRO-foxglove-bridge \ && apt-get autoremove -y \ && apt-get clean -y \ && rm -rf /var/lib/apt/lists/* diff --git a/blue_bringup/launch/bluerov2.launch.py b/blue_bringup/launch/bluerov2.launch.py index d92ab0b7..3e837b7c 100644 --- a/blue_bringup/launch/bluerov2.launch.py +++ b/blue_bringup/launch/bluerov2.launch.py @@ -29,6 +29,7 @@ from launch.substitutions import LaunchConfiguration, PathJoinSubstitution from launch_ros.actions import Node from launch_ros.substitutions import FindPackageShare +from launch_xml.launch_description_sources import XMLLaunchDescriptionSource def generate_launch_description() -> LaunchDescription: @@ -106,12 +107,19 @@ def generate_launch_description() -> LaunchDescription: " simulation." ), ), + DeclareLaunchArgument( + "foxglove_bridge_address", + default_value="127.0.0.1", + description="The Foxglove Studio datasource address.", + ), + DeclareLaunchArgument( + "foxglove_bridge_port", + default_value="8765", + description="The Foxglove Studio datasource port.", + ), ] - description_package = LaunchConfiguration("description_package") use_sim = LaunchConfiguration("use_sim") - use_foxglove = LaunchConfiguration("use_foxglove") - gazebo_world_file = LaunchConfiguration("gazebo_world_file") config_filepath = PathJoinSubstitution( [ @@ -157,10 +165,10 @@ def generate_launch_description() -> LaunchDescription: "-r", PathJoinSubstitution( [ - FindPackageShare(description_package), + FindPackageShare(LaunchConfiguration("description_package")), "gazebo", "worlds", - gazebo_world_file, + LaunchConfiguration("gazebo_world_file"), ] ), ], @@ -220,12 +228,19 @@ def generate_launch_description() -> LaunchDescription: }.items(), ), IncludeLaunchDescription( - PythonLaunchDescriptionSource( + XMLLaunchDescriptionSource( PathJoinSubstitution( - [FindPackageShare("foxglove_bridge"), "foxglove_bridge_launch.xml"] - ) + [ + FindPackageShare("foxglove_bridge"), + "foxglove_bridge_launch.xml", + ] + ), ), - condition=IfCondition(use_foxglove), + launch_arguments={ + "address": LaunchConfiguration("foxglove_bridge_address"), + "port": LaunchConfiguration("foxglove_bridge_port"), + }, + condition=IfCondition(LaunchConfiguration("use_foxglove")), ), ] diff --git a/blue_bringup/launch/bluerov2_heavy.launch.py b/blue_bringup/launch/bluerov2_heavy.launch.py index 9e53e694..35b4c435 100644 --- a/blue_bringup/launch/bluerov2_heavy.launch.py +++ b/blue_bringup/launch/bluerov2_heavy.launch.py @@ -29,6 +29,7 @@ from launch.substitutions import LaunchConfiguration, PathJoinSubstitution from launch_ros.actions import Node from launch_ros.substitutions import FindPackageShare +from launch_xml.launch_description_sources import XMLLaunchDescriptionSource def generate_launch_description() -> LaunchDescription: @@ -106,12 +107,19 @@ def generate_launch_description() -> LaunchDescription: " simulation." ), ), + DeclareLaunchArgument( + "foxglove_bridge_address", + default_value="127.0.0.1", + description="The Foxglove Studio datasource address.", + ), + DeclareLaunchArgument( + "foxglove_bridge_port", + default_value="8765", + description="The Foxglove Studio datasource port.", + ), ] - description_package = LaunchConfiguration("description_package") use_sim = LaunchConfiguration("use_sim") - use_foxglove = LaunchConfiguration("use_foxglove") - gazebo_world_file = LaunchConfiguration("gazebo_world_file") config_filepath = PathJoinSubstitution( [ @@ -157,10 +165,10 @@ def generate_launch_description() -> LaunchDescription: "-r", PathJoinSubstitution( [ - FindPackageShare(description_package), + FindPackageShare(LaunchConfiguration("description_package")), "gazebo", "worlds", - gazebo_world_file, + LaunchConfiguration("gazebo_world_file"), ] ), ], @@ -220,12 +228,19 @@ def generate_launch_description() -> LaunchDescription: }.items(), ), IncludeLaunchDescription( - PythonLaunchDescriptionSource( + XMLLaunchDescriptionSource( PathJoinSubstitution( - [FindPackageShare("foxglove_bridge"), "foxglove_bridge_launch.xml"] - ) + [ + FindPackageShare("foxglove_bridge"), + "foxglove_bridge_launch.xml", + ] + ), ), - condition=IfCondition(use_foxglove), + launch_arguments={ + "address": LaunchConfiguration("foxglove_bridge_address"), + "port": LaunchConfiguration("foxglove_bridge_port"), + }.items(), + condition=IfCondition(LaunchConfiguration("use_foxglove")), ), ] diff --git a/blue_bringup/package.xml b/blue_bringup/package.xml index a619e712..2096e94c 100644 --- a/blue_bringup/package.xml +++ b/blue_bringup/package.xml @@ -17,7 +17,7 @@ mavros mavros_extras python3-matplotlib - foxglove-bridge + foxglove_bridge ament_copyright ament_flake8 diff --git a/blue_msgs/msg/Reference.msg b/blue_msgs/msg/Reference.msg index abf872b6..1563ab7b 100644 --- a/blue_msgs/msg/Reference.msg +++ b/blue_msgs/msg/Reference.msg @@ -1,4 +1,3 @@ # Defines the desired reference values for a controller. -std_msgs/Header header geometry_msgs/Twist twist geometry_msgs/Accel accel From 8082620ca62e7df3616cddd718bb1b3d254f2eb1 Mon Sep 17 00:00:00 2001 From: Evan Palmer Date: Fri, 23 Jun 2023 17:58:46 -0700 Subject: [PATCH 10/16] Renamed reference to twistaccelcmd --- blue_control/CMakeLists.txt | 1 - blue_control/include/blue_control/frames.hpp | 4 ++-- blue_control/include/blue_control/ismc.hpp | 6 +++--- blue_control/src/controller.cpp | 4 ++-- blue_control/src/ismc.cpp | 8 ++++---- blue_msgs/CMakeLists.txt | 2 +- blue_msgs/msg/Reference.msg | 3 --- blue_msgs/msg/TwistAccelCmd.msg | 4 ++++ 8 files changed, 16 insertions(+), 16 deletions(-) delete mode 100644 blue_msgs/msg/Reference.msg create mode 100644 blue_msgs/msg/TwistAccelCmd.msg diff --git a/blue_control/CMakeLists.txt b/blue_control/CMakeLists.txt index 2d6b1fc3..e5b363d6 100644 --- a/blue_control/CMakeLists.txt +++ b/blue_control/CMakeLists.txt @@ -27,7 +27,6 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS eigen3_cmake_module Eigen3 tf2_eigen - tf2_geometry_msgs ) foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) diff --git a/blue_control/include/blue_control/frames.hpp b/blue_control/include/blue_control/frames.hpp index 1514901d..120f5eb4 100644 --- a/blue_control/include/blue_control/frames.hpp +++ b/blue_control/include/blue_control/frames.hpp @@ -26,7 +26,7 @@ namespace blue::transforms // Coordinate frame IDs const std::string kMapFrameId{"map"}; const std::string kMapNedFrameId{"map_ned"}; -const std::string kBaseFrameId{"base_link"}; -const std::string kBaseNedFrameId{"base_link_frd"}; +const std::string kBaseLinkFrameId{"base_link"}; +const std::string kBaseLinkFrdFrameId{"base_link_frd"}; } // namespace blue::transforms diff --git a/blue_control/include/blue_control/ismc.hpp b/blue_control/include/blue_control/ismc.hpp index 781a8aee..2c1f4e39 100644 --- a/blue_control/include/blue_control/ismc.hpp +++ b/blue_control/include/blue_control/ismc.hpp @@ -24,7 +24,7 @@ #include "blue_control/controller.hpp" #include "blue_dynamics/thruster_dynamics.hpp" -#include "blue_msgs/msg/reference.hpp" +#include "blue_msgs/msg/twist_accel_cmd.hpp" #include "geometry_msgs/msg/wrench_stamped.hpp" #include "mavros_msgs/msg/override_rc_in.hpp" @@ -63,13 +63,13 @@ class ISMC : public Controller bool use_battery_state_; // Reference signal - blue_msgs::msg::Reference cmd_; + blue_msgs::msg::TwistAccelCmd cmd_; // Publishers rclcpp::Publisher::SharedPtr desired_wrench_pub_; // Subscribers - rclcpp::Subscription::SharedPtr cmd_sub_; + rclcpp::Subscription::SharedPtr cmd_sub_; }; } // namespace blue::control diff --git a/blue_control/src/controller.cpp b/blue_control/src/controller.cpp index fe7614e6..248d1265 100644 --- a/blue_control/src/controller.cpp +++ b/blue_control/src/controller.cpp @@ -204,7 +204,7 @@ void Controller::updateOdomCb(nav_msgs::msg::Odometry::ConstSharedPtr msg) accel_ = accel; geometry_msgs::msg::AccelStamped accel_stamped; - accel_stamped.header.frame_id = blue::transforms::kBaseFrameId; + accel_stamped.header.frame_id = blue::transforms::kBaseLinkFrameId; accel_stamped.header.stamp = this->get_clock()->now(); accel_stamped.accel = accel_; @@ -215,7 +215,7 @@ void Controller::updateOdomCb(nav_msgs::msg::Odometry::ConstSharedPtr msg) // Publish the map -> base_link transform tf_map_base_.header.stamp = this->get_clock()->now(); tf_map_base_.header.frame_id = blue::transforms::kMapFrameId; - tf_map_base_.child_frame_id = blue::transforms::kBaseFrameId; + tf_map_base_.child_frame_id = blue::transforms::kBaseLinkFrameId; tf_map_base_.transform.translation.x = odom_.pose.pose.position.x; tf_map_base_.transform.translation.y = odom_.pose.pose.position.y; diff --git a/blue_control/src/ismc.cpp b/blue_control/src/ismc.cpp index 15edac35..d2c1ad4f 100644 --- a/blue_control/src/ismc.cpp +++ b/blue_control/src/ismc.cpp @@ -25,7 +25,6 @@ #include "blue_dynamics/thruster_dynamics.hpp" #include "tf2_eigen/tf2_eigen.hpp" -#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" namespace blue::control { @@ -60,8 +59,9 @@ ISMC::ISMC() this->create_publisher("/blue/ismc/desired_wrench", 1); // 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; }); + cmd_sub_ = this->create_subscription( + "/blue/ismc/cmd", 1, + [this](blue_msgs::msg::TwistAccelCmd::ConstSharedPtr msg) { cmd_ = *msg; }); } void ISMC::onArm() @@ -148,7 +148,7 @@ mavros_msgs::msg::OverrideRCIn ISMC::calculateControlInput() // Publish the desired torques to help with debugging and visualization geometry_msgs::msg::WrenchStamped wrench; - wrench.header.frame_id = blue::transforms::kBaseFrameId; + wrench.header.frame_id = blue::transforms::kBaseLinkFrameId; wrench.header.stamp = this->get_clock()->now(); wrench.wrench.force.x = forces[0]; wrench.wrench.force.y = forces[1]; diff --git a/blue_msgs/CMakeLists.txt b/blue_msgs/CMakeLists.txt index efa6dc4e..9c981621 100644 --- a/blue_msgs/CMakeLists.txt +++ b/blue_msgs/CMakeLists.txt @@ -11,7 +11,7 @@ find_package(geometry_msgs REQUIRED) find_package(rosidl_default_generators REQUIRED) set(msg_files - "msg/Reference.msg" + "msg/TwistAccelCmd.msg" ) rosidl_generate_interfaces(${PROJECT_NAME} diff --git a/blue_msgs/msg/Reference.msg b/blue_msgs/msg/Reference.msg deleted file mode 100644 index 1563ab7b..00000000 --- a/blue_msgs/msg/Reference.msg +++ /dev/null @@ -1,3 +0,0 @@ -# Defines the desired reference values for a controller. -geometry_msgs/Twist twist -geometry_msgs/Accel accel diff --git a/blue_msgs/msg/TwistAccelCmd.msg b/blue_msgs/msg/TwistAccelCmd.msg new file mode 100644 index 00000000..c5d43ca8 --- /dev/null +++ b/blue_msgs/msg/TwistAccelCmd.msg @@ -0,0 +1,4 @@ +# Defines a desired twist and acceleration command. +std_msgs/Header header +geometry_msgs/Twist twist +geometry_msgs/Accel accel From ae08ae1be70442d902a6aeb6c12f8d007baeef1c Mon Sep 17 00:00:00 2001 From: Evan Palmer Date: Fri, 23 Jun 2023 19:59:58 -0700 Subject: [PATCH 11/16] Started to resolve pr comments --- blue_bringup/scripts/disarm.sh | 1 - blue_control/package.xml | 9 +++++++-- blue_control/src/controller.cpp | 2 -- blue_control/src/ismc.cpp | 4 ++-- blue_localization/blue_localization/localizer.py | 2 +- 5 files changed, 10 insertions(+), 8 deletions(-) diff --git a/blue_bringup/scripts/disarm.sh b/blue_bringup/scripts/disarm.sh index 1fefbb73..7953707d 100755 --- a/blue_bringup/scripts/disarm.sh +++ b/blue_bringup/scripts/disarm.sh @@ -5,4 +5,3 @@ ros2 service call /blue/control/arm std_srvs/srv/SetBool data:\ false\ ros2 service call /blue/manager/enable_passthrough std_srvs/srv/SetBool data:\ false\ - diff --git a/blue_control/package.xml b/blue_control/package.xml index b4e6a39d..4f7457fa 100644 --- a/blue_control/package.xml +++ b/blue_control/package.xml @@ -1,12 +1,19 @@ + blue_control 0.0.1 Control interface for the BlueROV2 + Evan Palmer MIT + https://github.com/evan-palmer/blue.git + https://github.com/evan-palmer/blue/issues + + Evan Palmer + nav_msgs sensor_msgs mavros_msgs @@ -22,8 +29,6 @@ ament_cmake - ament_cmake_gtest - ament_cmake_gmock ament_lint_auto ament_lint_common diff --git a/blue_control/src/controller.cpp b/blue_control/src/controller.cpp index 248d1265..2dcd5889 100644 --- a/blue_control/src/controller.cpp +++ b/blue_control/src/controller.cpp @@ -20,8 +20,6 @@ #include "blue_control/controller.hpp" -#include "tf2/transform_datatypes.h" - namespace blue::control { diff --git a/blue_control/src/ismc.cpp b/blue_control/src/ismc.cpp index d2c1ad4f..72b5efdc 100644 --- a/blue_control/src/ismc.cpp +++ b/blue_control/src/ismc.cpp @@ -83,7 +83,7 @@ void ISMC::onArm() tf2::fromMsg(cmd_.twist, initial_velocity_error_); initial_velocity_error_ -= velocity; - // Now calculate the accleration error i.c. + // Now calculate the acceleration error i.c. blue::dynamics::Vector6d accel; accel << accel_.linear.x, accel_.linear.y, accel_.linear.z, accel_.angular.x, accel_.angular.y, accel_.angular.z; @@ -101,7 +101,7 @@ void ISMC::onDisarm() // Reset the total velocity error on disarm just to be safe total_velocity_error_ = blue::dynamics::Vector6d::Zero(); - // Reset the intial conditions too + // Reset the initial conditions too initial_velocity_error_ = blue::dynamics::Vector6d::Zero(); initial_acceleration_error_ = blue::dynamics::Vector6d::Zero(); }; diff --git a/blue_localization/blue_localization/localizer.py b/blue_localization/blue_localization/localizer.py index 7b99f0f1..fe86c9e1 100644 --- a/blue_localization/blue_localization/localizer.py +++ b/blue_localization/blue_localization/localizer.py @@ -28,12 +28,12 @@ import tf_transformations as tf from cv_bridge import CvBridge from geometry_msgs.msg import PoseStamped +from nav_msgs.msg import Odometry from rclpy.node import Node from sensor_msgs.msg import Image from tf2_ros import TransformException from tf2_ros.buffer import Buffer from tf2_ros.transform_listener import TransformListener -from nav_msgs.msg import Odometry class Localizer(Node, ABC): From 456ab2c0a076a6966b23778762e20fb221f7b699 Mon Sep 17 00:00:00 2001 From: Evan Palmer Date: Sat, 24 Jun 2023 18:57:32 -0700 Subject: [PATCH 12/16] WIP: tuning the controller for the bluerov2 base configuration --- blue_bringup/config/bluerov2.yaml | 8 ++++---- blue_bringup/launch/bluerov2.launch.py | 2 +- blue_control/include/blue_control/ismc.hpp | 2 ++ blue_control/src/ismc.cpp | 18 +++++++++++++++++- .../blue_localization/localizer.py | 11 ++++++++++- 5 files changed, 34 insertions(+), 7 deletions(-) diff --git a/blue_bringup/config/bluerov2.yaml b/blue_bringup/config/bluerov2.yaml index 979b6c29..93e65856 100644 --- a/blue_bringup/config/bluerov2.yaml +++ b/blue_bringup/config/bluerov2.yaml @@ -27,10 +27,10 @@ ismc: msg_ids: [31, 32] msg_rates: [100.0, 100.0] control_rate: 200.0 - proportional_gain: [10.0, 10.0, 10.0, 0.001, 0.001, 10.0] - integral_gain: [3.0, 3.0, 3.0, 0.001, 0.001, 3.0] - sliding_gain: 10.0 - boundary_thickness: 100.0 + proportional_gain: [2.0, 2.0, 2.0, 2.0, 2.0, 2.0] + integral_gain: [0.01, 0.01, 0.01, 0.01, 0.01, 0.01] + sliding_gain: 0.5 + boundary_thickness: 0.1 use_battery_state: false # Localization sources diff --git a/blue_bringup/launch/bluerov2.launch.py b/blue_bringup/launch/bluerov2.launch.py index 3e837b7c..90c2a6a5 100644 --- a/blue_bringup/launch/bluerov2.launch.py +++ b/blue_bringup/launch/bluerov2.launch.py @@ -239,7 +239,7 @@ def generate_launch_description() -> LaunchDescription: launch_arguments={ "address": LaunchConfiguration("foxglove_bridge_address"), "port": LaunchConfiguration("foxglove_bridge_port"), - }, + }.items(), condition=IfCondition(LaunchConfiguration("use_foxglove")), ), ] diff --git a/blue_control/include/blue_control/ismc.hpp b/blue_control/include/blue_control/ismc.hpp index 2c1f4e39..67be1f2a 100644 --- a/blue_control/include/blue_control/ismc.hpp +++ b/blue_control/include/blue_control/ismc.hpp @@ -25,6 +25,7 @@ #include "blue_control/controller.hpp" #include "blue_dynamics/thruster_dynamics.hpp" #include "blue_msgs/msg/twist_accel_cmd.hpp" +#include "geometry_msgs/msg/twist_stamped.hpp" #include "geometry_msgs/msg/wrench_stamped.hpp" #include "mavros_msgs/msg/override_rc_in.hpp" @@ -67,6 +68,7 @@ class ISMC : public Controller // Publishers rclcpp::Publisher::SharedPtr desired_wrench_pub_; + rclcpp::Publisher::SharedPtr velocity_error_pub_; // Subscribers rclcpp::Subscription::SharedPtr cmd_sub_; diff --git a/blue_control/src/ismc.cpp b/blue_control/src/ismc.cpp index 72b5efdc..66c946c6 100644 --- a/blue_control/src/ismc.cpp +++ b/blue_control/src/ismc.cpp @@ -54,9 +54,11 @@ ISMC::ISMC() boundary_thickness_ = this->get_parameter("boundary_thickness").as_double(); use_battery_state_ = this->get_parameter("use_battery_state").as_bool(); - // Publish the desired wrench to help with tuning and visualization + // Publish the desired wrench and errors to help with tuning and visualization desired_wrench_pub_ = this->create_publisher("/blue/ismc/desired_wrench", 1); + velocity_error_pub_ = + this->create_publisher("/blue/ismc/velocity_error", 1); // Update the reference signal when a new command is received cmd_sub_ = this->create_subscription( @@ -116,6 +118,20 @@ mavros_msgs::msg::OverrideRCIn ISMC::calculateControlInput() tf2::fromMsg(cmd_.twist, velocity_error); velocity_error -= velocity; + // Publish the velocity error to help with debugging + geometry_msgs::msg::TwistStamped velocity_error_msg; + velocity_error_msg.header.frame_id = blue::transforms::kBaseLinkFrdFrameId; + velocity_error_msg.header.stamp = this->get_clock()->now(); + + velocity_error_msg.twist.linear.x = velocity[0]; + velocity_error_msg.twist.linear.y = velocity[1]; + velocity_error_msg.twist.linear.z = velocity[2]; + velocity_error_msg.twist.angular.x = velocity[3]; + velocity_error_msg.twist.angular.y = velocity[4]; + velocity_error_msg.twist.angular.z = velocity[5]; + + velocity_error_pub_->publish(velocity_error_msg); + // :( blue::dynamics::Vector6d desired_accel; // NOLINT desired_accel << cmd_.accel.linear.x, cmd_.accel.linear.y, cmd_.accel.linear.z, diff --git a/blue_localization/blue_localization/localizer.py b/blue_localization/blue_localization/localizer.py index fe86c9e1..48ec6dbd 100644 --- a/blue_localization/blue_localization/localizer.py +++ b/blue_localization/blue_localization/localizer.py @@ -312,7 +312,16 @@ def proxy_odom_cb(self, msg: Odometry) -> None: # We only need the pose; we don't need the covariance pose.pose = msg.pose.pose - self.localization_pub.publish(pose) + # Transform the pose into the map_ned frame for ArduSub + to_frame = "map_ned" + + try: + self.localization_pub.publish(self.tf_buffer.transform(pose, to_frame)) + except Exception as e: + self.get_logger().warning( + f"Failed to transform the Gazebo pose from {pose.header.frame_id} to" + f" {to_frame}, {e}" + ) def main_aruco(args: list[str] | None = None): From 3170a72396bbf0f84af0e3ffddddb4383f030b31 Mon Sep 17 00:00:00 2001 From: Evan Palmer Date: Sun, 25 Jun 2023 00:09:01 -0700 Subject: [PATCH 13/16] WIP: Im tired of this --- blue_bringup/config/bluerov2.yaml | 2 +- blue_bringup/config/bluerov2_heavy.yaml | 6 ++-- blue_control/CMakeLists.txt | 1 + blue_control/src/ismc.cpp | 48 +++++++++++++++++++++---- 4 files changed, 46 insertions(+), 11 deletions(-) diff --git a/blue_bringup/config/bluerov2.yaml b/blue_bringup/config/bluerov2.yaml index 93e65856..04571c14 100644 --- a/blue_bringup/config/bluerov2.yaml +++ b/blue_bringup/config/bluerov2.yaml @@ -27,7 +27,7 @@ ismc: msg_ids: [31, 32] msg_rates: [100.0, 100.0] control_rate: 200.0 - proportional_gain: [2.0, 2.0, 2.0, 2.0, 2.0, 2.0] + proportional_gain: [1.0, 1.0, 1.0, 1.0, 1.0, 1.0] integral_gain: [0.01, 0.01, 0.01, 0.01, 0.01, 0.01] sliding_gain: 0.5 boundary_thickness: 0.1 diff --git a/blue_bringup/config/bluerov2_heavy.yaml b/blue_bringup/config/bluerov2_heavy.yaml index cd0504a4..1e8eca49 100644 --- a/blue_bringup/config/bluerov2_heavy.yaml +++ b/blue_bringup/config/bluerov2_heavy.yaml @@ -26,9 +26,9 @@ ismc: msg_ids: [31, 32] msg_rates: [100.0, 100.0] control_rate: 200.0 - proportional_gain: [10.0, 10.0, 10.0, 10.0, 10.0, 10.0] - integral_gain: [3.0, 3.0, 3.0, 3.0, 3.0, 3.0] - sliding_gain: 10.0 + proportional_gain: [1.0, 1.0, 1.0, 1.0, 1.0, 1.0] + integral_gain: [0.01, 0.01, 0.01, 0.01, 0.01, 0.01] + sliding_gain: 1000.0 boundary_thickness: 100.0 use_battery_state: false diff --git a/blue_control/CMakeLists.txt b/blue_control/CMakeLists.txt index e5b363d6..2d6b1fc3 100644 --- a/blue_control/CMakeLists.txt +++ b/blue_control/CMakeLists.txt @@ -27,6 +27,7 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS eigen3_cmake_module Eigen3 tf2_eigen + tf2_geometry_msgs ) foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) diff --git a/blue_control/src/ismc.cpp b/blue_control/src/ismc.cpp index 66c946c6..0c7ac95a 100644 --- a/blue_control/src/ismc.cpp +++ b/blue_control/src/ismc.cpp @@ -24,7 +24,9 @@ #include #include "blue_dynamics/thruster_dynamics.hpp" +#include "tf2/transform_datatypes.h" #include "tf2_eigen/tf2_eigen.hpp" +#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" namespace blue::control { @@ -96,6 +98,9 @@ void ISMC::onArm() cmd_.accel.angular.x, cmd_.accel.angular.y, cmd_.accel.angular.z; initial_acceleration_error_ = accel_error; initial_acceleration_error_ -= accel; + + // !REMOVE + cmd_.twist.linear.x = 1.0; }; void ISMC::onDisarm() @@ -162,7 +167,17 @@ mavros_msgs::msg::OverrideRCIn ISMC::calculateControlInput() hydrodynamics_.restoring_forces.calculateRestoringForces(orientation.toRotationMatrix()) + sliding_gain_ * surface; - // Publish the desired torques to help with debugging and visualization + // Initialize a OverrideRCIn message with no change for the PWM values + mavros_msgs::msg::OverrideRCIn msg; + + // Set all channels to "NOCHANGE" by default + for (uint16_t & channel : msg.channels) { + channel = mavros_msgs::msg::OverrideRCIn::CHAN_NOCHANGE; + } + + // The torques have been calculated for the base_link frame, but we need to transform them + // to the base_link_frd frame for ArduSub. We can just use the wrench message that we are already + // planning to publish to help us do the transform. geometry_msgs::msg::WrenchStamped wrench; wrench.header.frame_id = blue::transforms::kBaseLinkFrameId; wrench.header.stamp = this->get_clock()->now(); @@ -173,15 +188,34 @@ mavros_msgs::msg::OverrideRCIn ISMC::calculateControlInput() wrench.wrench.torque.y = forces[4]; wrench.wrench.torque.z = forces[5]; - desired_wrench_pub_->publish(wrench); - - mavros_msgs::msg::OverrideRCIn msg; + geometry_msgs::msg::TransformStamped transform; - // Set all channels to "NOCHANGE" by default - for (uint16_t & channel : msg.channels) { - channel = mavros_msgs::msg::OverrideRCIn::CHAN_NOCHANGE; + try { + transform = tf_buffer_->lookupTransform( + blue::transforms::kBaseLinkFrdFrameId, blue::transforms::kBaseLinkFrameId, + tf2::TimePointZero); + } + catch (const tf2::TransformException & e) { + RCLCPP_INFO( + this->get_logger(), "Could not transform %s to %s: %s", + blue::transforms::kBaseLinkFrdFrameId.c_str(), blue::transforms::kBaseLinkFrameId.c_str(), + e.what()); + return msg; } + geometry_msgs::msg::WrenchStamped wrench_frd; + + tf2::doTransform(wrench, wrench_frd, transform); + + desired_wrench_pub_->publish(wrench_frd); + + forces[0] = wrench_frd.wrench.force.x; + forces[1] = wrench_frd.wrench.force.y; + forces[2] = wrench_frd.wrench.force.z; + forces[3] = wrench_frd.wrench.torque.x; + forces[4] = wrench_frd.wrench.torque.y; + forces[5] = wrench_frd.wrench.torque.z; + // Multiply the desired forces by the pseudoinverse of the thruster configuration matrix // The size of this vector will depend on the number of thrusters so we don't assign it to a // fixed-size matrix From 283b504e94924af65572f8071921070d043ca546 Mon Sep 17 00:00:00 2001 From: Evan Palmer Date: Mon, 26 Jun 2023 23:13:03 -0700 Subject: [PATCH 14/16] WIP: resolved bugs in transforms and tcm --- .gitignore | 4 + blue_bringup/config/bluerov2_heavy.yaml | 32 +++-- blue_bringup/scripts/arm.sh | 7 - blue_bringup/scripts/disarm.sh | 7 - blue_control/CMakeLists.txt | 3 +- .../include/blue_control/controller.hpp | 29 +--- blue_control/include/blue_control/ismc.hpp | 7 +- blue_control/package.xml | 2 +- blue_control/src/controller.cpp | 43 +++--- blue_control/src/ismc.cpp | 125 ++++++++---------- .../blue_localization/localizer.py | 60 +++++++-- blue_manager/blue_manager/manager.py | 2 +- blue_msgs/CMakeLists.txt | 29 ---- blue_msgs/msg/TwistAccelCmd.msg | 4 - blue_utils/CMakeLists.txt | 68 ++++++++++ {blue_msgs => blue_utils}/LICENSE | 0 .../include/blue_utils}/frames.hpp | 0 blue_utils/include/blue_utils/utils.hpp | 80 +++++++++++ {blue_msgs => blue_utils}/package.xml | 14 +- blue_utils/scripts/arm.sh | 7 + blue_utils/scripts/disarm.sh | 7 + blue_utils/scripts/send_cmd.sh | 5 + blue_utils/scripts/send_pwm.sh | 5 + 23 files changed, 332 insertions(+), 208 deletions(-) delete mode 100755 blue_bringup/scripts/arm.sh delete mode 100755 blue_bringup/scripts/disarm.sh delete mode 100644 blue_msgs/CMakeLists.txt delete mode 100644 blue_msgs/msg/TwistAccelCmd.msg create mode 100644 blue_utils/CMakeLists.txt rename {blue_msgs => blue_utils}/LICENSE (100%) rename {blue_control/include/blue_control => blue_utils/include/blue_utils}/frames.hpp (100%) create mode 100644 blue_utils/include/blue_utils/utils.hpp rename {blue_msgs => blue_utils}/package.xml (66%) create mode 100755 blue_utils/scripts/arm.sh create mode 100755 blue_utils/scripts/disarm.sh create mode 100755 blue_utils/scripts/send_cmd.sh create mode 100755 blue_utils/scripts/send_pwm.sh diff --git a/.gitignore b/.gitignore index 53a92ef5..5f181a1a 100644 --- a/.gitignore +++ b/.gitignore @@ -48,3 +48,7 @@ ros_gz/ # ArduSub Sim eeprom.bin +mav.parm +mav.tlog +mav.tlog.raw +logs/ diff --git a/blue_bringup/config/bluerov2_heavy.yaml b/blue_bringup/config/bluerov2_heavy.yaml index 1e8eca49..1b472baf 100644 --- a/blue_bringup/config/bluerov2_heavy.yaml +++ b/blue_bringup/config/bluerov2_heavy.yaml @@ -6,7 +6,7 @@ blue_manager: ismc: ros__parameters: - mass: 11.5 + mass: 13.5 buoyancy: 112.80 weight: 114.80 inertia_tensor_coeff: [0.16, 0.16, 0.16] @@ -17,19 +17,26 @@ ismc: center_of_buoyancy: [0.0, 0.0, 0.0] ocean_current: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] num_thrusters: 8 - tcm: [ 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.0, 0.0, 0.0, 0.0, 0.218, 0.218, -0.218, -0.218, - 0.0, 0.0, 0.0, 0.0, 0.12, -0.12, 0.12, -0.12, - -0.1888, 0.1888, 0.1888, -0.1888, 0.0, 0.0, 0.0, 0.0] + tcm: [-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.0, -0.0, 0.0, -0.0, -0.21805, 0.21805, -0.21805, 0.21805, + 0.0, -0.0, 0.0, 0.0, -0.12, -0.12, 0.12, 0.12, + 0.1888, -0.1888, -0.1888, 0.1888, 0.0, 0.0, 0.0 , 0.0] + # tcm: [-0.70710678, -0.70710678, 0.70710678, 0.70710678, 0.0, 0.0, 0.0, 0.0, + # -0.70710678, 0.70710678, -0.70710678, 0.70710678, 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.0, -0.0, 0.0, -0.0, -0.21805, 0.21805, -0.21805, 0.21805, + # 0.0, -0.0, 0.0, 0.0, -0.12, -0.12, 0.12, 0.12, + # -0.17005918, 0.17005918, 0.17005918, -0.17005918, 0.0, 0.0, 0.0, 0.0] msg_ids: [31, 32] msg_rates: [100.0, 100.0] control_rate: 200.0 - proportional_gain: [1.0, 1.0, 1.0, 1.0, 1.0, 1.0] - integral_gain: [0.01, 0.01, 0.01, 0.01, 0.01, 0.01] - sliding_gain: 1000.0 - boundary_thickness: 100.0 + proportional_gain: [6.0, 6.0, 6.0, 6.0, 6.0, 6.0] + integral_gain: [0.001, 0.001, 0.001, 0.001, 0.001, 0.001] + derivative_gain: [0.1, 0.1, 0.1, 0.1, 0.1, 0.1] + sliding_gain: 20.0 + boundary_thickness: 200.0 use_battery_state: false aruco_marker_localizer: @@ -86,3 +93,6 @@ mavros/local_position: frame_id: "map" tf: send: false + frame_id: "map" + child_frame_id: "base_link" + send_fcu: false diff --git a/blue_bringup/scripts/arm.sh b/blue_bringup/scripts/arm.sh deleted file mode 100755 index 5b032278..00000000 --- a/blue_bringup/scripts/arm.sh +++ /dev/null @@ -1,7 +0,0 @@ -#!/usr/bin/env bash - -# This is a simple script used to make your life easier when arming a custom controller - -ros2 service call /blue/manager/enable_passthrough std_srvs/srv/SetBool data:\ true\ - -ros2 service call /blue/control/arm std_srvs/srv/SetBool data:\ true\ diff --git a/blue_bringup/scripts/disarm.sh b/blue_bringup/scripts/disarm.sh deleted file mode 100755 index 7953707d..00000000 --- a/blue_bringup/scripts/disarm.sh +++ /dev/null @@ -1,7 +0,0 @@ -#!/usr/bin/env bash - -# This is a simple script used to make your life easier when disarming a custom controller - -ros2 service call /blue/control/arm std_srvs/srv/SetBool data:\ false\ - -ros2 service call /blue/manager/enable_passthrough std_srvs/srv/SetBool data:\ false\ diff --git a/blue_control/CMakeLists.txt b/blue_control/CMakeLists.txt index 2d6b1fc3..eef384b8 100644 --- a/blue_control/CMakeLists.txt +++ b/blue_control/CMakeLists.txt @@ -19,10 +19,11 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS rclcpp ament_cmake blue_dynamics - blue_msgs + blue_utils nav_msgs sensor_msgs mavros_msgs + geometry_msgs std_srvs eigen3_cmake_module Eigen3 diff --git a/blue_control/include/blue_control/controller.hpp b/blue_control/include/blue_control/controller.hpp index 7100abc8..a2fbc5bc 100644 --- a/blue_control/include/blue_control/controller.hpp +++ b/blue_control/include/blue_control/controller.hpp @@ -26,8 +26,8 @@ #include #include -#include "blue_control/frames.hpp" #include "blue_dynamics/hydrodynamics.hpp" +#include "blue_utils/frames.hpp" #include "geometry_msgs/msg/accel.hpp" #include "geometry_msgs/msg/accel_stamped.hpp" #include "geometry_msgs/msg/pose_stamped.hpp" @@ -59,30 +59,6 @@ class Controller : public rclcpp::Node explicit Controller(const std::string & node_name); protected: - /** - * @brief Convert an std::vector into an Eigen::Matrix - * - * @tparam T The type of values held by the vector. - * @tparam major The order to copy over the elements in (e.g., ``Eigen::RowMajor``) - * @param rows The number of rows in the resulting matrix. - * @param cols The number of columns in the resulting matrix. - * @param vec The vector to convert into a matrix. - * @return The converted Eigen matrix. - */ - template - static inline Eigen::Matrix convertVectorToEigenMatrix( - const std::vector & vec, int rows, int cols) - { - // While it would be preferable to define the rows and cols as template parameters, the - // primary use-case for this method within the scope of this implementation is to use it with - // ROS 2 parameters which are not always known at compile time (e.g., TCM). Therefore, the rows - // and cols are made to be function parameters. - typedef const Eigen::Matrix M; - Eigen::Map mat(vec.data(), rows, cols); - - return mat; - } - /** * @brief Function executed when the controller is armed. */ @@ -185,9 +161,6 @@ class Controller : public rclcpp::Node // Manages whether or not control inputs are sent to ArduSub bool armed_; - // Dynamic transforms - geometry_msgs::msg::TransformStamped tf_map_base_; - // TF2 std::unique_ptr tf_listener_; diff --git a/blue_control/include/blue_control/ismc.hpp b/blue_control/include/blue_control/ismc.hpp index 67be1f2a..3e545ac6 100644 --- a/blue_control/include/blue_control/ismc.hpp +++ b/blue_control/include/blue_control/ismc.hpp @@ -24,7 +24,7 @@ #include "blue_control/controller.hpp" #include "blue_dynamics/thruster_dynamics.hpp" -#include "blue_msgs/msg/twist_accel_cmd.hpp" +#include "geometry_msgs/msg/twist.hpp" #include "geometry_msgs/msg/twist_stamped.hpp" #include "geometry_msgs/msg/wrench_stamped.hpp" #include "mavros_msgs/msg/override_rc_in.hpp" @@ -52,6 +52,7 @@ class ISMC : public Controller // ISMC gains blue::dynamics::Matrix6d integral_gain_; blue::dynamics::Matrix6d proportional_gain_; + blue::dynamics::Matrix6d derivative_gain_; double sliding_gain_; double boundary_thickness_; @@ -64,14 +65,14 @@ class ISMC : public Controller bool use_battery_state_; // Reference signal - blue_msgs::msg::TwistAccelCmd cmd_; + geometry_msgs::msg::Twist cmd_; // Publishers rclcpp::Publisher::SharedPtr desired_wrench_pub_; rclcpp::Publisher::SharedPtr velocity_error_pub_; // Subscribers - rclcpp::Subscription::SharedPtr cmd_sub_; + rclcpp::Subscription::SharedPtr cmd_sub_; }; } // namespace blue::control diff --git a/blue_control/package.xml b/blue_control/package.xml index 4f7457fa..9cc622f8 100644 --- a/blue_control/package.xml +++ b/blue_control/package.xml @@ -19,8 +19,8 @@ mavros_msgs rclcpp blue_dynamics + blue_utils eigen - blue_msgs std_srvs tf2_eigen tf2 diff --git a/blue_control/src/controller.cpp b/blue_control/src/controller.cpp index 2dcd5889..1fc31aa8 100644 --- a/blue_control/src/controller.cpp +++ b/blue_control/src/controller.cpp @@ -20,6 +20,8 @@ #include "blue_control/controller.hpp" +#include "blue_utils/utils.hpp" + namespace blue::control { @@ -45,17 +47,21 @@ Controller::Controller(const std::string & node_name) "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( + "frame", std::vector({true, true, false, false, true, false, false, true})); // clang-format off 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.0, 0.0, 0.0, 0.0, 0.218, 0.218, -0.218, -0.218, - 0.0, 0.0, 0.0, 0.0, 0.12, -0.12, 0.12, -0.12, - -0.1888, 0.1888, 0.1888, -0.1888, 0.0, 0.0, 0.0, 0.0})); + "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.0, 0.0, 0.0, 0.0, -0.218, -0.218, 0.218, 0.218, + 0.0, 0.0, 0.0, 0.0, -0.12, 0.12, -0.12, 0.12, + 0.1888, -0.1888, -0.1888, 0.1888, 0.0, 0.0, 0.0, 0.0})); // clang-format on + using blue::utils::convertVectorToEigenMatrix; + // Get hydrodynamic parameters const double mass = this->get_parameter("mass").as_double(); const double buoyancy = this->get_parameter("buoyancy").as_double(); @@ -90,7 +96,7 @@ Controller::Controller(const std::string & node_name) blue::dynamics::CurrentEffects(ocean_current)); // Setup the ROS things - tf_buffer_ = std::make_shared(get_clock()); + tf_buffer_ = std::make_shared(this->get_clock()); tf_listener_ = std::make_unique(*tf_buffer_); tf_broadcaster_ = std::make_unique(this); @@ -107,9 +113,9 @@ Controller::Controller(const std::string & node_name) [this](nav_msgs::msg::Odometry::ConstSharedPtr msg) { updateOdomCb(msg); }); arm_srv_ = this->create_service( - "blue/control/arm", [this]( - const std::shared_ptr request, - std::shared_ptr response) { + "blue/cmd/arm", [this]( + const std::shared_ptr request, + std::shared_ptr response) { armControllerCb(request, response); }); @@ -208,23 +214,8 @@ void Controller::updateOdomCb(nav_msgs::msg::Odometry::ConstSharedPtr msg) accel_pub_->publish(accel_stamped); + // Update the current Odometry reading odom_ = *msg; - - // Publish the map -> base_link transform - tf_map_base_.header.stamp = this->get_clock()->now(); - tf_map_base_.header.frame_id = blue::transforms::kMapFrameId; - tf_map_base_.child_frame_id = blue::transforms::kBaseLinkFrameId; - - tf_map_base_.transform.translation.x = odom_.pose.pose.position.x; - tf_map_base_.transform.translation.y = odom_.pose.pose.position.y; - tf_map_base_.transform.translation.z = odom_.pose.pose.position.z; - - tf_map_base_.transform.rotation.x = odom_.pose.pose.orientation.x; - tf_map_base_.transform.rotation.y = odom_.pose.pose.orientation.y; - tf_map_base_.transform.rotation.z = odom_.pose.pose.orientation.z; - tf_map_base_.transform.rotation.w = odom_.pose.pose.orientation.w; - - tf_broadcaster_->sendTransform(tf_map_base_); } void Controller::setMessageRates( diff --git a/blue_control/src/ismc.cpp b/blue_control/src/ismc.cpp index 0c7ac95a..e0882e2c 100644 --- a/blue_control/src/ismc.cpp +++ b/blue_control/src/ismc.cpp @@ -24,6 +24,7 @@ #include #include "blue_dynamics/thruster_dynamics.hpp" +#include "blue_utils/utils.hpp" #include "tf2/transform_datatypes.h" #include "tf2_eigen/tf2_eigen.hpp" #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" @@ -40,18 +41,22 @@ ISMC::ISMC() // Declare the ROS parameters specific to this controller this->declare_parameter("integral_gain", std::vector({1.0, 1.0, 1.0, 1.0, 1.0, 1.0})); this->declare_parameter("proportional_gain", std::vector({1.0, 1.0, 1.0, 1.0, 1.0, 1.0})); - this->declare_parameter("sliding_gain", 0.0); - this->declare_parameter("boundary_thickness", 0.0); + this->declare_parameter("derivative_gain", std::vector({1.0, 1.0, 1.0, 1.0, 1.0, 1.0})); + this->declare_parameter("sliding_gain", 20.0); + this->declare_parameter("boundary_thickness", 100.0); this->declare_parameter("use_battery_state", false); // Get the gain matrices - Eigen::VectorXd integral_gain_coeff = convertVectorToEigenMatrix( - this->get_parameter("integral_gain").as_double_array(), 6, 1); - Eigen::VectorXd proportional_gain_coeff = convertVectorToEigenMatrix( + Eigen::VectorXd integral_gain_coeff = blue::utils::convertVectorToEigenMatrix( this->get_parameter("integral_gain").as_double_array(), 6, 1); + Eigen::VectorXd proportional_gain_coeff = blue::utils::convertVectorToEigenMatrix( + this->get_parameter("proportional_gain").as_double_array(), 6, 1); + Eigen::VectorXd derivative_gain_coeff = blue::utils::convertVectorToEigenMatrix( + this->get_parameter("derivative_gain").as_double_array(), 6, 1); integral_gain_ = integral_gain_coeff.asDiagonal().toDenseMatrix(); proportional_gain_ = proportional_gain_coeff.asDiagonal().toDenseMatrix(); + derivative_gain_ = derivative_gain_coeff.asDiagonal().toDenseMatrix(); sliding_gain_ = this->get_parameter("sliding_gain").as_double(); boundary_thickness_ = this->get_parameter("boundary_thickness").as_double(); use_battery_state_ = this->get_parameter("use_battery_state").as_bool(); @@ -63,9 +68,9 @@ ISMC::ISMC() this->create_publisher("/blue/ismc/velocity_error", 1); // Update the reference signal when a new command is received - cmd_sub_ = this->create_subscription( - "/blue/ismc/cmd", 1, - [this](blue_msgs::msg::TwistAccelCmd::ConstSharedPtr msg) { cmd_ = *msg; }); + cmd_sub_ = this->create_subscription( + "/blue/ismc/cmd_vel", 1, + [this](geometry_msgs::msg::Twist::ConstSharedPtr msg) { cmd_ = *msg; }); } void ISMC::onArm() @@ -84,23 +89,14 @@ void ISMC::onArm() blue::dynamics::Vector6d velocity; tf2::fromMsg(odom_.twist.twist, velocity); - tf2::fromMsg(cmd_.twist, initial_velocity_error_); + tf2::fromMsg(cmd_, initial_velocity_error_); initial_velocity_error_ -= velocity; // Now calculate the acceleration error i.c. + // Assume that the desired acceleration is 0 blue::dynamics::Vector6d accel; - accel << accel_.linear.x, accel_.linear.y, accel_.linear.z, accel_.angular.x, accel_.angular.y, - accel_.angular.z; - - // There is no suitable tf2_eigen function for Accel types :( - blue::dynamics::Vector6d accel_error; - accel_error << cmd_.accel.linear.x, cmd_.accel.linear.y, cmd_.accel.linear.z, - cmd_.accel.angular.x, cmd_.accel.angular.y, cmd_.accel.angular.z; - initial_acceleration_error_ = accel_error; + blue::utils::fromMsg(accel_, accel); initial_acceleration_error_ -= accel; - - // !REMOVE - cmd_.twist.linear.x = 1.0; }; void ISMC::onDisarm() @@ -120,29 +116,23 @@ mavros_msgs::msg::OverrideRCIn ISMC::calculateControlInput() // Calculate the velocity error blue::dynamics::Vector6d velocity_error; - tf2::fromMsg(cmd_.twist, velocity_error); + tf2::fromMsg(cmd_, velocity_error); velocity_error -= velocity; + // Calculate the acceleration error; assume that the desired acceleration is 0 + blue::dynamics::Vector6d accel_error; + blue::utils::fromMsg(accel_, accel_error); + accel_error *= -1; + // Publish the velocity error to help with debugging geometry_msgs::msg::TwistStamped velocity_error_msg; - velocity_error_msg.header.frame_id = blue::transforms::kBaseLinkFrdFrameId; + velocity_error_msg.header.frame_id = blue::transforms::kBaseLinkFrameId; velocity_error_msg.header.stamp = this->get_clock()->now(); - - velocity_error_msg.twist.linear.x = velocity[0]; - velocity_error_msg.twist.linear.y = velocity[1]; - velocity_error_msg.twist.linear.z = velocity[2]; - velocity_error_msg.twist.angular.x = velocity[3]; - velocity_error_msg.twist.angular.y = velocity[4]; - velocity_error_msg.twist.angular.z = velocity[5]; - + velocity_error_msg.twist = tf2::toMsg(velocity_error); velocity_error_pub_->publish(velocity_error_msg); - // :( - blue::dynamics::Vector6d desired_accel; // 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 (map) frame + // This is effectively the transformation of the orientation to the body (base_link) frame Eigen::Quaterniond orientation; tf2::fromMsg(odom_.pose.pose.orientation, orientation); @@ -152,22 +142,37 @@ mavros_msgs::msg::OverrideRCIn ISMC::calculateControlInput() // Calculate the sliding surface blue::dynamics::Vector6d surface = - proportional_gain_ * velocity_error + integral_gain_ * total_velocity_error_ - - proportional_gain_ * initial_velocity_error_ - initial_acceleration_error_; // NOLINT + proportional_gain_ * velocity_error + integral_gain_ * total_velocity_error_ + + derivative_gain_ * accel_error - proportional_gain_ * initial_velocity_error_ - + initial_acceleration_error_; // Apply the sign function to the surface + // We use the tanh function to help reduce some of the chatter surface = surface.unaryExpr([this](double x) { return tanh(x / boundary_thickness_); }); - // Calculate the desired torques - blue::dynamics::Vector6d forces = + // Calculate the computed torque control + blue::dynamics::Vector6d tau0 = hydrodynamics_.inertia.getInertia() * - (desired_accel + proportional_gain_.inverse() * integral_gain_ * velocity_error) + - hydrodynamics_.coriolis.calculateCoriolis(velocity) * velocity + - hydrodynamics_.damping.calculateDamping(velocity) * velocity + - hydrodynamics_.restoring_forces.calculateRestoringForces(orientation.toRotationMatrix()) + - sliding_gain_ * surface; + (proportional_gain_ * velocity_error + integral_gain_ * total_velocity_error_ + + derivative_gain_ * accel_error) + + (hydrodynamics_.coriolis.calculateCoriolis(velocity) + + hydrodynamics_.damping.calculateDamping(velocity)) * + velocity + + hydrodynamics_.restoring_forces.calculateRestoringForces(orientation.toRotationMatrix()); + + // Calculate the disturbance rejection torque + blue::dynamics::Vector6d tau1 = -sliding_gain_ * surface; + + blue::dynamics::Vector6d forces = tau0 + tau1; - // Initialize a OverrideRCIn message with no change for the PWM values + // Publish the desired wrench for debugging purposes + geometry_msgs::msg::WrenchStamped wrench; + wrench.header.frame_id = blue::transforms::kBaseLinkFrameId; + wrench.header.stamp = this->get_clock()->now(); + wrench.wrench = blue::utils::toMsg(forces); + desired_wrench_pub_->publish(wrench); + + // Initialize an OverrideRCIn message with no change for the PWM values mavros_msgs::msg::OverrideRCIn msg; // Set all channels to "NOCHANGE" by default @@ -176,18 +181,7 @@ mavros_msgs::msg::OverrideRCIn ISMC::calculateControlInput() } // The torques have been calculated for the base_link frame, but we need to transform them - // to the base_link_frd frame for ArduSub. We can just use the wrench message that we are already - // planning to publish to help us do the transform. - geometry_msgs::msg::WrenchStamped wrench; - wrench.header.frame_id = blue::transforms::kBaseLinkFrameId; - wrench.header.stamp = this->get_clock()->now(); - wrench.wrench.force.x = forces[0]; - wrench.wrench.force.y = forces[1]; - wrench.wrench.force.z = forces[2]; - wrench.wrench.torque.x = forces[3]; - wrench.wrench.torque.y = forces[4]; - wrench.wrench.torque.z = forces[5]; - + // to the base_link_frd frame for ArduSub. geometry_msgs::msg::TransformStamped transform; try { @@ -196,25 +190,16 @@ mavros_msgs::msg::OverrideRCIn ISMC::calculateControlInput() tf2::TimePointZero); } catch (const tf2::TransformException & e) { - RCLCPP_INFO( - this->get_logger(), "Could not transform %s to %s: %s", - blue::transforms::kBaseLinkFrdFrameId.c_str(), blue::transforms::kBaseLinkFrameId.c_str(), + RCLCPP_INFO( // NOLINT + this->get_logger(), "Could not transform from %s to %s: %s", + blue::transforms::kBaseLinkFrameId.c_str(), blue::transforms::kBaseLinkFrdFrameId.c_str(), e.what()); return msg; } geometry_msgs::msg::WrenchStamped wrench_frd; - tf2::doTransform(wrench, wrench_frd, transform); - - desired_wrench_pub_->publish(wrench_frd); - - forces[0] = wrench_frd.wrench.force.x; - forces[1] = wrench_frd.wrench.force.y; - forces[2] = wrench_frd.wrench.force.z; - forces[3] = wrench_frd.wrench.torque.x; - forces[4] = wrench_frd.wrench.torque.y; - forces[5] = wrench_frd.wrench.torque.z; + blue::utils::fromMsg(wrench_frd.wrench, forces); // Multiply the desired forces by the pseudoinverse of the thruster configuration matrix // The size of this vector will depend on the number of thrusters so we don't assign it to a diff --git a/blue_localization/blue_localization/localizer.py b/blue_localization/blue_localization/localizer.py index 48ec6dbd..1eb4b412 100644 --- a/blue_localization/blue_localization/localizer.py +++ b/blue_localization/blue_localization/localizer.py @@ -27,18 +27,24 @@ import tf2_geometry_msgs # noqa import tf_transformations as tf from cv_bridge import CvBridge -from geometry_msgs.msg import PoseStamped +from geometry_msgs.msg import Pose, PoseStamped, TransformStamped from nav_msgs.msg import Odometry from rclpy.node import Node from sensor_msgs.msg import Image from tf2_ros import TransformException from tf2_ros.buffer import Buffer +from tf2_ros.transform_broadcaster import TransformBroadcaster from tf2_ros.transform_listener import TransformListener class Localizer(Node, ABC): """Base class for implementing a visual localization interface.""" + MAP_FRAME = "map" + MAP_NED_FRAME = "map_ned" + BASE_LINK_FRAME = "base_link" + BASE_LINK_FRD_FRAME = "base_link_frd" + def __init__(self, node_name: str) -> None: """Create a new localizer. @@ -51,11 +57,45 @@ def __init__(self, node_name: str) -> None: # Provide access to TF2 self.tf_buffer = Buffer() self.tf_listener = TransformListener(self.tf_buffer, self) + self.tf_broadcaster = TransformBroadcaster(self, 1) # Poses are sent to the ArduPilot EKF self.localization_pub = self.create_publisher( PoseStamped, "/mavros/vision_pose/pose", 1 ) + self.odometry_pub = self.create_publisher(Odometry, "/mavros/odometry/out", 1) + + @staticmethod + def convert_pose_to_transform( + node: Node, pose: Pose, reference_frame: str, child_frame: str + ) -> TransformStamped: + """Convert a Pose message into a TransformStamped. + + Args: + node: The ROS 2 node that is calling the method. + pose: The Pose message to convert into a TransformStamped message. + reference_frame: The frame that the TransformStamped transforms from. + child_frame: The frame that the TrasnformStamped transforms to. + + Returns: + The converted TransformStamped message. + """ + tf = TransformStamped() + + tf.header.stamp = node.get_clock().now().to_msg() + tf.header.frame_id = reference_frame + tf.child_frame_id = child_frame + + tf.transform.translation.x = pose.position.x + tf.transform.translation.y = pose.position.y + tf.transform.translation.z = pose.position.z + + tf.transform.rotation.x = pose.orientation.x + tf.transform.rotation.y = pose.orientation.y + tf.transform.rotation.z = pose.orientation.z + tf.transform.rotation.w = pose.orientation.w + + return tf class ArucoMarkerLocalizer(Localizer): @@ -303,6 +343,13 @@ def proxy_odom_cb(self, msg: Odometry) -> None: Args: msg: The Gazebo ground-truth odometry for the BlueROV2. """ + # Use the Odometry message to publish the transform from the map frame to the + # base_link frame + tf_map_base = Localizer.convert_pose_to_transform( + self, msg.pose.pose, self.MAP_FRAME, self.BASE_LINK_FRAME + ) + self.tf_broadcaster.sendTransform(tf_map_base) + pose = PoseStamped() # Pose is provided in the parent header frame @@ -312,16 +359,7 @@ def proxy_odom_cb(self, msg: Odometry) -> None: # We only need the pose; we don't need the covariance pose.pose = msg.pose.pose - # Transform the pose into the map_ned frame for ArduSub - to_frame = "map_ned" - - try: - self.localization_pub.publish(self.tf_buffer.transform(pose, to_frame)) - except Exception as e: - self.get_logger().warning( - f"Failed to transform the Gazebo pose from {pose.header.frame_id} to" - f" {to_frame}, {e}" - ) + self.localization_pub.publish(pose) def main_aruco(args: list[str] | None = None): diff --git a/blue_manager/blue_manager/manager.py b/blue_manager/blue_manager/manager.py index 909750a5..2275ae31 100644 --- a/blue_manager/blue_manager/manager.py +++ b/blue_manager/blue_manager/manager.py @@ -68,7 +68,7 @@ def __init__(self) -> None: # Services self.set_pwm_passthrough_srv = self.create_service( SetBool, - "/blue/manager/enable_passthrough", + "/blue/cmd/enable_passthrough", self.set_rc_passthrough_mode_cb, callback_group=reentrant_callback_group, ) diff --git a/blue_msgs/CMakeLists.txt b/blue_msgs/CMakeLists.txt deleted file mode 100644 index 9c981621..00000000 --- a/blue_msgs/CMakeLists.txt +++ /dev/null @@ -1,29 +0,0 @@ -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/TwistAccelCmd.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/msg/TwistAccelCmd.msg b/blue_msgs/msg/TwistAccelCmd.msg deleted file mode 100644 index c5d43ca8..00000000 --- a/blue_msgs/msg/TwistAccelCmd.msg +++ /dev/null @@ -1,4 +0,0 @@ -# Defines a desired twist and acceleration command. -std_msgs/Header header -geometry_msgs/Twist twist -geometry_msgs/Accel accel diff --git a/blue_utils/CMakeLists.txt b/blue_utils/CMakeLists.txt new file mode 100644 index 00000000..6ab6b4b1 --- /dev/null +++ b/blue_utils/CMakeLists.txt @@ -0,0 +1,68 @@ +cmake_minimum_required(VERSION 3.8) +project(blue_utils) + +# 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 + geometry_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} INTERFACE) + +target_link_libraries(${PROJECT_NAME} INTERFACE ${rclcpp_LIBRARIES}) +target_include_directories(${PROJECT_NAME} INTERFACE) +ament_target_dependencies(${PROJECT_NAME} INTERFACE ${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) + + # 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_export_targets(${PROJECT_NAME} HAS_LIBRARY_TARGET) +ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS}) + +ament_package() diff --git a/blue_msgs/LICENSE b/blue_utils/LICENSE similarity index 100% rename from blue_msgs/LICENSE rename to blue_utils/LICENSE diff --git a/blue_control/include/blue_control/frames.hpp b/blue_utils/include/blue_utils/frames.hpp similarity index 100% rename from blue_control/include/blue_control/frames.hpp rename to blue_utils/include/blue_utils/frames.hpp diff --git a/blue_utils/include/blue_utils/utils.hpp b/blue_utils/include/blue_utils/utils.hpp new file mode 100644 index 00000000..15f63261 --- /dev/null +++ b/blue_utils/include/blue_utils/utils.hpp @@ -0,0 +1,80 @@ +// 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 "blue_dynamics/hydrodynamics.hpp" +#include "geometry_msgs/msg/accel.hpp" +#include "geometry_msgs/msg/wrench.hpp" + +namespace blue::utils +{ + +/** + * @brief Convert an std::vector into an Eigen::Matrix + * + * @tparam T The type of values held by the vector. + * @tparam major The order to copy over the elements in (e.g., ``Eigen::RowMajor``) + * @param rows The number of rows in the resulting matrix. + * @param cols The number of columns in the resulting matrix. + * @param vec The vector to convert into a matrix. + * @return The converted Eigen matrix. + */ +template +inline Eigen::Matrix convertVectorToEigenMatrix( + const std::vector & vec, int rows, int cols) +{ + // While it would be preferable to define the rows and cols as template parameters, the + // primary use-case for this method within the scope of this implementation is to use it with + // ROS 2 parameters which are not always known at compile time (e.g., TCM). Therefore, the rows + // and cols are made to be function parameters. + typedef const Eigen::Matrix M; + Eigen::Map mat(vec.data(), rows, cols); + + return mat; +} + +inline void fromMsg(const geometry_msgs::msg::Accel & in, blue::dynamics::Vector6d & out) +{ + blue::dynamics::Vector6d v; + v << in.linear.x, in.linear.y, in.linear.z, in.angular.x, in.angular.y, in.angular.z; + out = v; +} + +inline void fromMsg(const geometry_msgs::msg::Wrench & in, blue::dynamics::Vector6d & out) +{ + blue::dynamics::Vector6d v; + v << in.force.x, in.force.y, in.force.z, in.torque.x, in.torque.y, in.torque.z; + out = v; +} + +inline geometry_msgs::msg::Wrench toMsg(const blue::dynamics::Vector6d & in) +{ + geometry_msgs::msg::Wrench msg; + msg.force.x = in[0]; + msg.force.y = in[1]; + msg.force.z = in[2]; + msg.torque.x = in[3]; + msg.torque.y = in[4]; + msg.torque.z = in[5]; + return msg; +} + +} // namespace blue::utils diff --git a/blue_msgs/package.xml b/blue_utils/package.xml similarity index 66% rename from blue_msgs/package.xml rename to blue_utils/package.xml index e3baa42a..c766fe05 100644 --- a/blue_msgs/package.xml +++ b/blue_utils/package.xml @@ -2,9 +2,9 @@ - blue_msgs + blue_utils 0.0.1 - Custom messages available for use by the controllers + Common helper functions. Evan Palmer MIT @@ -14,18 +14,14 @@ Evan Palmer + blue_dynamics + eigen + ament_cmake ament_lint_auto ament_lint_common - std_msgs - geometry_msgs - - rosidl_default_generators - rosidl_default_runtime - rosidl_interface_packages - ament_cmake diff --git a/blue_utils/scripts/arm.sh b/blue_utils/scripts/arm.sh new file mode 100755 index 00000000..ace7be04 --- /dev/null +++ b/blue_utils/scripts/arm.sh @@ -0,0 +1,7 @@ +#!/usr/bin/env bash + +# This is a simple script used to make your life easier when arming a custom controller + +ros2 service call /blue/cmd/enable_passthrough std_srvs/srv/SetBool data:\ true\ + +ros2 service call /blue/cmd/arm std_srvs/srv/SetBool data:\ true\ diff --git a/blue_utils/scripts/disarm.sh b/blue_utils/scripts/disarm.sh new file mode 100755 index 00000000..778dd254 --- /dev/null +++ b/blue_utils/scripts/disarm.sh @@ -0,0 +1,7 @@ +#!/usr/bin/env bash + +# This is a simple script used to make your life easier when disarming a custom controller + +ros2 service call /blue/cmd/arm std_srvs/srv/SetBool data:\ false\ + +ros2 service call /blue/cmd/enable_passthrough std_srvs/srv/SetBool data:\ false\ diff --git a/blue_utils/scripts/send_cmd.sh b/blue_utils/scripts/send_cmd.sh new file mode 100755 index 00000000..34d782d4 --- /dev/null +++ b/blue_utils/scripts/send_cmd.sh @@ -0,0 +1,5 @@ +#!/usr/bin/env bash + +# Send a velocity command to the BlueROV2 + +ros2 topic pub /blue/ismc/cmd_vel geometry_msgs/msg/Twist '{linear: {x: 0.2, y: 0.0, z: 0.2}, angular: {x: 0.0, y: 0.0, z: 0.0}}' diff --git a/blue_utils/scripts/send_pwm.sh b/blue_utils/scripts/send_pwm.sh new file mode 100755 index 00000000..a982b596 --- /dev/null +++ b/blue_utils/scripts/send_pwm.sh @@ -0,0 +1,5 @@ +#!/usr/bin/env bash + +# Send a PWM command to the BlueROV2 + +ros2 topic pub /mavros/rc/override mavros_msgs/msg/OverrideRCIn "{channels: [0, 0, 0, 0, 1800, 1100, 1100, 1800, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]}" From 760d81819955d5c6f794d690d98894c0413023c1 Mon Sep 17 00:00:00 2001 From: Evan Palmer Date: Tue, 27 Jun 2023 19:31:35 -0700 Subject: [PATCH 15/16] Final fixes for pr and added keyboard teleop --- blue_bringup/config/bluerov2.yaml | 19 ++++++++++--------- blue_bringup/config/bluerov2_heavy.yaml | 10 ++-------- blue_bringup/launch/bluerov2.launch.py | 2 +- blue_bringup/launch/bluerov2_heavy.launch.py | 2 +- blue_bringup/package.xml | 1 + blue_control/package.xml | 2 -- blue_utils/scripts/send_cmd.sh | 2 +- 7 files changed, 16 insertions(+), 22 deletions(-) diff --git a/blue_bringup/config/bluerov2.yaml b/blue_bringup/config/bluerov2.yaml index 04571c14..c1c371c9 100644 --- a/blue_bringup/config/bluerov2.yaml +++ b/blue_bringup/config/bluerov2.yaml @@ -18,19 +18,20 @@ ismc: center_of_buoyancy: [0.0, 0.0, 0.2] ocean_current: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] num_thrusters: 6 - tcm: [ 0.707, 0.707, -0.707, -0.707, 0.0, 0.0, - -0.707, 0.707, -0.707, 0.707, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, -1.0, 1.0, - 0.0, 0.0, 0.0, 0.0, 0.218, 0.218, - 0.0, 0.0, 0.0, 0.0, 0.12, -0.12, - -0.1888, 0.1888, 0.1888, -0.1888, 0.0, 0.0] + tcm: [ -0.707, -0.707, 0.707, 0.707, 0.0, 0.0, + 0.707, -0.707, 0.707, -0.707, 0.0, 0.0, + 0.0, 0.0, -0.0, -0.0, 1.0, 1.0, + 0.0, -0.0, 0.0, -0.0, -0.21805, 0.21805, + 0.0, -0.0, 0.0, 0.0, -0.12, -0.12, + -0.1888, 0.1888, 0.1888, -0.1888, 0.0, 0.0] msg_ids: [31, 32] msg_rates: [100.0, 100.0] control_rate: 200.0 - proportional_gain: [1.0, 1.0, 1.0, 1.0, 1.0, 1.0] + proportional_gain: [10.0, 10.0, 6.0, 6.0, 6.0, 10.0] integral_gain: [0.01, 0.01, 0.01, 0.01, 0.01, 0.01] - sliding_gain: 0.5 - boundary_thickness: 0.1 + derivative_gain: [0.1, 0.1, 0.1, 0.1, 0.1, 0.1] + sliding_gain: 20.0 + boundary_thickness: 200.0 use_battery_state: false # Localization sources diff --git a/blue_bringup/config/bluerov2_heavy.yaml b/blue_bringup/config/bluerov2_heavy.yaml index 1b472baf..4a10cd78 100644 --- a/blue_bringup/config/bluerov2_heavy.yaml +++ b/blue_bringup/config/bluerov2_heavy.yaml @@ -23,17 +23,11 @@ ismc: 0.0, -0.0, 0.0, -0.0, -0.21805, 0.21805, -0.21805, 0.21805, 0.0, -0.0, 0.0, 0.0, -0.12, -0.12, 0.12, 0.12, 0.1888, -0.1888, -0.1888, 0.1888, 0.0, 0.0, 0.0 , 0.0] - # tcm: [-0.70710678, -0.70710678, 0.70710678, 0.70710678, 0.0, 0.0, 0.0, 0.0, - # -0.70710678, 0.70710678, -0.70710678, 0.70710678, 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.0, -0.0, 0.0, -0.0, -0.21805, 0.21805, -0.21805, 0.21805, - # 0.0, -0.0, 0.0, 0.0, -0.12, -0.12, 0.12, 0.12, - # -0.17005918, 0.17005918, 0.17005918, -0.17005918, 0.0, 0.0, 0.0, 0.0] msg_ids: [31, 32] msg_rates: [100.0, 100.0] control_rate: 200.0 - proportional_gain: [6.0, 6.0, 6.0, 6.0, 6.0, 6.0] - integral_gain: [0.001, 0.001, 0.001, 0.001, 0.001, 0.001] + proportional_gain: [10.0, 10.0, 6.0, 6.0, 6.0, 10.0] + integral_gain: [0.01, 0.01, 0.01, 0.01, 0.01, 0.01] derivative_gain: [0.1, 0.1, 0.1, 0.1, 0.1, 0.1] sliding_gain: 20.0 boundary_thickness: 200.0 diff --git a/blue_bringup/launch/bluerov2.launch.py b/blue_bringup/launch/bluerov2.launch.py index 90c2a6a5..f390e19c 100644 --- a/blue_bringup/launch/bluerov2.launch.py +++ b/blue_bringup/launch/bluerov2.launch.py @@ -78,7 +78,7 @@ def generate_launch_description() -> LaunchDescription: DeclareLaunchArgument( "use_sim", default_value="false", - description="Automatically start Gazebo.", + description="Launch the Gazebo + ArduSub simulator.", ), DeclareLaunchArgument( "use_foxglove", diff --git a/blue_bringup/launch/bluerov2_heavy.launch.py b/blue_bringup/launch/bluerov2_heavy.launch.py index 35b4c435..1b7e06d8 100644 --- a/blue_bringup/launch/bluerov2_heavy.launch.py +++ b/blue_bringup/launch/bluerov2_heavy.launch.py @@ -78,7 +78,7 @@ def generate_launch_description() -> LaunchDescription: DeclareLaunchArgument( "use_sim", default_value="false", - description="Automatically start Gazebo.", + description="Launch the Gazebo + ArduSub simulator.", ), DeclareLaunchArgument( "use_foxglove", diff --git a/blue_bringup/package.xml b/blue_bringup/package.xml index 2096e94c..c68ab0b2 100644 --- a/blue_bringup/package.xml +++ b/blue_bringup/package.xml @@ -18,6 +18,7 @@ mavros_extras python3-matplotlib foxglove_bridge + teleop_twist_keyboard ament_copyright ament_flake8 diff --git a/blue_control/package.xml b/blue_control/package.xml index 9cc622f8..931e2144 100644 --- a/blue_control/package.xml +++ b/blue_control/package.xml @@ -18,8 +18,6 @@ sensor_msgs mavros_msgs rclcpp - blue_dynamics - blue_utils eigen std_srvs tf2_eigen diff --git a/blue_utils/scripts/send_cmd.sh b/blue_utils/scripts/send_cmd.sh index 34d782d4..643409fd 100755 --- a/blue_utils/scripts/send_cmd.sh +++ b/blue_utils/scripts/send_cmd.sh @@ -2,4 +2,4 @@ # Send a velocity command to the BlueROV2 -ros2 topic pub /blue/ismc/cmd_vel geometry_msgs/msg/Twist '{linear: {x: 0.2, y: 0.0, z: 0.2}, angular: {x: 0.0, y: 0.0, z: 0.0}}' +ros2 topic pub /blue/ismc/cmd_vel geometry_msgs/msg/Twist '{linear: {x: 0.0, y: 0.0, z: 0.0}, angular: {x: 0.0, y: 0.0, z: 0.0}}' From 25788da004b92c6f8b075c9f6057835b64567dc0 Mon Sep 17 00:00:00 2001 From: Evan Palmer Date: Tue, 27 Jun 2023 22:19:18 -0700 Subject: [PATCH 16/16] Resolved PR comments --- blue_utils/include/blue_utils/utils.hpp | 18 ++++++++++++++++++ blue_utils/package.xml | 2 +- blue_utils/scripts/send_pwm.sh | 2 +- 3 files changed, 20 insertions(+), 2 deletions(-) diff --git a/blue_utils/include/blue_utils/utils.hpp b/blue_utils/include/blue_utils/utils.hpp index 15f63261..de957878 100644 --- a/blue_utils/include/blue_utils/utils.hpp +++ b/blue_utils/include/blue_utils/utils.hpp @@ -51,6 +51,12 @@ inline Eigen::Matrix convertVectorToEigenMatr return mat; } +/** + * @brief Convert a geometry_msgs::msg::Accel message into an Eigen vector. + * + * @param in The Accel message to convert. + * @param out The Eigen vector that should be populated with the Accel data. + */ inline void fromMsg(const geometry_msgs::msg::Accel & in, blue::dynamics::Vector6d & out) { blue::dynamics::Vector6d v; @@ -58,6 +64,12 @@ inline void fromMsg(const geometry_msgs::msg::Accel & in, blue::dynamics::Vector out = v; } +/** + * @brief Convert a geometry_msgs::msg::Wrench into an Eigen vector. + * + * @param in The Wrench message to convert. + * @param out The Eigen vector that should be populated with the Wrench data. + */ inline void fromMsg(const geometry_msgs::msg::Wrench & in, blue::dynamics::Vector6d & out) { blue::dynamics::Vector6d v; @@ -65,6 +77,12 @@ inline void fromMsg(const geometry_msgs::msg::Wrench & in, blue::dynamics::Vecto out = v; } +/** + * @brief Convert an Eigen vector into a geometry_msgs::msg::Wrench message. + * + * @param in The Eigen vector to convert into a Wrench message. + * @return geometry_msgs::msg::Wrench + */ inline geometry_msgs::msg::Wrench toMsg(const blue::dynamics::Vector6d & in) { geometry_msgs::msg::Wrench msg; diff --git a/blue_utils/package.xml b/blue_utils/package.xml index c766fe05..f598fb6f 100644 --- a/blue_utils/package.xml +++ b/blue_utils/package.xml @@ -4,7 +4,7 @@ blue_utils 0.0.1 - Common helper functions. + Common helper functions and constants Evan Palmer MIT diff --git a/blue_utils/scripts/send_pwm.sh b/blue_utils/scripts/send_pwm.sh index a982b596..39576ce2 100755 --- a/blue_utils/scripts/send_pwm.sh +++ b/blue_utils/scripts/send_pwm.sh @@ -2,4 +2,4 @@ # Send a PWM command to the BlueROV2 -ros2 topic pub /mavros/rc/override mavros_msgs/msg/OverrideRCIn "{channels: [0, 0, 0, 0, 1800, 1100, 1100, 1800, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]}" +ros2 topic pub /mavros/rc/override mavros_msgs/msg/OverrideRCIn "{channels: [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]}"