From b5fcbadc3922974889ef6f459a456c49229d6a62 Mon Sep 17 00:00:00 2001 From: Evan Palmer Date: Mon, 27 Mar 2023 23:57:10 -0700 Subject: [PATCH 01/34] started porting bridge to base controller --- blue_control/CMakeLists.txt | 55 +++++++++++++++ blue_control/LICENSE | 17 +++++ .../include/blue_control/base_controller.hpp | 50 ++++++++++++++ blue_control/package.xml | 26 +++++++ blue_control/src/base_controller.cpp | 68 +++++++++++++++++++ 5 files changed, 216 insertions(+) create mode 100644 blue_control/CMakeLists.txt create mode 100644 blue_control/LICENSE create mode 100644 blue_control/include/blue_control/base_controller.hpp create mode 100644 blue_control/package.xml create mode 100644 blue_control/src/base_controller.cpp diff --git a/blue_control/CMakeLists.txt b/blue_control/CMakeLists.txt new file mode 100644 index 00000000..1b1da5df --- /dev/null +++ b/blue_control/CMakeLists.txt @@ -0,0 +1,55 @@ +cmake_minimum_required(VERSION 3.8) +project(blue_control) + +# Default to C99 +if(NOT CMAKE_C_STANDARD) + set(CMAKE_C_STANDARD 99) +endif() + +# Default to C++ 17 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 17) +endif() + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +set(THIS_PACKAGE_INCLUDE_DEPENDS + rclcpp + ament_cmake + mavros_msgs + std_msgs + std_srvs +) + +foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) + find_package(${Dependency} REQUIRED) +endforeach() + +include_directories(include) + +add_executable(${PROJECT_NAME} src/bridge.cpp) +ament_target_dependencies(${PROJECT_NAME} ${THIS_PACKAGE_INCLUDE_DEPENDS}) + +install( + TARGETS ${PROJECT_NAME} + DESTINATION lib/${PROJECT_NAME} +) + +install( + DIRECTORY include/ + DESTINATION include +) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + + # Run linters found in package.xml except those below + set(ament_cmake_uncrustify_FOUND TRUE) + + ament_lint_auto_find_test_dependencies() +endif() + +ament_package() + diff --git a/blue_control/LICENSE b/blue_control/LICENSE new file mode 100644 index 00000000..30e8e2ec --- /dev/null +++ b/blue_control/LICENSE @@ -0,0 +1,17 @@ +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: + +The above copyright notice and this permission notice shall be included in +all copies or substantial portions of the Software. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL +THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +THE SOFTWARE. diff --git a/blue_control/include/blue_control/base_controller.hpp b/blue_control/include/blue_control/base_controller.hpp new file mode 100644 index 00000000..a509f136 --- /dev/null +++ b/blue_control/include/blue_control/base_controller.hpp @@ -0,0 +1,50 @@ +// 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 "mavros_msgs/msg/override_rc_in.hpp" +#include "rclcpp/rclcpp.hpp" +#include "std_srvs/srv/set_bool.hpp" + +namespace blue_control +{ + +class BaseController : public rclcpp::Node +{ +public: + virtual ~BaseController() = default; // NOLINT + +protected: + BaseController(); + +private: + void publishOverrideRCIn() const; + void enableOverride( + std::shared_ptr request, + std::shared_ptr response); + + bool override_enabled_; + + mavros_msgs::msg::OverrideRCIn current_pwm_values_; + rclcpp::Publisher::SharedPtr rc_override_publisher_; + rclcpp::Service::SharedPtr enable_override_service_; + rclcpp::TimerBase::SharedPtr timer_; +}; + +} // namespace blue_control diff --git a/blue_control/package.xml b/blue_control/package.xml new file mode 100644 index 00000000..e975c64b --- /dev/null +++ b/blue_control/package.xml @@ -0,0 +1,26 @@ + + + + blue_control + 0.0.0 + TODO: Package description + Evan Palmer + MIT + + ament_cmake + + mavros + mavros_extras + + std_msgs + std_srvs + mavros_msgs + geographic_msgs + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/blue_control/src/base_controller.cpp b/blue_control/src/base_controller.cpp new file mode 100644 index 00000000..1cb4fafd --- /dev/null +++ b/blue_control/src/base_controller.cpp @@ -0,0 +1,68 @@ +// Copyright 2023, Evan Palmer +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: + +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. + +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL +// THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +// THE SOFTWARE. + +#include "blue_control/base_controller.hpp" + +#include +#include + +#include "rclcpp/rclcpp.hpp" + +using namespace std::chrono_literals; + +namespace blue_control +{ + +BaseController::BaseController() +: Node("blue_control") +{ + // Create a publisher to publish the current desired RC values + rc_override_publisher_ = + this->create_publisher("/mavros/rc/override", 1); + + // Start a timer to publish the current desired PWM values at a frequency of 50hz + timer_ = create_wall_timer(20ms, std::bind(&Bridge::publishOverrideRCIn, this)); + + // Set up a service to manage whether or not the RC values will be overridden + enable_override_service_ = this->create_service( + "/blue_control/rc/override/enable", + std::bind(&Bridge::enableOverride, this, std::placeholders::_1, std::placeholders::_2)); +} + +void Bridge::publishOverrideRCIn() const +{ + // Only publish the override values if the bridge has been enabled + if (override_enabled_) { + rc_override_publisher_->publish(current_pwm_values_); + } +} + +void Bridge::enableOverride( + const std::shared_ptr request, // NOLINT + std::shared_ptr response) // NOLINT +{ + // Enable/disable publishing the override messages + override_enabled_ = request->data; + + // Set the response according to whether or not the update was done properly + response->success = (override_enabled_ == request->data); +} + +} // namespace blue_control From 8586481f44d240aca40c98d794e1e2e6a36233d9 Mon Sep 17 00:00:00 2001 From: Evan Palmer Date: Tue, 28 Mar 2023 23:58:10 -0700 Subject: [PATCH 02/34] continued base controller implementation --- blue_bridge/CMakeLists.txt | 54 ----------- blue_bridge/src/bridge.cpp | 90 ------------------- blue_control/CMakeLists.txt | 15 +--- .../include/blue_control/base_controller.hpp | 34 +++++-- .../include/blue_control/dynamic_model.hpp | 28 ++---- blue_control/package.xml | 1 + blue_control/src/base_controller.cpp | 25 +++--- blue_control/src/dynamic_model.cpp | 24 +++++ blue_utils/CMakeLists.txt | 26 ++++++ {blue_bridge => blue_utils}/LICENSE | 0 blue_utils/include/blue_utils/pwm.hpp | 0 {blue_bridge => blue_utils}/package.xml | 14 ++- blue_utils/src/pwm.cpp | 0 13 files changed, 103 insertions(+), 208 deletions(-) delete mode 100644 blue_bridge/CMakeLists.txt delete mode 100644 blue_bridge/src/bridge.cpp rename blue_bridge/include/blue_bridge/bridge.hpp => blue_control/include/blue_control/dynamic_model.hpp (55%) create mode 100644 blue_control/src/dynamic_model.cpp create mode 100644 blue_utils/CMakeLists.txt rename {blue_bridge => blue_utils}/LICENSE (100%) create mode 100644 blue_utils/include/blue_utils/pwm.hpp rename {blue_bridge => blue_utils}/package.xml (52%) create mode 100644 blue_utils/src/pwm.cpp diff --git a/blue_bridge/CMakeLists.txt b/blue_bridge/CMakeLists.txt deleted file mode 100644 index 7e9ad530..00000000 --- a/blue_bridge/CMakeLists.txt +++ /dev/null @@ -1,54 +0,0 @@ -cmake_minimum_required(VERSION 3.8) -project(blue_bridge) - -# 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 - mavros_msgs - std_msgs - std_srvs -) - -foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) - find_package(${Dependency} REQUIRED) -endforeach() - -include_directories(include) - -add_executable(${PROJECT_NAME} src/bridge.cpp) -ament_target_dependencies(${PROJECT_NAME} ${THIS_PACKAGE_INCLUDE_DEPENDS}) - -install( - TARGETS ${PROJECT_NAME} - DESTINATION lib/${PROJECT_NAME} -) - -install( - DIRECTORY include/ - DESTINATION include -) - -if(BUILD_TESTING) - find_package(ament_lint_auto REQUIRED) - - # Run linters found in package.xml except those below - set(ament_cmake_uncrustify_FOUND TRUE) - - ament_lint_auto_find_test_dependencies() -endif() - -ament_package() diff --git a/blue_bridge/src/bridge.cpp b/blue_bridge/src/bridge.cpp deleted file mode 100644 index 15d34521..00000000 --- a/blue_bridge/src/bridge.cpp +++ /dev/null @@ -1,90 +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. - -#include "blue_bridge/bridge.hpp" - -#include -#include - -#include "rclcpp/rclcpp.hpp" - -using namespace std::chrono_literals; - -namespace blue_bridge -{ - -Bridge::Bridge() -: Node("blue_bridge"), - bridge_running_(false) -{ - // Create a subscription to get the desired PWM values - rc_override_subscription_ = this->create_subscription( - "/blue_bridge/rc/override", 1, - std::bind(&Bridge::updateCurrentPwmValues, this, std::placeholders::_1)); - - // Create a publisher to publish the current desired RC values - rc_override_publisher_ = - this->create_publisher("/mavros/rc/override", 1); - - // Start a timer to publish the current desired PWM values at a frequency of 50hz - timer_ = create_wall_timer(20ms, std::bind(&Bridge::publishOverrideRCIn, this)); - - // Set up a service to manage control - enable_override_service_ = this->create_service( - "/blue_bridge/rc/override/enable", - std::bind(&Bridge::enableOverride, this, std::placeholders::_1, std::placeholders::_2)); -} - -bool Bridge::active() const { return bridge_running_; } - -void Bridge::publishOverrideRCIn() const -{ - // Only publish the override values if the bridge has been enabled - if (active()) { - rc_override_publisher_->publish(current_pwm_values_); - } -} - -void Bridge::updateCurrentPwmValues(const mavros_msgs::msg::OverrideRCIn & desired_pwm_values_msg) -{ - // Update the desired RC override values - current_pwm_values_ = desired_pwm_values_msg; -} - -void Bridge::enableOverride( - const std::shared_ptr request, // NOLINT - std::shared_ptr response) // NOLINT -{ - // Enable/disable publishing the override messages - bridge_running_ = request->data; - - // Set the response according to whether or not the update was done properly - response->success = (bridge_running_ == request->data); -} - -} // namespace blue_bridge - -int main(int argc, char ** argv) -{ - rclcpp::init(argc, argv); - rclcpp::spin(std::make_shared()); - rclcpp::shutdown(); - return 0; -} diff --git a/blue_control/CMakeLists.txt b/blue_control/CMakeLists.txt index 1b1da5df..c019742b 100644 --- a/blue_control/CMakeLists.txt +++ b/blue_control/CMakeLists.txt @@ -21,26 +21,13 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS mavros_msgs std_msgs std_srvs + geometry_msgs ) foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) find_package(${Dependency} REQUIRED) endforeach() -include_directories(include) - -add_executable(${PROJECT_NAME} src/bridge.cpp) -ament_target_dependencies(${PROJECT_NAME} ${THIS_PACKAGE_INCLUDE_DEPENDS}) - -install( - TARGETS ${PROJECT_NAME} - DESTINATION lib/${PROJECT_NAME} -) - -install( - DIRECTORY include/ - DESTINATION include -) if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) diff --git a/blue_control/include/blue_control/base_controller.hpp b/blue_control/include/blue_control/base_controller.hpp index a509f136..44a97f44 100644 --- a/blue_control/include/blue_control/base_controller.hpp +++ b/blue_control/include/blue_control/base_controller.hpp @@ -18,11 +18,14 @@ // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN // THE SOFTWARE. +#include "geometry_msgs/msg/pose_stamped.hpp" #include "mavros_msgs/msg/override_rc_in.hpp" +#include "mavros_msgs/msg/state.hpp" +#include "nav_msgs/msg/odometry.hpp" #include "rclcpp/rclcpp.hpp" #include "std_srvs/srv/set_bool.hpp" -namespace blue_control +namespace blue::control { class BaseController : public rclcpp::Node @@ -30,21 +33,40 @@ class BaseController : public rclcpp::Node public: virtual ~BaseController() = default; // NOLINT + // TODO(evan-palmer): move these to a library + double convertVoltageToPwm(double voltage); + double convertForceToPwm(double force); + + void setControlSignal(const mavros_msgs::msg::OverrideRCIn & control_input); + protected: - BaseController(); + BaseController(const std::string & node_name, const rclcpp::NodeOptions & options); private: - void publishOverrideRCIn() const; + void publishRC() const; void enableOverride( std::shared_ptr request, std::shared_ptr response); + // BlueROV2 state bool override_enabled_; + mavros_msgs::msg::OverrideRCIn control_signal_; + geometry_msgs::msg::PoseStamped pose_; + + // Subscriptions + rclcpp::Subscription::SharedPtr state_sub_; + rclcpp::Subscription::SharedPtr pose_sub_; + rclcpp::Subscription::SharedPtr slam_pose_sub_; - mavros_msgs::msg::OverrideRCIn current_pwm_values_; - rclcpp::Publisher::SharedPtr rc_override_publisher_; + // Publishers + rclcpp::Publisher::SharedPtr rc_override_pub_; + rclcpp::Publisher::SharedPtr ext_nav_pub_; + + // Services rclcpp::Service::SharedPtr enable_override_service_; + + // PWM timer rclcpp::TimerBase::SharedPtr timer_; }; -} // namespace blue_control +} // namespace blue::control diff --git a/blue_bridge/include/blue_bridge/bridge.hpp b/blue_control/include/blue_control/dynamic_model.hpp similarity index 55% rename from blue_bridge/include/blue_bridge/bridge.hpp rename to blue_control/include/blue_control/dynamic_model.hpp index 51e7082b..ab8cecfe 100644 --- a/blue_bridge/include/blue_bridge/bridge.hpp +++ b/blue_control/include/blue_control/dynamic_model.hpp @@ -18,34 +18,16 @@ // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN // THE SOFTWARE. -#include "mavros_msgs/msg/override_rc_in.hpp" -#include "rclcpp/rclcpp.hpp" -#include "std_srvs/srv/set_bool.hpp" - -namespace blue_bridge +namespace blue::control { -class Bridge : public rclcpp::Node +class DynamicModel { public: - Bridge(); - - bool active() const; + DynamicModel(/* args */); private: - void publishOverrideRCIn() const; - void updateCurrentPwmValues(const mavros_msgs::msg::OverrideRCIn & desired_pwm_values_msg); - void enableOverride( - std::shared_ptr request, - std::shared_ptr response); - - bool bridge_running_; - - mavros_msgs::msg::OverrideRCIn current_pwm_values_; - rclcpp::Subscription::SharedPtr rc_override_subscription_; - rclcpp::Publisher::SharedPtr rc_override_publisher_; - rclcpp::Service::SharedPtr enable_override_service_; - rclcpp::TimerBase::SharedPtr timer_; + /* data */ }; -} // namespace blue_bridge +} // namespace blue::control diff --git a/blue_control/package.xml b/blue_control/package.xml index e975c64b..d43c6351 100644 --- a/blue_control/package.xml +++ b/blue_control/package.xml @@ -14,6 +14,7 @@ std_msgs std_srvs + nav_msgs mavros_msgs geographic_msgs diff --git a/blue_control/src/base_controller.cpp b/blue_control/src/base_controller.cpp index 1cb4fafd..560be72c 100644 --- a/blue_control/src/base_controller.cpp +++ b/blue_control/src/base_controller.cpp @@ -27,34 +27,35 @@ using namespace std::chrono_literals; -namespace blue_control +namespace blue::control { -BaseController::BaseController() -: Node("blue_control") +BaseController::BaseController(const std::string & node_name, const rclcpp::NodeOptions & options) +: Node(node_name, options) { // Create a publisher to publish the current desired RC values - rc_override_publisher_ = + rc_override_pub_ = this->create_publisher("/mavros/rc/override", 1); // Start a timer to publish the current desired PWM values at a frequency of 50hz - timer_ = create_wall_timer(20ms, std::bind(&Bridge::publishOverrideRCIn, this)); + timer_ = create_wall_timer(20ms, std::bind(&BaseController::publishRC, this)); // Set up a service to manage whether or not the RC values will be overridden enable_override_service_ = this->create_service( "/blue_control/rc/override/enable", - std::bind(&Bridge::enableOverride, this, std::placeholders::_1, std::placeholders::_2)); + std::bind(&BaseController::enableOverride, this, std::placeholders::_1, std::placeholders::_2)); } -void Bridge::publishOverrideRCIn() const +void BaseController::publishRC() const { - // Only publish the override values if the bridge has been enabled - if (override_enabled_) { - rc_override_publisher_->publish(current_pwm_values_); + // Only publish the override values if the bridge has been enabled and the subscription + // has been set up + if (override_enabled_ && (rc_override_pub_->get_subscription_count() > 0)) { + rc_override_pub_->publish(control_signal_); } } -void Bridge::enableOverride( +void BaseController::enableOverride( const std::shared_ptr request, // NOLINT std::shared_ptr response) // NOLINT { @@ -65,4 +66,4 @@ void Bridge::enableOverride( response->success = (override_enabled_ == request->data); } -} // namespace blue_control +} // namespace blue::control diff --git a/blue_control/src/dynamic_model.cpp b/blue_control/src/dynamic_model.cpp new file mode 100644 index 00000000..cf31174b --- /dev/null +++ b/blue_control/src/dynamic_model.cpp @@ -0,0 +1,24 @@ +// 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. + +namespace blue::control +{ + +} // namespace blue::control diff --git a/blue_utils/CMakeLists.txt b/blue_utils/CMakeLists.txt new file mode 100644 index 00000000..087e9e24 --- /dev/null +++ b/blue_utils/CMakeLists.txt @@ -0,0 +1,26 @@ +cmake_minimum_required(VERSION 3.8) +project(blue_utils) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +# uncomment the following section in order to fill in +# further dependencies manually. +# find_package( REQUIRED) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + # comment the line when a copyright and license is added to all source files + set(ament_cmake_copyright_FOUND TRUE) + # the following line skips cpplint (only works in a git repo) + # comment the line when this package is in a git repo and when + # a copyright and license is added to all source files + set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +ament_package() diff --git a/blue_bridge/LICENSE b/blue_utils/LICENSE similarity index 100% rename from blue_bridge/LICENSE rename to blue_utils/LICENSE diff --git a/blue_utils/include/blue_utils/pwm.hpp b/blue_utils/include/blue_utils/pwm.hpp new file mode 100644 index 00000000..e69de29b diff --git a/blue_bridge/package.xml b/blue_utils/package.xml similarity index 52% rename from blue_bridge/package.xml rename to blue_utils/package.xml index 75f7a231..243e9068 100644 --- a/blue_bridge/package.xml +++ b/blue_utils/package.xml @@ -1,20 +1,16 @@ - blue_bridge - 0.0.1 - mavros interface between the BlueROV2 hardware and the driver. + blue_utils + 0.0.0 + TODO: Package description Evan Palmer MIT ament_cmake - mavros - mavros_extras - std_msgs - std_srvs - mavros_msgs - geographic_msgs + ament_lint_auto + ament_lint_common ament_cmake diff --git a/blue_utils/src/pwm.cpp b/blue_utils/src/pwm.cpp new file mode 100644 index 00000000..e69de29b From d6cff38351b3a848d86c23955f720f2ea1ae5106 Mon Sep 17 00:00:00 2001 From: Evan Palmer Date: Wed, 29 Mar 2023 00:13:34 -0700 Subject: [PATCH 03/34] bedtime --- .../include/blue_control/base_controller.hpp | 4 --- blue_control/src/base_controller.cpp | 5 ++++ blue_utils/include/blue_utils/pwm.hpp | 27 +++++++++++++++++++ blue_utils/src/pwm.cpp | 27 +++++++++++++++++++ 4 files changed, 59 insertions(+), 4 deletions(-) diff --git a/blue_control/include/blue_control/base_controller.hpp b/blue_control/include/blue_control/base_controller.hpp index 44a97f44..935cc2ba 100644 --- a/blue_control/include/blue_control/base_controller.hpp +++ b/blue_control/include/blue_control/base_controller.hpp @@ -33,10 +33,6 @@ class BaseController : public rclcpp::Node public: virtual ~BaseController() = default; // NOLINT - // TODO(evan-palmer): move these to a library - double convertVoltageToPwm(double voltage); - double convertForceToPwm(double force); - void setControlSignal(const mavros_msgs::msg::OverrideRCIn & control_input); protected: diff --git a/blue_control/src/base_controller.cpp b/blue_control/src/base_controller.cpp index 560be72c..c9180228 100644 --- a/blue_control/src/base_controller.cpp +++ b/blue_control/src/base_controller.cpp @@ -46,6 +46,11 @@ BaseController::BaseController(const std::string & node_name, const rclcpp::Node std::bind(&BaseController::enableOverride, this, std::placeholders::_1, std::placeholders::_2)); } +void BaseController::setControlSignal(const mavros_msgs::msg::OverrideRCIn & control_input) +{ + control_signal_ = control_input; +} + void BaseController::publishRC() const { // Only publish the override values if the bridge has been enabled and the subscription diff --git a/blue_utils/include/blue_utils/pwm.hpp b/blue_utils/include/blue_utils/pwm.hpp index e69de29b..4d204b67 100644 --- a/blue_utils/include/blue_utils/pwm.hpp +++ b/blue_utils/include/blue_utils/pwm.hpp @@ -0,0 +1,27 @@ +// 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. + +namespace blue::utils +{ + +int convertVoltageToPwm(double voltage); +int convertForceToPwm(double force); + +} // namespace blue::utils diff --git a/blue_utils/src/pwm.cpp b/blue_utils/src/pwm.cpp index e69de29b..6e628801 100644 --- a/blue_utils/src/pwm.cpp +++ b/blue_utils/src/pwm.cpp @@ -0,0 +1,27 @@ +// 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. + +namespace blue::utils +{ + +int convertVoltageToPwm(double voltage) { return 0; } +int convertForceToPwm(double force) { return 0; } + +} // namespace blue::utils From b29613afff801e1a9a46df44ed6006c8a64ca1b0 Mon Sep 17 00:00:00 2001 From: Evan Palmer Date: Wed, 29 Mar 2023 11:48:56 -0700 Subject: [PATCH 04/34] Added parameter to control publish rate --- .../{base_controller.hpp => controller.hpp} | 17 ++++--- .../include/blue_control}/pwm.hpp | 0 .../{base_controller.cpp => controller.cpp} | 45 +++++++++++++++---- {blue_utils => blue_control}/src/pwm.cpp | 0 blue_utils/CMakeLists.txt | 26 ----------- blue_utils/LICENSE | 17 ------- blue_utils/package.xml | 18 -------- 7 files changed, 47 insertions(+), 76 deletions(-) rename blue_control/include/blue_control/{base_controller.hpp => controller.hpp} (83%) rename {blue_utils/include/blue_utils => blue_control/include/blue_control}/pwm.hpp (100%) rename blue_control/src/{base_controller.cpp => controller.cpp} (61%) rename {blue_utils => blue_control}/src/pwm.cpp (100%) delete mode 100644 blue_utils/CMakeLists.txt delete mode 100644 blue_utils/LICENSE delete mode 100644 blue_utils/package.xml diff --git a/blue_control/include/blue_control/base_controller.hpp b/blue_control/include/blue_control/controller.hpp similarity index 83% rename from blue_control/include/blue_control/base_controller.hpp rename to blue_control/include/blue_control/controller.hpp index 935cc2ba..0a4c4e9f 100644 --- a/blue_control/include/blue_control/base_controller.hpp +++ b/blue_control/include/blue_control/controller.hpp @@ -28,26 +28,31 @@ namespace blue::control { -class BaseController : public rclcpp::Node +class Controller : public rclcpp::Node { public: - virtual ~BaseController() = default; // NOLINT + virtual ~Controller() = default; // NOLINT void setControlSignal(const mavros_msgs::msg::OverrideRCIn & control_input); protected: - BaseController(const std::string & node_name, const rclcpp::NodeOptions & options); + Controller(const std::string & node_name, const rclcpp::NodeOptions & options); + + double control_loop_freq_; private: - void publishRC() const; - void enableOverride( + // Callbacks + void publishRcCb() const; + void enableOverrideCb( std::shared_ptr request, std::shared_ptr response); + void setOdomPoseCb(const geometry_msgs::msg::PoseStamped & pose); + void proxySlamPoseCb(const geometry_msgs::msg::PoseStamped & pose); // BlueROV2 state bool override_enabled_; mavros_msgs::msg::OverrideRCIn control_signal_; - geometry_msgs::msg::PoseStamped pose_; + geometry_msgs::msg::PoseStamped odom_pose_; // Subscriptions rclcpp::Subscription::SharedPtr state_sub_; diff --git a/blue_utils/include/blue_utils/pwm.hpp b/blue_control/include/blue_control/pwm.hpp similarity index 100% rename from blue_utils/include/blue_utils/pwm.hpp rename to blue_control/include/blue_control/pwm.hpp diff --git a/blue_control/src/base_controller.cpp b/blue_control/src/controller.cpp similarity index 61% rename from blue_control/src/base_controller.cpp rename to blue_control/src/controller.cpp index c9180228..18df510e 100644 --- a/blue_control/src/base_controller.cpp +++ b/blue_control/src/controller.cpp @@ -18,7 +18,7 @@ // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN // THE SOFTWARE. -#include "blue_control/base_controller.hpp" +#include "blue_control/controller.hpp" #include #include @@ -30,28 +30,43 @@ using namespace std::chrono_literals; namespace blue::control { -BaseController::BaseController(const std::string & node_name, const rclcpp::NodeOptions & options) -: Node(node_name, options) +Controller::Controller(const std::string & node_name, const rclcpp::NodeOptions & options) +: Node(node_name, options), + control_loop_freq_(250) { + { + rcl_interfaces::msg::ParameterDescriptor desc; + desc.name = "control_loop_freq"; + desc.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE; + desc.description = "The frequency at which the control loop should run"; + desc.read_only = true; + control_loop_freq_ = declare_parameter(desc.name, control_loop_freq_, desc); + } + // Create a publisher to publish the current desired RC values rc_override_pub_ = this->create_publisher("/mavros/rc/override", 1); - // Start a timer to publish the current desired PWM values at a frequency of 50hz - timer_ = create_wall_timer(20ms, std::bind(&BaseController::publishRC, this)); + // If the control loop frequency is greater than 50hz, publish PWM values at the same rate as the + // control loop, otherwise publish PWM values at 50hz. + const double pwm_freq_sec = control_loop_freq_ > 50 ? (1 / control_loop_freq_) : 0.02; + + // Start a timer to publish the current desired PWM values at the + timer_ = create_wall_timer( + std::chrono::duration(pwm_freq_sec), std::bind(&Controller::publishRcCb, this)); // Set up a service to manage whether or not the RC values will be overridden enable_override_service_ = this->create_service( "/blue_control/rc/override/enable", - std::bind(&BaseController::enableOverride, this, std::placeholders::_1, std::placeholders::_2)); + std::bind(&Controller::enableOverrideCb, this, std::placeholders::_1, std::placeholders::_2)); } -void BaseController::setControlSignal(const mavros_msgs::msg::OverrideRCIn & control_input) +void Controller::setControlSignal(const mavros_msgs::msg::OverrideRCIn & control_input) { control_signal_ = control_input; } -void BaseController::publishRC() const +void Controller::publishRcCb() const { // Only publish the override values if the bridge has been enabled and the subscription // has been set up @@ -60,7 +75,7 @@ void BaseController::publishRC() const } } -void BaseController::enableOverride( +void Controller::enableOverrideCb( const std::shared_ptr request, // NOLINT std::shared_ptr response) // NOLINT { @@ -71,4 +86,16 @@ void BaseController::enableOverride( response->success = (override_enabled_ == request->data); } +void Controller::setOdomPoseCb(const geometry_msgs::msg::PoseStamped & pose) +{ + // TODO(evan-palmer): update transforms here + odom_pose_ = pose; +} + +void Controller::proxySlamPoseCb(const geometry_msgs::msg::PoseStamped & pose) +{ + // TODO(evan-palmer): update to use correct coordinate frames here + ext_nav_pub_->publish(pose); +} + } // namespace blue::control diff --git a/blue_utils/src/pwm.cpp b/blue_control/src/pwm.cpp similarity index 100% rename from blue_utils/src/pwm.cpp rename to blue_control/src/pwm.cpp diff --git a/blue_utils/CMakeLists.txt b/blue_utils/CMakeLists.txt deleted file mode 100644 index 087e9e24..00000000 --- a/blue_utils/CMakeLists.txt +++ /dev/null @@ -1,26 +0,0 @@ -cmake_minimum_required(VERSION 3.8) -project(blue_utils) - -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic) -endif() - -# find dependencies -find_package(ament_cmake REQUIRED) -# uncomment the following section in order to fill in -# further dependencies manually. -# find_package( REQUIRED) - -if(BUILD_TESTING) - find_package(ament_lint_auto REQUIRED) - # the following line skips the linter which checks for copyrights - # comment the line when a copyright and license is added to all source files - set(ament_cmake_copyright_FOUND TRUE) - # the following line skips cpplint (only works in a git repo) - # comment the line when this package is in a git repo and when - # a copyright and license is added to all source files - set(ament_cmake_cpplint_FOUND TRUE) - ament_lint_auto_find_test_dependencies() -endif() - -ament_package() diff --git a/blue_utils/LICENSE b/blue_utils/LICENSE deleted file mode 100644 index 30e8e2ec..00000000 --- a/blue_utils/LICENSE +++ /dev/null @@ -1,17 +0,0 @@ -Permission is hereby granted, free of charge, to any person obtaining a copy -of this software and associated documentation files (the "Software"), to deal -in the Software without restriction, including without limitation the rights -to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -copies of the Software, and to permit persons to whom the Software is -furnished to do so, subject to the following conditions: - -The above copyright notice and this permission notice shall be included in -all copies or substantial portions of the Software. - -THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL -THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN -THE SOFTWARE. diff --git a/blue_utils/package.xml b/blue_utils/package.xml deleted file mode 100644 index 243e9068..00000000 --- a/blue_utils/package.xml +++ /dev/null @@ -1,18 +0,0 @@ - - - - blue_utils - 0.0.0 - TODO: Package description - Evan Palmer - MIT - - ament_cmake - - ament_lint_auto - ament_lint_common - - - ament_cmake - - From 168cc3f5a2d74aa00c04e06aea0ebda9a9bfe6cd Mon Sep 17 00:00:00 2001 From: Evan Palmer Date: Thu, 30 Mar 2023 16:33:25 -0700 Subject: [PATCH 05/34] Continued controller implementation and fixed docker errors --- .clang-tidy | 3 +- .docker/Dockerfile | 31 +++++--- .dockerignore | 2 +- blue_control/CMakeLists.txt | 1 - .../include/blue_control/controller.hpp | 34 ++++----- blue_control/package.xml | 4 +- blue_control/src/controller.cpp | 74 +++++++++---------- 7 files changed, 75 insertions(+), 74 deletions(-) diff --git a/.clang-tidy b/.clang-tidy index 22487524..a6bfad84 100644 --- a/.clang-tidy +++ b/.clang-tidy @@ -25,8 +25,7 @@ Checks: > -bugprone-narrowing-conversions, -bugprone-easily-swappable-parameters, -bugprone-implicit-widening-of-multiplication-result, - -modernize-avoid-bind, - -modernize-use-nodiscard + -modernize-avoid-bind WarningsAsErrors: "*" CheckOptions: - key: readability-braces-around-statements.ShortStatementLines diff --git a/.docker/Dockerfile b/.docker/Dockerfile index 6372a20b..edf397c6 100644 --- a/.docker/Dockerfile +++ b/.docker/Dockerfile @@ -4,12 +4,15 @@ FROM ros:$ROS_DISTRO-ros-base as ci LABEL maintainer="Evan Palmer" LABEL maintainer-email="evanp922@gmail.com" +ENV DEBIAN_FRONTEND=noninteractive WORKDIR /root/ws_blue COPY . src/blue -# Install the core apt packages -RUN apt-get update && apt-get upgrade -y && apt-get install -y --no-install-recommends \ +# Install apt packages +RUN apt-get -q update \ + && apt-get -q -y upgrade \ + && apt-get -q install --no-install-recommends -y \ git \ wget \ curl \ @@ -30,10 +33,10 @@ RUN [ "/bin/bash" , "-c" , "\ && sudo ./install_geographiclib_datasets.sh" ] # Install all ROS dependencies -RUN apt-get update && apt-get upgrade -y \ +RUN apt-get -q update \ + && apt-get -q -y upgrade \ && rosdep update \ - && DEBIAN_FRONTEND=noninteractive \ - rosdep install -y --from-paths src --ignore-src --rosdistro ${ROS_DISTRO} --as-root=apt:false \ + && rosdep install -y --from-paths src --ignore-src --rosdistro ${ROS_DISTRO} --as-root=apt:false \ && rm -rf src \ && apt-get autoremove -y \ && apt-get clean -y \ @@ -42,14 +45,15 @@ RUN apt-get update && apt-get upgrade -y \ FROM ci as source ENV ROS_UNDERLAY /root/ws_blue/install +ENV DEBIAN_FRONTEND=noninteractive WORKDIR $ROS_UNDERLAY/.. COPY . src/blue -RUN apt-get update && apt-get upgrade -y \ +RUN apt-get -q update \ + && apt-get -q -y upgrade \ && rosdep update \ - && DEBIAN_FRONTEND=noninteractive \ - rosdep install -y --from-paths src --ignore-src --rosdistro ${ROS_DISTRO} --as-root=apt:false \ + && rosdep install -y --from-paths src --ignore-src --rosdistro ${ROS_DISTRO} --as-root=apt:false \ && apt-get autoremove -y \ && apt-get clean -y \ && rm -rf /var/lib/apt/lists/* @@ -65,21 +69,24 @@ RUN . "/opt/ros/${ROS_DISTRO}/setup.sh" \ FROM ci as develop ENV ROS_UNDERLAY /root/ws_blue/install +ENV DEBIAN_FRONTEND=noninteractive WORKDIR $ROS_UNDERLAY/.. COPY . src/blue -RUN apt-get update && apt-get upgrade -y \ +RUN apt-get -q update \ + && apt-get -q -y upgrade \ && rosdep update \ - && DEBIAN_FRONTEND=noninteractive \ - rosdep install -y --from-paths src --ignore-src --rosdistro ${ROS_DISTRO} --as-root=apt:false \ + && rosdep install -y --from-paths src --ignore-src --rosdistro ${ROS_DISTRO} --as-root=apt:false \ && rm -rf src \ && apt-get autoremove -y \ && apt-get clean -y \ && rm -rf /var/lib/apt/lists/* # Install development tools -RUN apt-get update && apt-get upgrade -y && apt-get install -y --no-install-recommends \ +RUN apt-get -q update \ + && apt-get -q -y upgrade \ + && apt-get -q install --no-install-recommends -y \ python3-dev \ python3-pip \ iputils-ping \ diff --git a/.dockerignore b/.dockerignore index 55c94c45..e61184bc 100644 --- a/.dockerignore +++ b/.dockerignore @@ -2,4 +2,4 @@ * # Except the following -!blue_bridge +!blue_control diff --git a/blue_control/CMakeLists.txt b/blue_control/CMakeLists.txt index c019742b..f653c837 100644 --- a/blue_control/CMakeLists.txt +++ b/blue_control/CMakeLists.txt @@ -28,7 +28,6 @@ foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) find_package(${Dependency} REQUIRED) endforeach() - if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) diff --git a/blue_control/include/blue_control/controller.hpp b/blue_control/include/blue_control/controller.hpp index 0a4c4e9f..7e43b9fc 100644 --- a/blue_control/include/blue_control/controller.hpp +++ b/blue_control/include/blue_control/controller.hpp @@ -18,6 +18,10 @@ // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN // THE SOFTWARE. +#include +#include +#include + #include "geometry_msgs/msg/pose_stamped.hpp" #include "mavros_msgs/msg/override_rc_in.hpp" #include "mavros_msgs/msg/state.hpp" @@ -32,39 +36,33 @@ class Controller : public rclcpp::Node { public: virtual ~Controller() = default; // NOLINT - - void setControlSignal(const mavros_msgs::msg::OverrideRCIn & control_input); - -protected: Controller(const std::string & node_name, const rclcpp::NodeOptions & options); - double control_loop_freq_; + [[nodiscard]] bool running() const; -private: - // Callbacks - void publishRcCb() const; - void enableOverrideCb( - std::shared_ptr request, - std::shared_ptr response); - void setOdomPoseCb(const geometry_msgs::msg::PoseStamped & pose); - void proxySlamPoseCb(const geometry_msgs::msg::PoseStamped & pose); +protected: + virtual mavros_msgs::msg::OverrideRCIn update(); // BlueROV2 state - bool override_enabled_; - mavros_msgs::msg::OverrideRCIn control_signal_; geometry_msgs::msg::PoseStamped odom_pose_; + bool running_; + +private: + void runControlLoopCb(); + void startControlCb( + const std::shared_ptr & request, + const std::shared_ptr & response); + void setOdomPoseCb(geometry_msgs::msg::PoseStamped::ConstSharedPtr pose); // Subscriptions rclcpp::Subscription::SharedPtr state_sub_; rclcpp::Subscription::SharedPtr pose_sub_; - rclcpp::Subscription::SharedPtr slam_pose_sub_; // Publishers rclcpp::Publisher::SharedPtr rc_override_pub_; - rclcpp::Publisher::SharedPtr ext_nav_pub_; // Services - rclcpp::Service::SharedPtr enable_override_service_; + rclcpp::Service::SharedPtr start_control_; // PWM timer rclcpp::TimerBase::SharedPtr timer_; diff --git a/blue_control/package.xml b/blue_control/package.xml index d43c6351..019b5fd0 100644 --- a/blue_control/package.xml +++ b/blue_control/package.xml @@ -2,8 +2,8 @@ blue_control - 0.0.0 - TODO: Package description + 0.0.1 + Control interface for the BlueROV2. Evan Palmer MIT diff --git a/blue_control/src/controller.cpp b/blue_control/src/controller.cpp index 18df510e..ec0a30af 100644 --- a/blue_control/src/controller.cpp +++ b/blue_control/src/controller.cpp @@ -31,71 +31,69 @@ namespace blue::control { Controller::Controller(const std::string & node_name, const rclcpp::NodeOptions & options) -: Node(node_name, options), - control_loop_freq_(250) +: Node(node_name, options) { + double control_loop_freq = 250; { rcl_interfaces::msg::ParameterDescriptor desc; desc.name = "control_loop_freq"; desc.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE; - desc.description = "The frequency at which the control loop should run"; + desc.description = + "The frequency at which the control loop should run. This must be greater than 50Hz to " + "override the RC inputs."; desc.read_only = true; - control_loop_freq_ = declare_parameter(desc.name, control_loop_freq_, desc); + control_loop_freq = declare_parameter(desc.name, control_loop_freq, desc); + } + + if (control_loop_freq < 50) { + throw std::invalid_argument( + "The control loop frequency must be greater than 50Hz to override the RC inputs!"); } - // Create a publisher to publish the current desired RC values rc_override_pub_ = this->create_publisher("/mavros/rc/override", 1); - // If the control loop frequency is greater than 50hz, publish PWM values at the same rate as the - // control loop, otherwise publish PWM values at 50hz. - const double pwm_freq_sec = control_loop_freq_ > 50 ? (1 / control_loop_freq_) : 0.02; + pose_sub_ = this->create_subscription( + "/mavros/local_position/pose", rclcpp::QoS(rclcpp::KeepLast(1)).best_effort(), + [this](geometry_msgs::msg::PoseStamped::ConstSharedPtr pose) -> void { + setOdomPoseCb(std::move(pose)); + }); - // Start a timer to publish the current desired PWM values at the - timer_ = create_wall_timer( - std::chrono::duration(pwm_freq_sec), std::bind(&Controller::publishRcCb, this)); + start_control_ = this->create_service( + "/blue/control/run", + [this]( + const std::shared_ptr & request, + const std::shared_ptr & response) -> void { + startControlCb(request, response); + }); - // Set up a service to manage whether or not the RC values will be overridden - enable_override_service_ = this->create_service( - "/blue_control/rc/override/enable", - std::bind(&Controller::enableOverrideCb, this, std::placeholders::_1, std::placeholders::_2)); + timer_ = create_wall_timer( + std::chrono::duration(1 / control_loop_freq), [this]() -> void { runControlLoopCb(); }); } -void Controller::setControlSignal(const mavros_msgs::msg::OverrideRCIn & control_input) -{ - control_signal_ = control_input; -} +bool Controller::running() const { return running_; } -void Controller::publishRcCb() const +void Controller::runControlLoopCb() { - // Only publish the override values if the bridge has been enabled and the subscription - // has been set up - if (override_enabled_ && (rc_override_pub_->get_subscription_count() > 0)) { - rc_override_pub_->publish(control_signal_); + if (running() && (rc_override_pub_->get_subscription_count() > 0)) { + rc_override_pub_->publish(update()); } } -void Controller::enableOverrideCb( - const std::shared_ptr request, // NOLINT - std::shared_ptr response) // NOLINT +void Controller::startControlCb( + const std::shared_ptr & request, + const std::shared_ptr & response) { - // Enable/disable publishing the override messages - override_enabled_ = request->data; + running_ = request->data; // Set the response according to whether or not the update was done properly - response->success = (override_enabled_ == request->data); + response->success = (running() == request->data); } -void Controller::setOdomPoseCb(const geometry_msgs::msg::PoseStamped & pose) +void Controller::setOdomPoseCb(geometry_msgs::msg::PoseStamped::ConstSharedPtr pose) // NOLINT { // TODO(evan-palmer): update transforms here - odom_pose_ = pose; -} - -void Controller::proxySlamPoseCb(const geometry_msgs::msg::PoseStamped & pose) -{ - // TODO(evan-palmer): update to use correct coordinate frames here - ext_nav_pub_->publish(pose); + odom_pose_ = *pose; } } // namespace blue::control From 1fafaac4f32eba213986082286186ce2a007daec Mon Sep 17 00:00:00 2001 From: Evan Palmer Date: Fri, 31 Mar 2023 12:18:58 -0700 Subject: [PATCH 06/34] save point --- blue_control/include/blue_control/controller.hpp | 15 +++++++-------- blue_control/include/blue_control/manager.hpp | 0 blue_control/package.xml | 1 + blue_control/src/controller.cpp | 1 + blue_control/src/manager.cpp | 0 5 files changed, 9 insertions(+), 8 deletions(-) create mode 100644 blue_control/include/blue_control/manager.hpp create mode 100644 blue_control/src/manager.cpp diff --git a/blue_control/include/blue_control/controller.hpp b/blue_control/include/blue_control/controller.hpp index 7e43b9fc..39371d46 100644 --- a/blue_control/include/blue_control/controller.hpp +++ b/blue_control/include/blue_control/controller.hpp @@ -18,15 +18,13 @@ // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN // THE SOFTWARE. -#include -#include -#include - #include "geometry_msgs/msg/pose_stamped.hpp" +#include "geometry_msgs/msg/twist_stamped.hpp" #include "mavros_msgs/msg/override_rc_in.hpp" #include "mavros_msgs/msg/state.hpp" #include "nav_msgs/msg/odometry.hpp" #include "rclcpp/rclcpp.hpp" +#include "sensor_msgs/msg/imu.hpp" #include "std_srvs/srv/set_bool.hpp" namespace blue::control @@ -43,10 +41,6 @@ class Controller : public rclcpp::Node protected: virtual mavros_msgs::msg::OverrideRCIn update(); - // BlueROV2 state - geometry_msgs::msg::PoseStamped odom_pose_; - bool running_; - private: void runControlLoopCb(); void startControlCb( @@ -66,6 +60,11 @@ class Controller : public rclcpp::Node // PWM timer rclcpp::TimerBase::SharedPtr timer_; + + // BlueROV2 state; these are fused to create the public odometry msg in the desired frame + geometry_msgs::msg::PoseStamped odom_pose_; + geometry_msgs::msg::TwistStamped odom_twist_; + bool running_; }; } // namespace blue::control diff --git a/blue_control/include/blue_control/manager.hpp b/blue_control/include/blue_control/manager.hpp new file mode 100644 index 00000000..e69de29b diff --git a/blue_control/package.xml b/blue_control/package.xml index 019b5fd0..5e983f2e 100644 --- a/blue_control/package.xml +++ b/blue_control/package.xml @@ -16,6 +16,7 @@ std_srvs nav_msgs mavros_msgs + sensor_msgs geographic_msgs ament_lint_auto diff --git a/blue_control/src/controller.cpp b/blue_control/src/controller.cpp index ec0a30af..9458c49d 100644 --- a/blue_control/src/controller.cpp +++ b/blue_control/src/controller.cpp @@ -93,6 +93,7 @@ void Controller::startControlCb( void Controller::setOdomPoseCb(geometry_msgs::msg::PoseStamped::ConstSharedPtr pose) // NOLINT { // TODO(evan-palmer): update transforms here + // TODO(evan-palmer): calculate and update linear velocity odom_pose_ = *pose; } diff --git a/blue_control/src/manager.cpp b/blue_control/src/manager.cpp new file mode 100644 index 00000000..e69de29b From 93ef5e68ff78204360edc3a45876c012fa52d70a Mon Sep 17 00:00:00 2001 From: Evan Palmer Date: Fri, 31 Mar 2023 13:12:24 -0700 Subject: [PATCH 07/34] reincluded bridge to separate concerns --- blue_bridge/CMakeLists.txt | 54 +++++++++ blue_bridge/LICENSE | 17 +++ blue_bridge/include/blue_bridge/bridge.hpp | 51 ++++++++ .../include/blue_bridge}/pwm.hpp | 0 blue_bridge/package.xml | 22 ++++ blue_bridge/src/bridge.cpp | 112 ++++++++++++++++++ {blue_control => blue_bridge}/src/pwm.cpp | 0 .../include/blue_control/controller.hpp | 10 +- blue_control/src/controller.cpp | 11 +- blue_dynamics/CMakeLists.txt | 26 ++++ blue_dynamics/LICENSE | 17 +++ blue_dynamics/package.xml | 18 +++ blue_manager/CMakeLists.txt | 26 ++++ blue_manager/LICENSE | 17 +++ .../include/blue_manager}/manager.hpp | 0 blue_manager/package.xml | 18 +++ .../src/manager.cpp | 0 17 files changed, 382 insertions(+), 17 deletions(-) create mode 100644 blue_bridge/CMakeLists.txt create mode 100644 blue_bridge/LICENSE create mode 100644 blue_bridge/include/blue_bridge/bridge.hpp rename {blue_control/include/blue_control => blue_bridge/include/blue_bridge}/pwm.hpp (100%) create mode 100644 blue_bridge/package.xml create mode 100644 blue_bridge/src/bridge.cpp rename {blue_control => blue_bridge}/src/pwm.cpp (100%) create mode 100644 blue_dynamics/CMakeLists.txt create mode 100644 blue_dynamics/LICENSE create mode 100644 blue_dynamics/package.xml create mode 100644 blue_manager/CMakeLists.txt create mode 100644 blue_manager/LICENSE rename {blue_control/include/blue_control => blue_manager/include/blue_manager}/manager.hpp (100%) create mode 100644 blue_manager/package.xml rename {blue_control => blue_manager}/src/manager.cpp (100%) diff --git a/blue_bridge/CMakeLists.txt b/blue_bridge/CMakeLists.txt new file mode 100644 index 00000000..7e9ad530 --- /dev/null +++ b/blue_bridge/CMakeLists.txt @@ -0,0 +1,54 @@ +cmake_minimum_required(VERSION 3.8) +project(blue_bridge) + +# 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 + mavros_msgs + std_msgs + std_srvs +) + +foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) + find_package(${Dependency} REQUIRED) +endforeach() + +include_directories(include) + +add_executable(${PROJECT_NAME} src/bridge.cpp) +ament_target_dependencies(${PROJECT_NAME} ${THIS_PACKAGE_INCLUDE_DEPENDS}) + +install( + TARGETS ${PROJECT_NAME} + DESTINATION lib/${PROJECT_NAME} +) + +install( + DIRECTORY include/ + DESTINATION include +) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + + # Run linters found in package.xml except those below + set(ament_cmake_uncrustify_FOUND TRUE) + + ament_lint_auto_find_test_dependencies() +endif() + +ament_package() diff --git a/blue_bridge/LICENSE b/blue_bridge/LICENSE new file mode 100644 index 00000000..30e8e2ec --- /dev/null +++ b/blue_bridge/LICENSE @@ -0,0 +1,17 @@ +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: + +The above copyright notice and this permission notice shall be included in +all copies or substantial portions of the Software. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL +THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +THE SOFTWARE. diff --git a/blue_bridge/include/blue_bridge/bridge.hpp b/blue_bridge/include/blue_bridge/bridge.hpp new file mode 100644 index 00000000..f1b86607 --- /dev/null +++ b/blue_bridge/include/blue_bridge/bridge.hpp @@ -0,0 +1,51 @@ +// 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 "mavros_msgs/msg/override_rc_in.hpp" +#include "rclcpp/rclcpp.hpp" +#include "std_srvs/srv/set_bool.hpp" + +namespace blue::bridge +{ + +class Bridge : public rclcpp::Node +{ +public: + Bridge(); + + [[nodiscard]] bool active() const; + +private: + void publishOverrideRcInCb() const; + void setCurrentPwmValuesCb(mavros_msgs::msg::OverrideRCIn::ConstSharedPtr pwm); + void enableOverrideCb( + const std::shared_ptr & request, + const std::shared_ptr & response); + + bool bridge_running_; + mavros_msgs::msg::OverrideRCIn current_pwm_values_; + + rclcpp::Subscription::SharedPtr rc_override_sub_; + rclcpp::Publisher::SharedPtr rc_override_pub_; + rclcpp::Service::SharedPtr enable_override_service_; + rclcpp::TimerBase::SharedPtr timer_; +}; + +} // namespace blue::bridge diff --git a/blue_control/include/blue_control/pwm.hpp b/blue_bridge/include/blue_bridge/pwm.hpp similarity index 100% rename from blue_control/include/blue_control/pwm.hpp rename to blue_bridge/include/blue_bridge/pwm.hpp diff --git a/blue_bridge/package.xml b/blue_bridge/package.xml new file mode 100644 index 00000000..75f7a231 --- /dev/null +++ b/blue_bridge/package.xml @@ -0,0 +1,22 @@ + + + + blue_bridge + 0.0.1 + mavros interface between the BlueROV2 hardware and the driver. + Evan Palmer + MIT + + ament_cmake + + mavros + mavros_extras + std_msgs + std_srvs + mavros_msgs + geographic_msgs + + + ament_cmake + + diff --git a/blue_bridge/src/bridge.cpp b/blue_bridge/src/bridge.cpp new file mode 100644 index 00000000..326ebb33 --- /dev/null +++ b/blue_bridge/src/bridge.cpp @@ -0,0 +1,112 @@ +// Copyright 2023, Evan Palmer +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: + +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. + +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL +// THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +// THE SOFTWARE. + +#include "blue_bridge/bridge.hpp" + +#include +#include + +#include "rclcpp/rclcpp.hpp" + +using namespace std::chrono_literals; + +namespace blue::bridge +{ + +Bridge::Bridge() +: Node("blue_bridge"), + bridge_running_(false) +{ + double override_freq = 250; + { + rcl_interfaces::msg::ParameterDescriptor desc; + desc.name = "override_freq"; + desc.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE; + desc.description = + "The rate that the PWM inputs should be published at. This must be at least 50Hz to " + "override the RC inputs. If the control loop frequency is greater than 50Hz, this should " + "match the rate of the control loop."; + desc.read_only = true; + override_freq = declare_parameter(desc.name, override_freq, desc); + } + + if (override_freq < 50) { + throw std::invalid_argument( + "The override frequency must be greater than 50Hz to override the RC inputs!"); + } + + rc_override_sub_ = this->create_subscription( + "/blue_bridge/rc/override", rclcpp::QoS(rclcpp::KeepLast(1)).reliable(), + [this](mavros_msgs::msg::OverrideRCIn::ConstSharedPtr pwm) -> void { + setCurrentPwmValuesCb(pwm); // NOLINT + }); + + rc_override_pub_ = this->create_publisher( + "/mavros/rc/override", rclcpp::QoS(rclcpp::KeepLast(1)).reliable()); + + timer_ = create_wall_timer(std::chrono::duration(1 / override_freq), [this]() -> void { + publishOverrideRcInCb(); + }); + + enable_override_service_ = this->create_service( + "/blue_bridge/rc/override/enable", + [this]( + const std::shared_ptr & request, + const std::shared_ptr & response) -> void { + enableOverrideCb(request, response); + }); +} + +bool Bridge::active() const { return bridge_running_; } + +void Bridge::publishOverrideRcInCb() const +{ + // Only publish the override values if the bridge has been enabled + if (active() && (rc_override_pub_->get_subscription_count() > 0)) { + rc_override_pub_->publish(current_pwm_values_); + } +} + +void Bridge::setCurrentPwmValuesCb(mavros_msgs::msg::OverrideRCIn::ConstSharedPtr pwm) // NOLINT +{ + // Update the desired RC override values + current_pwm_values_ = *pwm; +} + +void Bridge::enableOverrideCb( + const std::shared_ptr & request, + const std::shared_ptr & response) +{ + // Enable/disable publishing the override messages + bridge_running_ = request->data; + + // Set the response according to whether or not the update was done properly + response->success = (bridge_running_ == request->data); +} + +} // namespace blue::bridge + +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + return 0; +} diff --git a/blue_control/src/pwm.cpp b/blue_bridge/src/pwm.cpp similarity index 100% rename from blue_control/src/pwm.cpp rename to blue_bridge/src/pwm.cpp diff --git a/blue_control/include/blue_control/controller.hpp b/blue_control/include/blue_control/controller.hpp index 39371d46..99ef5694 100644 --- a/blue_control/include/blue_control/controller.hpp +++ b/blue_control/include/blue_control/controller.hpp @@ -21,7 +21,6 @@ #include "geometry_msgs/msg/pose_stamped.hpp" #include "geometry_msgs/msg/twist_stamped.hpp" #include "mavros_msgs/msg/override_rc_in.hpp" -#include "mavros_msgs/msg/state.hpp" #include "nav_msgs/msg/odometry.hpp" #include "rclcpp/rclcpp.hpp" #include "sensor_msgs/msg/imu.hpp" @@ -48,17 +47,10 @@ class Controller : public rclcpp::Node const std::shared_ptr & response); void setOdomPoseCb(geometry_msgs::msg::PoseStamped::ConstSharedPtr pose); - // Subscriptions - rclcpp::Subscription::SharedPtr state_sub_; rclcpp::Subscription::SharedPtr pose_sub_; - - // Publishers + rclcpp::Subscription::SharedPtr imu_sub_; rclcpp::Publisher::SharedPtr rc_override_pub_; - - // Services rclcpp::Service::SharedPtr start_control_; - - // PWM timer rclcpp::TimerBase::SharedPtr timer_; // BlueROV2 state; these are fused to create the public odometry msg in the desired frame diff --git a/blue_control/src/controller.cpp b/blue_control/src/controller.cpp index 9458c49d..eed68452 100644 --- a/blue_control/src/controller.cpp +++ b/blue_control/src/controller.cpp @@ -45,13 +45,8 @@ Controller::Controller(const std::string & node_name, const rclcpp::NodeOptions control_loop_freq = declare_parameter(desc.name, control_loop_freq, desc); } - if (control_loop_freq < 50) { - throw std::invalid_argument( - "The control loop frequency must be greater than 50Hz to override the RC inputs!"); - } - - rc_override_pub_ = - this->create_publisher("/mavros/rc/override", 1); + rc_override_pub_ = this->create_publisher( + "/blue_bridge/rc/override", rclcpp::QoS(rclcpp::KeepLast(1)).reliable()); pose_sub_ = this->create_subscription( "/mavros/local_position/pose", rclcpp::QoS(rclcpp::KeepLast(1)).best_effort(), @@ -75,7 +70,7 @@ bool Controller::running() const { return running_; } void Controller::runControlLoopCb() { - if (running() && (rc_override_pub_->get_subscription_count() > 0)) { + if (running()) { rc_override_pub_->publish(update()); } } diff --git a/blue_dynamics/CMakeLists.txt b/blue_dynamics/CMakeLists.txt new file mode 100644 index 00000000..ca500318 --- /dev/null +++ b/blue_dynamics/CMakeLists.txt @@ -0,0 +1,26 @@ +cmake_minimum_required(VERSION 3.8) +project(blue_dynamics) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +# uncomment the following section in order to fill in +# further dependencies manually. +# find_package( REQUIRED) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + # comment the line when a copyright and license is added to all source files + set(ament_cmake_copyright_FOUND TRUE) + # the following line skips cpplint (only works in a git repo) + # comment the line when this package is in a git repo and when + # a copyright and license is added to all source files + set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +ament_package() diff --git a/blue_dynamics/LICENSE b/blue_dynamics/LICENSE new file mode 100644 index 00000000..30e8e2ec --- /dev/null +++ b/blue_dynamics/LICENSE @@ -0,0 +1,17 @@ +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: + +The above copyright notice and this permission notice shall be included in +all copies or substantial portions of the Software. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL +THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +THE SOFTWARE. diff --git a/blue_dynamics/package.xml b/blue_dynamics/package.xml new file mode 100644 index 00000000..8d2b537a --- /dev/null +++ b/blue_dynamics/package.xml @@ -0,0 +1,18 @@ + + + + blue_dynamics + 0.0.0 + TODO: Package description + Evan Palmer + MIT + + ament_cmake + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/blue_manager/CMakeLists.txt b/blue_manager/CMakeLists.txt new file mode 100644 index 00000000..e2cf6831 --- /dev/null +++ b/blue_manager/CMakeLists.txt @@ -0,0 +1,26 @@ +cmake_minimum_required(VERSION 3.8) +project(blue_manager) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +# uncomment the following section in order to fill in +# further dependencies manually. +# find_package( REQUIRED) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + # comment the line when a copyright and license is added to all source files + set(ament_cmake_copyright_FOUND TRUE) + # the following line skips cpplint (only works in a git repo) + # comment the line when this package is in a git repo and when + # a copyright and license is added to all source files + set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +ament_package() diff --git a/blue_manager/LICENSE b/blue_manager/LICENSE new file mode 100644 index 00000000..30e8e2ec --- /dev/null +++ b/blue_manager/LICENSE @@ -0,0 +1,17 @@ +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: + +The above copyright notice and this permission notice shall be included in +all copies or substantial portions of the Software. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL +THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +THE SOFTWARE. diff --git a/blue_control/include/blue_control/manager.hpp b/blue_manager/include/blue_manager/manager.hpp similarity index 100% rename from blue_control/include/blue_control/manager.hpp rename to blue_manager/include/blue_manager/manager.hpp diff --git a/blue_manager/package.xml b/blue_manager/package.xml new file mode 100644 index 00000000..7ab5180b --- /dev/null +++ b/blue_manager/package.xml @@ -0,0 +1,18 @@ + + + + blue_manager + 0.0.0 + TODO: Package description + Evan Palmer + MIT + + ament_cmake + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/blue_control/src/manager.cpp b/blue_manager/src/manager.cpp similarity index 100% rename from blue_control/src/manager.cpp rename to blue_manager/src/manager.cpp From fc51e3eae0866ff28859c3412c25d9be75c0294f Mon Sep 17 00:00:00 2001 From: Evan Palmer Date: Sat, 1 Apr 2023 00:27:06 -0700 Subject: [PATCH 08/34] continued base controller implementation --- blue_bridge/src/bridge.cpp | 2 +- .../include/blue_control/controller.hpp | 13 +++++-------- blue_control/src/controller.cpp | 18 +++++++++++------- 3 files changed, 17 insertions(+), 16 deletions(-) diff --git a/blue_bridge/src/bridge.cpp b/blue_bridge/src/bridge.cpp index 326ebb33..643301b8 100644 --- a/blue_bridge/src/bridge.cpp +++ b/blue_bridge/src/bridge.cpp @@ -106,7 +106,7 @@ void Bridge::enableOverrideCb( int main(int argc, char ** argv) { rclcpp::init(argc, argv); - rclcpp::spin(std::make_shared()); + rclcpp::spin(std::make_shared()); rclcpp::shutdown(); return 0; } diff --git a/blue_control/include/blue_control/controller.hpp b/blue_control/include/blue_control/controller.hpp index 99ef5694..c22f1a99 100644 --- a/blue_control/include/blue_control/controller.hpp +++ b/blue_control/include/blue_control/controller.hpp @@ -35,28 +35,25 @@ class Controller : public rclcpp::Node virtual ~Controller() = default; // NOLINT Controller(const std::string & node_name, const rclcpp::NodeOptions & options); - [[nodiscard]] bool running() const; - protected: virtual mavros_msgs::msg::OverrideRCIn update(); + bool running_; + nav_msgs::msg::Odometry odom_; + private: void runControlLoopCb(); void startControlCb( const std::shared_ptr & request, const std::shared_ptr & response); - void setOdomPoseCb(geometry_msgs::msg::PoseStamped::ConstSharedPtr pose); + void updatePoseCb(geometry_msgs::msg::PoseStamped::ConstSharedPtr pose); + void updateAngularVelCb(sensor_msgs::msg::Imu::ConstSharedPtr imu); rclcpp::Subscription::SharedPtr pose_sub_; rclcpp::Subscription::SharedPtr imu_sub_; rclcpp::Publisher::SharedPtr rc_override_pub_; rclcpp::Service::SharedPtr start_control_; rclcpp::TimerBase::SharedPtr timer_; - - // BlueROV2 state; these are fused to create the public odometry msg in the desired frame - geometry_msgs::msg::PoseStamped odom_pose_; - geometry_msgs::msg::TwistStamped odom_twist_; - bool running_; }; } // namespace blue::control diff --git a/blue_control/src/controller.cpp b/blue_control/src/controller.cpp index eed68452..58ffd6b2 100644 --- a/blue_control/src/controller.cpp +++ b/blue_control/src/controller.cpp @@ -51,7 +51,7 @@ Controller::Controller(const std::string & node_name, const rclcpp::NodeOptions pose_sub_ = this->create_subscription( "/mavros/local_position/pose", rclcpp::QoS(rclcpp::KeepLast(1)).best_effort(), [this](geometry_msgs::msg::PoseStamped::ConstSharedPtr pose) -> void { - setOdomPoseCb(std::move(pose)); + updatePoseCb(std::move(pose)); }); start_control_ = this->create_service( @@ -66,11 +66,9 @@ Controller::Controller(const std::string & node_name, const rclcpp::NodeOptions std::chrono::duration(1 / control_loop_freq), [this]() -> void { runControlLoopCb(); }); } -bool Controller::running() const { return running_; } - void Controller::runControlLoopCb() { - if (running()) { + if (running_) { rc_override_pub_->publish(update()); } } @@ -82,14 +80,20 @@ void Controller::startControlCb( running_ = request->data; // Set the response according to whether or not the update was done properly - response->success = (running() == request->data); + response->success = (running_ == request->data); } -void Controller::setOdomPoseCb(geometry_msgs::msg::PoseStamped::ConstSharedPtr pose) // NOLINT +void Controller::updatePoseCb(geometry_msgs::msg::PoseStamped::ConstSharedPtr pose) // NOLINT { // TODO(evan-palmer): update transforms here // TODO(evan-palmer): calculate and update linear velocity - odom_pose_ = *pose; + odom_.pose.pose = pose->pose; +} + +void Controller::updateAngularVelCb(sensor_msgs::msg::Imu::ConstSharedPtr imu) // NOLINT +{ + // TODO(evan-palmer): update transforms here (if necessary) + odom_.twist.twist.angular = imu->angular_velocity; } } // namespace blue::control From 3886d10dd05b031cbd07315f293b9dd84a7a84be Mon Sep 17 00:00:00 2001 From: Evan Palmer Date: Sun, 2 Apr 2023 20:07:52 -0700 Subject: [PATCH 09/34] added linear velocity calculation --- .../include/blue_control/controller.hpp | 12 +++++++++- blue_control/src/controller.cpp | 22 +++++++++++++++---- 2 files changed, 29 insertions(+), 5 deletions(-) diff --git a/blue_control/include/blue_control/controller.hpp b/blue_control/include/blue_control/controller.hpp index c22f1a99..f431332d 100644 --- a/blue_control/include/blue_control/controller.hpp +++ b/blue_control/include/blue_control/controller.hpp @@ -38,22 +38,32 @@ class Controller : public rclcpp::Node protected: virtual mavros_msgs::msg::OverrideRCIn update(); + // BlueROV2 state bool running_; nav_msgs::msg::Odometry odom_; private: void runControlLoopCb(); - void startControlCb( + void enableControlCb( const std::shared_ptr & request, const std::shared_ptr & response); void updatePoseCb(geometry_msgs::msg::PoseStamped::ConstSharedPtr pose); void updateAngularVelCb(sensor_msgs::msg::Imu::ConstSharedPtr imu); + // Subscriptions rclcpp::Subscription::SharedPtr pose_sub_; rclcpp::Subscription::SharedPtr imu_sub_; + + // Publishers rclcpp::Publisher::SharedPtr rc_override_pub_; + + // Services rclcpp::Service::SharedPtr start_control_; + + // Timers rclcpp::TimerBase::SharedPtr timer_; + + rclcpp::Time pose_time_; }; } // namespace blue::control diff --git a/blue_control/src/controller.cpp b/blue_control/src/controller.cpp index 58ffd6b2..eaf8834f 100644 --- a/blue_control/src/controller.cpp +++ b/blue_control/src/controller.cpp @@ -59,11 +59,14 @@ Controller::Controller(const std::string & node_name, const rclcpp::NodeOptions [this]( const std::shared_ptr & request, const std::shared_ptr & response) -> void { - startControlCb(request, response); + enableControlCb(request, response); }); timer_ = create_wall_timer( std::chrono::duration(1 / control_loop_freq), [this]() -> void { runControlLoopCb(); }); + + // Set the initial pose time + pose_time_ = this->get_clock()->now(); } void Controller::runControlLoopCb() @@ -73,7 +76,7 @@ void Controller::runControlLoopCb() } } -void Controller::startControlCb( +void Controller::enableControlCb( const std::shared_ptr & request, const std::shared_ptr & response) { @@ -85,9 +88,20 @@ void Controller::startControlCb( void Controller::updatePoseCb(geometry_msgs::msg::PoseStamped::ConstSharedPtr pose) // NOLINT { - // TODO(evan-palmer): update transforms here - // TODO(evan-palmer): calculate and update linear velocity + // Update the linear velocity + double duration = rclcpp::Duration(pose->header.stamp - pose_time_).seconds(); // NOLINT + + odom_.twist.twist.linear.x = (pose->pose.position.x - odom_.pose.pose.position.x) / duration; + odom_.twist.twist.linear.y = (pose->pose.position.y - odom_.pose.pose.position.y) / duration; + odom_.twist.twist.linear.z = (pose->pose.position.z - odom_.pose.pose.position.z) / duration; + + // Now we can update the pose odom_.pose.pose = pose->pose; + + // Save the timestamp for the next measurement + pose_time_ = pose->header.stamp; + + // TODO(evan-palmer): update transforms here } void Controller::updateAngularVelCb(sensor_msgs::msg::Imu::ConstSharedPtr imu) // NOLINT From d35103dc3e94f63b2f5cbf93e3dbc802484decbf Mon Sep 17 00:00:00 2001 From: Evan Palmer Date: Mon, 3 Apr 2023 23:41:21 -0700 Subject: [PATCH 10/34] cleaned up lambdas and continued base controller --- .dockerignore | 1 + blue_bridge/include/blue_bridge/bridge.hpp | 6 --- blue_bridge/src/bridge.cpp | 44 ++++++------------- .../include/blue_control/controller.hpp | 14 ++++-- blue_control/package.xml | 3 ++ blue_control/src/controller.cpp | 18 ++------ 6 files changed, 32 insertions(+), 54 deletions(-) diff --git a/.dockerignore b/.dockerignore index e61184bc..711874cd 100644 --- a/.dockerignore +++ b/.dockerignore @@ -2,4 +2,5 @@ * # Except the following +!blue_bridge !blue_control diff --git a/blue_bridge/include/blue_bridge/bridge.hpp b/blue_bridge/include/blue_bridge/bridge.hpp index f1b86607..28d58a6f 100644 --- a/blue_bridge/include/blue_bridge/bridge.hpp +++ b/blue_bridge/include/blue_bridge/bridge.hpp @@ -33,12 +33,6 @@ class Bridge : public rclcpp::Node [[nodiscard]] bool active() const; private: - void publishOverrideRcInCb() const; - void setCurrentPwmValuesCb(mavros_msgs::msg::OverrideRCIn::ConstSharedPtr pwm); - void enableOverrideCb( - const std::shared_ptr & request, - const std::shared_ptr & response); - bool bridge_running_; mavros_msgs::msg::OverrideRCIn current_pwm_values_; diff --git a/blue_bridge/src/bridge.cpp b/blue_bridge/src/bridge.cpp index 643301b8..aec532ae 100644 --- a/blue_bridge/src/bridge.cpp +++ b/blue_bridge/src/bridge.cpp @@ -52,55 +52,37 @@ Bridge::Bridge() "The override frequency must be greater than 50Hz to override the RC inputs!"); } + // Set the PWM values that should be used when overriding the RC inputs rc_override_sub_ = this->create_subscription( - "/blue_bridge/rc/override", rclcpp::QoS(rclcpp::KeepLast(1)).reliable(), - [this](mavros_msgs::msg::OverrideRCIn::ConstSharedPtr pwm) -> void { - setCurrentPwmValuesCb(pwm); // NOLINT + "/blue/rc/override", rclcpp::QoS(rclcpp::KeepLast(1)).reliable(), + [this](mavros_msgs::msg::OverrideRCIn::ConstSharedPtr pwm) -> void { // NOLINT + current_pwm_values_ = *pwm; }); rc_override_pub_ = this->create_publisher( "/mavros/rc/override", rclcpp::QoS(rclcpp::KeepLast(1)).reliable()); + // Publish the Override RC values at the specified frequency timer_ = create_wall_timer(std::chrono::duration(1 / override_freq), [this]() -> void { - publishOverrideRcInCb(); + // Only publish the override values if the bridge has been enabled + if (active() && (rc_override_pub_->get_subscription_count() > 0)) { + rc_override_pub_->publish(current_pwm_values_); + } }); + // Enable/disable RC override enable_override_service_ = this->create_service( - "/blue_bridge/rc/override/enable", + "/blue/rc/override/enable", [this]( const std::shared_ptr & request, const std::shared_ptr & response) -> void { - enableOverrideCb(request, response); + bridge_running_ = request->data; + response->success = (bridge_running_ == request->data); }); } bool Bridge::active() const { return bridge_running_; } -void Bridge::publishOverrideRcInCb() const -{ - // Only publish the override values if the bridge has been enabled - if (active() && (rc_override_pub_->get_subscription_count() > 0)) { - rc_override_pub_->publish(current_pwm_values_); - } -} - -void Bridge::setCurrentPwmValuesCb(mavros_msgs::msg::OverrideRCIn::ConstSharedPtr pwm) // NOLINT -{ - // Update the desired RC override values - current_pwm_values_ = *pwm; -} - -void Bridge::enableOverrideCb( - const std::shared_ptr & request, - const std::shared_ptr & response) -{ - // Enable/disable publishing the override messages - bridge_running_ = request->data; - - // Set the response according to whether or not the update was done properly - response->success = (bridge_running_ == request->data); -} - } // namespace blue::bridge int main(int argc, char ** argv) diff --git a/blue_control/include/blue_control/controller.hpp b/blue_control/include/blue_control/controller.hpp index f431332d..c1663764 100644 --- a/blue_control/include/blue_control/controller.hpp +++ b/blue_control/include/blue_control/controller.hpp @@ -25,6 +25,8 @@ #include "rclcpp/rclcpp.hpp" #include "sensor_msgs/msg/imu.hpp" #include "std_srvs/srv/set_bool.hpp" +#include "tf2_ros/transform_broadcaster.h" +#include "tf2_ros/transform_listener.h" namespace blue::control { @@ -42,11 +44,17 @@ class Controller : public rclcpp::Node bool running_; nav_msgs::msg::Odometry odom_; + // Static transforms + tf2::Transform tf_cam_base_; + + // Dynamic transforms + tf2::Transform tf_odom_base_; + + // Inverse transforms + tf2::Transform tf_base_odom_; + private: void runControlLoopCb(); - void enableControlCb( - const std::shared_ptr & request, - const std::shared_ptr & response); void updatePoseCb(geometry_msgs::msg::PoseStamped::ConstSharedPtr pose); void updateAngularVelCb(sensor_msgs::msg::Imu::ConstSharedPtr imu); diff --git a/blue_control/package.xml b/blue_control/package.xml index 5e983f2e..23c33ef2 100644 --- a/blue_control/package.xml +++ b/blue_control/package.xml @@ -18,6 +18,9 @@ mavros_msgs sensor_msgs geographic_msgs + tf2 + tf2_geometry_msgs + tf2_ros ament_lint_auto ament_lint_common diff --git a/blue_control/src/controller.cpp b/blue_control/src/controller.cpp index eaf8834f..c748d930 100644 --- a/blue_control/src/controller.cpp +++ b/blue_control/src/controller.cpp @@ -25,8 +25,6 @@ #include "rclcpp/rclcpp.hpp" -using namespace std::chrono_literals; - namespace blue::control { @@ -46,7 +44,7 @@ Controller::Controller(const std::string & node_name, const rclcpp::NodeOptions } rc_override_pub_ = this->create_publisher( - "/blue_bridge/rc/override", rclcpp::QoS(rclcpp::KeepLast(1)).reliable()); + "/blue/rc/override", rclcpp::QoS(rclcpp::KeepLast(1)).reliable()); pose_sub_ = this->create_subscription( "/mavros/local_position/pose", rclcpp::QoS(rclcpp::KeepLast(1)).best_effort(), @@ -59,7 +57,9 @@ Controller::Controller(const std::string & node_name, const rclcpp::NodeOptions [this]( const std::shared_ptr & request, const std::shared_ptr & response) -> void { - enableControlCb(request, response); + // Enable external control + running_ = request->data; + response->success = (running_ == request->data); }); timer_ = create_wall_timer( @@ -76,16 +76,6 @@ void Controller::runControlLoopCb() } } -void Controller::enableControlCb( - const std::shared_ptr & request, - const std::shared_ptr & response) -{ - running_ = request->data; - - // Set the response according to whether or not the update was done properly - response->success = (running_ == request->data); -} - void Controller::updatePoseCb(geometry_msgs::msg::PoseStamped::ConstSharedPtr pose) // NOLINT { // Update the linear velocity From 3c8ffe44106415951e0281c61aee78c4d331b7f1 Mon Sep 17 00:00:00 2001 From: Evan Palmer Date: Fri, 21 Apr 2023 19:21:03 -0700 Subject: [PATCH 11/34] Started implementing vehicle dynamics --- .../include/blue_control/controller.hpp | 8 +- .../include/blue_control/dynamic_model.hpp | 33 ---- blue_control/package.xml | 1 + blue_control/src/controller.cpp | 4 +- .../include/blue_dynamics/hydrodynamics.hpp | 150 ++++++++++++++++++ .../blue_dynamics/thruster_dynamics.hpp | 0 .../blue_dynamics/vehicle_dynamics.hpp | 4 +- setup.cfg | 3 +- 8 files changed, 158 insertions(+), 45 deletions(-) delete mode 100644 blue_control/include/blue_control/dynamic_model.hpp create mode 100644 blue_dynamics/include/blue_dynamics/hydrodynamics.hpp create mode 100644 blue_dynamics/include/blue_dynamics/thruster_dynamics.hpp rename blue_control/src/dynamic_model.cpp => blue_dynamics/include/blue_dynamics/vehicle_dynamics.hpp (95%) diff --git a/blue_control/include/blue_control/controller.hpp b/blue_control/include/blue_control/controller.hpp index c1663764..8d4a2ea8 100644 --- a/blue_control/include/blue_control/controller.hpp +++ b/blue_control/include/blue_control/controller.hpp @@ -19,7 +19,6 @@ // THE SOFTWARE. #include "geometry_msgs/msg/pose_stamped.hpp" -#include "geometry_msgs/msg/twist_stamped.hpp" #include "mavros_msgs/msg/override_rc_in.hpp" #include "nav_msgs/msg/odometry.hpp" #include "rclcpp/rclcpp.hpp" @@ -55,15 +54,12 @@ class Controller : public rclcpp::Node private: void runControlLoopCb(); - void updatePoseCb(geometry_msgs::msg::PoseStamped::ConstSharedPtr pose); - void updateAngularVelCb(sensor_msgs::msg::Imu::ConstSharedPtr imu); // Subscriptions - rclcpp::Subscription::SharedPtr pose_sub_; - rclcpp::Subscription::SharedPtr imu_sub_; + rclcpp::Subscription::SharedPtr odom_sub_; // Publishers - rclcpp::Publisher::SharedPtr rc_override_pub_; + rclcpp::Publisher::SharedPtr rc_pub_; // Services rclcpp::Service::SharedPtr start_control_; diff --git a/blue_control/include/blue_control/dynamic_model.hpp b/blue_control/include/blue_control/dynamic_model.hpp deleted file mode 100644 index ab8cecfe..00000000 --- a/blue_control/include/blue_control/dynamic_model.hpp +++ /dev/null @@ -1,33 +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. - -namespace blue::control -{ - -class DynamicModel -{ -public: - DynamicModel(/* args */); - -private: - /* data */ -}; - -} // namespace blue::control diff --git a/blue_control/package.xml b/blue_control/package.xml index 23c33ef2..3f9e65dd 100644 --- a/blue_control/package.xml +++ b/blue_control/package.xml @@ -12,6 +12,7 @@ mavros mavros_extras + rclcpp std_msgs std_srvs nav_msgs diff --git a/blue_control/src/controller.cpp b/blue_control/src/controller.cpp index c748d930..9ed92fa5 100644 --- a/blue_control/src/controller.cpp +++ b/blue_control/src/controller.cpp @@ -43,7 +43,7 @@ Controller::Controller(const std::string & node_name, const rclcpp::NodeOptions control_loop_freq = declare_parameter(desc.name, control_loop_freq, desc); } - rc_override_pub_ = this->create_publisher( + rc_pub_ = this->create_publisher( "/blue/rc/override", rclcpp::QoS(rclcpp::KeepLast(1)).reliable()); pose_sub_ = this->create_subscription( @@ -72,7 +72,7 @@ Controller::Controller(const std::string & node_name, const rclcpp::NodeOptions void Controller::runControlLoopCb() { if (running_) { - rc_override_pub_->publish(update()); + rc_pub_->publish(update()); } } diff --git a/blue_dynamics/include/blue_dynamics/hydrodynamics.hpp b/blue_dynamics/include/blue_dynamics/hydrodynamics.hpp new file mode 100644 index 00000000..81a63fac --- /dev/null +++ b/blue_dynamics/include/blue_dynamics/hydrodynamics.hpp @@ -0,0 +1,150 @@ +// Copyright 2023, Evan Palmer +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: + +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. + +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL +// THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +// THE SOFTWARE. + +namespace dynamics +{ + +/** + * @brief + * + */ +struct HydrodynamicsBase +{ + double x; + double y; + double z; + double k; + double m; + double n; + + /** + * @brief Construct a new Hydrodynamics Base object + * + * @param x + * @param y + * @param z + * @param k + * @param m + * @param n + */ + HydrodynamicsBase(double x, double y, double z, double k, double m, double n) + : x(x), + y(y), + z(z), + k(k), + m(m), + n(n) + { + } +}; + +/** + * @brief + * + */ +struct MomentsOfInertia +{ + double x; + double y; + double z; + + /** + * @brief Construct a new Moments Of Inertia object + * + * @param x + * @param y + * @param z + */ + MomentsOfInertia(double x, double y, double z) + : x(x), + y(y), + z(z) + { + } +}; + +/** + * @brief + * + */ +struct AddedMass : HydrodynamicsBase +{ + /** + * @brief Construct a new Added Mass object + * + * @param x_u_dot + * @param y_v_dot + * @param z_w_dot + * @param k_p_dot + * @param m_q_dot + * @param n_r_dot + */ + AddedMass( + double x_u_dot, double y_v_dot, double z_w_dot, double k_p_dot, double m_q_dot, double n_r_dot) + : HydrodynamicsBase(x_u_dot, y_v_dot, z_w_dot, k_p_dot, m_q_dot, n_r_dot) + { + } +}; + +/** + * @brief + * + */ +struct LinearDamping : HydrodynamicsBase +{ + /** + * @brief Construct a new Linear Damping object + * + * @param x_u + * @param y_v + * @param z_w + * @param k_p + * @param m_q + * @param n_r + */ + LinearDamping(double x_u, double y_v, double z_w, double k_p, double m_q, double n_r) + : HydrodynamicsBase(x_u, y_v, z_w, k_p, m_q, n_r) + { + } +}; + +/** + * @brief + * + */ +struct NonlinearDamping : HydrodynamicsBase +{ + /** + * @brief Construct a new Nonlinear Damping object + * + * @param x_uu + * @param y_vv + * @param z_ww + * @param k_pp + * @param m_qq + * @param n_rr + */ + NonlinearDamping(double x_uu, double y_vv, double z_ww, double k_pp, double m_qq, double n_rr) + : HydrodynamicsBase(x_uu, y_vv, z_ww, k_pp, m_qq, n_rr) + { + } +}; + +} // namespace dynamics diff --git a/blue_dynamics/include/blue_dynamics/thruster_dynamics.hpp b/blue_dynamics/include/blue_dynamics/thruster_dynamics.hpp new file mode 100644 index 00000000..e69de29b diff --git a/blue_control/src/dynamic_model.cpp b/blue_dynamics/include/blue_dynamics/vehicle_dynamics.hpp similarity index 95% rename from blue_control/src/dynamic_model.cpp rename to blue_dynamics/include/blue_dynamics/vehicle_dynamics.hpp index cf31174b..a7e8ddb9 100644 --- a/blue_control/src/dynamic_model.cpp +++ b/blue_dynamics/include/blue_dynamics/vehicle_dynamics.hpp @@ -18,7 +18,7 @@ // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN // THE SOFTWARE. -namespace blue::control +namespace dynamics { -} // namespace blue::control +} // namespace dynamics diff --git a/setup.cfg b/setup.cfg index 33e87d65..424125b8 100644 --- a/setup.cfg +++ b/setup.cfg @@ -12,5 +12,4 @@ ignore = profile=black [pydocstyle] -ignore = D100 -convention = google +ignore = D203,D213,D215,D406,D407,D408,D409,D413,D100,D104 From 9665b66b75cb6cd438ef1b7d7e55dadd0d5bdd9c Mon Sep 17 00:00:00 2001 From: Evan Palmer Date: Sat, 22 Apr 2023 02:16:19 -0700 Subject: [PATCH 12/34] Continued vehicle dynamics implementation --- .devcontainer.json | 13 +- .dockerignore | 1 + blue_dynamics/CMakeLists.txt | 67 ++++++-- .../include/blue_dynamics/hydrodynamics.hpp | 97 ++++++------ .../blue_dynamics/vehicle_dynamics.hpp | 59 ++++++- blue_dynamics/package.xml | 6 + blue_dynamics/src/vehicle_dynamics.cpp | 149 ++++++++++++++++++ 7 files changed, 330 insertions(+), 62 deletions(-) create mode 100644 blue_dynamics/src/vehicle_dynamics.cpp diff --git a/.devcontainer.json b/.devcontainer.json index 8dd28b60..461b19f5 100644 --- a/.devcontainer.json +++ b/.devcontainer.json @@ -36,6 +36,7 @@ "files.insertFinalNewline": true, "files.trimTrailingWhitespace": true, "editor.formatOnSave": true, + "editor.tabSize": 2, "xml.format.maxLineWidth": 100, "json.format.enable": true, "python.linting.enabled": true, @@ -45,14 +46,20 @@ "python.linting.mypyEnabled": true, "python.formatting.provider": "black", "autoDocstring.startOnNewLine": false, - "autoDocstring.docstringFormat": "google", + "autoDocstring.docstringFormat": "google-notypes", "isort.args": ["--profile", "black"], "isort.check": true, "python.autoComplete.extraPaths": [ - "/opt/ros/humble/lib/python3.10/site-packages/" + "/opt/ros/humble/lib/python3.10/site-packages/", + "/opt/ros/humble/local/lib/python3.10/dist-packages/", + "/opt/ros/rolling/lib/python3.10/site-packages/", + "/opt/ros/rolling/local/lib/python3.10/dist-packages/" ], "python.analysis.extraPaths": [ - "/opt/ros/humble/lib/python3.10/site-packages/" + "/opt/ros/humble/lib/python3.10/site-packages/", + "/opt/ros/humble/local/lib/python3.10/dist-packages/", + "/opt/ros/rolling/lib/python3.10/site-packages/", + "/opt/ros/rolling/local/lib/python3.10/dist-packages/" ], "C_Cpp.default.intelliSenseMode": "linux-gcc-x86", "C_Cpp.clang_format_fallbackStyle": "Google", diff --git a/.dockerignore b/.dockerignore index 711874cd..b389be55 100644 --- a/.dockerignore +++ b/.dockerignore @@ -4,3 +4,4 @@ # Except the following !blue_bridge !blue_control +!blue_dynamics diff --git a/blue_dynamics/CMakeLists.txt b/blue_dynamics/CMakeLists.txt index ca500318..ae80fe2a 100644 --- a/blue_dynamics/CMakeLists.txt +++ b/blue_dynamics/CMakeLists.txt @@ -1,26 +1,71 @@ cmake_minimum_required(VERSION 3.8) project(blue_dynamics) +# 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() -# find dependencies -find_package(ament_cmake REQUIRED) -# uncomment the following section in order to fill in -# further dependencies manually. -# find_package( REQUIRED) +set(THIS_PACKAGE_INCLUDE_DEPENDS + rclcpp + ament_cmake + eigen3_cmake_module + Eigen3 + geometry_msgs +) + +foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) + find_package(${Dependency} REQUIRED) +endforeach() + +include_directories( + include +) + +add_library(${PROJECT_NAME} SHARED + src/vehicle_dynamics.cpp +) + +target_link_libraries(${PROJECT_NAME} ${rclcpp_LIBRARIES}) +target_include_directories(${PROJECT_NAME} PRIVATE include) +ament_target_dependencies(${PROJECT_NAME} ${THIS_PACKAGE_INCLUDE_DEPENDS}) + +install( + DIRECTORY include/ + DESTINATION include +) + +install( + TARGETS ${PROJECT_NAME} + EXPORT ${PROJECT_NAME} + RUNTIME DESTINATION bin + LIBRARY DESTINATION lib + ARCHIVE DESTINATION lib + INCLUDES DESTINATION include +) if(BUILD_TESTING) + find_package(ament_cmake_gtest REQUIRED) + find_package(ament_cmake_gmock REQUIRED) find_package(ament_lint_auto REQUIRED) - # the following line skips the linter which checks for copyrights - # comment the line when a copyright and license is added to all source files + + # Run linters found in package.xml except the two below set(ament_cmake_copyright_FOUND TRUE) - # the following line skips cpplint (only works in a git repo) - # comment the line when this package is in a git repo and when - # a copyright and license is added to all source files - set(ament_cmake_cpplint_FOUND TRUE) + set(ament_cmake_uncrustify_FOUND TRUE) + ament_lint_auto_find_test_dependencies() endif() +ament_export_targets(${PROJECT_NAME} HAS_LIBRARY_TARGET) +ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS}) + ament_package() diff --git a/blue_dynamics/include/blue_dynamics/hydrodynamics.hpp b/blue_dynamics/include/blue_dynamics/hydrodynamics.hpp index 81a63fac..2bd4a372 100644 --- a/blue_dynamics/include/blue_dynamics/hydrodynamics.hpp +++ b/blue_dynamics/include/blue_dynamics/hydrodynamics.hpp @@ -18,14 +18,12 @@ // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN // THE SOFTWARE. -namespace dynamics +#include + +namespace blue::dynamics { -/** - * @brief - * - */ -struct HydrodynamicsBase +struct Hydrodynamics6d { double x; double y; @@ -34,17 +32,7 @@ struct HydrodynamicsBase double m; double n; - /** - * @brief Construct a new Hydrodynamics Base object - * - * @param x - * @param y - * @param z - * @param k - * @param m - * @param n - */ - HydrodynamicsBase(double x, double y, double z, double k, double m, double n) + Hydrodynamics6d(double x, double y, double z, double k, double m, double n) : x(x), y(y), z(z), @@ -53,41 +41,58 @@ struct HydrodynamicsBase n(n) { } + + [[nodiscard]] virtual Eigen::MatrixXd toMatrix() const + { + Eigen::VectorXd vec; + vec << x, y, z, k, m, n; + + return vec.asDiagonal().toDenseMatrix(); + }; }; -/** - * @brief - * - */ -struct MomentsOfInertia +struct Hydrodynamics3d { double x; double y; double z; - /** - * @brief Construct a new Moments Of Inertia object - * - * @param x - * @param y - * @param z - */ - MomentsOfInertia(double x, double y, double z) + Hydrodynamics3d(double x, double y, double z) : x(x), y(y), z(z) { } + + [[nodiscard]] virtual Eigen::Matrix3d toMatrix() const + { + Eigen::Vector3d vec; + vec << x, y, z; + + return vec.asDiagonal().toDenseMatrix(); + }; }; -/** - * @brief - * - */ -struct AddedMass : HydrodynamicsBase +struct MomentsOfInertia : Hydrodynamics3d +{ + MomentsOfInertia(double x, double y, double z) + : Hydrodynamics3d(x, y, z) + { + } +}; + +struct CenterOfBuoyancy : Hydrodynamics3d +{ + CenterOfBuoyancy(double x, double y, double z) + : Hydrodynamics3d(x, y, z) + { + } +}; + +struct AddedMass : Hydrodynamics6d { /** - * @brief Construct a new Added Mass object + * @brief Construct a new AddedMass object. * * @param x_u_dot * @param y_v_dot @@ -98,19 +103,19 @@ struct AddedMass : HydrodynamicsBase */ AddedMass( double x_u_dot, double y_v_dot, double z_w_dot, double k_p_dot, double m_q_dot, double n_r_dot) - : HydrodynamicsBase(x_u_dot, y_v_dot, z_w_dot, k_p_dot, m_q_dot, n_r_dot) + : Hydrodynamics6d(x_u_dot, y_v_dot, z_w_dot, k_p_dot, m_q_dot, n_r_dot) { } }; /** - * @brief + * @brief Linear damping coefficients. * */ -struct LinearDamping : HydrodynamicsBase +struct LinearDamping : Hydrodynamics6d { /** - * @brief Construct a new Linear Damping object + * @brief Construct a new LinearDamping object. * * @param x_u * @param y_v @@ -120,19 +125,19 @@ struct LinearDamping : HydrodynamicsBase * @param n_r */ LinearDamping(double x_u, double y_v, double z_w, double k_p, double m_q, double n_r) - : HydrodynamicsBase(x_u, y_v, z_w, k_p, m_q, n_r) + : Hydrodynamics6d(x_u, y_v, z_w, k_p, m_q, n_r) { } }; /** - * @brief + * @brief Nonlinear damping coefficients. * */ -struct NonlinearDamping : HydrodynamicsBase +struct NonlinearDamping : Hydrodynamics6d { /** - * @brief Construct a new Nonlinear Damping object + * @brief Construct a new NonlinearDamping object * * @param x_uu * @param y_vv @@ -142,9 +147,9 @@ struct NonlinearDamping : HydrodynamicsBase * @param n_rr */ NonlinearDamping(double x_uu, double y_vv, double z_ww, double k_pp, double m_qq, double n_rr) - : HydrodynamicsBase(x_uu, y_vv, z_ww, k_pp, m_qq, n_rr) + : Hydrodynamics6d(x_uu, y_vv, z_ww, k_pp, m_qq, n_rr) { } }; -} // namespace dynamics +} // namespace blue::dynamics diff --git a/blue_dynamics/include/blue_dynamics/vehicle_dynamics.hpp b/blue_dynamics/include/blue_dynamics/vehicle_dynamics.hpp index a7e8ddb9..98fed4a1 100644 --- a/blue_dynamics/include/blue_dynamics/vehicle_dynamics.hpp +++ b/blue_dynamics/include/blue_dynamics/vehicle_dynamics.hpp @@ -18,7 +18,62 @@ // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN // THE SOFTWARE. -namespace dynamics +#include + +#include "blue_dynamics/hydrodynamics.hpp" +#include "geometry_msgs/msg/point.hpp" +#include "geometry_msgs/msg/pose_stamped.hpp" +#include "geometry_msgs/msg/twist_stamped.hpp" + +namespace blue::dynamics +{ + +struct VehicleDynamics { + double mass; + double weight; + double buoyancy; + MomentsOfInertia moments; + AddedMass added_mass; + LinearDamping linear_damping; + NonlinearDamping quadratic_damping; + CenterOfBuoyancy center_of_buoyancy; + + VehicleDynamics( + double mass, double weight, double buoyancy, const MomentsOfInertia & moments, + const AddedMass & added_mass, const LinearDamping & linear_damping, + const NonlinearDamping & quadratic_damping, const CenterOfBuoyancy & center_of_buoyancy); + + [[nodiscard]] static Eigen::Matrix3d createSkewSymmetricMatrix(double a1, double a2, double a3); + + [[nodiscard]] Eigen::MatrixXd calculateInertiaMatrix() const; + + [[nodiscard]] static Eigen::MatrixXd calculateRigidBodyMatrix( + double mass, const MomentsOfInertia & moments); + + [[nodiscard]] static Eigen::MatrixXd calculateAddedMassMatrix(const AddedMass & added_mass); + + [[nodiscard]] Eigen::MatrixXd calculateCoriolisMatrix( + const geometry_msgs::msg::TwistStamped & velocity) const; + + [[nodiscard]] static Eigen::MatrixXd calculateRigidBodyCoriolisMatrix( + double mass, const MomentsOfInertia & moments, + const geometry_msgs::msg::TwistStamped & velocity); + + [[nodiscard]] static Eigen::MatrixXd calculateHydrodynamicCoriolixMatrix( + const AddedMass & added_mass, const geometry_msgs::msg::TwistStamped & velocity); + + [[nodiscard]] Eigen::MatrixXd calculateDampingMatrix( + const geometry_msgs::msg::TwistStamped & velocity) const; + + [[nodiscard]] static Eigen::MatrixXd calculateLinearDampingMatrix( + const LinearDamping & linear_damping); + + [[nodiscard]] static Eigen::MatrixXd calculateNonlinearDampingMatrix( + const NonlinearDamping & quadratic_damping, const geometry_msgs::msg::TwistStamped & velocity); + + [[nodiscard]] Eigen::VectorXd calculateRestoringForcesVector( + const geometry_msgs::msg::PoseStamped & pose) const; +}; -} // namespace dynamics +} // namespace blue::dynamics diff --git a/blue_dynamics/package.xml b/blue_dynamics/package.xml index 8d2b537a..ed08bc1a 100644 --- a/blue_dynamics/package.xml +++ b/blue_dynamics/package.xml @@ -8,6 +8,12 @@ MIT ament_cmake + eigen3_cmake_module + + eigen + geometry_msgs + + eigen3_cmake_module ament_lint_auto ament_lint_common diff --git a/blue_dynamics/src/vehicle_dynamics.cpp b/blue_dynamics/src/vehicle_dynamics.cpp new file mode 100644 index 00000000..07e8e607 --- /dev/null +++ b/blue_dynamics/src/vehicle_dynamics.cpp @@ -0,0 +1,149 @@ +// Copyright 2023, Evan Palmer +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: + +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. + +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL +// THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +// THE SOFTWARE. + +#include "blue_dynamics/vehicle_dynamics.hpp" + +#include + +namespace blue::dynamics +{ + +VehicleDynamics::VehicleDynamics( + double mass, double weight, double buoyancy, const MomentsOfInertia & moments, + const AddedMass & added_mass, const LinearDamping & linear_damping, + const NonlinearDamping & quadratic_damping, const CenterOfBuoyancy & center_of_buoyancy) +: mass(mass), + weight(weight), + buoyancy(buoyancy), + moments(std::move(moments)), + added_mass(std::move(added_mass)), + linear_damping(std::move(linear_damping)), + quadratic_damping(std::move(quadratic_damping)), + center_of_buoyancy(std::move(center_of_buoyancy)) +{ +} + +[[nodiscard]] Eigen::Matrix3d VehicleDynamics::createSkewSymmetricMatrix( + double a1, double a2, double a3) +{ + Eigen::Matrix3d mat; + mat << 0, -a3, a2, a3, 0, -a1, -a2, a1, 0; + + return mat; +} + +[[nodiscard]] Eigen::MatrixXd VehicleDynamics::calculateInertiaMatrix() const +{ + return calculateRigidBodyMatrix(mass, moments) + calculateAddedMassMatrix(added_mass); +} + +[[nodiscard]] Eigen::MatrixXd VehicleDynamics::calculateRigidBodyMatrix( + double mass, const MomentsOfInertia & moments) +{ + Eigen::MatrixXd mat(6, 6); + + mat.topLeftCorner(3, 3) = mass * Eigen::MatrixXd::Identity(3, 3); + mat.bottomRightCorner(3, 3) = moments.toMatrix(); + + return mat; +} + +[[nodiscard]] Eigen::MatrixXd VehicleDynamics::calculateAddedMassMatrix( + const AddedMass & added_mass) +{ + return added_mass.toMatrix(); +} + +[[nodiscard]] Eigen::MatrixXd VehicleDynamics::calculateCoriolisMatrix( + const geometry_msgs::msg::TwistStamped & velocity) const +{ + return calculateRigidBodyCoriolisMatrix(mass, moments, velocity) + + calculateHydrodynamicCoriolixMatrix(added_mass, velocity); +} + +[[nodiscard]] Eigen::MatrixXd VehicleDynamics::calculateRigidBodyCoriolisMatrix( + double mass, const MomentsOfInertia & moments, const geometry_msgs::msg::TwistStamped & velocity) +{ + Eigen::MatrixXd mat(6, 6); + + Eigen::Matrix3d linear_vel = + mass * createSkewSymmetricMatrix( + velocity.twist.linear.x, velocity.twist.linear.y, velocity.twist.linear.z); + + Eigen::Matrix3d angular_vel = createSkewSymmetricMatrix( + moments.x * velocity.twist.angular.x, moments.y * velocity.twist.angular.y, + moments.z * velocity.twist.angular.z); + + mat.topRightCorner(3, 3) = linear_vel; + mat.bottomLeftCorner(3, 3) = linear_vel; + mat.bottomRightCorner(3, 3) = angular_vel; + + return mat; +} + +[[nodiscard]] Eigen::MatrixXd VehicleDynamics::calculateHydrodynamicCoriolixMatrix( + const AddedMass & added_mass, const geometry_msgs::msg::TwistStamped & velocity) +{ + Eigen::MatrixXd mat(6, 6); + + Eigen::Matrix3d linear_vel = createSkewSymmetricMatrix( + added_mass.x * velocity.twist.linear.x, added_mass.y * velocity.twist.linear.y, + added_mass.z * velocity.twist.linear.z); + + Eigen::Matrix3d angular_vel = createSkewSymmetricMatrix( + added_mass.k * velocity.twist.angular.x, added_mass.m * velocity.twist.angular.y, + added_mass.n * velocity.twist.angular.z); + + mat.topRightCorner(3, 3) = linear_vel; + mat.bottomLeftCorner(3, 3) = linear_vel; + mat.bottomRightCorner(3, 3) = angular_vel; + + return mat; +} + +[[nodiscard]] Eigen::MatrixXd VehicleDynamics::calculateDampingMatrix( + const geometry_msgs::msg::TwistStamped & velocity) const +{ + return calculateLinearDampingMatrix(linear_damping) + + calculateNonlinearDampingMatrix(quadratic_damping, velocity); +} + +[[nodiscard]] Eigen::MatrixXd VehicleDynamics::calculateLinearDampingMatrix( + const LinearDamping & linear_damping) +{ + return -1 * linear_damping.toMatrix(); +} + +[[nodiscard]] Eigen::MatrixXd VehicleDynamics::calculateNonlinearDampingMatrix( + const NonlinearDamping & quadratic_damping, const geometry_msgs::msg::TwistStamped & velocity) +{ + Eigen::VectorXd vec(6); + vec << std::abs(velocity.twist.linear.x), std::abs(velocity.twist.linear.y), + std::abs(velocity.twist.linear.z), std::abs(velocity.twist.angular.x), + std::abs(velocity.twist.angular.y), std::abs(velocity.twist.angular.z); + + return -1 * quadratic_damping.toMatrix() * vec; +} + +[[nodiscard]] Eigen::VectorXd VehicleDynamics::calculateRestoringForcesVector( + const geometry_msgs::msg::PoseStamped & pose) const +{ +} +} // namespace blue::dynamics From 024dff53f0668ba72efb2e500663bd46c4e28248 Mon Sep 17 00:00:00 2001 From: Evan Palmer Date: Sat, 22 Apr 2023 19:46:05 -0700 Subject: [PATCH 13/34] Finished initial untested version of vehicle dynamics --- .../blue_dynamics/current_dynamics.hpp | 0 .../include/blue_dynamics/hydrodynamics.hpp | 28 ++- .../blue_dynamics/vehicle_dynamics.hpp | 160 ++++++++++++++++-- blue_dynamics/src/vehicle_dynamics.cpp | 52 ++++-- 4 files changed, 209 insertions(+), 31 deletions(-) create mode 100644 blue_dynamics/include/blue_dynamics/current_dynamics.hpp diff --git a/blue_dynamics/include/blue_dynamics/current_dynamics.hpp b/blue_dynamics/include/blue_dynamics/current_dynamics.hpp new file mode 100644 index 00000000..e69de29b diff --git a/blue_dynamics/include/blue_dynamics/hydrodynamics.hpp b/blue_dynamics/include/blue_dynamics/hydrodynamics.hpp index 2bd4a372..d442d6ba 100644 --- a/blue_dynamics/include/blue_dynamics/hydrodynamics.hpp +++ b/blue_dynamics/include/blue_dynamics/hydrodynamics.hpp @@ -48,7 +48,15 @@ struct Hydrodynamics6d vec << x, y, z, k, m, n; return vec.asDiagonal().toDenseMatrix(); - }; + } + + [[nodiscard]] virtual Eigen::VectorXd toVector() const + { + Eigen::VectorXd vec; + vec << x, y, z, k, m, n; + + return vec; + } }; struct Hydrodynamics3d @@ -70,7 +78,15 @@ struct Hydrodynamics3d vec << x, y, z; return vec.asDiagonal().toDenseMatrix(); - }; + } + + [[nodiscard]] virtual Eigen::Vector3d toVector() const + { + Eigen::Vector3d vec; + vec << x, y, z; + + return vec; + } }; struct MomentsOfInertia : Hydrodynamics3d @@ -89,6 +105,14 @@ struct CenterOfBuoyancy : Hydrodynamics3d } }; +struct CenterOfGravity : Hydrodynamics3d +{ + CenterOfGravity(double x, double y, double z) + : Hydrodynamics3d(x, y, z) + { + } +}; + struct AddedMass : Hydrodynamics6d { /** diff --git a/blue_dynamics/include/blue_dynamics/vehicle_dynamics.hpp b/blue_dynamics/include/blue_dynamics/vehicle_dynamics.hpp index 98fed4a1..0230fad3 100644 --- a/blue_dynamics/include/blue_dynamics/vehicle_dynamics.hpp +++ b/blue_dynamics/include/blue_dynamics/vehicle_dynamics.hpp @@ -30,48 +30,182 @@ namespace blue::dynamics struct VehicleDynamics { - double mass; - double weight; - double buoyancy; - MomentsOfInertia moments; - AddedMass added_mass; - LinearDamping linear_damping; - NonlinearDamping quadratic_damping; - CenterOfBuoyancy center_of_buoyancy; - + double mass; // The total mass of the vehicle (kg). + double weight; // The weight of the vehicle in the inertial frame. + double buoyancy; // The buoyance force acting on the vehicle in the inertial frame. + MomentsOfInertia moments; // The moments of inertia. + AddedMass added_mass; // The added mass coefficients. + LinearDamping linear_damping; // The linear damping coefficients. + NonlinearDamping quadratic_damping; // The nonlinear damping coefficients. + CenterOfBuoyancy center_of_buoyancy; // The center of buoyancy. + CenterOfGravity center_of_gravity; // The center of gravity. + + /** + * @brief Create a new VehicleDynamics object. + * + * @note This implementation draws from Thor I. Fossen's "Handbook of Marine Craft Hydrodynamics + * and Motion Control" (2011) and Gianluca Antonelli's "Underwater Robots" (2014) for the + * hydrodynamic model. Relevant equations and definitions used are documented with each method. + * + * + * @param mass The total mass of the vehicle (kg). + * @param weight The weight of the vehicle in the inertial frame. This is denoted as `W` in + * Section 2.5 of Gianluca Antonelli's "Underwater Robots". + * @param buoyancy The buoyancy force acting on the vehicle. This is denoted as `B` in Section 2.5 + * of Gianluca Antonelli's "Underwater Robots". + * @param moments The moments of inertia. + * @param added_mass The added mass coefficients. These are defined in Section 2.4.1 of Gianluca + * Antonelli's "Underwater Robots". + * @param linear_damping The rigid body linear damping coefficients. These are defined in Section + * 2.4.2 of Gianluca Antonelli's "Underwater Robots". + * @param quadratic_damping The rigid body nonlinear damping coefficients. These are defined in + * Section 2.4.2 of Gianluca Antonelli's "Underwater Robots". + * @param center_of_buoyancy The center of buoyancy. + * @param center_of_gravity The center of gravity relative to the body frame. + */ VehicleDynamics( double mass, double weight, double buoyancy, const MomentsOfInertia & moments, const AddedMass & added_mass, const LinearDamping & linear_damping, - const NonlinearDamping & quadratic_damping, const CenterOfBuoyancy & center_of_buoyancy); - + const NonlinearDamping & quadratic_damping, const CenterOfBuoyancy & center_of_buoyancy, + const CenterOfGravity & center_of_gravity); + + /** + * @brief Create a skew-symmetric matrix from the provided coefficients. + * + * @note This is also referred to as the "Cross-Product Operator" by some textbooks. + * + * @param a1 Coefficient one. + * @param a2 Coefficient two. + * @param a3 Coefficient three. + * @return Eigen::Matrix3d + */ [[nodiscard]] static Eigen::Matrix3d createSkewSymmetricMatrix(double a1, double a2, double a3); + /** + * @brief Calculate the vehicle's inertia matrix. + * + * @note The inertia matrix, commonly denoted by textbooks as `M`, is the sum of the rigid body + * mass `M_RB` and the added mass `M_A` such that `M = M_RB + M_A`. + * + * @return Eigen::MatrixXd + */ [[nodiscard]] Eigen::MatrixXd calculateInertiaMatrix() const; - [[nodiscard]] static Eigen::MatrixXd calculateRigidBodyMatrix( + /** + * @brief Calculate the rigid body inertia matrix. + * + * @note The definition used for the rigid body inertia matrix `M_RB` is provided by + * Thor I. Fossen's textbook "Handbook of Marine Craft Hydrodynamics and Motion Control" in + * Equation 3.44. Note that, in this model, we define the body frame to be coincident with the + * center of mass, such that r_g^b = 0. The result is that `M_RB` is a diagonal matrix. + * + * @param mass The total mass of the vehicle (kg). + * @param moments The moments of inertia. This is assumed to be a diagonal matrix. + * @return Eigen::MatrixXd + */ + [[nodiscard]] static Eigen::MatrixXd calculateRigidBodyMassMatrix( double mass, const MomentsOfInertia & moments); + /** + * @brief Calculate the added mass inertia matrix. + * + * @note The definition used for the added mass inertia matrix `M_A` is provided by Gianluca + * Antonelli's textbook "Underwater Robots" in Section 2.4.1. + * + * @param added_mass The added mass coefficients. + * @return Eigen::MatrixXd + */ [[nodiscard]] static Eigen::MatrixXd calculateAddedMassMatrix(const AddedMass & added_mass); + /** + * @brief Calculate the Coriolis and centripetal force matrix. + * + * @note The Coriolis and centripetal force matrix `C` is the sum of the rigid body Coriolis + * forces and the added Coriolis forces such that `C = C_RB + C_A`. + * + * @param velocity The current velocity of the vehicle in the body frame. + * @return Eigen::MatrixXd + */ [[nodiscard]] Eigen::MatrixXd calculateCoriolisMatrix( const geometry_msgs::msg::TwistStamped & velocity) const; + /** + * @brief Calculate the rigid body Coriolis-centripetal matrix. + * + * @note The definition of the rigid body Coriolis-centripetal matrix `C_RB` used in this work is + * provided by Thor I. Fossen's textbook "Handbook of Marine Craft Hydrodynamics and Motion + * Control" in Equation 3.57. Note that, in this model, we define the body frame to be coincident + * with the center of mass, such that r_g^b = 0. + * + * @param mass The total mass of the vehicle (kg). + * @param moments The moments of inertia. + * @param velocity The current velocity of the vehicle in the body frame. + * @return Eigen::MatrixXd + */ [[nodiscard]] static Eigen::MatrixXd calculateRigidBodyCoriolisMatrix( double mass, const MomentsOfInertia & moments, const geometry_msgs::msg::TwistStamped & velocity); - [[nodiscard]] static Eigen::MatrixXd calculateHydrodynamicCoriolixMatrix( + /** + * @brief Calculate the added Coriolis-centripetal matrix. + * + * @note The definition of the added Coriolis-centripetal matrix `C_A` used in this work is + * provided by Gianluca Antonelli's "Underwater Robots" in Section 2.4.1. + * + * @param added_mass The added mass coefficients. + * @param velocity The current velocity of the vehicle in the body frame. + * @return Eigen::MatrixXd + */ + [[nodiscard]] static Eigen::MatrixXd calculateAddedCoriolixMatrix( const AddedMass & added_mass, const geometry_msgs::msg::TwistStamped & velocity); + /** + * @brief Calculate the rigid body damping matrix. + * + * @note The rigid body damping matrix `D` is defined as the sum of the linear and nonlinear + * damping coefficients. + * + * @param velocity The current velocity of the vehicle in the body frame. + * @return Eigen::MatrixXd + */ [[nodiscard]] Eigen::MatrixXd calculateDampingMatrix( const geometry_msgs::msg::TwistStamped & velocity) const; + /** + * @brief Calculate the linear damping matrix. + * + * @note The definition of the linear damping matrix used in this work is provided by Gianluca + * Antonelli's "Underwater Robots" in Section 2.4.2. + * + * @param linear_damping The linear damping coefficients. + * @return Eigen::MatrixXd + */ [[nodiscard]] static Eigen::MatrixXd calculateLinearDampingMatrix( const LinearDamping & linear_damping); + /** + * @brief Calculate the nonlinear damping matrix. + * + * @note The definition of the nonlinear damping matrix used in this work is provided by Gianluca + * Antonelli's "Underwater Robots" in Section 2.4.2. + * + * @param quadratic_damping The nonlinear damping coefficients. + * @param velocity The current velocity of the vehicle in the body frame. + * @return Eigen::MatrixXd + */ [[nodiscard]] static Eigen::MatrixXd calculateNonlinearDampingMatrix( const NonlinearDamping & quadratic_damping, const geometry_msgs::msg::TwistStamped & velocity); + /** + * @brief Calculate the vector of force/moment due to gravity and buoyancy in the body-fixed + * frame. + * + * @note The definition used for the gravity and buoyancy vector `g` is given by Gianluca + * Antonelli's "Underwater Robots" in Section 2.5. + * + * @param pose The current pose of the vehicle in the inertial frame. + * @return Eigen::VectorXd + */ [[nodiscard]] Eigen::VectorXd calculateRestoringForcesVector( const geometry_msgs::msg::PoseStamped & pose) const; }; diff --git a/blue_dynamics/src/vehicle_dynamics.cpp b/blue_dynamics/src/vehicle_dynamics.cpp index 07e8e607..6203454c 100644 --- a/blue_dynamics/src/vehicle_dynamics.cpp +++ b/blue_dynamics/src/vehicle_dynamics.cpp @@ -28,7 +28,8 @@ namespace blue::dynamics VehicleDynamics::VehicleDynamics( double mass, double weight, double buoyancy, const MomentsOfInertia & moments, const AddedMass & added_mass, const LinearDamping & linear_damping, - const NonlinearDamping & quadratic_damping, const CenterOfBuoyancy & center_of_buoyancy) + const NonlinearDamping & quadratic_damping, const CenterOfBuoyancy & center_of_buoyancy, + const CenterOfGravity & center_of_gravity) : mass(mass), weight(weight), buoyancy(buoyancy), @@ -36,7 +37,8 @@ VehicleDynamics::VehicleDynamics( added_mass(std::move(added_mass)), linear_damping(std::move(linear_damping)), quadratic_damping(std::move(quadratic_damping)), - center_of_buoyancy(std::move(center_of_buoyancy)) + center_of_buoyancy(std::move(center_of_buoyancy)), + center_of_gravity(std::move(center_of_gravity)) { } @@ -51,10 +53,10 @@ VehicleDynamics::VehicleDynamics( [[nodiscard]] Eigen::MatrixXd VehicleDynamics::calculateInertiaMatrix() const { - return calculateRigidBodyMatrix(mass, moments) + calculateAddedMassMatrix(added_mass); + return calculateRigidBodyMassMatrix(mass, moments) + calculateAddedMassMatrix(added_mass); } -[[nodiscard]] Eigen::MatrixXd VehicleDynamics::calculateRigidBodyMatrix( +[[nodiscard]] Eigen::MatrixXd VehicleDynamics::calculateRigidBodyMassMatrix( double mass, const MomentsOfInertia & moments) { Eigen::MatrixXd mat(6, 6); @@ -68,14 +70,14 @@ VehicleDynamics::VehicleDynamics( [[nodiscard]] Eigen::MatrixXd VehicleDynamics::calculateAddedMassMatrix( const AddedMass & added_mass) { - return added_mass.toMatrix(); + return -1 * added_mass.toMatrix(); } [[nodiscard]] Eigen::MatrixXd VehicleDynamics::calculateCoriolisMatrix( const geometry_msgs::msg::TwistStamped & velocity) const { return calculateRigidBodyCoriolisMatrix(mass, moments, velocity) + - calculateHydrodynamicCoriolixMatrix(added_mass, velocity); + calculateAddedCoriolixMatrix(added_mass, velocity); } [[nodiscard]] Eigen::MatrixXd VehicleDynamics::calculateRigidBodyCoriolisMatrix( @@ -83,22 +85,19 @@ VehicleDynamics::VehicleDynamics( { Eigen::MatrixXd mat(6, 6); - Eigen::Matrix3d linear_vel = - mass * createSkewSymmetricMatrix( - velocity.twist.linear.x, velocity.twist.linear.y, velocity.twist.linear.z); + Eigen::Vector3d v2; + v2 << velocity.twist.angular.x, velocity.twist.angular.y, velocity.twist.angular.z; - Eigen::Matrix3d angular_vel = createSkewSymmetricMatrix( - moments.x * velocity.twist.angular.x, moments.y * velocity.twist.angular.y, - moments.z * velocity.twist.angular.z); + const Eigen::Vector3d moments_v2 = moments.toMatrix() * v2; - mat.topRightCorner(3, 3) = linear_vel; - mat.bottomLeftCorner(3, 3) = linear_vel; - mat.bottomRightCorner(3, 3) = angular_vel; + mat.topLeftCorner(3, 3) = mass * createSkewSymmetricMatrix(v2(0), v2(1), v2(2)); + mat.bottomRightCorner(3, 3) = + -1 * createSkewSymmetricMatrix(moments_v2(0), moments_v2(1), moments_v2(2)); return mat; } -[[nodiscard]] Eigen::MatrixXd VehicleDynamics::calculateHydrodynamicCoriolixMatrix( +[[nodiscard]] Eigen::MatrixXd VehicleDynamics::calculateAddedCoriolixMatrix( const AddedMass & added_mass, const geometry_msgs::msg::TwistStamped & velocity) { Eigen::MatrixXd mat(6, 6); @@ -145,5 +144,26 @@ VehicleDynamics::VehicleDynamics( [[nodiscard]] Eigen::VectorXd VehicleDynamics::calculateRestoringForcesVector( const geometry_msgs::msg::PoseStamped & pose) const { + Eigen::Quaterniond q( + pose.pose.orientation.w, pose.pose.orientation.x, pose.pose.orientation.y, + pose.pose.orientation.z); + + Eigen::Matrix3d rot = q.toRotationMatrix(); + + // The Z-axis points downwards, so gravity is positive and buoyancy is negative + Eigen::Vector3d fg; + fg << 0, 0, weight; + + Eigen::Vector3d fb; + fb << 0, 0, buoyancy; + fb *= -1; + + Eigen::VectorXd g_rb(6); + g_rb.topRows(3) = rot * (fg + fb); + g_rb.bottomRows(3) = + center_of_gravity.toVector().cross(rot * fg) + center_of_buoyancy.toVector().cross(rot * fb); + g_rb *= -1; + + return g_rb; } } // namespace blue::dynamics From fbaa3d6a0681a424c7fbe95371e643dec30a17ed Mon Sep 17 00:00:00 2001 From: Evan Palmer Date: Sat, 22 Apr 2023 19:59:35 -0700 Subject: [PATCH 14/34] Resolve initial bugs --- blue_dynamics/src/vehicle_dynamics.cpp | 15 +++++++++++---- 1 file changed, 11 insertions(+), 4 deletions(-) diff --git a/blue_dynamics/src/vehicle_dynamics.cpp b/blue_dynamics/src/vehicle_dynamics.cpp index 6203454c..ade69209 100644 --- a/blue_dynamics/src/vehicle_dynamics.cpp +++ b/blue_dynamics/src/vehicle_dynamics.cpp @@ -62,6 +62,8 @@ VehicleDynamics::VehicleDynamics( Eigen::MatrixXd mat(6, 6); mat.topLeftCorner(3, 3) = mass * Eigen::MatrixXd::Identity(3, 3); + mat.topRightCorner(3, 3) = Eigen::MatrixXd::Zero(3, 3); + mat.bottomLeftCorner(3, 3) = Eigen::MatrixXd::Zero(3, 3); mat.bottomRightCorner(3, 3) = moments.toMatrix(); return mat; @@ -91,6 +93,8 @@ VehicleDynamics::VehicleDynamics( const Eigen::Vector3d moments_v2 = moments.toMatrix() * v2; mat.topLeftCorner(3, 3) = mass * createSkewSymmetricMatrix(v2(0), v2(1), v2(2)); + mat.topRightCorner(3, 3) = Eigen::MatrixXd::Zero(3, 3); + mat.bottomLeftCorner(3, 3) = Eigen::MatrixXd::Zero(3, 3); mat.bottomRightCorner(3, 3) = -1 * createSkewSymmetricMatrix(moments_v2(0), moments_v2(1), moments_v2(2)); @@ -110,6 +114,7 @@ VehicleDynamics::VehicleDynamics( added_mass.k * velocity.twist.angular.x, added_mass.m * velocity.twist.angular.y, added_mass.n * velocity.twist.angular.z); + mat.topLeftCorner(3, 3) = Eigen::MatrixXd::Zero(3, 3); mat.topRightCorner(3, 3) = linear_vel; mat.bottomLeftCorner(3, 3) = linear_vel; mat.bottomRightCorner(3, 3) = angular_vel; @@ -134,11 +139,13 @@ VehicleDynamics::VehicleDynamics( const NonlinearDamping & quadratic_damping, const geometry_msgs::msg::TwistStamped & velocity) { Eigen::VectorXd vec(6); - vec << std::abs(velocity.twist.linear.x), std::abs(velocity.twist.linear.y), - std::abs(velocity.twist.linear.z), std::abs(velocity.twist.angular.x), - std::abs(velocity.twist.angular.y), std::abs(velocity.twist.angular.z); + vec << velocity.twist.linear.x, velocity.twist.linear.y, velocity.twist.linear.z, + velocity.twist.angular.x, velocity.twist.angular.y, velocity.twist.angular.z; - return -1 * quadratic_damping.toMatrix() * vec; + // Take the absolute value of each coefficient + vec = vec.cwiseAbs(); + + return -1 * (quadratic_damping.toMatrix() * vec).asDiagonal(); } [[nodiscard]] Eigen::VectorXd VehicleDynamics::calculateRestoringForcesVector( From 5ab75cc5cfa16f60faa5faced806114025078d67 Mon Sep 17 00:00:00 2001 From: Evan Palmer Date: Sat, 22 Apr 2023 20:02:16 -0700 Subject: [PATCH 15/34] Started working on unit testing the dynamics --- blue_dynamics/test/test_vehicle_dynamics.cpp | 26 ++++++++++++++++++++ 1 file changed, 26 insertions(+) create mode 100644 blue_dynamics/test/test_vehicle_dynamics.cpp diff --git a/blue_dynamics/test/test_vehicle_dynamics.cpp b/blue_dynamics/test/test_vehicle_dynamics.cpp new file mode 100644 index 00000000..e7eddf05 --- /dev/null +++ b/blue_dynamics/test/test_vehicle_dynamics.cpp @@ -0,0 +1,26 @@ +// Copyright 2023, Evan Palmer +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: + +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. + +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL +// THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +// THE SOFTWARE. + +#include "blue_dynamics/vehicle_dynamics.hpp" + +namespace blue::dynamics::test +{ + +} // namespace blue::dynamics::test From 975284487d02a67ad1d19e0f90dcde86a66d031b Mon Sep 17 00:00:00 2001 From: Evan Palmer Date: Sat, 22 Apr 2023 21:44:42 -0700 Subject: [PATCH 16/34] i dont remember --- .../blue_dynamics/current_dynamics.hpp | 19 +++ .../include/blue_dynamics/hydrodynamics.hpp | 116 +++++++++++++++--- .../blue_dynamics/thruster_dynamics.hpp | 19 +++ .../blue_dynamics/vehicle_dynamics.hpp | 17 +-- blue_dynamics/test/test_vehicle_dynamics.cpp | 21 ++++ 5 files changed, 167 insertions(+), 25 deletions(-) diff --git a/blue_dynamics/include/blue_dynamics/current_dynamics.hpp b/blue_dynamics/include/blue_dynamics/current_dynamics.hpp index e69de29b..0a25e9f7 100644 --- a/blue_dynamics/include/blue_dynamics/current_dynamics.hpp +++ b/blue_dynamics/include/blue_dynamics/current_dynamics.hpp @@ -0,0 +1,19 @@ +// Copyright 2023, Evan Palmer +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: + +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. + +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL +// THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +// THE SOFTWARE. diff --git a/blue_dynamics/include/blue_dynamics/hydrodynamics.hpp b/blue_dynamics/include/blue_dynamics/hydrodynamics.hpp index d442d6ba..1ae2fded 100644 --- a/blue_dynamics/include/blue_dynamics/hydrodynamics.hpp +++ b/blue_dynamics/include/blue_dynamics/hydrodynamics.hpp @@ -23,6 +23,9 @@ namespace blue::dynamics { +/** + * @brief A base struct that can be used to define a collection of 6-D hydrodynamic coefficients. + */ struct Hydrodynamics6d { double x; @@ -32,6 +35,16 @@ struct Hydrodynamics6d double m; double n; + /** + * @brief Construct a new Hydrodynamics6d object. + * + * @param x The X coefficient. + * @param y The Y coefficient. + * @param z The Z coefficient. + * @param k The K coefficient. + * @param m The M coefficient. + * @param n The Z coefficient. + */ Hydrodynamics6d(double x, double y, double z, double k, double m, double n) : x(x), y(y), @@ -42,6 +55,13 @@ struct Hydrodynamics6d { } + /** + * @brief Create a matrix using the coefficients. + * + * @note The base struct creates a diagonal matrix from the coefficients. + * + * @return Eigen::MatrixXd + */ [[nodiscard]] virtual Eigen::MatrixXd toMatrix() const { Eigen::VectorXd vec; @@ -50,6 +70,11 @@ struct Hydrodynamics6d return vec.asDiagonal().toDenseMatrix(); } + /** + * @brief Create a vector using the coefficients. + * + * @return Eigen::VectorXd + */ [[nodiscard]] virtual Eigen::VectorXd toVector() const { Eigen::VectorXd vec; @@ -59,12 +84,22 @@ struct Hydrodynamics6d } }; +/** + * @brief A base struct that can be used to define a collection of 3-D hydrodynamic coefficients. + */ struct Hydrodynamics3d { double x; double y; double z; + /** + * @brief Construct a new Hydrodynamics3d object + * + * @param x The X coefficient. + * @param y The Y coefficient. + * @param z The Z coefficient. + */ Hydrodynamics3d(double x, double y, double z) : x(x), y(y), @@ -72,6 +107,13 @@ struct Hydrodynamics3d { } + /** + * @brief Create a matrix from the coefficients. + * + * @note The base struct creates a diagonal matrix from the coefficients. + * + * @return Eigen::Matrix3d + */ [[nodiscard]] virtual Eigen::Matrix3d toMatrix() const { Eigen::Vector3d vec; @@ -80,6 +122,11 @@ struct Hydrodynamics3d return vec.asDiagonal().toDenseMatrix(); } + /** + * @brief Create a vector from the coefficients. + * + * @return Eigen::Vector3d + */ [[nodiscard]] virtual Eigen::Vector3d toVector() const { Eigen::Vector3d vec; @@ -89,41 +136,74 @@ struct Hydrodynamics3d } }; +/** + * @brief The moments of inertia of a rigid body. + */ struct MomentsOfInertia : Hydrodynamics3d { + /** + * @brief Construct a new MomentsOfInertia object. + * + * @param x The I_xx coefficient. + * @param y The I_yy coefficient. + * @param z The I_zz coefficient. + */ MomentsOfInertia(double x, double y, double z) : Hydrodynamics3d(x, y, z) { } }; +/** + * @brief The center of buoyancy of a submerged body. + */ struct CenterOfBuoyancy : Hydrodynamics3d { + /** + * @brief Construct a new CenterOfBuoyancy object. + * + * @param x The center of buoyancy along the x-axis. + * @param y The center of buoyancy along the y-axis. + * @param z The center of buoyancy along the z-axis. + */ CenterOfBuoyancy(double x, double y, double z) : Hydrodynamics3d(x, y, z) { } }; +/** + * @brief The center of gravity of a submerged body. + */ struct CenterOfGravity : Hydrodynamics3d { + /** + * @brief Construct a new CenterOfGravity object. + * + * @param x The center of gravity along the x-axis. + * @param y The center of gravity along the y-axis. + * @param z The center of gravity along the z-axis. + */ CenterOfGravity(double x, double y, double z) : Hydrodynamics3d(x, y, z) { } }; +/** + * @brief The added mass hydrodynamic coefficients. + */ struct AddedMass : Hydrodynamics6d { /** * @brief Construct a new AddedMass object. * - * @param x_u_dot - * @param y_v_dot - * @param z_w_dot - * @param k_p_dot - * @param m_q_dot - * @param n_r_dot + * @param x_u_dot The X_dot{u} coefficient. + * @param y_v_dot The X_dot{v} coefficient. + * @param z_w_dot The Z_dot{w} coefficient. + * @param k_p_dot The K_dot{p} coefficient. + * @param m_q_dot The K_dot{q} coefficient. + * @param n_r_dot The N_dot{r} coefficient. */ AddedMass( double x_u_dot, double y_v_dot, double z_w_dot, double k_p_dot, double m_q_dot, double n_r_dot) @@ -141,12 +221,12 @@ struct LinearDamping : Hydrodynamics6d /** * @brief Construct a new LinearDamping object. * - * @param x_u - * @param y_v - * @param z_w - * @param k_p - * @param m_q - * @param n_r + * @param x_u The X_u coefficient. + * @param y_v The Y_v coefficient. + * @param z_w The Z_w coefficient. + * @param k_p The K_p coefficient. + * @param m_q The M_q coefficient. + * @param n_r The N_r coefficient. */ LinearDamping(double x_u, double y_v, double z_w, double k_p, double m_q, double n_r) : Hydrodynamics6d(x_u, y_v, z_w, k_p, m_q, n_r) @@ -163,12 +243,12 @@ struct NonlinearDamping : Hydrodynamics6d /** * @brief Construct a new NonlinearDamping object * - * @param x_uu - * @param y_vv - * @param z_ww - * @param k_pp - * @param m_qq - * @param n_rr + * @param x_uu The X_{u|u|} coefficient. + * @param y_vv The Y_{v|v|} coefficient. + * @param z_ww The Z_{w|w|} coefficient. + * @param k_pp The K_{p|p|} coefficient. + * @param m_qq The M_{q|q|} coefficient. + * @param n_rr The N_{r|r|} coefficient. */ NonlinearDamping(double x_uu, double y_vv, double z_ww, double k_pp, double m_qq, double n_rr) : Hydrodynamics6d(x_uu, y_vv, z_ww, k_pp, m_qq, n_rr) diff --git a/blue_dynamics/include/blue_dynamics/thruster_dynamics.hpp b/blue_dynamics/include/blue_dynamics/thruster_dynamics.hpp index e69de29b..0a25e9f7 100644 --- a/blue_dynamics/include/blue_dynamics/thruster_dynamics.hpp +++ b/blue_dynamics/include/blue_dynamics/thruster_dynamics.hpp @@ -0,0 +1,19 @@ +// Copyright 2023, Evan Palmer +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: + +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. + +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL +// THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +// THE SOFTWARE. diff --git a/blue_dynamics/include/blue_dynamics/vehicle_dynamics.hpp b/blue_dynamics/include/blue_dynamics/vehicle_dynamics.hpp index 0230fad3..d6ef3718 100644 --- a/blue_dynamics/include/blue_dynamics/vehicle_dynamics.hpp +++ b/blue_dynamics/include/blue_dynamics/vehicle_dynamics.hpp @@ -28,6 +28,14 @@ namespace blue::dynamics { +/** + * @brief The hydrodynamic model for an underwater vehicle. + * + * @note This implementation draws from Thor I. Fossen's "Handbook of Marine Craft Hydrodynamics + * and Motion Control" (2011) and Gianluca Antonelli's "Underwater Robots" (2014) for the + * hydrodynamic model. Relevant equations and definitions used are documented with each method. + * + */ struct VehicleDynamics { double mass; // The total mass of the vehicle (kg). @@ -43,11 +51,6 @@ struct VehicleDynamics /** * @brief Create a new VehicleDynamics object. * - * @note This implementation draws from Thor I. Fossen's "Handbook of Marine Craft Hydrodynamics - * and Motion Control" (2011) and Gianluca Antonelli's "Underwater Robots" (2014) for the - * hydrodynamic model. Relevant equations and definitions used are documented with each method. - * - * * @param mass The total mass of the vehicle (kg). * @param weight The weight of the vehicle in the inertial frame. This is denoted as `W` in * Section 2.5 of Gianluca Antonelli's "Underwater Robots". @@ -84,8 +87,8 @@ struct VehicleDynamics /** * @brief Calculate the vehicle's inertia matrix. * - * @note The inertia matrix, commonly denoted by textbooks as `M`, is the sum of the rigid body - * mass `M_RB` and the added mass `M_A` such that `M = M_RB + M_A`. + * @note The inertia matrix `M` is the sum of the rigid body mass `M_RB` and the added mass `M_A` + * such that `M = M_RB + M_A`. * * @return Eigen::MatrixXd */ diff --git a/blue_dynamics/test/test_vehicle_dynamics.cpp b/blue_dynamics/test/test_vehicle_dynamics.cpp index e7eddf05..51b221a3 100644 --- a/blue_dynamics/test/test_vehicle_dynamics.cpp +++ b/blue_dynamics/test/test_vehicle_dynamics.cpp @@ -18,9 +18,30 @@ // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN // THE SOFTWARE. +#include +#include + +#include + #include "blue_dynamics/vehicle_dynamics.hpp" namespace blue::dynamics::test { +TEST(VehicleDynamicsTest, CreatesSkewSymmetricMatrix) {} + +TEST(VehicleDynamicsTest, CalculatesRigidBodyMassMatrix) {} + +TEST(VehicleDynamicsTest, CalculatesAddedMassMatrix) {} + +TEST(VehicleDynamicsTest, CalculatesRigidBodyCoriolisMatrix) {} + +TEST(VehicleDynamicsTest, CalculatesAddedCoriolisMatrix) {} + +TEST(VehicleDynamicsTest, CalculatesLinearMatrix) {} + +TEST(VehicleDynamicsTest, CalculatesNonlinearMatrix) {} + +TEST(VehicleDynamicsTest, CalculatesRestoringForcesVector) {} + } // namespace blue::dynamics::test From 8ab67d558b940a41c03e7efcaa606575d42e75c5 Mon Sep 17 00:00:00 2001 From: Evan Palmer Date: Mon, 24 Apr 2023 20:46:32 -0700 Subject: [PATCH 17/34] Started implementing test cases and squashed some bugs --- blue_bridge/CMakeLists.txt | 3 +- blue_control/CMakeLists.txt | 3 +- .../include/blue_control/controller.hpp | 4 + blue_dynamics/CMakeLists.txt | 8 +- .../blue_dynamics/current_dynamics.hpp | 2 + .../include/blue_dynamics/hydrodynamics.hpp | 52 +++--- .../blue_dynamics/thruster_dynamics.hpp | 2 + .../blue_dynamics/vehicle_dynamics.hpp | 2 + blue_dynamics/package.xml | 2 + blue_dynamics/src/vehicle_dynamics.cpp | 14 +- blue_dynamics/test/test_vehicle_dynamics.cpp | 153 +++++++++++++++++- 11 files changed, 199 insertions(+), 46 deletions(-) diff --git a/blue_bridge/CMakeLists.txt b/blue_bridge/CMakeLists.txt index 7e9ad530..06f5f877 100644 --- a/blue_bridge/CMakeLists.txt +++ b/blue_bridge/CMakeLists.txt @@ -45,7 +45,8 @@ install( if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) - # Run linters found in package.xml except those below + # 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() diff --git a/blue_control/CMakeLists.txt b/blue_control/CMakeLists.txt index f653c837..849366c8 100644 --- a/blue_control/CMakeLists.txt +++ b/blue_control/CMakeLists.txt @@ -31,7 +31,8 @@ endforeach() if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) - # Run linters found in package.xml except those below + # 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() diff --git a/blue_control/include/blue_control/controller.hpp b/blue_control/include/blue_control/controller.hpp index 8d4a2ea8..109c3c2a 100644 --- a/blue_control/include/blue_control/controller.hpp +++ b/blue_control/include/blue_control/controller.hpp @@ -18,6 +18,10 @@ // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN // THE SOFTWARE. +#pragma once + +#include + #include "geometry_msgs/msg/pose_stamped.hpp" #include "mavros_msgs/msg/override_rc_in.hpp" #include "nav_msgs/msg/odometry.hpp" diff --git a/blue_dynamics/CMakeLists.txt b/blue_dynamics/CMakeLists.txt index ae80fe2a..09bbd554 100644 --- a/blue_dynamics/CMakeLists.txt +++ b/blue_dynamics/CMakeLists.txt @@ -55,7 +55,6 @@ install( if(BUILD_TESTING) find_package(ament_cmake_gtest REQUIRED) - find_package(ament_cmake_gmock REQUIRED) find_package(ament_lint_auto REQUIRED) # Run linters found in package.xml except the two below @@ -63,6 +62,13 @@ if(BUILD_TESTING) set(ament_cmake_uncrustify_FOUND TRUE) ament_lint_auto_find_test_dependencies() + + # Setup unit tests + ament_add_gtest( + test_vehicle_dynamics + test/test_vehicle_dynamics.cpp + ) + target_link_libraries(test_vehicle_dynamics ${PROJECT_NAME}) endif() ament_export_targets(${PROJECT_NAME} HAS_LIBRARY_TARGET) diff --git a/blue_dynamics/include/blue_dynamics/current_dynamics.hpp b/blue_dynamics/include/blue_dynamics/current_dynamics.hpp index 0a25e9f7..d7711b1c 100644 --- a/blue_dynamics/include/blue_dynamics/current_dynamics.hpp +++ b/blue_dynamics/include/blue_dynamics/current_dynamics.hpp @@ -17,3 +17,5 @@ // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN // THE SOFTWARE. + +#pragma once diff --git a/blue_dynamics/include/blue_dynamics/hydrodynamics.hpp b/blue_dynamics/include/blue_dynamics/hydrodynamics.hpp index 1ae2fded..ecc10349 100644 --- a/blue_dynamics/include/blue_dynamics/hydrodynamics.hpp +++ b/blue_dynamics/include/blue_dynamics/hydrodynamics.hpp @@ -18,6 +18,8 @@ // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN // THE SOFTWARE. +#pragma once + #include namespace blue::dynamics @@ -56,31 +58,28 @@ struct Hydrodynamics6d } /** - * @brief Create a matrix using the coefficients. - * - * @note The base struct creates a diagonal matrix from the coefficients. + * @brief Create a vector using the coefficients. * - * @return Eigen::MatrixXd + * @return Eigen::VectorXd */ - [[nodiscard]] virtual Eigen::MatrixXd toMatrix() const + [[nodiscard]] virtual Eigen::VectorXd toVector() const { - Eigen::VectorXd vec; + Eigen::VectorXd vec(6); // NOLINT vec << x, y, z, k, m, n; - return vec.asDiagonal().toDenseMatrix(); + return vec; } /** - * @brief Create a vector using the coefficients. + * @brief Create a matrix using the coefficients. * - * @return Eigen::VectorXd + * @note The base struct creates a diagonal matrix from the coefficients. + * + * @return Eigen::MatrixXd */ - [[nodiscard]] virtual Eigen::VectorXd toVector() const + [[nodiscard]] virtual Eigen::MatrixXd toMatrix() const { - Eigen::VectorXd vec; - vec << x, y, z, k, m, n; - - return vec; + return toVector().asDiagonal().toDenseMatrix(); } }; @@ -108,31 +107,28 @@ struct Hydrodynamics3d } /** - * @brief Create a matrix from the coefficients. - * - * @note The base struct creates a diagonal matrix from the coefficients. + * @brief Create a vector from the coefficients. * - * @return Eigen::Matrix3d + * @return Eigen::Vector3d */ - [[nodiscard]] virtual Eigen::Matrix3d toMatrix() const + [[nodiscard]] virtual Eigen::Vector3d toVector() const { - Eigen::Vector3d vec; + Eigen::Vector3d vec(3); // NOLINT vec << x, y, z; - return vec.asDiagonal().toDenseMatrix(); + return vec; } /** - * @brief Create a vector from the coefficients. + * @brief Create a matrix from the coefficients. * - * @return Eigen::Vector3d + * @note The base struct creates a diagonal matrix from the coefficients. + * + * @return Eigen::Matrix3d */ - [[nodiscard]] virtual Eigen::Vector3d toVector() const + [[nodiscard]] virtual Eigen::Matrix3d toMatrix() const { - Eigen::Vector3d vec; - vec << x, y, z; - - return vec; + return toVector().asDiagonal().toDenseMatrix(); } }; diff --git a/blue_dynamics/include/blue_dynamics/thruster_dynamics.hpp b/blue_dynamics/include/blue_dynamics/thruster_dynamics.hpp index 0a25e9f7..d7711b1c 100644 --- a/blue_dynamics/include/blue_dynamics/thruster_dynamics.hpp +++ b/blue_dynamics/include/blue_dynamics/thruster_dynamics.hpp @@ -17,3 +17,5 @@ // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN // THE SOFTWARE. + +#pragma once diff --git a/blue_dynamics/include/blue_dynamics/vehicle_dynamics.hpp b/blue_dynamics/include/blue_dynamics/vehicle_dynamics.hpp index d6ef3718..d164597c 100644 --- a/blue_dynamics/include/blue_dynamics/vehicle_dynamics.hpp +++ b/blue_dynamics/include/blue_dynamics/vehicle_dynamics.hpp @@ -18,6 +18,8 @@ // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN // THE SOFTWARE. +#pragma once + #include #include "blue_dynamics/hydrodynamics.hpp" diff --git a/blue_dynamics/package.xml b/blue_dynamics/package.xml index ed08bc1a..dd9a697e 100644 --- a/blue_dynamics/package.xml +++ b/blue_dynamics/package.xml @@ -15,6 +15,8 @@ eigen3_cmake_module + ament_cmake_gtest + ament_cmake_gmock ament_lint_auto ament_lint_common diff --git a/blue_dynamics/src/vehicle_dynamics.cpp b/blue_dynamics/src/vehicle_dynamics.cpp index ade69209..9ed9af69 100644 --- a/blue_dynamics/src/vehicle_dynamics.cpp +++ b/blue_dynamics/src/vehicle_dynamics.cpp @@ -72,7 +72,7 @@ VehicleDynamics::VehicleDynamics( [[nodiscard]] Eigen::MatrixXd VehicleDynamics::calculateAddedMassMatrix( const AddedMass & added_mass) { - return -1 * added_mass.toMatrix(); + return -added_mass.toMatrix(); } [[nodiscard]] Eigen::MatrixXd VehicleDynamics::calculateCoriolisMatrix( @@ -87,7 +87,7 @@ VehicleDynamics::VehicleDynamics( { Eigen::MatrixXd mat(6, 6); - Eigen::Vector3d v2; + Eigen::Vector3d v2(3); // NOLINT v2 << velocity.twist.angular.x, velocity.twist.angular.y, velocity.twist.angular.z; const Eigen::Vector3d moments_v2 = moments.toMatrix() * v2; @@ -96,7 +96,7 @@ VehicleDynamics::VehicleDynamics( mat.topRightCorner(3, 3) = Eigen::MatrixXd::Zero(3, 3); mat.bottomLeftCorner(3, 3) = Eigen::MatrixXd::Zero(3, 3); mat.bottomRightCorner(3, 3) = - -1 * createSkewSymmetricMatrix(moments_v2(0), moments_v2(1), moments_v2(2)); + -createSkewSymmetricMatrix(moments_v2(0), moments_v2(1), moments_v2(2)); return mat; } @@ -132,7 +132,7 @@ VehicleDynamics::VehicleDynamics( [[nodiscard]] Eigen::MatrixXd VehicleDynamics::calculateLinearDampingMatrix( const LinearDamping & linear_damping) { - return -1 * linear_damping.toMatrix(); + return -linear_damping.toMatrix(); } [[nodiscard]] Eigen::MatrixXd VehicleDynamics::calculateNonlinearDampingMatrix( @@ -145,7 +145,7 @@ VehicleDynamics::VehicleDynamics( // Take the absolute value of each coefficient vec = vec.cwiseAbs(); - return -1 * (quadratic_damping.toMatrix() * vec).asDiagonal(); + return -(quadratic_damping.toMatrix() * vec).asDiagonal().toDenseMatrix(); } [[nodiscard]] Eigen::VectorXd VehicleDynamics::calculateRestoringForcesVector( @@ -158,10 +158,10 @@ VehicleDynamics::VehicleDynamics( Eigen::Matrix3d rot = q.toRotationMatrix(); // The Z-axis points downwards, so gravity is positive and buoyancy is negative - Eigen::Vector3d fg; + Eigen::Vector3d fg(3); // NOLINT fg << 0, 0, weight; - Eigen::Vector3d fb; + Eigen::Vector3d fb(3); fb << 0, 0, buoyancy; fb *= -1; diff --git a/blue_dynamics/test/test_vehicle_dynamics.cpp b/blue_dynamics/test/test_vehicle_dynamics.cpp index 51b221a3..fcbd4f64 100644 --- a/blue_dynamics/test/test_vehicle_dynamics.cpp +++ b/blue_dynamics/test/test_vehicle_dynamics.cpp @@ -18,30 +18,167 @@ // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN // THE SOFTWARE. -#include #include #include +#include #include "blue_dynamics/vehicle_dynamics.hpp" +#include "geometry_msgs/msg/twist_stamped.hpp" namespace blue::dynamics::test { -TEST(VehicleDynamicsTest, CreatesSkewSymmetricMatrix) {} +TEST(VehicleDynamicsTest, CreatesSkewSymmetricMatrix) +{ + const double a1 = 1.0; + const double a2 = 2.0; + const double a3 = 3.0; + + Eigen::Matrix3d expected; + expected << 0, -a3, a2, a3, 0, -a1, -a2, a1, 0; + + const Eigen::Matrix3d actual = + blue::dynamics::VehicleDynamics::createSkewSymmetricMatrix(a1, a2, a3); + + ASSERT_TRUE(expected.isApprox(actual)); +} + +TEST(VehicleDynamicsTest, CalculatesRigidBodyMassMatrix) +{ + const auto moments = blue::dynamics::MomentsOfInertia(1.0, 2.0, 3.0); + const double mass = 5.0; + + Eigen::VectorXd diagonal(6); // NOLINT + diagonal << mass, mass, mass, moments.x, moments.y, moments.z; + + const Eigen::MatrixXd expected = diagonal.asDiagonal().toDenseMatrix(); + + const Eigen::MatrixXd actual = + blue::dynamics::VehicleDynamics::calculateRigidBodyMassMatrix(mass, moments); + + ASSERT_TRUE(expected.isApprox(actual)); +} + +TEST(VehicleDynamicsTest, CalculatesAddedMassMatrix) +{ + const blue::dynamics::AddedMass added_mass = + blue::dynamics::AddedMass(1.0, 2.0, 3.0, 4.0, 5.0, 6.0); + + const Eigen::MatrixXd expected = -added_mass.toMatrix(); + + const Eigen::MatrixXd actual = + blue::dynamics::VehicleDynamics::calculateAddedMassMatrix(added_mass); + + ASSERT_TRUE(expected.isApprox(actual)); +} + +TEST(VehicleDynamicsTest, CalculatesRigidBodyCoriolisMatrix) +{ + auto moments = blue::dynamics::MomentsOfInertia(1.0, 2.0, 3.0); + const double mass = 5.0; + + geometry_msgs::msg::TwistStamped velocity; + velocity.twist.angular.x = 1.0; + velocity.twist.angular.y = 2.0; + velocity.twist.angular.z = 3.0; + + Eigen::MatrixXd expected(6, 6); + expected.topRightCorner(3, 3) = Eigen::MatrixXd::Zero(3, 3); + expected.bottomLeftCorner(3, 3) = Eigen::MatrixXd::Zero(3, 3); + expected.topLeftCorner(3, 3) = + mass * blue::dynamics::VehicleDynamics::createSkewSymmetricMatrix( + velocity.twist.angular.x, velocity.twist.angular.y, velocity.twist.angular.z); -TEST(VehicleDynamicsTest, CalculatesRigidBodyMassMatrix) {} + Eigen::Vector3d v2(3); + v2 << velocity.twist.angular.x, velocity.twist.angular.y, velocity.twist.angular.z; -TEST(VehicleDynamicsTest, CalculatesAddedMassMatrix) {} + const Eigen::Vector3d moments_v2 = moments.toMatrix() * v2; -TEST(VehicleDynamicsTest, CalculatesRigidBodyCoriolisMatrix) {} + expected.bottomRightCorner(3, 3) = -blue::dynamics::VehicleDynamics::createSkewSymmetricMatrix( + moments_v2(0), moments_v2(1), moments_v2(2)); -TEST(VehicleDynamicsTest, CalculatesAddedCoriolisMatrix) {} + Eigen::MatrixXd actual = + blue::dynamics::VehicleDynamics::calculateRigidBodyCoriolisMatrix(mass, moments, velocity); -TEST(VehicleDynamicsTest, CalculatesLinearMatrix) {} + ASSERT_TRUE(expected.isApprox(actual)); +} -TEST(VehicleDynamicsTest, CalculatesNonlinearMatrix) {} +TEST(VehicleDynamicsTest, CalculatesAddedCoriolisMatrix) +{ + const blue::dynamics::AddedMass added_mass = + blue::dynamics::AddedMass(1.0, 2.0, 3.0, 4.0, 5.0, 6.0); + + geometry_msgs::msg::TwistStamped velocity; + velocity.twist.linear.x = 1.0; + velocity.twist.linear.y = 2.0; + velocity.twist.linear.z = 3.0; + velocity.twist.angular.x = 1.0; + velocity.twist.angular.y = 2.0; + velocity.twist.angular.z = 3.0; + + Eigen::MatrixXd linear = blue::dynamics::VehicleDynamics::createSkewSymmetricMatrix( + added_mass.x * velocity.twist.linear.x, added_mass.y * velocity.twist.linear.y, + added_mass.z * velocity.twist.linear.z); + + Eigen::MatrixXd expected(6, 6); + expected.topLeftCorner(3, 3) = Eigen::MatrixXd::Zero(3, 3); + expected.topRightCorner(3, 3) = linear; + expected.bottomLeftCorner(3, 3) = linear; + expected.bottomRightCorner(3, 3) = blue::dynamics::VehicleDynamics::createSkewSymmetricMatrix( + added_mass.k * velocity.twist.angular.x, added_mass.m * velocity.twist.angular.y, + added_mass.n * velocity.twist.angular.z); + + Eigen::MatrixXd actual = + blue::dynamics::VehicleDynamics::calculateAddedCoriolixMatrix(added_mass, velocity); + + ASSERT_TRUE(expected.isApprox(actual)); +} + +TEST(VehicleDynamicsTest, CalculatesLinearDampingMatrix) +{ + const auto linear = blue::dynamics::LinearDamping(1.0, 2.0, 3.0, 4.0, 5.0, 6.0); + const Eigen::MatrixXd expected = -linear.toMatrix(); + + const Eigen::MatrixXd actual = + blue::dynamics::VehicleDynamics::calculateLinearDampingMatrix(linear); + + ASSERT_TRUE(expected.isApprox(actual)); +} + +TEST(VehicleDynamicsTest, CalculatesNonlinearDampingMatrix) +{ + geometry_msgs::msg::TwistStamped velocity; + velocity.twist.linear.x = -1.0; + velocity.twist.linear.y = -2.0; + velocity.twist.linear.z = -3.0; + velocity.twist.angular.x = -1.0; + velocity.twist.angular.y = -2.0; + velocity.twist.angular.z = -3.0; + + Eigen::VectorXd velocity_vect(6); // NOLINT + velocity_vect << std::abs(velocity.twist.linear.x), std::abs(velocity.twist.linear.y), + std::abs(velocity.twist.linear.z), std::abs(velocity.twist.angular.x), + std::abs(velocity.twist.angular.y), std::abs(velocity.twist.angular.z); + + const auto nonlinear = blue::dynamics::NonlinearDamping(1.0, 2.0, 3.0, 4.0, 5.0, 6.0); + const Eigen::MatrixXd expected = + -(nonlinear.toMatrix() * velocity_vect).asDiagonal().toDenseMatrix(); + + const Eigen::MatrixXd actual = + blue::dynamics::VehicleDynamics::calculateNonlinearDampingMatrix(nonlinear, velocity); + + ASSERT_TRUE(expected.isApprox(actual)); +} TEST(VehicleDynamicsTest, CalculatesRestoringForcesVector) {} } // namespace blue::dynamics::test + +int main(int argc, char ** argv) +{ + ::testing::InitGoogleTest(&argc, argv); + const int result = RUN_ALL_TESTS(); + + return result; +} From dec9c6ba61881b73610e8cd63d915af811312b7d Mon Sep 17 00:00:00 2001 From: Evan Palmer Date: Tue, 25 Apr 2023 11:45:31 -0700 Subject: [PATCH 18/34] Finished vehicle dynamics test cases and added current effects --- blue_dynamics/CMakeLists.txt | 2 + .../include/blue_dynamics/current_effects.hpp | 61 +++++++ .../include/blue_dynamics/hydrodynamics.hpp | 153 +++++++++--------- .../blue_dynamics/thruster_dynamics.hpp | 12 ++ .../blue_dynamics/vehicle_dynamics.hpp | 24 ++- blue_dynamics/package.xml | 4 +- .../current_effects.cpp} | 33 +++- blue_dynamics/src/hydrodynamics.cpp | 131 +++++++++++++++ blue_dynamics/src/vehicle_dynamics.cpp | 15 +- blue_dynamics/test/test_vehicle_dynamics.cpp | 38 ++++- 10 files changed, 372 insertions(+), 101 deletions(-) create mode 100644 blue_dynamics/include/blue_dynamics/current_effects.hpp rename blue_dynamics/{include/blue_dynamics/current_dynamics.hpp => src/current_effects.cpp} (60%) create mode 100644 blue_dynamics/src/hydrodynamics.cpp diff --git a/blue_dynamics/CMakeLists.txt b/blue_dynamics/CMakeLists.txt index 09bbd554..a8c347a9 100644 --- a/blue_dynamics/CMakeLists.txt +++ b/blue_dynamics/CMakeLists.txt @@ -33,6 +33,8 @@ include_directories( add_library(${PROJECT_NAME} SHARED src/vehicle_dynamics.cpp + src/current_effects.cpp + src/hydrodynamics.cpp ) target_link_libraries(${PROJECT_NAME} ${rclcpp_LIBRARIES}) diff --git a/blue_dynamics/include/blue_dynamics/current_effects.hpp b/blue_dynamics/include/blue_dynamics/current_effects.hpp new file mode 100644 index 00000000..e2adeea6 --- /dev/null +++ b/blue_dynamics/include/blue_dynamics/current_effects.hpp @@ -0,0 +1,61 @@ +// Copyright 2023, Evan Palmer +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: + +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. + +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL +// THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +// THE SOFTWARE. + +#pragma once + +#include + +#include "geometry_msgs/msg/pose_stamped.hpp" + +namespace blue::dynamics +{ + +struct CurrentEffects +{ + double v_cx; // The linear flow rate in the x direction. + double v_cy; // The linear flow rate in the y direction. + double v_cz; // The linear flow rate in the z direction. + double v_cp; // The rotational flow rate around the x axis. + double v_cq; // The rotational flow rate around the y axis. + double v_cr; // The rotational flow rate around the z axis. + + /** + * @brief Construct a new CurrentEffects object. + * + * @param v_cx The linear flow rate in the x direction. + * @param v_cy The linear flow rate in the y direction. + * @param v_cz The linear flow rate in the z direction. + * @param v_cp The rotational flow rate around the x axis. + * @param v_cq The rotational flow rate around the y axis. + * @param v_cr The rotational flow rate around the z axis. + */ + CurrentEffects(double v_cx, double v_cy, double v_cz, double v_cp, double v_cq, double v_cr); + + /** + * @brief Calculate the ocean current velocity with respect to the body frame. + * + * @param pose The current pose of the vehicle in the inertial frame. + * @return The ocean current velocity in the body frame. + */ + [[nodiscard]] Eigen::VectorXd calculateCurrentEffects( + const geometry_msgs::msg::PoseStamped & pose) const; +}; + +} // namespace blue::dynamics diff --git a/blue_dynamics/include/blue_dynamics/hydrodynamics.hpp b/blue_dynamics/include/blue_dynamics/hydrodynamics.hpp index ecc10349..69aaad2d 100644 --- a/blue_dynamics/include/blue_dynamics/hydrodynamics.hpp +++ b/blue_dynamics/include/blue_dynamics/hydrodynamics.hpp @@ -25,17 +25,37 @@ namespace blue::dynamics { +/** + * @brief A common base class to use for defining a collection of N-D hydrodynamics coefficients. + */ +struct HydrodynamicsXd +{ + /** + * @brief Convert the coefficients into a column vector. + * + * @return The coefficients as a vector. + */ + [[nodiscard]] virtual Eigen::VectorXd toVector() const = 0; + + /** + * @brief Convert the coefficients into a matrix. + * + * @return The coefficients as a matrix. + */ + [[nodiscard]] virtual Eigen::MatrixXd toMatrix() const = 0; +}; + /** * @brief A base struct that can be used to define a collection of 6-D hydrodynamic coefficients. */ -struct Hydrodynamics6d +struct Hydrodynamics6d : HydrodynamicsXd { - double x; - double y; - double z; - double k; - double m; - double n; + double x; // The X coefficient. + double y; // The Y coefficient. + double z; // The Z coefficient. + double k; // The K coefficient. + double m; // The M coefficient. + double n; // The Z coefficient. /** * @brief Construct a new Hydrodynamics6d object. @@ -47,50 +67,33 @@ struct Hydrodynamics6d * @param m The M coefficient. * @param n The Z coefficient. */ - Hydrodynamics6d(double x, double y, double z, double k, double m, double n) - : x(x), - y(y), - z(z), - k(k), - m(m), - n(n) - { - } + Hydrodynamics6d(double x, double y, double z, double k, double m, double n); /** * @brief Create a vector using the coefficients. * - * @return Eigen::VectorXd + * @return The coefficients as a column vector. */ - [[nodiscard]] virtual Eigen::VectorXd toVector() const - { - Eigen::VectorXd vec(6); // NOLINT - vec << x, y, z, k, m, n; - - return vec; - } + [[nodiscard]] Eigen::VectorXd toVector() const override; /** * @brief Create a matrix using the coefficients. * * @note The base struct creates a diagonal matrix from the coefficients. * - * @return Eigen::MatrixXd + * @return The coefficients as a 6x6 diagonal matrix. */ - [[nodiscard]] virtual Eigen::MatrixXd toMatrix() const - { - return toVector().asDiagonal().toDenseMatrix(); - } + [[nodiscard]] Eigen::MatrixXd toMatrix() const override; }; /** * @brief A base struct that can be used to define a collection of 3-D hydrodynamic coefficients. */ -struct Hydrodynamics3d +struct Hydrodynamics3d : HydrodynamicsXd { - double x; - double y; - double z; + double x; // The X coefficient. + double y; // The Y coefficient. + double z; // The Z coefficient. /** * @brief Construct a new Hydrodynamics3d object @@ -99,37 +102,23 @@ struct Hydrodynamics3d * @param y The Y coefficient. * @param z The Z coefficient. */ - Hydrodynamics3d(double x, double y, double z) - : x(x), - y(y), - z(z) - { - } + Hydrodynamics3d(double x, double y, double z); /** * @brief Create a vector from the coefficients. * - * @return Eigen::Vector3d + * @return The coefficients as a column vector. */ - [[nodiscard]] virtual Eigen::Vector3d toVector() const - { - Eigen::Vector3d vec(3); // NOLINT - vec << x, y, z; - - return vec; - } + [[nodiscard]] Eigen::VectorXd toVector() const override; /** * @brief Create a matrix from the coefficients. * * @note The base struct creates a diagonal matrix from the coefficients. * - * @return Eigen::Matrix3d + * @return The coefficients as a 3x3 diagonal matrix. */ - [[nodiscard]] virtual Eigen::Matrix3d toMatrix() const - { - return toVector().asDiagonal().toDenseMatrix(); - } + [[nodiscard]] Eigen::MatrixXd toMatrix() const override; }; /** @@ -137,6 +126,11 @@ struct Hydrodynamics3d */ struct MomentsOfInertia : Hydrodynamics3d { + /** + * @brief Construct a new default MomentsOfInertia object. + */ + MomentsOfInertia(); + /** * @brief Construct a new MomentsOfInertia object. * @@ -144,10 +138,7 @@ struct MomentsOfInertia : Hydrodynamics3d * @param y The I_yy coefficient. * @param z The I_zz coefficient. */ - MomentsOfInertia(double x, double y, double z) - : Hydrodynamics3d(x, y, z) - { - } + MomentsOfInertia(double x, double y, double z); }; /** @@ -155,6 +146,11 @@ struct MomentsOfInertia : Hydrodynamics3d */ struct CenterOfBuoyancy : Hydrodynamics3d { + /** + * @brief Construct a new default CenterOfBuoyancy object. + */ + CenterOfBuoyancy(); + /** * @brief Construct a new CenterOfBuoyancy object. * @@ -162,10 +158,7 @@ struct CenterOfBuoyancy : Hydrodynamics3d * @param y The center of buoyancy along the y-axis. * @param z The center of buoyancy along the z-axis. */ - CenterOfBuoyancy(double x, double y, double z) - : Hydrodynamics3d(x, y, z) - { - } + CenterOfBuoyancy(double x, double y, double z); }; /** @@ -173,6 +166,11 @@ struct CenterOfBuoyancy : Hydrodynamics3d */ struct CenterOfGravity : Hydrodynamics3d { + /** + * @brief Construct a new default CenterOfGravity object. + */ + CenterOfGravity(); + /** * @brief Construct a new CenterOfGravity object. * @@ -180,10 +178,7 @@ struct CenterOfGravity : Hydrodynamics3d * @param y The center of gravity along the y-axis. * @param z The center of gravity along the z-axis. */ - CenterOfGravity(double x, double y, double z) - : Hydrodynamics3d(x, y, z) - { - } + CenterOfGravity(double x, double y, double z); }; /** @@ -191,6 +186,11 @@ struct CenterOfGravity : Hydrodynamics3d */ struct AddedMass : Hydrodynamics6d { + /** + * @brief Construct a new default AddedMass object. + */ + AddedMass(); + /** * @brief Construct a new AddedMass object. * @@ -202,10 +202,7 @@ struct AddedMass : Hydrodynamics6d * @param n_r_dot The N_dot{r} coefficient. */ AddedMass( - double x_u_dot, double y_v_dot, double z_w_dot, double k_p_dot, double m_q_dot, double n_r_dot) - : Hydrodynamics6d(x_u_dot, y_v_dot, z_w_dot, k_p_dot, m_q_dot, n_r_dot) - { - } + double x_u_dot, double y_v_dot, double z_w_dot, double k_p_dot, double m_q_dot, double n_r_dot); }; /** @@ -214,6 +211,11 @@ struct AddedMass : Hydrodynamics6d */ struct LinearDamping : Hydrodynamics6d { + /** + * @brief Construct a new default LinearDamping object. + */ + LinearDamping(); + /** * @brief Construct a new LinearDamping object. * @@ -224,10 +226,7 @@ struct LinearDamping : Hydrodynamics6d * @param m_q The M_q coefficient. * @param n_r The N_r coefficient. */ - LinearDamping(double x_u, double y_v, double z_w, double k_p, double m_q, double n_r) - : Hydrodynamics6d(x_u, y_v, z_w, k_p, m_q, n_r) - { - } + LinearDamping(double x_u, double y_v, double z_w, double k_p, double m_q, double n_r); }; /** @@ -236,6 +235,11 @@ struct LinearDamping : Hydrodynamics6d */ struct NonlinearDamping : Hydrodynamics6d { + /** + * @brief Construct a new default NonlinearDamping object. + */ + NonlinearDamping(); + /** * @brief Construct a new NonlinearDamping object * @@ -246,10 +250,7 @@ struct NonlinearDamping : Hydrodynamics6d * @param m_qq The M_{q|q|} coefficient. * @param n_rr The N_{r|r|} coefficient. */ - NonlinearDamping(double x_uu, double y_vv, double z_ww, double k_pp, double m_qq, double n_rr) - : Hydrodynamics6d(x_uu, y_vv, z_ww, k_pp, m_qq, n_rr) - { - } + NonlinearDamping(double x_uu, double y_vv, double z_ww, double k_pp, double m_qq, double n_rr); }; } // namespace blue::dynamics diff --git a/blue_dynamics/include/blue_dynamics/thruster_dynamics.hpp b/blue_dynamics/include/blue_dynamics/thruster_dynamics.hpp index d7711b1c..c67d4f7b 100644 --- a/blue_dynamics/include/blue_dynamics/thruster_dynamics.hpp +++ b/blue_dynamics/include/blue_dynamics/thruster_dynamics.hpp @@ -19,3 +19,15 @@ // THE SOFTWARE. #pragma once + +#include + +namespace blue::dynamics +{ + +struct ThrusterDynamics +{ + ThrusterDynamics(); +}; + +} // namespace blue::dynamics diff --git a/blue_dynamics/include/blue_dynamics/vehicle_dynamics.hpp b/blue_dynamics/include/blue_dynamics/vehicle_dynamics.hpp index d164597c..921682a7 100644 --- a/blue_dynamics/include/blue_dynamics/vehicle_dynamics.hpp +++ b/blue_dynamics/include/blue_dynamics/vehicle_dynamics.hpp @@ -77,12 +77,10 @@ struct VehicleDynamics /** * @brief Create a skew-symmetric matrix from the provided coefficients. * - * @note This is also referred to as the "Cross-Product Operator" by some textbooks. - * * @param a1 Coefficient one. * @param a2 Coefficient two. * @param a3 Coefficient three. - * @return Eigen::Matrix3d + * @return The calculated skew-symmetric matrix. */ [[nodiscard]] static Eigen::Matrix3d createSkewSymmetricMatrix(double a1, double a2, double a3); @@ -92,7 +90,7 @@ struct VehicleDynamics * @note The inertia matrix `M` is the sum of the rigid body mass `M_RB` and the added mass `M_A` * such that `M = M_RB + M_A`. * - * @return Eigen::MatrixXd + * @return The inertia matrix `M`. */ [[nodiscard]] Eigen::MatrixXd calculateInertiaMatrix() const; @@ -106,7 +104,7 @@ struct VehicleDynamics * * @param mass The total mass of the vehicle (kg). * @param moments The moments of inertia. This is assumed to be a diagonal matrix. - * @return Eigen::MatrixXd + * @return The rigid body inertia matrix `M_RB`. */ [[nodiscard]] static Eigen::MatrixXd calculateRigidBodyMassMatrix( double mass, const MomentsOfInertia & moments); @@ -118,7 +116,7 @@ struct VehicleDynamics * Antonelli's textbook "Underwater Robots" in Section 2.4.1. * * @param added_mass The added mass coefficients. - * @return Eigen::MatrixXd + * @return The added mass inertia matrix `M_A`. */ [[nodiscard]] static Eigen::MatrixXd calculateAddedMassMatrix(const AddedMass & added_mass); @@ -129,7 +127,7 @@ struct VehicleDynamics * forces and the added Coriolis forces such that `C = C_RB + C_A`. * * @param velocity The current velocity of the vehicle in the body frame. - * @return Eigen::MatrixXd + * @return The Coriolis-centripetal force matrix `C`. */ [[nodiscard]] Eigen::MatrixXd calculateCoriolisMatrix( const geometry_msgs::msg::TwistStamped & velocity) const; @@ -145,7 +143,7 @@ struct VehicleDynamics * @param mass The total mass of the vehicle (kg). * @param moments The moments of inertia. * @param velocity The current velocity of the vehicle in the body frame. - * @return Eigen::MatrixXd + * @return The rigid body Coriolis-centripetal force matrix `C_RB`. */ [[nodiscard]] static Eigen::MatrixXd calculateRigidBodyCoriolisMatrix( double mass, const MomentsOfInertia & moments, @@ -159,7 +157,7 @@ struct VehicleDynamics * * @param added_mass The added mass coefficients. * @param velocity The current velocity of the vehicle in the body frame. - * @return Eigen::MatrixXd + * @return The added Coriolis-centripetal force matrix `C_A`. */ [[nodiscard]] static Eigen::MatrixXd calculateAddedCoriolixMatrix( const AddedMass & added_mass, const geometry_msgs::msg::TwistStamped & velocity); @@ -171,7 +169,7 @@ struct VehicleDynamics * damping coefficients. * * @param velocity The current velocity of the vehicle in the body frame. - * @return Eigen::MatrixXd + * @return The damping matrix `D`. */ [[nodiscard]] Eigen::MatrixXd calculateDampingMatrix( const geometry_msgs::msg::TwistStamped & velocity) const; @@ -183,7 +181,7 @@ struct VehicleDynamics * Antonelli's "Underwater Robots" in Section 2.4.2. * * @param linear_damping The linear damping coefficients. - * @return Eigen::MatrixXd + * @return A matrix of linear damping coefficients. */ [[nodiscard]] static Eigen::MatrixXd calculateLinearDampingMatrix( const LinearDamping & linear_damping); @@ -196,7 +194,7 @@ struct VehicleDynamics * * @param quadratic_damping The nonlinear damping coefficients. * @param velocity The current velocity of the vehicle in the body frame. - * @return Eigen::MatrixXd + * @return A matrix of nonlinear damping coefficients. */ [[nodiscard]] static Eigen::MatrixXd calculateNonlinearDampingMatrix( const NonlinearDamping & quadratic_damping, const geometry_msgs::msg::TwistStamped & velocity); @@ -209,7 +207,7 @@ struct VehicleDynamics * Antonelli's "Underwater Robots" in Section 2.5. * * @param pose The current pose of the vehicle in the inertial frame. - * @return Eigen::VectorXd + * @return Calculate the vector of restoring forces `g`. */ [[nodiscard]] Eigen::VectorXd calculateRestoringForcesVector( const geometry_msgs::msg::PoseStamped & pose) const; diff --git a/blue_dynamics/package.xml b/blue_dynamics/package.xml index dd9a697e..d4dea7a4 100644 --- a/blue_dynamics/package.xml +++ b/blue_dynamics/package.xml @@ -2,8 +2,8 @@ blue_dynamics - 0.0.0 - TODO: Package description + 0.0.1 + Library used to define and calculate the hydrodynamics of the BlueROV2. Evan Palmer MIT diff --git a/blue_dynamics/include/blue_dynamics/current_dynamics.hpp b/blue_dynamics/src/current_effects.cpp similarity index 60% rename from blue_dynamics/include/blue_dynamics/current_dynamics.hpp rename to blue_dynamics/src/current_effects.cpp index d7711b1c..2683d71e 100644 --- a/blue_dynamics/include/blue_dynamics/current_dynamics.hpp +++ b/blue_dynamics/src/current_effects.cpp @@ -18,4 +18,35 @@ // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN // THE SOFTWARE. -#pragma once +#include "blue_dynamics/current_effects.hpp" + +namespace blue::dynamics +{ + +CurrentEffects::CurrentEffects( + double v_cx, double v_cy, double v_cz, double v_cp = 0, double v_cq = 0, double v_cr = 0) +: v_cx(v_cx), + v_cy(v_cy), + v_cz(v_cz), + v_cp(v_cp), + v_cq(v_cq), + v_cr(v_cr) +{ +} + +[[nodiscard]] Eigen::VectorXd CurrentEffects::calculateCurrentEffects( + const geometry_msgs::msg::PoseStamped & pose) const +{ + Eigen::Quaterniond q( + pose.pose.orientation.w, pose.pose.orientation.x, pose.pose.orientation.y, + pose.pose.orientation.z); + + Eigen::Matrix3d rot = q.toRotationMatrix(); + + Eigen::VectorXd vec(6); // NOLINT + vec << v_cx, v_cy, v_cz, v_cp, v_cq, v_cr; + + return rot * vec; +} + +} // namespace blue::dynamics diff --git a/blue_dynamics/src/hydrodynamics.cpp b/blue_dynamics/src/hydrodynamics.cpp new file mode 100644 index 00000000..4e59199f --- /dev/null +++ b/blue_dynamics/src/hydrodynamics.cpp @@ -0,0 +1,131 @@ +// Copyright 2023, Evan Palmer +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: + +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. + +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL +// THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +// THE SOFTWARE. + +#include "blue_dynamics/hydrodynamics.hpp" + +namespace blue::dynamics +{ + +Hydrodynamics6d::Hydrodynamics6d(double x, double y, double z, double k, double m, double n) +: x(x), + y(y), + z(z), + k(k), + m(m), + n(n) +{ +} + +[[nodiscard]] Eigen::VectorXd Hydrodynamics6d::toVector() const +{ + Eigen::VectorXd vec(6); // NOLINT + vec << x, y, z, k, m, n; + + return vec; +} + +[[nodiscard]] Eigen::MatrixXd Hydrodynamics6d::toMatrix() const +{ + return toVector().asDiagonal().toDenseMatrix(); +} + +Hydrodynamics3d::Hydrodynamics3d(double x, double y, double z) +: x(x), + y(y), + z(z) +{ +} + +[[nodiscard]] Eigen::VectorXd Hydrodynamics3d::toVector() const +{ + Eigen::VectorXd vec(3); // NOLINT + vec << x, y, z; + + return vec; +} + +[[nodiscard]] Eigen::MatrixXd Hydrodynamics3d::toMatrix() const +{ + return toVector().asDiagonal().toDenseMatrix(); +} + +MomentsOfInertia::MomentsOfInertia() +: Hydrodynamics3d(0.0, 0.0, 0.0) +{ +} + +MomentsOfInertia::MomentsOfInertia(double x, double y, double z) +: Hydrodynamics3d(x, y, z) +{ +} + +CenterOfBuoyancy::CenterOfBuoyancy() +: Hydrodynamics3d(0.0, 0.0, 0.0) +{ +} + +CenterOfBuoyancy::CenterOfBuoyancy(double x, double y, double z) +: Hydrodynamics3d(x, y, z) +{ +} + +CenterOfGravity::CenterOfGravity() +: Hydrodynamics3d(0.0, 0.0, 0.0) +{ +} + +CenterOfGravity::CenterOfGravity(double x, double y, double z) +: Hydrodynamics3d(x, y, z) +{ +} + +AddedMass::AddedMass() +: Hydrodynamics6d(0.0, 0.0, 0.0, 0.0, 0.0, 0.0) +{ +} + +AddedMass::AddedMass( + double x_u_dot, double y_v_dot, double z_w_dot, double k_p_dot, double m_q_dot, double n_r_dot) +: Hydrodynamics6d(x_u_dot, y_v_dot, z_w_dot, k_p_dot, m_q_dot, n_r_dot) +{ +} + +LinearDamping::LinearDamping() +: Hydrodynamics6d(0.0, 0.0, 0.0, 0.0, 0.0, 0.0) +{ +} + +LinearDamping::LinearDamping(double x_u, double y_v, double z_w, double k_p, double m_q, double n_r) +: Hydrodynamics6d(x_u, y_v, z_w, k_p, m_q, n_r) +{ +} + +NonlinearDamping::NonlinearDamping() +: Hydrodynamics6d(0.0, 0.0, 0.0, 0.0, 0.0, 0.0) +{ +} + +NonlinearDamping::NonlinearDamping( + double x_uu, double y_vv, double z_ww, double k_pp, double m_qq, double n_rr) +: Hydrodynamics6d(x_uu, y_vv, z_ww, k_pp, m_qq, n_rr) +{ +} + +} // namespace blue::dynamics diff --git a/blue_dynamics/src/vehicle_dynamics.cpp b/blue_dynamics/src/vehicle_dynamics.cpp index 9ed9af69..9ae97bcc 100644 --- a/blue_dynamics/src/vehicle_dynamics.cpp +++ b/blue_dynamics/src/vehicle_dynamics.cpp @@ -158,17 +158,16 @@ VehicleDynamics::VehicleDynamics( Eigen::Matrix3d rot = q.toRotationMatrix(); // The Z-axis points downwards, so gravity is positive and buoyancy is negative - Eigen::Vector3d fg(3); // NOLINT - fg << 0, 0, weight; - - Eigen::Vector3d fb(3); - fb << 0, 0, buoyancy; - fb *= -1; + Eigen::Vector3d fg(0, 0, weight); // NOLINT + Eigen::Vector3d fb(0, 0, -buoyancy); // NOLINT Eigen::VectorXd g_rb(6); g_rb.topRows(3) = rot * (fg + fb); - g_rb.bottomRows(3) = - center_of_gravity.toVector().cross(rot * fg) + center_of_buoyancy.toVector().cross(rot * fb); + + // These vectors already 3d, but are returned as "Xd" vectors and need to be casted to "3d" + // vectors to support using the `cross` method. + g_rb.bottomRows(3) = static_cast(center_of_gravity.toVector()).cross(rot * fg) + + static_cast(center_of_buoyancy.toVector()).cross(rot * fb); g_rb *= -1; return g_rb; diff --git a/blue_dynamics/test/test_vehicle_dynamics.cpp b/blue_dynamics/test/test_vehicle_dynamics.cpp index fcbd4f64..a257a549 100644 --- a/blue_dynamics/test/test_vehicle_dynamics.cpp +++ b/blue_dynamics/test/test_vehicle_dynamics.cpp @@ -24,6 +24,7 @@ #include #include "blue_dynamics/vehicle_dynamics.hpp" +#include "geometry_msgs/msg/pose_stamped.hpp" #include "geometry_msgs/msg/twist_stamped.hpp" namespace blue::dynamics::test @@ -171,7 +172,42 @@ TEST(VehicleDynamicsTest, CalculatesNonlinearDampingMatrix) ASSERT_TRUE(expected.isApprox(actual)); } -TEST(VehicleDynamicsTest, CalculatesRestoringForcesVector) {} +TEST(VehicleDynamicsTest, CalculatesRestoringForcesVector) +{ + const double weight = 10.0; + const double buoyancy = 7.0; + auto pose = geometry_msgs::msg::PoseStamped(); + + const auto center_of_gravity = blue::dynamics::CenterOfGravity(0.1, 0.1, 0.3); + + // Buoyancy is coincident with the center of gravity + const auto center_of_buoyancy = blue::dynamics::CenterOfBuoyancy(0.1, 0.1, 0.3); + + // No rotation + pose.pose.orientation.x = 0.0; + pose.pose.orientation.y = 0.0; + pose.pose.orientation.z = 0.0; + pose.pose.orientation.w = 1.0; + + Eigen::VectorXd expected(6); // NOLINT + + Eigen::Vector3d fg(0, 0, weight); // NOLINT + Eigen::Vector3d fb(0, 0, -buoyancy); // NOLINT + + expected.topRows(3) = fg + fb; + expected.bottomRows(3) = static_cast(center_of_gravity.toVector()).cross(fg) + + static_cast(center_of_buoyancy.toVector()).cross(fb); + expected *= -1; + + const auto vehicle_dynamics = blue::dynamics::VehicleDynamics( + 0.0, weight, buoyancy, blue::dynamics::MomentsOfInertia(), blue::dynamics::AddedMass(), + blue::dynamics::LinearDamping(), blue::dynamics::NonlinearDamping(), center_of_buoyancy, + center_of_gravity); + + Eigen::VectorXd actual = vehicle_dynamics.calculateRestoringForcesVector(pose); + + ASSERT_TRUE(expected.isApprox(actual)); +} } // namespace blue::dynamics::test From 14fd359da2a1c10f739cdb9a49634ba59a3f89dc Mon Sep 17 00:00:00 2001 From: Evan Palmer Date: Tue, 25 Apr 2023 18:46:53 -0700 Subject: [PATCH 19/34] Added TCM and started thruster dynamics --- blue_bridge/CMakeLists.txt | 55 ----------- blue_bridge/LICENSE | 17 ---- blue_bridge/include/blue_bridge/bridge.hpp | 45 --------- blue_bridge/include/blue_bridge/pwm.hpp | 27 ------ blue_bridge/package.xml | 22 ----- blue_bridge/src/bridge.cpp | 94 ------------------- blue_bridge/src/pwm.cpp | 27 ------ .../include/blue_dynamics/current_effects.hpp | 6 ++ .../blue_dynamics/thruster_configuration.hpp | 53 +++++++++++ .../blue_dynamics/thruster_dynamics.hpp | 7 +- blue_dynamics/src/thruster_dynamics.cpp | 0 11 files changed, 65 insertions(+), 288 deletions(-) delete mode 100644 blue_bridge/CMakeLists.txt delete mode 100644 blue_bridge/LICENSE delete mode 100644 blue_bridge/include/blue_bridge/bridge.hpp delete mode 100644 blue_bridge/include/blue_bridge/pwm.hpp delete mode 100644 blue_bridge/package.xml delete mode 100644 blue_bridge/src/bridge.cpp delete mode 100644 blue_bridge/src/pwm.cpp create mode 100644 blue_dynamics/include/blue_dynamics/thruster_configuration.hpp create mode 100644 blue_dynamics/src/thruster_dynamics.cpp diff --git a/blue_bridge/CMakeLists.txt b/blue_bridge/CMakeLists.txt deleted file mode 100644 index 06f5f877..00000000 --- a/blue_bridge/CMakeLists.txt +++ /dev/null @@ -1,55 +0,0 @@ -cmake_minimum_required(VERSION 3.8) -project(blue_bridge) - -# 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 - mavros_msgs - std_msgs - std_srvs -) - -foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) - find_package(${Dependency} REQUIRED) -endforeach() - -include_directories(include) - -add_executable(${PROJECT_NAME} src/bridge.cpp) -ament_target_dependencies(${PROJECT_NAME} ${THIS_PACKAGE_INCLUDE_DEPENDS}) - -install( - TARGETS ${PROJECT_NAME} - DESTINATION lib/${PROJECT_NAME} -) - -install( - DIRECTORY include/ - DESTINATION include -) - -if(BUILD_TESTING) - find_package(ament_lint_auto REQUIRED) - - # Run linters found in package.xml except the two below - set(ament_cmake_copyright_FOUND TRUE) - set(ament_cmake_uncrustify_FOUND TRUE) - - ament_lint_auto_find_test_dependencies() -endif() - -ament_package() diff --git a/blue_bridge/LICENSE b/blue_bridge/LICENSE deleted file mode 100644 index 30e8e2ec..00000000 --- a/blue_bridge/LICENSE +++ /dev/null @@ -1,17 +0,0 @@ -Permission is hereby granted, free of charge, to any person obtaining a copy -of this software and associated documentation files (the "Software"), to deal -in the Software without restriction, including without limitation the rights -to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -copies of the Software, and to permit persons to whom the Software is -furnished to do so, subject to the following conditions: - -The above copyright notice and this permission notice shall be included in -all copies or substantial portions of the Software. - -THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL -THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN -THE SOFTWARE. diff --git a/blue_bridge/include/blue_bridge/bridge.hpp b/blue_bridge/include/blue_bridge/bridge.hpp deleted file mode 100644 index 28d58a6f..00000000 --- a/blue_bridge/include/blue_bridge/bridge.hpp +++ /dev/null @@ -1,45 +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. - -#include "mavros_msgs/msg/override_rc_in.hpp" -#include "rclcpp/rclcpp.hpp" -#include "std_srvs/srv/set_bool.hpp" - -namespace blue::bridge -{ - -class Bridge : public rclcpp::Node -{ -public: - Bridge(); - - [[nodiscard]] bool active() const; - -private: - bool bridge_running_; - mavros_msgs::msg::OverrideRCIn current_pwm_values_; - - rclcpp::Subscription::SharedPtr rc_override_sub_; - rclcpp::Publisher::SharedPtr rc_override_pub_; - rclcpp::Service::SharedPtr enable_override_service_; - rclcpp::TimerBase::SharedPtr timer_; -}; - -} // namespace blue::bridge diff --git a/blue_bridge/include/blue_bridge/pwm.hpp b/blue_bridge/include/blue_bridge/pwm.hpp deleted file mode 100644 index 4d204b67..00000000 --- a/blue_bridge/include/blue_bridge/pwm.hpp +++ /dev/null @@ -1,27 +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. - -namespace blue::utils -{ - -int convertVoltageToPwm(double voltage); -int convertForceToPwm(double force); - -} // namespace blue::utils diff --git a/blue_bridge/package.xml b/blue_bridge/package.xml deleted file mode 100644 index 75f7a231..00000000 --- a/blue_bridge/package.xml +++ /dev/null @@ -1,22 +0,0 @@ - - - - blue_bridge - 0.0.1 - mavros interface between the BlueROV2 hardware and the driver. - Evan Palmer - MIT - - ament_cmake - - mavros - mavros_extras - std_msgs - std_srvs - mavros_msgs - geographic_msgs - - - ament_cmake - - diff --git a/blue_bridge/src/bridge.cpp b/blue_bridge/src/bridge.cpp deleted file mode 100644 index aec532ae..00000000 --- a/blue_bridge/src/bridge.cpp +++ /dev/null @@ -1,94 +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. - -#include "blue_bridge/bridge.hpp" - -#include -#include - -#include "rclcpp/rclcpp.hpp" - -using namespace std::chrono_literals; - -namespace blue::bridge -{ - -Bridge::Bridge() -: Node("blue_bridge"), - bridge_running_(false) -{ - double override_freq = 250; - { - rcl_interfaces::msg::ParameterDescriptor desc; - desc.name = "override_freq"; - desc.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE; - desc.description = - "The rate that the PWM inputs should be published at. This must be at least 50Hz to " - "override the RC inputs. If the control loop frequency is greater than 50Hz, this should " - "match the rate of the control loop."; - desc.read_only = true; - override_freq = declare_parameter(desc.name, override_freq, desc); - } - - if (override_freq < 50) { - throw std::invalid_argument( - "The override frequency must be greater than 50Hz to override the RC inputs!"); - } - - // Set the PWM values that should be used when overriding the RC inputs - rc_override_sub_ = this->create_subscription( - "/blue/rc/override", rclcpp::QoS(rclcpp::KeepLast(1)).reliable(), - [this](mavros_msgs::msg::OverrideRCIn::ConstSharedPtr pwm) -> void { // NOLINT - current_pwm_values_ = *pwm; - }); - - rc_override_pub_ = this->create_publisher( - "/mavros/rc/override", rclcpp::QoS(rclcpp::KeepLast(1)).reliable()); - - // Publish the Override RC values at the specified frequency - timer_ = create_wall_timer(std::chrono::duration(1 / override_freq), [this]() -> void { - // Only publish the override values if the bridge has been enabled - if (active() && (rc_override_pub_->get_subscription_count() > 0)) { - rc_override_pub_->publish(current_pwm_values_); - } - }); - - // Enable/disable RC override - enable_override_service_ = this->create_service( - "/blue/rc/override/enable", - [this]( - const std::shared_ptr & request, - const std::shared_ptr & response) -> void { - bridge_running_ = request->data; - response->success = (bridge_running_ == request->data); - }); -} - -bool Bridge::active() const { return bridge_running_; } - -} // namespace blue::bridge - -int main(int argc, char ** argv) -{ - rclcpp::init(argc, argv); - rclcpp::spin(std::make_shared()); - rclcpp::shutdown(); - return 0; -} diff --git a/blue_bridge/src/pwm.cpp b/blue_bridge/src/pwm.cpp deleted file mode 100644 index 6e628801..00000000 --- a/blue_bridge/src/pwm.cpp +++ /dev/null @@ -1,27 +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. - -namespace blue::utils -{ - -int convertVoltageToPwm(double voltage) { return 0; } -int convertForceToPwm(double force) { return 0; } - -} // namespace blue::utils diff --git a/blue_dynamics/include/blue_dynamics/current_effects.hpp b/blue_dynamics/include/blue_dynamics/current_effects.hpp index e2adeea6..82f12962 100644 --- a/blue_dynamics/include/blue_dynamics/current_effects.hpp +++ b/blue_dynamics/include/blue_dynamics/current_effects.hpp @@ -27,6 +27,9 @@ namespace blue::dynamics { +/** + * @brief The velocity of the fluid in which the BlueROV2 is operating. + */ struct CurrentEffects { double v_cx; // The linear flow rate in the x direction. @@ -51,6 +54,9 @@ struct CurrentEffects /** * @brief Calculate the ocean current velocity with respect to the body frame. * + * @note The formula used to implement the current effect calculate is obtained from Gianluca + * Antonelli's "Underwater Robots" in Section 2.4.3. + * * @param pose The current pose of the vehicle in the inertial frame. * @return The ocean current velocity in the body frame. */ diff --git a/blue_dynamics/include/blue_dynamics/thruster_configuration.hpp b/blue_dynamics/include/blue_dynamics/thruster_configuration.hpp new file mode 100644 index 00000000..88ae1fdb --- /dev/null +++ b/blue_dynamics/include/blue_dynamics/thruster_configuration.hpp @@ -0,0 +1,53 @@ +// Copyright 2023, Evan Palmer +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: + +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. + +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL +// THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +// THE SOFTWARE. + +#pragma once + +#include + +namespace blue::dynamics +{ + +/** + * @brief The thruster configuration matrix (TCM) for the BlueROV2. + * + * @note This is generally denoted as `B` in literature. + */ +const Eigen::MatrixXd kBlueRov2Tcm((Eigen::MatrixXd(6, 6) << 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) + .finished()); + +/** + * @brief The thruster configuration matrix (TCM) for the BlueROV2 Heavy. + * + * @note This is generally denoted as `B` in literature. + */ +const Eigen::MatrixXd kBlueRov2HeavyTcm((Eigen::MatrixXd(6, 8) << 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) + .finished()); + +} // namespace blue::dynamics diff --git a/blue_dynamics/include/blue_dynamics/thruster_dynamics.hpp b/blue_dynamics/include/blue_dynamics/thruster_dynamics.hpp index c67d4f7b..651aa876 100644 --- a/blue_dynamics/include/blue_dynamics/thruster_dynamics.hpp +++ b/blue_dynamics/include/blue_dynamics/thruster_dynamics.hpp @@ -27,7 +27,12 @@ namespace blue::dynamics struct ThrusterDynamics { - ThrusterDynamics(); + [[nodiscard]] virtual Eigen::MatrixXd calculateThrusterDynamics() const = 0; +}; + +struct LumpedParameterThrusterDynamics : ThrusterDynamics +{ + [[nodiscard]] Eigen::MatrixXd calculateThrusterDynamics() const override; }; } // namespace blue::dynamics diff --git a/blue_dynamics/src/thruster_dynamics.cpp b/blue_dynamics/src/thruster_dynamics.cpp new file mode 100644 index 00000000..e69de29b From eeacad61508eec69581c5e40b95a36f3080113fc Mon Sep 17 00:00:00 2001 From: Evan Palmer Date: Wed, 26 Apr 2023 13:00:50 -0700 Subject: [PATCH 20/34] Removed unrelevant files --- blue_control/CMakeLists.txt | 42 ------- blue_control/LICENSE | 17 --- .../include/blue_control/controller.hpp | 77 ------------- blue_control/package.xml | 32 ------ blue_control/src/controller.cpp | 103 ------------------ blue_manager/CMakeLists.txt | 26 ----- blue_manager/LICENSE | 17 --- blue_manager/include/blue_manager/manager.hpp | 0 blue_manager/package.xml | 18 --- blue_manager/src/manager.cpp | 0 10 files changed, 332 deletions(-) delete mode 100644 blue_control/CMakeLists.txt delete mode 100644 blue_control/LICENSE delete mode 100644 blue_control/include/blue_control/controller.hpp delete mode 100644 blue_control/package.xml delete mode 100644 blue_control/src/controller.cpp delete mode 100644 blue_manager/CMakeLists.txt delete mode 100644 blue_manager/LICENSE delete mode 100644 blue_manager/include/blue_manager/manager.hpp delete mode 100644 blue_manager/package.xml delete mode 100644 blue_manager/src/manager.cpp diff --git a/blue_control/CMakeLists.txt b/blue_control/CMakeLists.txt deleted file mode 100644 index 849366c8..00000000 --- a/blue_control/CMakeLists.txt +++ /dev/null @@ -1,42 +0,0 @@ -cmake_minimum_required(VERSION 3.8) -project(blue_control) - -# Default to C99 -if(NOT CMAKE_C_STANDARD) - set(CMAKE_C_STANDARD 99) -endif() - -# Default to C++ 17 -if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 17) -endif() - -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic) -endif() - -set(THIS_PACKAGE_INCLUDE_DEPENDS - rclcpp - ament_cmake - mavros_msgs - std_msgs - std_srvs - geometry_msgs -) - -foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) - find_package(${Dependency} REQUIRED) -endforeach() - -if(BUILD_TESTING) - find_package(ament_lint_auto REQUIRED) - - # Run linters found in package.xml except the two below - set(ament_cmake_copyright_FOUND TRUE) - set(ament_cmake_uncrustify_FOUND TRUE) - - ament_lint_auto_find_test_dependencies() -endif() - -ament_package() - diff --git a/blue_control/LICENSE b/blue_control/LICENSE deleted file mode 100644 index 30e8e2ec..00000000 --- a/blue_control/LICENSE +++ /dev/null @@ -1,17 +0,0 @@ -Permission is hereby granted, free of charge, to any person obtaining a copy -of this software and associated documentation files (the "Software"), to deal -in the Software without restriction, including without limitation the rights -to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -copies of the Software, and to permit persons to whom the Software is -furnished to do so, subject to the following conditions: - -The above copyright notice and this permission notice shall be included in -all copies or substantial portions of the Software. - -THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL -THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN -THE SOFTWARE. diff --git a/blue_control/include/blue_control/controller.hpp b/blue_control/include/blue_control/controller.hpp deleted file mode 100644 index 109c3c2a..00000000 --- a/blue_control/include/blue_control/controller.hpp +++ /dev/null @@ -1,77 +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. - -#pragma once - -#include - -#include "geometry_msgs/msg/pose_stamped.hpp" -#include "mavros_msgs/msg/override_rc_in.hpp" -#include "nav_msgs/msg/odometry.hpp" -#include "rclcpp/rclcpp.hpp" -#include "sensor_msgs/msg/imu.hpp" -#include "std_srvs/srv/set_bool.hpp" -#include "tf2_ros/transform_broadcaster.h" -#include "tf2_ros/transform_listener.h" - -namespace blue::control -{ - -class Controller : public rclcpp::Node -{ -public: - virtual ~Controller() = default; // NOLINT - Controller(const std::string & node_name, const rclcpp::NodeOptions & options); - -protected: - virtual mavros_msgs::msg::OverrideRCIn update(); - - // BlueROV2 state - bool running_; - nav_msgs::msg::Odometry odom_; - - // Static transforms - tf2::Transform tf_cam_base_; - - // Dynamic transforms - tf2::Transform tf_odom_base_; - - // Inverse transforms - tf2::Transform tf_base_odom_; - -private: - void runControlLoopCb(); - - // Subscriptions - rclcpp::Subscription::SharedPtr odom_sub_; - - // Publishers - rclcpp::Publisher::SharedPtr rc_pub_; - - // Services - rclcpp::Service::SharedPtr start_control_; - - // Timers - rclcpp::TimerBase::SharedPtr timer_; - - rclcpp::Time pose_time_; -}; - -} // namespace blue::control diff --git a/blue_control/package.xml b/blue_control/package.xml deleted file mode 100644 index 3f9e65dd..00000000 --- a/blue_control/package.xml +++ /dev/null @@ -1,32 +0,0 @@ - - - - blue_control - 0.0.1 - Control interface for the BlueROV2. - Evan Palmer - MIT - - ament_cmake - - mavros - mavros_extras - - rclcpp - std_msgs - std_srvs - nav_msgs - mavros_msgs - sensor_msgs - geographic_msgs - tf2 - tf2_geometry_msgs - tf2_ros - - ament_lint_auto - ament_lint_common - - - ament_cmake - - diff --git a/blue_control/src/controller.cpp b/blue_control/src/controller.cpp deleted file mode 100644 index 9ed92fa5..00000000 --- a/blue_control/src/controller.cpp +++ /dev/null @@ -1,103 +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. - -#include "blue_control/controller.hpp" - -#include -#include - -#include "rclcpp/rclcpp.hpp" - -namespace blue::control -{ - -Controller::Controller(const std::string & node_name, const rclcpp::NodeOptions & options) -: Node(node_name, options) -{ - double control_loop_freq = 250; - { - rcl_interfaces::msg::ParameterDescriptor desc; - desc.name = "control_loop_freq"; - desc.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE; - desc.description = - "The frequency at which the control loop should run. This must be greater than 50Hz to " - "override the RC inputs."; - desc.read_only = true; - control_loop_freq = declare_parameter(desc.name, control_loop_freq, desc); - } - - rc_pub_ = this->create_publisher( - "/blue/rc/override", rclcpp::QoS(rclcpp::KeepLast(1)).reliable()); - - pose_sub_ = this->create_subscription( - "/mavros/local_position/pose", rclcpp::QoS(rclcpp::KeepLast(1)).best_effort(), - [this](geometry_msgs::msg::PoseStamped::ConstSharedPtr pose) -> void { - updatePoseCb(std::move(pose)); - }); - - start_control_ = this->create_service( - "/blue/control/run", - [this]( - const std::shared_ptr & request, - const std::shared_ptr & response) -> void { - // Enable external control - running_ = request->data; - response->success = (running_ == request->data); - }); - - timer_ = create_wall_timer( - std::chrono::duration(1 / control_loop_freq), [this]() -> void { runControlLoopCb(); }); - - // Set the initial pose time - pose_time_ = this->get_clock()->now(); -} - -void Controller::runControlLoopCb() -{ - if (running_) { - rc_pub_->publish(update()); - } -} - -void Controller::updatePoseCb(geometry_msgs::msg::PoseStamped::ConstSharedPtr pose) // NOLINT -{ - // Update the linear velocity - double duration = rclcpp::Duration(pose->header.stamp - pose_time_).seconds(); // NOLINT - - odom_.twist.twist.linear.x = (pose->pose.position.x - odom_.pose.pose.position.x) / duration; - odom_.twist.twist.linear.y = (pose->pose.position.y - odom_.pose.pose.position.y) / duration; - odom_.twist.twist.linear.z = (pose->pose.position.z - odom_.pose.pose.position.z) / duration; - - // Now we can update the pose - odom_.pose.pose = pose->pose; - - // Save the timestamp for the next measurement - pose_time_ = pose->header.stamp; - - // TODO(evan-palmer): update transforms here -} - -void Controller::updateAngularVelCb(sensor_msgs::msg::Imu::ConstSharedPtr imu) // NOLINT -{ - // TODO(evan-palmer): update transforms here (if necessary) - odom_.twist.twist.angular = imu->angular_velocity; -} - -} // namespace blue::control diff --git a/blue_manager/CMakeLists.txt b/blue_manager/CMakeLists.txt deleted file mode 100644 index e2cf6831..00000000 --- a/blue_manager/CMakeLists.txt +++ /dev/null @@ -1,26 +0,0 @@ -cmake_minimum_required(VERSION 3.8) -project(blue_manager) - -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic) -endif() - -# find dependencies -find_package(ament_cmake REQUIRED) -# uncomment the following section in order to fill in -# further dependencies manually. -# find_package( REQUIRED) - -if(BUILD_TESTING) - find_package(ament_lint_auto REQUIRED) - # the following line skips the linter which checks for copyrights - # comment the line when a copyright and license is added to all source files - set(ament_cmake_copyright_FOUND TRUE) - # the following line skips cpplint (only works in a git repo) - # comment the line when this package is in a git repo and when - # a copyright and license is added to all source files - set(ament_cmake_cpplint_FOUND TRUE) - ament_lint_auto_find_test_dependencies() -endif() - -ament_package() diff --git a/blue_manager/LICENSE b/blue_manager/LICENSE deleted file mode 100644 index 30e8e2ec..00000000 --- a/blue_manager/LICENSE +++ /dev/null @@ -1,17 +0,0 @@ -Permission is hereby granted, free of charge, to any person obtaining a copy -of this software and associated documentation files (the "Software"), to deal -in the Software without restriction, including without limitation the rights -to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -copies of the Software, and to permit persons to whom the Software is -furnished to do so, subject to the following conditions: - -The above copyright notice and this permission notice shall be included in -all copies or substantial portions of the Software. - -THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL -THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN -THE SOFTWARE. diff --git a/blue_manager/include/blue_manager/manager.hpp b/blue_manager/include/blue_manager/manager.hpp deleted file mode 100644 index e69de29b..00000000 diff --git a/blue_manager/package.xml b/blue_manager/package.xml deleted file mode 100644 index 7ab5180b..00000000 --- a/blue_manager/package.xml +++ /dev/null @@ -1,18 +0,0 @@ - - - - blue_manager - 0.0.0 - TODO: Package description - Evan Palmer - MIT - - ament_cmake - - ament_lint_auto - ament_lint_common - - - ament_cmake - - diff --git a/blue_manager/src/manager.cpp b/blue_manager/src/manager.cpp deleted file mode 100644 index e69de29b..00000000 From f534ddd1d0acb472923a3d31a5224e6714bcd102 Mon Sep 17 00:00:00 2001 From: Evan Palmer Date: Wed, 26 Apr 2023 17:22:15 -0700 Subject: [PATCH 21/34] Fixed comments --- .../blue_dynamics/thruster_configuration.hpp | 53 ------------------- .../blue_dynamics/vehicle_dynamics.hpp | 12 ++--- 2 files changed, 6 insertions(+), 59 deletions(-) delete mode 100644 blue_dynamics/include/blue_dynamics/thruster_configuration.hpp diff --git a/blue_dynamics/include/blue_dynamics/thruster_configuration.hpp b/blue_dynamics/include/blue_dynamics/thruster_configuration.hpp deleted file mode 100644 index 88ae1fdb..00000000 --- a/blue_dynamics/include/blue_dynamics/thruster_configuration.hpp +++ /dev/null @@ -1,53 +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. - -#pragma once - -#include - -namespace blue::dynamics -{ - -/** - * @brief The thruster configuration matrix (TCM) for the BlueROV2. - * - * @note This is generally denoted as `B` in literature. - */ -const Eigen::MatrixXd kBlueRov2Tcm((Eigen::MatrixXd(6, 6) << 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) - .finished()); - -/** - * @brief The thruster configuration matrix (TCM) for the BlueROV2 Heavy. - * - * @note This is generally denoted as `B` in literature. - */ -const Eigen::MatrixXd kBlueRov2HeavyTcm((Eigen::MatrixXd(6, 8) << 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) - .finished()); - -} // namespace blue::dynamics diff --git a/blue_dynamics/include/blue_dynamics/vehicle_dynamics.hpp b/blue_dynamics/include/blue_dynamics/vehicle_dynamics.hpp index 921682a7..f14e3028 100644 --- a/blue_dynamics/include/blue_dynamics/vehicle_dynamics.hpp +++ b/blue_dynamics/include/blue_dynamics/vehicle_dynamics.hpp @@ -55,16 +55,16 @@ struct VehicleDynamics * * @param mass The total mass of the vehicle (kg). * @param weight The weight of the vehicle in the inertial frame. This is denoted as `W` in - * Section 2.5 of Gianluca Antonelli's "Underwater Robots". + * Section 2.5 of Gianluca Antonelli's "Underwater Robots". * @param buoyancy The buoyancy force acting on the vehicle. This is denoted as `B` in Section 2.5 - * of Gianluca Antonelli's "Underwater Robots". + * of Gianluca Antonelli's "Underwater Robots". * @param moments The moments of inertia. * @param added_mass The added mass coefficients. These are defined in Section 2.4.1 of Gianluca - * Antonelli's "Underwater Robots". + * Antonelli's "Underwater Robots". * @param linear_damping The rigid body linear damping coefficients. These are defined in Section - * 2.4.2 of Gianluca Antonelli's "Underwater Robots". + * 2.4.2 of Gianluca Antonelli's "Underwater Robots". * @param quadratic_damping The rigid body nonlinear damping coefficients. These are defined in - * Section 2.4.2 of Gianluca Antonelli's "Underwater Robots". + * Section 2.4.2 of Gianluca Antonelli's "Underwater Robots". * @param center_of_buoyancy The center of buoyancy. * @param center_of_gravity The center of gravity relative to the body frame. */ @@ -80,7 +80,7 @@ struct VehicleDynamics * @param a1 Coefficient one. * @param a2 Coefficient two. * @param a3 Coefficient three. - * @return The calculated skew-symmetric matrix. + * @return The created skew-symmetric matrix. */ [[nodiscard]] static Eigen::Matrix3d createSkewSymmetricMatrix(double a1, double a2, double a3); From cb4e649bbdc313c18df143c0d552e4718b91ad16 Mon Sep 17 00:00:00 2001 From: Evan Palmer Date: Tue, 2 May 2023 18:44:50 -0700 Subject: [PATCH 22/34] started implementation of manager --- blue_manager/LICENSE | 17 ++ blue_manager/blue_manager/__init__.py | 0 blue_manager/blue_manager/manager.py | 213 ++++++++++++++++++++++++++ blue_manager/package.xml | 18 +++ blue_manager/resource/blue_manager | 0 blue_manager/setup.cfg | 4 + blue_manager/setup.py | 25 +++ blue_manager/test/test_copyright.py | 25 +++ blue_manager/test/test_flake8.py | 25 +++ blue_manager/test/test_pep257.py | 23 +++ blue_msgs/CMakeLists.txt | 30 ++++ blue_msgs/LICENSE | 17 ++ blue_msgs/action/Arm.action | 11 ++ blue_msgs/action/Connect.action | 11 ++ blue_msgs/package.xml | 26 ++++ 15 files changed, 445 insertions(+) create mode 100644 blue_manager/LICENSE create mode 100644 blue_manager/blue_manager/__init__.py create mode 100644 blue_manager/blue_manager/manager.py create mode 100644 blue_manager/package.xml create mode 100644 blue_manager/resource/blue_manager create mode 100644 blue_manager/setup.cfg create mode 100644 blue_manager/setup.py create mode 100644 blue_manager/test/test_copyright.py create mode 100644 blue_manager/test/test_flake8.py create mode 100644 blue_manager/test/test_pep257.py create mode 100644 blue_msgs/CMakeLists.txt create mode 100644 blue_msgs/LICENSE create mode 100644 blue_msgs/action/Arm.action create mode 100644 blue_msgs/action/Connect.action create mode 100644 blue_msgs/package.xml diff --git a/blue_manager/LICENSE b/blue_manager/LICENSE new file mode 100644 index 00000000..30e8e2ec --- /dev/null +++ b/blue_manager/LICENSE @@ -0,0 +1,17 @@ +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: + +The above copyright notice and this permission notice shall be included in +all copies or substantial portions of the Software. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL +THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +THE SOFTWARE. diff --git a/blue_manager/blue_manager/__init__.py b/blue_manager/blue_manager/__init__.py new file mode 100644 index 00000000..e69de29b diff --git a/blue_manager/blue_manager/manager.py b/blue_manager/blue_manager/manager.py new file mode 100644 index 00000000..08a01f94 --- /dev/null +++ b/blue_manager/blue_manager/manager.py @@ -0,0 +1,213 @@ +# 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 typing import Any + +import rclpy +from mavros_msgs.srv import MessageInterval, ParamGet +from rclpy.action import ActionServer +from rclpy.node import Node +from sensor_msgs.msg import BatteryState + +from blue_msgs.action import Arm, Connect + + +class Manager(Node): + def __init__(self) -> None: + super().__init__("blue_manager") + + self.connected = False + self.armed = False + self.backup_settings: dict[str, Any] = {} + self.battery_charge = 0.0 + + # Declare the manager parameters + self.declare_parameters( + "", + [ + ("min_battery_percent", 15.0), + ("stopped_pwm", 1500), + ("rov_config", "heavy"), + ("max_connection_attempts", 3), + ("connection_attempt_timeout", 1.0), + ], + ) + + # Load the parameter values + self.total_battery_cells = ( + self.get_parameter("total_battery_cells") + .get_parameter_value() + .integer_value + ) + self.min_battery_percent = ( + self.get_parameter("min_battery_percent").get_parameter_value().double_value + ) + self.rov_config = ( + self.get_parameter("rov_config").get_parameter_value().string_value + ) + self.max_connection_attempts = ( + self.get_parameter("max_connection_attempts") + .get_parameter_value() + .integer_value + ) + self.connection_attempt_timeout = ( + self.get_parameter("connection_attempt_timeout") + .get_parameter_value() + .double_value + ) + self.stopped_pwm = ( + self.get_parameter("stopped_pwm").get_parameter_value().integer_value + ) + + # Service clients + self.set_message_interval_client = self.create_client( + MessageInterval, "/mavros/set_message_interval", 1 + ) + + # Action servers + self.connect_action_server = ActionServer( + self, Connect, "/blue/connect", self._connect_control_manager_cb + ) + self.arm_action_server = ActionServer( + self, Arm, "/blue/arm", self._arm_controller_cb + ) + + # Subscribers + self.battery_sub = self.create_subscription( + BatteryState, "/mavros/battery", self._update_battery_level_cb + ) + + def _connect_control_manager_cb( + self, connection_request: Connect.Goal + ) -> Connect.Result: + connection_result = Connect.Result() + + if connection_request.connect: + result = self.connect( + self.max_connection_attempts, self.connection_attempt_timeout + ) + else: + result = self.disconnect( + self.max_connection_attempts, self.connection_attempt_timeout + ) + + connection_result.success = result + + return connection_result + + def _arm_controller_cb(self, arm_request: Arm.Goal) -> Arm.Result: + arm_result = Connect.Result() + + if arm_request.arm: + result = self.arm() + else: + result = self.disarm() + + arm_result.success = result + + return arm_result + + def set_message_intervals(self) -> None: + ... + + def connect(self, max_attempts: int, timeout: float) -> bool: + for _ in range(max_attempts): + self.connected = self.backup_motor_params(timeout) + + if self.connected: + break + + return self.connected + + def disconnect(self, max_attempts: int, timeout: float) -> bool: + disarmed = False + backup_restored = False + + for _ in range(max_attempts): + if not disarmed: + disarmed = self.disarm() + + if not backup_restored: + backup_restored = self.restore_param_backup(timeout) + + if disarmed and backup_restored: + break + + success = disarmed and backup_restored + self.connected = not success + + return success + + def arm(self) -> bool: + if not self.check_battery_level(): + self.get_logger().warning( + f"Failed to arm the system: the current battery level (" + f" {self.battery_charge}%) is below the minimum battery level." + ) + return False + + self.armed = self.enable_pwm_passthrough() + + return self.armed + + def disarm(self) -> bool: + success = self.stop_thrusters() and self.disable_pwm_passthrough() + self.armed = not success + return success + + def backup_motor_params(self, timeout: float) -> bool: + ... + + def restore_param_backup(self, timeout: float) -> bool: + ... + + def stop_thrusters(self) -> bool: + ... + + def enable_pwm_passthrough(self) -> bool: + ... + + def disable_pwm_passthrough(self) -> bool: + ... + + def check_battery_level(self) -> bool: + return self.battery_charge < self.min_battery_percent + + def _update_battery_level_cb(self, battery_state: BatteryState) -> float: + self.battery_charge = battery_state.percentage + + if not self.check_battery_level(): + self.get_logger().warning( + f"The current battery level ({self.battery_charge * 100}%) is below" + f" the minimum charge level. Disarming the system." + ) + + self.disarm() + + +def main(args: list[str] | None = None): + """Run the ROV manager.""" + rclpy.init(args=args) + + node = Manager() + rclpy.spin(node) + + node.destroy_node() + rclpy.shutdown() diff --git a/blue_manager/package.xml b/blue_manager/package.xml new file mode 100644 index 00000000..c2f7afc3 --- /dev/null +++ b/blue_manager/package.xml @@ -0,0 +1,18 @@ + + + + blue_manager + 0.0.0 + TODO: Package description + Evan Palmer + MIT + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + + ament_python + + diff --git a/blue_manager/resource/blue_manager b/blue_manager/resource/blue_manager new file mode 100644 index 00000000..e69de29b diff --git a/blue_manager/setup.cfg b/blue_manager/setup.cfg new file mode 100644 index 00000000..036fce1c --- /dev/null +++ b/blue_manager/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/blue_manager +[install] +install_scripts=$base/lib/blue_manager diff --git a/blue_manager/setup.py b/blue_manager/setup.py new file mode 100644 index 00000000..9ef2bf98 --- /dev/null +++ b/blue_manager/setup.py @@ -0,0 +1,25 @@ +from setuptools import setup + +package_name = 'blue_manager' + +setup( + name=package_name, + version='0.0.0', + packages=[package_name], + data_files=[ + ('share/ament_index/resource_index/packages', + ['resource/' + package_name]), + ('share/' + package_name, ['package.xml']), + ], + install_requires=['setuptools'], + zip_safe=True, + maintainer='Evan Palmer', + maintainer_email='evanp922@gmail.com', + description='TODO: Package description', + license='MIT', + tests_require=['pytest'], + entry_points={ + 'console_scripts': [ + ], + }, +) diff --git a/blue_manager/test/test_copyright.py b/blue_manager/test/test_copyright.py new file mode 100644 index 00000000..97a39196 --- /dev/null +++ b/blue_manager/test/test_copyright.py @@ -0,0 +1,25 @@ +# 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. + +from ament_copyright.main import main +import pytest + + +# 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_manager/test/test_flake8.py b/blue_manager/test/test_flake8.py new file mode 100644 index 00000000..27ee1078 --- /dev/null +++ b/blue_manager/test/test_flake8.py @@ -0,0 +1,25 @@ +# 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. + +from ament_flake8.main import main_with_errors +import pytest + + +@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_manager/test/test_pep257.py b/blue_manager/test/test_pep257.py new file mode 100644 index 00000000..b234a384 --- /dev/null +++ b/blue_manager/test/test_pep257.py @@ -0,0 +1,23 @@ +# 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. + +from ament_pep257.main import main +import pytest + + +@pytest.mark.linter +@pytest.mark.pep257 +def test_pep257(): + rc = main(argv=['.', 'test']) + assert rc == 0, 'Found code style errors / warnings' diff --git a/blue_msgs/CMakeLists.txt b/blue_msgs/CMakeLists.txt new file mode 100644 index 00000000..ec036f0c --- /dev/null +++ b/blue_msgs/CMakeLists.txt @@ -0,0 +1,30 @@ +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(action_msgs REQUIRED) +find_package(rosidl_default_generators REQUIRED) + +set(action_files + "action/Arm.action" + "action/Connect.action" +) + +rosidl_generate_interfaces(${PROJECT_NAME} + ${action_files} + DEPENDENCIES builtin_interfaces std_msgs action_msgs +) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() +endif() + +ament_export_dependencies(rosidl_default_runtime) + +ament_package() diff --git a/blue_msgs/LICENSE b/blue_msgs/LICENSE new file mode 100644 index 00000000..30e8e2ec --- /dev/null +++ b/blue_msgs/LICENSE @@ -0,0 +1,17 @@ +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: + +The above copyright notice and this permission notice shall be included in +all copies or substantial portions of the Software. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL +THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +THE SOFTWARE. diff --git a/blue_msgs/action/Arm.action b/blue_msgs/action/Arm.action new file mode 100644 index 00000000..43addb5d --- /dev/null +++ b/blue_msgs/action/Arm.action @@ -0,0 +1,11 @@ +# Arm/disarm the BlueROV2 thrusters. + +# Set to 'True' to arm the BlueROV2 thrusters. Set to 'False' +# to disarm the thrusters. +# bool arm +--- +# Returns 'True' if the arm/disarm attempt was successful. Returns +# 'False' otherwise. +bool success +--- +# No feedback is required. diff --git a/blue_msgs/action/Connect.action b/blue_msgs/action/Connect.action new file mode 100644 index 00000000..c95ff58c --- /dev/null +++ b/blue_msgs/action/Connect.action @@ -0,0 +1,11 @@ +# Connect/disconnect the control interface. + +# Set to 'True' to attempt to establish a connection. Set to +# 'False' otherwise. +bool connect +--- +# Returns 'True' if the connect/disconnect was successful. Returns +# 'False' otherwise. +bool success +--- +# No feedback is provided. diff --git a/blue_msgs/package.xml b/blue_msgs/package.xml new file mode 100644 index 00000000..92fb92bb --- /dev/null +++ b/blue_msgs/package.xml @@ -0,0 +1,26 @@ + + + + blue_msgs + 0.0.0 + TODO: Package description + Evan Palmer + MIT + + ament_cmake + + ament_lint_auto + ament_lint_common + + action_msgs + std_msgs + geometry_msgs + + rosidl_default_generators + rosidl_default_runtime + rosidl_interface_packages + + + ament_cmake + + From f3854b3c7676a91f4ed4b8295a88e342d5bda5ff Mon Sep 17 00:00:00 2001 From: Evan Palmer Date: Wed, 3 May 2023 21:01:33 -0700 Subject: [PATCH 23/34] Removed unused packages and cleaned up manager implementation --- blue_manager/blue_manager/manager.py | 194 ++++++++------------------- blue_manager/package.xml | 3 + 2 files changed, 58 insertions(+), 139 deletions(-) diff --git a/blue_manager/blue_manager/manager.py b/blue_manager/blue_manager/manager.py index 08a01f94..9a30d8db 100644 --- a/blue_manager/blue_manager/manager.py +++ b/blue_manager/blue_manager/manager.py @@ -21,185 +21,101 @@ from typing import Any import rclpy -from mavros_msgs.srv import MessageInterval, ParamGet -from rclpy.action import ActionServer +from mavros_msgs.srv import MessageInterval, ParamGet, ParamSetV2 from rclpy.node import Node from sensor_msgs.msg import BatteryState - -from blue_msgs.action import Arm, Connect +from std_srvs.srv import SetBool class Manager(Node): + STOPPED_PWM = 1500 + def __init__(self) -> None: super().__init__("blue_manager") + self.battery_state = BatteryState() self.connected = False self.armed = False - self.backup_settings: dict[str, Any] = {} - self.battery_charge = 0.0 - - # Declare the manager parameters - self.declare_parameters( - "", - [ - ("min_battery_percent", 15.0), - ("stopped_pwm", 1500), - ("rov_config", "heavy"), - ("max_connection_attempts", 3), - ("connection_attempt_timeout", 1.0), - ], - ) + self.rov_config = "heavy" + self.backup_params: dict[str, Any] = {} - # Load the parameter values - self.total_battery_cells = ( - self.get_parameter("total_battery_cells") - .get_parameter_value() - .integer_value - ) - self.min_battery_percent = ( - self.get_parameter("min_battery_percent").get_parameter_value().double_value - ) - self.rov_config = ( - self.get_parameter("rov_config").get_parameter_value().string_value - ) - self.max_connection_attempts = ( - self.get_parameter("max_connection_attempts") - .get_parameter_value() - .integer_value + # Subscribers + self.battery_sub = self.create_subscription( + BatteryState, "/mavros/battery", self._store_battery_state_cb ) - self.connection_attempt_timeout = ( - self.get_parameter("connection_attempt_timeout") - .get_parameter_value() - .double_value + + # Services + self.arm_srv = self.create_service( + SetBool, "/blue/connect", self._manage_connection_cb ) - self.stopped_pwm = ( - self.get_parameter("stopped_pwm").get_parameter_value().integer_value + self.connect_srv = self.create_service( + SetBool, "/blue/arm", self._manage_arming_cb ) # Service clients - self.set_message_interval_client = self.create_client( + self.set_message_rate_srv_client = self.create_client( MessageInterval, "/mavros/set_message_interval", 1 ) - - # Action servers - self.connect_action_server = ActionServer( - self, Connect, "/blue/connect", self._connect_control_manager_cb - ) - self.arm_action_server = ActionServer( - self, Arm, "/blue/arm", self._arm_controller_cb + self.get_param_srv_client = self.create_client(ParamGet, "/mavros/param/get", 1) + self.set_param_srv_client = self.create_client( + ParamSetV2, "/mavros/param/set", 1 ) - # Subscribers - self.battery_sub = self.create_subscription( - BatteryState, "/mavros/battery", self._update_battery_level_cb - ) + def _store_battery_state_cb(self, state: BatteryState) -> None: + self.battery_state = state - def _connect_control_manager_cb( - self, connection_request: Connect.Goal - ) -> Connect.Result: - connection_result = Connect.Result() + def check_battery_state(self) -> bool: + return self.battery_state.percentage <= 0.2 - if connection_request.connect: - result = self.connect( - self.max_connection_attempts, self.connection_attempt_timeout + def _manage_connection_cb(self, connect: bool) -> None: + if connect: + self.get_logger().info( + "Attempting to establish a connection with the BlueROV2..." ) - else: - result = self.disconnect( - self.max_connection_attempts, self.connection_attempt_timeout - ) - - connection_result.success = result - return connection_result + self.connected = self.connect() - def _arm_controller_cb(self, arm_request: Arm.Goal) -> Arm.Result: - arm_result = Connect.Result() - - if arm_request.arm: - result = self.arm() + if not self.connected: + self.get_logger().warning( + "Failed to establish a connection with the BlueROV2." + ) + else: + self.get_logger().info( + "Successfully established a connection with the BlueROV2." + ) else: - result = self.disarm() - - arm_result.success = result - - return arm_result - - def set_message_intervals(self) -> None: - ... - - def connect(self, max_attempts: int, timeout: float) -> bool: - for _ in range(max_attempts): - self.connected = self.backup_motor_params(timeout) - - if self.connected: - break - - return self.connected - - def disconnect(self, max_attempts: int, timeout: float) -> bool: - disarmed = False - backup_restored = False - - for _ in range(max_attempts): - if not disarmed: - disarmed = self.disarm() - - if not backup_restored: - backup_restored = self.restore_param_backup(timeout) - - if disarmed and backup_restored: - break - - success = disarmed and backup_restored - self.connected = not success - - return success - - def arm(self) -> bool: - if not self.check_battery_level(): - self.get_logger().warning( - f"Failed to arm the system: the current battery level (" - f" {self.battery_charge}%) is below the minimum battery level." + self.get_logger().info( + "Attempting to shutdown the connection to the BlueROV2..." ) - return False + self.connected = not self.disconnect() - self.armed = self.enable_pwm_passthrough() + if not self.connected: + self.get_logger().warning( + "Successfully shutdown the connection to the BlueROV2." + ) + else: + self.get_logger().info("Failed to perform all shutdown proceedures.") - return self.armed - - def disarm(self) -> bool: - success = self.stop_thrusters() and self.disable_pwm_passthrough() - self.armed = not success - return success - - def backup_motor_params(self, timeout: float) -> bool: + def connect(self) -> bool: ... - def restore_param_backup(self, timeout: float) -> bool: + def backup_motor_params(self) -> bool: ... - def stop_thrusters(self) -> bool: + def disconnect(self) -> bool: ... - def enable_pwm_passthrough(self) -> bool: + def restore_motor_param_backup(self) -> bool: ... - def disable_pwm_passthrough(self) -> bool: + def _manage_arming_cb(self, arm: bool) -> None: ... - def check_battery_level(self) -> bool: - return self.battery_charge < self.min_battery_percent - - def _update_battery_level_cb(self, battery_state: BatteryState) -> float: - self.battery_charge = battery_state.percentage - - if not self.check_battery_level(): - self.get_logger().warning( - f"The current battery level ({self.battery_charge * 100}%) is below" - f" the minimum charge level. Disarming the system." - ) + def arm(self) -> bool: + ... - self.disarm() + def disarm(self) -> bool: + ... def main(args: list[str] | None = None): diff --git a/blue_manager/package.xml b/blue_manager/package.xml index c2f7afc3..f79f3510 100644 --- a/blue_manager/package.xml +++ b/blue_manager/package.xml @@ -7,6 +7,9 @@ Evan Palmer MIT + mavros_msgs + std_srvs + ament_copyright ament_flake8 ament_pep257 From 80285061d77e2d798c189efc5bff52d01f158d4d Mon Sep 17 00:00:00 2001 From: Evan Palmer Date: Wed, 3 May 2023 21:02:28 -0700 Subject: [PATCH 24/34] Removed dynamics package as not in pr scope --- .dockerignore | 4 +- blue_dynamics/CMakeLists.txt | 79 ------ blue_dynamics/LICENSE | 17 -- .../include/blue_dynamics/current_effects.hpp | 67 ----- .../include/blue_dynamics/hydrodynamics.hpp | 256 ------------------ .../blue_dynamics/thruster_dynamics.hpp | 38 --- .../blue_dynamics/vehicle_dynamics.hpp | 216 --------------- blue_dynamics/package.xml | 26 -- blue_dynamics/src/current_effects.cpp | 52 ---- blue_dynamics/src/hydrodynamics.cpp | 131 --------- blue_dynamics/src/thruster_dynamics.cpp | 0 blue_dynamics/src/vehicle_dynamics.cpp | 175 ------------ blue_dynamics/test/test_vehicle_dynamics.cpp | 220 --------------- 13 files changed, 1 insertion(+), 1280 deletions(-) delete mode 100644 blue_dynamics/CMakeLists.txt delete mode 100644 blue_dynamics/LICENSE delete mode 100644 blue_dynamics/include/blue_dynamics/current_effects.hpp delete mode 100644 blue_dynamics/include/blue_dynamics/hydrodynamics.hpp delete mode 100644 blue_dynamics/include/blue_dynamics/thruster_dynamics.hpp delete mode 100644 blue_dynamics/include/blue_dynamics/vehicle_dynamics.hpp delete mode 100644 blue_dynamics/package.xml delete mode 100644 blue_dynamics/src/current_effects.cpp delete mode 100644 blue_dynamics/src/hydrodynamics.cpp delete mode 100644 blue_dynamics/src/thruster_dynamics.cpp delete mode 100644 blue_dynamics/src/vehicle_dynamics.cpp delete mode 100644 blue_dynamics/test/test_vehicle_dynamics.cpp diff --git a/.dockerignore b/.dockerignore index b389be55..9efc3ebb 100644 --- a/.dockerignore +++ b/.dockerignore @@ -2,6 +2,4 @@ * # Except the following -!blue_bridge -!blue_control -!blue_dynamics +!blue_manager diff --git a/blue_dynamics/CMakeLists.txt b/blue_dynamics/CMakeLists.txt deleted file mode 100644 index a8c347a9..00000000 --- a/blue_dynamics/CMakeLists.txt +++ /dev/null @@ -1,79 +0,0 @@ -cmake_minimum_required(VERSION 3.8) -project(blue_dynamics) - -# 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 - eigen3_cmake_module - Eigen3 - geometry_msgs -) - -foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) - find_package(${Dependency} REQUIRED) -endforeach() - -include_directories( - include -) - -add_library(${PROJECT_NAME} SHARED - src/vehicle_dynamics.cpp - src/current_effects.cpp - src/hydrodynamics.cpp -) - -target_link_libraries(${PROJECT_NAME} ${rclcpp_LIBRARIES}) -target_include_directories(${PROJECT_NAME} PRIVATE include) -ament_target_dependencies(${PROJECT_NAME} ${THIS_PACKAGE_INCLUDE_DEPENDS}) - -install( - DIRECTORY include/ - DESTINATION include -) - -install( - TARGETS ${PROJECT_NAME} - EXPORT ${PROJECT_NAME} - RUNTIME DESTINATION bin - LIBRARY DESTINATION lib - ARCHIVE DESTINATION lib - INCLUDES DESTINATION include -) - -if(BUILD_TESTING) - find_package(ament_cmake_gtest REQUIRED) - 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() - - # Setup unit tests - ament_add_gtest( - test_vehicle_dynamics - test/test_vehicle_dynamics.cpp - ) - target_link_libraries(test_vehicle_dynamics ${PROJECT_NAME}) -endif() - -ament_export_targets(${PROJECT_NAME} HAS_LIBRARY_TARGET) -ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS}) - -ament_package() diff --git a/blue_dynamics/LICENSE b/blue_dynamics/LICENSE deleted file mode 100644 index 30e8e2ec..00000000 --- a/blue_dynamics/LICENSE +++ /dev/null @@ -1,17 +0,0 @@ -Permission is hereby granted, free of charge, to any person obtaining a copy -of this software and associated documentation files (the "Software"), to deal -in the Software without restriction, including without limitation the rights -to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -copies of the Software, and to permit persons to whom the Software is -furnished to do so, subject to the following conditions: - -The above copyright notice and this permission notice shall be included in -all copies or substantial portions of the Software. - -THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL -THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN -THE SOFTWARE. diff --git a/blue_dynamics/include/blue_dynamics/current_effects.hpp b/blue_dynamics/include/blue_dynamics/current_effects.hpp deleted file mode 100644 index 82f12962..00000000 --- a/blue_dynamics/include/blue_dynamics/current_effects.hpp +++ /dev/null @@ -1,67 +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. - -#pragma once - -#include - -#include "geometry_msgs/msg/pose_stamped.hpp" - -namespace blue::dynamics -{ - -/** - * @brief The velocity of the fluid in which the BlueROV2 is operating. - */ -struct CurrentEffects -{ - double v_cx; // The linear flow rate in the x direction. - double v_cy; // The linear flow rate in the y direction. - double v_cz; // The linear flow rate in the z direction. - double v_cp; // The rotational flow rate around the x axis. - double v_cq; // The rotational flow rate around the y axis. - double v_cr; // The rotational flow rate around the z axis. - - /** - * @brief Construct a new CurrentEffects object. - * - * @param v_cx The linear flow rate in the x direction. - * @param v_cy The linear flow rate in the y direction. - * @param v_cz The linear flow rate in the z direction. - * @param v_cp The rotational flow rate around the x axis. - * @param v_cq The rotational flow rate around the y axis. - * @param v_cr The rotational flow rate around the z axis. - */ - CurrentEffects(double v_cx, double v_cy, double v_cz, double v_cp, double v_cq, double v_cr); - - /** - * @brief Calculate the ocean current velocity with respect to the body frame. - * - * @note The formula used to implement the current effect calculate is obtained from Gianluca - * Antonelli's "Underwater Robots" in Section 2.4.3. - * - * @param pose The current pose of the vehicle in the inertial frame. - * @return The ocean current velocity in the body frame. - */ - [[nodiscard]] Eigen::VectorXd calculateCurrentEffects( - const geometry_msgs::msg::PoseStamped & pose) const; -}; - -} // namespace blue::dynamics diff --git a/blue_dynamics/include/blue_dynamics/hydrodynamics.hpp b/blue_dynamics/include/blue_dynamics/hydrodynamics.hpp deleted file mode 100644 index 69aaad2d..00000000 --- a/blue_dynamics/include/blue_dynamics/hydrodynamics.hpp +++ /dev/null @@ -1,256 +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. - -#pragma once - -#include - -namespace blue::dynamics -{ - -/** - * @brief A common base class to use for defining a collection of N-D hydrodynamics coefficients. - */ -struct HydrodynamicsXd -{ - /** - * @brief Convert the coefficients into a column vector. - * - * @return The coefficients as a vector. - */ - [[nodiscard]] virtual Eigen::VectorXd toVector() const = 0; - - /** - * @brief Convert the coefficients into a matrix. - * - * @return The coefficients as a matrix. - */ - [[nodiscard]] virtual Eigen::MatrixXd toMatrix() const = 0; -}; - -/** - * @brief A base struct that can be used to define a collection of 6-D hydrodynamic coefficients. - */ -struct Hydrodynamics6d : HydrodynamicsXd -{ - double x; // The X coefficient. - double y; // The Y coefficient. - double z; // The Z coefficient. - double k; // The K coefficient. - double m; // The M coefficient. - double n; // The Z coefficient. - - /** - * @brief Construct a new Hydrodynamics6d object. - * - * @param x The X coefficient. - * @param y The Y coefficient. - * @param z The Z coefficient. - * @param k The K coefficient. - * @param m The M coefficient. - * @param n The Z coefficient. - */ - Hydrodynamics6d(double x, double y, double z, double k, double m, double n); - - /** - * @brief Create a vector using the coefficients. - * - * @return The coefficients as a column vector. - */ - [[nodiscard]] Eigen::VectorXd toVector() const override; - - /** - * @brief Create a matrix using the coefficients. - * - * @note The base struct creates a diagonal matrix from the coefficients. - * - * @return The coefficients as a 6x6 diagonal matrix. - */ - [[nodiscard]] Eigen::MatrixXd toMatrix() const override; -}; - -/** - * @brief A base struct that can be used to define a collection of 3-D hydrodynamic coefficients. - */ -struct Hydrodynamics3d : HydrodynamicsXd -{ - double x; // The X coefficient. - double y; // The Y coefficient. - double z; // The Z coefficient. - - /** - * @brief Construct a new Hydrodynamics3d object - * - * @param x The X coefficient. - * @param y The Y coefficient. - * @param z The Z coefficient. - */ - Hydrodynamics3d(double x, double y, double z); - - /** - * @brief Create a vector from the coefficients. - * - * @return The coefficients as a column vector. - */ - [[nodiscard]] Eigen::VectorXd toVector() const override; - - /** - * @brief Create a matrix from the coefficients. - * - * @note The base struct creates a diagonal matrix from the coefficients. - * - * @return The coefficients as a 3x3 diagonal matrix. - */ - [[nodiscard]] Eigen::MatrixXd toMatrix() const override; -}; - -/** - * @brief The moments of inertia of a rigid body. - */ -struct MomentsOfInertia : Hydrodynamics3d -{ - /** - * @brief Construct a new default MomentsOfInertia object. - */ - MomentsOfInertia(); - - /** - * @brief Construct a new MomentsOfInertia object. - * - * @param x The I_xx coefficient. - * @param y The I_yy coefficient. - * @param z The I_zz coefficient. - */ - MomentsOfInertia(double x, double y, double z); -}; - -/** - * @brief The center of buoyancy of a submerged body. - */ -struct CenterOfBuoyancy : Hydrodynamics3d -{ - /** - * @brief Construct a new default CenterOfBuoyancy object. - */ - CenterOfBuoyancy(); - - /** - * @brief Construct a new CenterOfBuoyancy object. - * - * @param x The center of buoyancy along the x-axis. - * @param y The center of buoyancy along the y-axis. - * @param z The center of buoyancy along the z-axis. - */ - CenterOfBuoyancy(double x, double y, double z); -}; - -/** - * @brief The center of gravity of a submerged body. - */ -struct CenterOfGravity : Hydrodynamics3d -{ - /** - * @brief Construct a new default CenterOfGravity object. - */ - CenterOfGravity(); - - /** - * @brief Construct a new CenterOfGravity object. - * - * @param x The center of gravity along the x-axis. - * @param y The center of gravity along the y-axis. - * @param z The center of gravity along the z-axis. - */ - CenterOfGravity(double x, double y, double z); -}; - -/** - * @brief The added mass hydrodynamic coefficients. - */ -struct AddedMass : Hydrodynamics6d -{ - /** - * @brief Construct a new default AddedMass object. - */ - AddedMass(); - - /** - * @brief Construct a new AddedMass object. - * - * @param x_u_dot The X_dot{u} coefficient. - * @param y_v_dot The X_dot{v} coefficient. - * @param z_w_dot The Z_dot{w} coefficient. - * @param k_p_dot The K_dot{p} coefficient. - * @param m_q_dot The K_dot{q} coefficient. - * @param n_r_dot The N_dot{r} coefficient. - */ - AddedMass( - double x_u_dot, double y_v_dot, double z_w_dot, double k_p_dot, double m_q_dot, double n_r_dot); -}; - -/** - * @brief Linear damping coefficients. - * - */ -struct LinearDamping : Hydrodynamics6d -{ - /** - * @brief Construct a new default LinearDamping object. - */ - LinearDamping(); - - /** - * @brief Construct a new LinearDamping object. - * - * @param x_u The X_u coefficient. - * @param y_v The Y_v coefficient. - * @param z_w The Z_w coefficient. - * @param k_p The K_p coefficient. - * @param m_q The M_q coefficient. - * @param n_r The N_r coefficient. - */ - LinearDamping(double x_u, double y_v, double z_w, double k_p, double m_q, double n_r); -}; - -/** - * @brief Nonlinear damping coefficients. - * - */ -struct NonlinearDamping : Hydrodynamics6d -{ - /** - * @brief Construct a new default NonlinearDamping object. - */ - NonlinearDamping(); - - /** - * @brief Construct a new NonlinearDamping object - * - * @param x_uu The X_{u|u|} coefficient. - * @param y_vv The Y_{v|v|} coefficient. - * @param z_ww The Z_{w|w|} coefficient. - * @param k_pp The K_{p|p|} coefficient. - * @param m_qq The M_{q|q|} coefficient. - * @param n_rr The N_{r|r|} coefficient. - */ - NonlinearDamping(double x_uu, double y_vv, double z_ww, double k_pp, double m_qq, double n_rr); -}; - -} // namespace blue::dynamics diff --git a/blue_dynamics/include/blue_dynamics/thruster_dynamics.hpp b/blue_dynamics/include/blue_dynamics/thruster_dynamics.hpp deleted file mode 100644 index 651aa876..00000000 --- a/blue_dynamics/include/blue_dynamics/thruster_dynamics.hpp +++ /dev/null @@ -1,38 +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. - -#pragma once - -#include - -namespace blue::dynamics -{ - -struct ThrusterDynamics -{ - [[nodiscard]] virtual Eigen::MatrixXd calculateThrusterDynamics() const = 0; -}; - -struct LumpedParameterThrusterDynamics : ThrusterDynamics -{ - [[nodiscard]] Eigen::MatrixXd calculateThrusterDynamics() const override; -}; - -} // namespace blue::dynamics diff --git a/blue_dynamics/include/blue_dynamics/vehicle_dynamics.hpp b/blue_dynamics/include/blue_dynamics/vehicle_dynamics.hpp deleted file mode 100644 index f14e3028..00000000 --- a/blue_dynamics/include/blue_dynamics/vehicle_dynamics.hpp +++ /dev/null @@ -1,216 +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. - -#pragma once - -#include - -#include "blue_dynamics/hydrodynamics.hpp" -#include "geometry_msgs/msg/point.hpp" -#include "geometry_msgs/msg/pose_stamped.hpp" -#include "geometry_msgs/msg/twist_stamped.hpp" - -namespace blue::dynamics -{ - -/** - * @brief The hydrodynamic model for an underwater vehicle. - * - * @note This implementation draws from Thor I. Fossen's "Handbook of Marine Craft Hydrodynamics - * and Motion Control" (2011) and Gianluca Antonelli's "Underwater Robots" (2014) for the - * hydrodynamic model. Relevant equations and definitions used are documented with each method. - * - */ -struct VehicleDynamics -{ - double mass; // The total mass of the vehicle (kg). - double weight; // The weight of the vehicle in the inertial frame. - double buoyancy; // The buoyance force acting on the vehicle in the inertial frame. - MomentsOfInertia moments; // The moments of inertia. - AddedMass added_mass; // The added mass coefficients. - LinearDamping linear_damping; // The linear damping coefficients. - NonlinearDamping quadratic_damping; // The nonlinear damping coefficients. - CenterOfBuoyancy center_of_buoyancy; // The center of buoyancy. - CenterOfGravity center_of_gravity; // The center of gravity. - - /** - * @brief Create a new VehicleDynamics object. - * - * @param mass The total mass of the vehicle (kg). - * @param weight The weight of the vehicle in the inertial frame. This is denoted as `W` in - * Section 2.5 of Gianluca Antonelli's "Underwater Robots". - * @param buoyancy The buoyancy force acting on the vehicle. This is denoted as `B` in Section 2.5 - * of Gianluca Antonelli's "Underwater Robots". - * @param moments The moments of inertia. - * @param added_mass The added mass coefficients. These are defined in Section 2.4.1 of Gianluca - * Antonelli's "Underwater Robots". - * @param linear_damping The rigid body linear damping coefficients. These are defined in Section - * 2.4.2 of Gianluca Antonelli's "Underwater Robots". - * @param quadratic_damping The rigid body nonlinear damping coefficients. These are defined in - * Section 2.4.2 of Gianluca Antonelli's "Underwater Robots". - * @param center_of_buoyancy The center of buoyancy. - * @param center_of_gravity The center of gravity relative to the body frame. - */ - VehicleDynamics( - double mass, double weight, double buoyancy, const MomentsOfInertia & moments, - const AddedMass & added_mass, const LinearDamping & linear_damping, - const NonlinearDamping & quadratic_damping, const CenterOfBuoyancy & center_of_buoyancy, - const CenterOfGravity & center_of_gravity); - - /** - * @brief Create a skew-symmetric matrix from the provided coefficients. - * - * @param a1 Coefficient one. - * @param a2 Coefficient two. - * @param a3 Coefficient three. - * @return The created skew-symmetric matrix. - */ - [[nodiscard]] static Eigen::Matrix3d createSkewSymmetricMatrix(double a1, double a2, double a3); - - /** - * @brief Calculate the vehicle's inertia matrix. - * - * @note The inertia matrix `M` is the sum of the rigid body mass `M_RB` and the added mass `M_A` - * such that `M = M_RB + M_A`. - * - * @return The inertia matrix `M`. - */ - [[nodiscard]] Eigen::MatrixXd calculateInertiaMatrix() const; - - /** - * @brief Calculate the rigid body inertia matrix. - * - * @note The definition used for the rigid body inertia matrix `M_RB` is provided by - * Thor I. Fossen's textbook "Handbook of Marine Craft Hydrodynamics and Motion Control" in - * Equation 3.44. Note that, in this model, we define the body frame to be coincident with the - * center of mass, such that r_g^b = 0. The result is that `M_RB` is a diagonal matrix. - * - * @param mass The total mass of the vehicle (kg). - * @param moments The moments of inertia. This is assumed to be a diagonal matrix. - * @return The rigid body inertia matrix `M_RB`. - */ - [[nodiscard]] static Eigen::MatrixXd calculateRigidBodyMassMatrix( - double mass, const MomentsOfInertia & moments); - - /** - * @brief Calculate the added mass inertia matrix. - * - * @note The definition used for the added mass inertia matrix `M_A` is provided by Gianluca - * Antonelli's textbook "Underwater Robots" in Section 2.4.1. - * - * @param added_mass The added mass coefficients. - * @return The added mass inertia matrix `M_A`. - */ - [[nodiscard]] static Eigen::MatrixXd calculateAddedMassMatrix(const AddedMass & added_mass); - - /** - * @brief Calculate the Coriolis and centripetal force matrix. - * - * @note The Coriolis and centripetal force matrix `C` is the sum of the rigid body Coriolis - * forces and the added Coriolis forces such that `C = C_RB + C_A`. - * - * @param velocity The current velocity of the vehicle in the body frame. - * @return The Coriolis-centripetal force matrix `C`. - */ - [[nodiscard]] Eigen::MatrixXd calculateCoriolisMatrix( - const geometry_msgs::msg::TwistStamped & velocity) const; - - /** - * @brief Calculate the rigid body Coriolis-centripetal matrix. - * - * @note The definition of the rigid body Coriolis-centripetal matrix `C_RB` used in this work is - * provided by Thor I. Fossen's textbook "Handbook of Marine Craft Hydrodynamics and Motion - * Control" in Equation 3.57. Note that, in this model, we define the body frame to be coincident - * with the center of mass, such that r_g^b = 0. - * - * @param mass The total mass of the vehicle (kg). - * @param moments The moments of inertia. - * @param velocity The current velocity of the vehicle in the body frame. - * @return The rigid body Coriolis-centripetal force matrix `C_RB`. - */ - [[nodiscard]] static Eigen::MatrixXd calculateRigidBodyCoriolisMatrix( - double mass, const MomentsOfInertia & moments, - const geometry_msgs::msg::TwistStamped & velocity); - - /** - * @brief Calculate the added Coriolis-centripetal matrix. - * - * @note The definition of the added Coriolis-centripetal matrix `C_A` used in this work is - * provided by Gianluca Antonelli's "Underwater Robots" in Section 2.4.1. - * - * @param added_mass The added mass coefficients. - * @param velocity The current velocity of the vehicle in the body frame. - * @return The added Coriolis-centripetal force matrix `C_A`. - */ - [[nodiscard]] static Eigen::MatrixXd calculateAddedCoriolixMatrix( - const AddedMass & added_mass, const geometry_msgs::msg::TwistStamped & velocity); - - /** - * @brief Calculate the rigid body damping matrix. - * - * @note The rigid body damping matrix `D` is defined as the sum of the linear and nonlinear - * damping coefficients. - * - * @param velocity The current velocity of the vehicle in the body frame. - * @return The damping matrix `D`. - */ - [[nodiscard]] Eigen::MatrixXd calculateDampingMatrix( - const geometry_msgs::msg::TwistStamped & velocity) const; - - /** - * @brief Calculate the linear damping matrix. - * - * @note The definition of the linear damping matrix used in this work is provided by Gianluca - * Antonelli's "Underwater Robots" in Section 2.4.2. - * - * @param linear_damping The linear damping coefficients. - * @return A matrix of linear damping coefficients. - */ - [[nodiscard]] static Eigen::MatrixXd calculateLinearDampingMatrix( - const LinearDamping & linear_damping); - - /** - * @brief Calculate the nonlinear damping matrix. - * - * @note The definition of the nonlinear damping matrix used in this work is provided by Gianluca - * Antonelli's "Underwater Robots" in Section 2.4.2. - * - * @param quadratic_damping The nonlinear damping coefficients. - * @param velocity The current velocity of the vehicle in the body frame. - * @return A matrix of nonlinear damping coefficients. - */ - [[nodiscard]] static Eigen::MatrixXd calculateNonlinearDampingMatrix( - const NonlinearDamping & quadratic_damping, const geometry_msgs::msg::TwistStamped & velocity); - - /** - * @brief Calculate the vector of force/moment due to gravity and buoyancy in the body-fixed - * frame. - * - * @note The definition used for the gravity and buoyancy vector `g` is given by Gianluca - * Antonelli's "Underwater Robots" in Section 2.5. - * - * @param pose The current pose of the vehicle in the inertial frame. - * @return Calculate the vector of restoring forces `g`. - */ - [[nodiscard]] Eigen::VectorXd calculateRestoringForcesVector( - const geometry_msgs::msg::PoseStamped & pose) const; -}; - -} // namespace blue::dynamics diff --git a/blue_dynamics/package.xml b/blue_dynamics/package.xml deleted file mode 100644 index d4dea7a4..00000000 --- a/blue_dynamics/package.xml +++ /dev/null @@ -1,26 +0,0 @@ - - - - blue_dynamics - 0.0.1 - Library used to define and calculate the hydrodynamics of the BlueROV2. - Evan Palmer - MIT - - ament_cmake - eigen3_cmake_module - - eigen - geometry_msgs - - eigen3_cmake_module - - ament_cmake_gtest - ament_cmake_gmock - ament_lint_auto - ament_lint_common - - - ament_cmake - - diff --git a/blue_dynamics/src/current_effects.cpp b/blue_dynamics/src/current_effects.cpp deleted file mode 100644 index 2683d71e..00000000 --- a/blue_dynamics/src/current_effects.cpp +++ /dev/null @@ -1,52 +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. - -#include "blue_dynamics/current_effects.hpp" - -namespace blue::dynamics -{ - -CurrentEffects::CurrentEffects( - double v_cx, double v_cy, double v_cz, double v_cp = 0, double v_cq = 0, double v_cr = 0) -: v_cx(v_cx), - v_cy(v_cy), - v_cz(v_cz), - v_cp(v_cp), - v_cq(v_cq), - v_cr(v_cr) -{ -} - -[[nodiscard]] Eigen::VectorXd CurrentEffects::calculateCurrentEffects( - const geometry_msgs::msg::PoseStamped & pose) const -{ - Eigen::Quaterniond q( - pose.pose.orientation.w, pose.pose.orientation.x, pose.pose.orientation.y, - pose.pose.orientation.z); - - Eigen::Matrix3d rot = q.toRotationMatrix(); - - Eigen::VectorXd vec(6); // NOLINT - vec << v_cx, v_cy, v_cz, v_cp, v_cq, v_cr; - - return rot * vec; -} - -} // namespace blue::dynamics diff --git a/blue_dynamics/src/hydrodynamics.cpp b/blue_dynamics/src/hydrodynamics.cpp deleted file mode 100644 index 4e59199f..00000000 --- a/blue_dynamics/src/hydrodynamics.cpp +++ /dev/null @@ -1,131 +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. - -#include "blue_dynamics/hydrodynamics.hpp" - -namespace blue::dynamics -{ - -Hydrodynamics6d::Hydrodynamics6d(double x, double y, double z, double k, double m, double n) -: x(x), - y(y), - z(z), - k(k), - m(m), - n(n) -{ -} - -[[nodiscard]] Eigen::VectorXd Hydrodynamics6d::toVector() const -{ - Eigen::VectorXd vec(6); // NOLINT - vec << x, y, z, k, m, n; - - return vec; -} - -[[nodiscard]] Eigen::MatrixXd Hydrodynamics6d::toMatrix() const -{ - return toVector().asDiagonal().toDenseMatrix(); -} - -Hydrodynamics3d::Hydrodynamics3d(double x, double y, double z) -: x(x), - y(y), - z(z) -{ -} - -[[nodiscard]] Eigen::VectorXd Hydrodynamics3d::toVector() const -{ - Eigen::VectorXd vec(3); // NOLINT - vec << x, y, z; - - return vec; -} - -[[nodiscard]] Eigen::MatrixXd Hydrodynamics3d::toMatrix() const -{ - return toVector().asDiagonal().toDenseMatrix(); -} - -MomentsOfInertia::MomentsOfInertia() -: Hydrodynamics3d(0.0, 0.0, 0.0) -{ -} - -MomentsOfInertia::MomentsOfInertia(double x, double y, double z) -: Hydrodynamics3d(x, y, z) -{ -} - -CenterOfBuoyancy::CenterOfBuoyancy() -: Hydrodynamics3d(0.0, 0.0, 0.0) -{ -} - -CenterOfBuoyancy::CenterOfBuoyancy(double x, double y, double z) -: Hydrodynamics3d(x, y, z) -{ -} - -CenterOfGravity::CenterOfGravity() -: Hydrodynamics3d(0.0, 0.0, 0.0) -{ -} - -CenterOfGravity::CenterOfGravity(double x, double y, double z) -: Hydrodynamics3d(x, y, z) -{ -} - -AddedMass::AddedMass() -: Hydrodynamics6d(0.0, 0.0, 0.0, 0.0, 0.0, 0.0) -{ -} - -AddedMass::AddedMass( - double x_u_dot, double y_v_dot, double z_w_dot, double k_p_dot, double m_q_dot, double n_r_dot) -: Hydrodynamics6d(x_u_dot, y_v_dot, z_w_dot, k_p_dot, m_q_dot, n_r_dot) -{ -} - -LinearDamping::LinearDamping() -: Hydrodynamics6d(0.0, 0.0, 0.0, 0.0, 0.0, 0.0) -{ -} - -LinearDamping::LinearDamping(double x_u, double y_v, double z_w, double k_p, double m_q, double n_r) -: Hydrodynamics6d(x_u, y_v, z_w, k_p, m_q, n_r) -{ -} - -NonlinearDamping::NonlinearDamping() -: Hydrodynamics6d(0.0, 0.0, 0.0, 0.0, 0.0, 0.0) -{ -} - -NonlinearDamping::NonlinearDamping( - double x_uu, double y_vv, double z_ww, double k_pp, double m_qq, double n_rr) -: Hydrodynamics6d(x_uu, y_vv, z_ww, k_pp, m_qq, n_rr) -{ -} - -} // namespace blue::dynamics diff --git a/blue_dynamics/src/thruster_dynamics.cpp b/blue_dynamics/src/thruster_dynamics.cpp deleted file mode 100644 index e69de29b..00000000 diff --git a/blue_dynamics/src/vehicle_dynamics.cpp b/blue_dynamics/src/vehicle_dynamics.cpp deleted file mode 100644 index 9ae97bcc..00000000 --- a/blue_dynamics/src/vehicle_dynamics.cpp +++ /dev/null @@ -1,175 +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. - -#include "blue_dynamics/vehicle_dynamics.hpp" - -#include - -namespace blue::dynamics -{ - -VehicleDynamics::VehicleDynamics( - double mass, double weight, double buoyancy, const MomentsOfInertia & moments, - const AddedMass & added_mass, const LinearDamping & linear_damping, - const NonlinearDamping & quadratic_damping, const CenterOfBuoyancy & center_of_buoyancy, - const CenterOfGravity & center_of_gravity) -: mass(mass), - weight(weight), - buoyancy(buoyancy), - moments(std::move(moments)), - added_mass(std::move(added_mass)), - linear_damping(std::move(linear_damping)), - quadratic_damping(std::move(quadratic_damping)), - center_of_buoyancy(std::move(center_of_buoyancy)), - center_of_gravity(std::move(center_of_gravity)) -{ -} - -[[nodiscard]] Eigen::Matrix3d VehicleDynamics::createSkewSymmetricMatrix( - double a1, double a2, double a3) -{ - Eigen::Matrix3d mat; - mat << 0, -a3, a2, a3, 0, -a1, -a2, a1, 0; - - return mat; -} - -[[nodiscard]] Eigen::MatrixXd VehicleDynamics::calculateInertiaMatrix() const -{ - return calculateRigidBodyMassMatrix(mass, moments) + calculateAddedMassMatrix(added_mass); -} - -[[nodiscard]] Eigen::MatrixXd VehicleDynamics::calculateRigidBodyMassMatrix( - double mass, const MomentsOfInertia & moments) -{ - Eigen::MatrixXd mat(6, 6); - - mat.topLeftCorner(3, 3) = mass * Eigen::MatrixXd::Identity(3, 3); - mat.topRightCorner(3, 3) = Eigen::MatrixXd::Zero(3, 3); - mat.bottomLeftCorner(3, 3) = Eigen::MatrixXd::Zero(3, 3); - mat.bottomRightCorner(3, 3) = moments.toMatrix(); - - return mat; -} - -[[nodiscard]] Eigen::MatrixXd VehicleDynamics::calculateAddedMassMatrix( - const AddedMass & added_mass) -{ - return -added_mass.toMatrix(); -} - -[[nodiscard]] Eigen::MatrixXd VehicleDynamics::calculateCoriolisMatrix( - const geometry_msgs::msg::TwistStamped & velocity) const -{ - return calculateRigidBodyCoriolisMatrix(mass, moments, velocity) + - calculateAddedCoriolixMatrix(added_mass, velocity); -} - -[[nodiscard]] Eigen::MatrixXd VehicleDynamics::calculateRigidBodyCoriolisMatrix( - double mass, const MomentsOfInertia & moments, const geometry_msgs::msg::TwistStamped & velocity) -{ - Eigen::MatrixXd mat(6, 6); - - Eigen::Vector3d v2(3); // NOLINT - v2 << velocity.twist.angular.x, velocity.twist.angular.y, velocity.twist.angular.z; - - const Eigen::Vector3d moments_v2 = moments.toMatrix() * v2; - - mat.topLeftCorner(3, 3) = mass * createSkewSymmetricMatrix(v2(0), v2(1), v2(2)); - mat.topRightCorner(3, 3) = Eigen::MatrixXd::Zero(3, 3); - mat.bottomLeftCorner(3, 3) = Eigen::MatrixXd::Zero(3, 3); - mat.bottomRightCorner(3, 3) = - -createSkewSymmetricMatrix(moments_v2(0), moments_v2(1), moments_v2(2)); - - return mat; -} - -[[nodiscard]] Eigen::MatrixXd VehicleDynamics::calculateAddedCoriolixMatrix( - const AddedMass & added_mass, const geometry_msgs::msg::TwistStamped & velocity) -{ - Eigen::MatrixXd mat(6, 6); - - Eigen::Matrix3d linear_vel = createSkewSymmetricMatrix( - added_mass.x * velocity.twist.linear.x, added_mass.y * velocity.twist.linear.y, - added_mass.z * velocity.twist.linear.z); - - Eigen::Matrix3d angular_vel = createSkewSymmetricMatrix( - added_mass.k * velocity.twist.angular.x, added_mass.m * velocity.twist.angular.y, - added_mass.n * velocity.twist.angular.z); - - mat.topLeftCorner(3, 3) = Eigen::MatrixXd::Zero(3, 3); - mat.topRightCorner(3, 3) = linear_vel; - mat.bottomLeftCorner(3, 3) = linear_vel; - mat.bottomRightCorner(3, 3) = angular_vel; - - return mat; -} - -[[nodiscard]] Eigen::MatrixXd VehicleDynamics::calculateDampingMatrix( - const geometry_msgs::msg::TwistStamped & velocity) const -{ - return calculateLinearDampingMatrix(linear_damping) + - calculateNonlinearDampingMatrix(quadratic_damping, velocity); -} - -[[nodiscard]] Eigen::MatrixXd VehicleDynamics::calculateLinearDampingMatrix( - const LinearDamping & linear_damping) -{ - return -linear_damping.toMatrix(); -} - -[[nodiscard]] Eigen::MatrixXd VehicleDynamics::calculateNonlinearDampingMatrix( - const NonlinearDamping & quadratic_damping, const geometry_msgs::msg::TwistStamped & velocity) -{ - Eigen::VectorXd vec(6); - vec << velocity.twist.linear.x, velocity.twist.linear.y, velocity.twist.linear.z, - velocity.twist.angular.x, velocity.twist.angular.y, velocity.twist.angular.z; - - // Take the absolute value of each coefficient - vec = vec.cwiseAbs(); - - return -(quadratic_damping.toMatrix() * vec).asDiagonal().toDenseMatrix(); -} - -[[nodiscard]] Eigen::VectorXd VehicleDynamics::calculateRestoringForcesVector( - const geometry_msgs::msg::PoseStamped & pose) const -{ - Eigen::Quaterniond q( - pose.pose.orientation.w, pose.pose.orientation.x, pose.pose.orientation.y, - pose.pose.orientation.z); - - Eigen::Matrix3d rot = q.toRotationMatrix(); - - // The Z-axis points downwards, so gravity is positive and buoyancy is negative - Eigen::Vector3d fg(0, 0, weight); // NOLINT - Eigen::Vector3d fb(0, 0, -buoyancy); // NOLINT - - Eigen::VectorXd g_rb(6); - g_rb.topRows(3) = rot * (fg + fb); - - // These vectors already 3d, but are returned as "Xd" vectors and need to be casted to "3d" - // vectors to support using the `cross` method. - g_rb.bottomRows(3) = static_cast(center_of_gravity.toVector()).cross(rot * fg) + - static_cast(center_of_buoyancy.toVector()).cross(rot * fb); - g_rb *= -1; - - return g_rb; -} -} // namespace blue::dynamics diff --git a/blue_dynamics/test/test_vehicle_dynamics.cpp b/blue_dynamics/test/test_vehicle_dynamics.cpp deleted file mode 100644 index a257a549..00000000 --- a/blue_dynamics/test/test_vehicle_dynamics.cpp +++ /dev/null @@ -1,220 +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. - -#include - -#include -#include - -#include "blue_dynamics/vehicle_dynamics.hpp" -#include "geometry_msgs/msg/pose_stamped.hpp" -#include "geometry_msgs/msg/twist_stamped.hpp" - -namespace blue::dynamics::test -{ - -TEST(VehicleDynamicsTest, CreatesSkewSymmetricMatrix) -{ - const double a1 = 1.0; - const double a2 = 2.0; - const double a3 = 3.0; - - Eigen::Matrix3d expected; - expected << 0, -a3, a2, a3, 0, -a1, -a2, a1, 0; - - const Eigen::Matrix3d actual = - blue::dynamics::VehicleDynamics::createSkewSymmetricMatrix(a1, a2, a3); - - ASSERT_TRUE(expected.isApprox(actual)); -} - -TEST(VehicleDynamicsTest, CalculatesRigidBodyMassMatrix) -{ - const auto moments = blue::dynamics::MomentsOfInertia(1.0, 2.0, 3.0); - const double mass = 5.0; - - Eigen::VectorXd diagonal(6); // NOLINT - diagonal << mass, mass, mass, moments.x, moments.y, moments.z; - - const Eigen::MatrixXd expected = diagonal.asDiagonal().toDenseMatrix(); - - const Eigen::MatrixXd actual = - blue::dynamics::VehicleDynamics::calculateRigidBodyMassMatrix(mass, moments); - - ASSERT_TRUE(expected.isApprox(actual)); -} - -TEST(VehicleDynamicsTest, CalculatesAddedMassMatrix) -{ - const blue::dynamics::AddedMass added_mass = - blue::dynamics::AddedMass(1.0, 2.0, 3.0, 4.0, 5.0, 6.0); - - const Eigen::MatrixXd expected = -added_mass.toMatrix(); - - const Eigen::MatrixXd actual = - blue::dynamics::VehicleDynamics::calculateAddedMassMatrix(added_mass); - - ASSERT_TRUE(expected.isApprox(actual)); -} - -TEST(VehicleDynamicsTest, CalculatesRigidBodyCoriolisMatrix) -{ - auto moments = blue::dynamics::MomentsOfInertia(1.0, 2.0, 3.0); - const double mass = 5.0; - - geometry_msgs::msg::TwistStamped velocity; - velocity.twist.angular.x = 1.0; - velocity.twist.angular.y = 2.0; - velocity.twist.angular.z = 3.0; - - Eigen::MatrixXd expected(6, 6); - expected.topRightCorner(3, 3) = Eigen::MatrixXd::Zero(3, 3); - expected.bottomLeftCorner(3, 3) = Eigen::MatrixXd::Zero(3, 3); - expected.topLeftCorner(3, 3) = - mass * blue::dynamics::VehicleDynamics::createSkewSymmetricMatrix( - velocity.twist.angular.x, velocity.twist.angular.y, velocity.twist.angular.z); - - Eigen::Vector3d v2(3); - v2 << velocity.twist.angular.x, velocity.twist.angular.y, velocity.twist.angular.z; - - const Eigen::Vector3d moments_v2 = moments.toMatrix() * v2; - - expected.bottomRightCorner(3, 3) = -blue::dynamics::VehicleDynamics::createSkewSymmetricMatrix( - moments_v2(0), moments_v2(1), moments_v2(2)); - - Eigen::MatrixXd actual = - blue::dynamics::VehicleDynamics::calculateRigidBodyCoriolisMatrix(mass, moments, velocity); - - ASSERT_TRUE(expected.isApprox(actual)); -} - -TEST(VehicleDynamicsTest, CalculatesAddedCoriolisMatrix) -{ - const blue::dynamics::AddedMass added_mass = - blue::dynamics::AddedMass(1.0, 2.0, 3.0, 4.0, 5.0, 6.0); - - geometry_msgs::msg::TwistStamped velocity; - velocity.twist.linear.x = 1.0; - velocity.twist.linear.y = 2.0; - velocity.twist.linear.z = 3.0; - velocity.twist.angular.x = 1.0; - velocity.twist.angular.y = 2.0; - velocity.twist.angular.z = 3.0; - - Eigen::MatrixXd linear = blue::dynamics::VehicleDynamics::createSkewSymmetricMatrix( - added_mass.x * velocity.twist.linear.x, added_mass.y * velocity.twist.linear.y, - added_mass.z * velocity.twist.linear.z); - - Eigen::MatrixXd expected(6, 6); - expected.topLeftCorner(3, 3) = Eigen::MatrixXd::Zero(3, 3); - expected.topRightCorner(3, 3) = linear; - expected.bottomLeftCorner(3, 3) = linear; - expected.bottomRightCorner(3, 3) = blue::dynamics::VehicleDynamics::createSkewSymmetricMatrix( - added_mass.k * velocity.twist.angular.x, added_mass.m * velocity.twist.angular.y, - added_mass.n * velocity.twist.angular.z); - - Eigen::MatrixXd actual = - blue::dynamics::VehicleDynamics::calculateAddedCoriolixMatrix(added_mass, velocity); - - ASSERT_TRUE(expected.isApprox(actual)); -} - -TEST(VehicleDynamicsTest, CalculatesLinearDampingMatrix) -{ - const auto linear = blue::dynamics::LinearDamping(1.0, 2.0, 3.0, 4.0, 5.0, 6.0); - const Eigen::MatrixXd expected = -linear.toMatrix(); - - const Eigen::MatrixXd actual = - blue::dynamics::VehicleDynamics::calculateLinearDampingMatrix(linear); - - ASSERT_TRUE(expected.isApprox(actual)); -} - -TEST(VehicleDynamicsTest, CalculatesNonlinearDampingMatrix) -{ - geometry_msgs::msg::TwistStamped velocity; - velocity.twist.linear.x = -1.0; - velocity.twist.linear.y = -2.0; - velocity.twist.linear.z = -3.0; - velocity.twist.angular.x = -1.0; - velocity.twist.angular.y = -2.0; - velocity.twist.angular.z = -3.0; - - Eigen::VectorXd velocity_vect(6); // NOLINT - velocity_vect << std::abs(velocity.twist.linear.x), std::abs(velocity.twist.linear.y), - std::abs(velocity.twist.linear.z), std::abs(velocity.twist.angular.x), - std::abs(velocity.twist.angular.y), std::abs(velocity.twist.angular.z); - - const auto nonlinear = blue::dynamics::NonlinearDamping(1.0, 2.0, 3.0, 4.0, 5.0, 6.0); - const Eigen::MatrixXd expected = - -(nonlinear.toMatrix() * velocity_vect).asDiagonal().toDenseMatrix(); - - const Eigen::MatrixXd actual = - blue::dynamics::VehicleDynamics::calculateNonlinearDampingMatrix(nonlinear, velocity); - - ASSERT_TRUE(expected.isApprox(actual)); -} - -TEST(VehicleDynamicsTest, CalculatesRestoringForcesVector) -{ - const double weight = 10.0; - const double buoyancy = 7.0; - auto pose = geometry_msgs::msg::PoseStamped(); - - const auto center_of_gravity = blue::dynamics::CenterOfGravity(0.1, 0.1, 0.3); - - // Buoyancy is coincident with the center of gravity - const auto center_of_buoyancy = blue::dynamics::CenterOfBuoyancy(0.1, 0.1, 0.3); - - // No rotation - pose.pose.orientation.x = 0.0; - pose.pose.orientation.y = 0.0; - pose.pose.orientation.z = 0.0; - pose.pose.orientation.w = 1.0; - - Eigen::VectorXd expected(6); // NOLINT - - Eigen::Vector3d fg(0, 0, weight); // NOLINT - Eigen::Vector3d fb(0, 0, -buoyancy); // NOLINT - - expected.topRows(3) = fg + fb; - expected.bottomRows(3) = static_cast(center_of_gravity.toVector()).cross(fg) + - static_cast(center_of_buoyancy.toVector()).cross(fb); - expected *= -1; - - const auto vehicle_dynamics = blue::dynamics::VehicleDynamics( - 0.0, weight, buoyancy, blue::dynamics::MomentsOfInertia(), blue::dynamics::AddedMass(), - blue::dynamics::LinearDamping(), blue::dynamics::NonlinearDamping(), center_of_buoyancy, - center_of_gravity); - - Eigen::VectorXd actual = vehicle_dynamics.calculateRestoringForcesVector(pose); - - ASSERT_TRUE(expected.isApprox(actual)); -} - -} // namespace blue::dynamics::test - -int main(int argc, char ** argv) -{ - ::testing::InitGoogleTest(&argc, argv); - const int result = RUN_ALL_TESTS(); - - return result; -} From 4f1230ce18c059dacdc5960b74c3e13e82a73353 Mon Sep 17 00:00:00 2001 From: Evan Palmer Date: Fri, 5 May 2023 00:53:01 -0700 Subject: [PATCH 25/34] Implemented and tested parameter backup procedure, started on passthrough mode enabling --- blue_manager/blue_manager/manager.py | 243 +++++++++++++++++++++------ blue_manager/setup.py | 24 +-- blue_msgs/CMakeLists.txt | 30 ---- blue_msgs/LICENSE | 17 -- blue_msgs/action/Arm.action | 11 -- blue_msgs/action/Connect.action | 11 -- blue_msgs/package.xml | 26 --- cspell.json | 14 ++ 8 files changed, 215 insertions(+), 161 deletions(-) delete mode 100644 blue_msgs/CMakeLists.txt delete mode 100644 blue_msgs/LICENSE delete mode 100644 blue_msgs/action/Arm.action delete mode 100644 blue_msgs/action/Connect.action delete mode 100644 blue_msgs/package.xml create mode 100644 cspell.json diff --git a/blue_manager/blue_manager/manager.py b/blue_manager/blue_manager/manager.py index 9a30d8db..f9e7222d 100644 --- a/blue_manager/blue_manager/manager.py +++ b/blue_manager/blue_manager/manager.py @@ -18,103 +18,238 @@ # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN # THE SOFTWARE. -from typing import Any +from copy import deepcopy +from functools import partial import rclpy -from mavros_msgs.srv import MessageInterval, ParamGet, ParamSetV2 +from mavros_msgs.msg import ParamEvent +from rcl_interfaces.msg import Parameter, ParameterType +from rcl_interfaces.srv import GetParameters, SetParameters from rclpy.node import Node -from sensor_msgs.msg import BatteryState +from rclpy.task import Future from std_srvs.srv import SetBool class Manager(Node): + """Provides custom controllers with an interface to the BlueROV2 thrusters.""" + STOPPED_PWM = 1500 def __init__(self) -> None: + """Create a new control manager.""" super().__init__("blue_manager") - self.battery_state = BatteryState() self.connected = False self.armed = False - self.rov_config = "heavy" - self.backup_params: dict[str, Any] = {} + + self.declare_parameters("", [("rov_config", "heavy")]) + + rov_config = self.get_parameter("rov_config").get_parameter_value().string_value + + # The BlueROV2 has two configurations: the heavy configuration with 8 thrusters + # and the standard configuration with 6 thrusters. + match rov_config: + case "heavy": + num_thrusters = 8 + case _: + num_thrusters = 6 + + # Maintain a backup of the thruster parameters so that we can restore them when + # disconnecting + self.thruster_params_backup: dict[str, Parameter | None] = { + f"SERVO{i}_FUNCTION": None for i in range(1, num_thrusters + 1) + } # Subscribers - self.battery_sub = self.create_subscription( - BatteryState, "/mavros/battery", self._store_battery_state_cb + self.param_event_sub = self.create_subscription( + ParamEvent, "/mavros/param/event", self.save_default_thruster_params_cb, 10 ) # Services - self.arm_srv = self.create_service( - SetBool, "/blue/connect", self._manage_connection_cb - ) self.connect_srv = self.create_service( - SetBool, "/blue/arm", self._manage_arming_cb + SetBool, "/blue/connect", self.manage_connection_cb ) + def wait_for_service(client): + while not client.wait_for_service(timeout_sec=1.0): + ... + # Service clients - self.set_message_rate_srv_client = self.create_client( - MessageInterval, "/mavros/set_message_interval", 1 + self.get_param_srv_client = self.create_client( + GetParameters, "/mavros/param/get_parameters" ) - self.get_param_srv_client = self.create_client(ParamGet, "/mavros/param/get", 1) + wait_for_service(self.get_param_srv_client) + self.set_param_srv_client = self.create_client( - ParamSetV2, "/mavros/param/set", 1 + SetParameters, "/mavros/param/set_parameters" ) + wait_for_service(self.set_param_srv_client) - def _store_battery_state_cb(self, state: BatteryState) -> None: - self.battery_state = state + @property + def thruster_params_backed_up(self) -> bool: + """All required thruster parameters have been successfully backed up. - def check_battery_state(self) -> bool: - return self.battery_state.percentage <= 0.2 + Returns: + Whether or not the thruster parameters have been backed up. + """ + return None not in self.thruster_params_backup.values() - def _manage_connection_cb(self, connect: bool) -> None: - if connect: - self.get_logger().info( - "Attempting to establish a connection with the BlueROV2..." - ) + def save_default_thruster_params_cb(self, event: ParamEvent) -> None: + """Save the default thruster parameter values to the parameter backup. - self.connected = self.connect() + MAVROS publishes the parameter values of all ArduSub parameters when + populating the parameter server. We subscribe to this topic to receive the + messages at the same time as MAVROS so that we can avoid duplicating requests + to the FCU. - if not self.connected: - self.get_logger().warning( - "Failed to establish a connection with the BlueROV2." - ) - else: - self.get_logger().info( - "Successfully established a connection with the BlueROV2." - ) - else: - self.get_logger().info( - "Attempting to shutdown the connection to the BlueROV2..." + Args: + event: The default parameter loading event triggered by MAVROS. + """ + if ( + event.param_id in self.thruster_params_backup + and self.thruster_params_backup[event.param_id] is None + ): + self.thruster_params_backup[event.param_id] = Parameter( + name=event.param_id, value=event.value ) - self.connected = not self.disconnect() - if not self.connected: - self.get_logger().warning( - "Successfully shutdown the connection to the BlueROV2." + if self.thruster_params_backed_up: + self.get_logger().info( + "Successfully backed up the ArduSub thruster parameters." ) + + def request_ardusub_params(self, param_ids: list[str]) -> Future: + """Request parameter values from ArduSub. + + Args: + param_ids: The names of the parameters whose values should be requested. + + Returns: + A future object which provides access to the parameter request result. + """ + return self.get_param_srv_client.call_async( + GetParameters.Request(names=param_ids) + ) + + def request_thruster_params(self, thruster_param_ids: list[str]) -> None: + """Request the thruster parameters which will be overwritten. + + The thruster parameters requested include all MOTORn_FUNCTION parameters + assigned to be motors. These are modified by the manager to support the PWM + Pass-through mode, but need to be restored when the manager is shutdown. + + Args: + thruster_param_ids: The names of the thruster parameters whose values + should be backed up. + """ + + def handle_request_future_cb(future: Future, param_ids: list[str]): + try: + response = future.result() + except Exception: + ... else: - self.get_logger().info("Failed to perform all shutdown proceedures.") + # The parameter values are provided in the same order as the names + # were provided in + for param_value, param_id in zip(response.values, param_ids): + self.thruster_params_backup[param_id] = Parameter( + name=param_id, value=param_value + ) + + future = self.request_ardusub_params(thruster_param_ids) + future.add_done_callback( + partial(handle_request_future_cb, param_ids=thruster_param_ids) + ) + + def manage_connection_cb( + self, request: SetBool.Request, response: SetBool.Response + ) -> SetBool.Response: + """Connect/disconnect the manager. + + Args: + request: The connection service request. + response: The connection service response. + + Returns: + The connection service response. + """ + if request.data: + response.success = self.connect() + else: + response.success = True + + # TODO(Evan): This is wrong + self.connected = response.success + + return response def connect(self) -> bool: - ... + self.get_logger().warning( + "Attempting to connect the manager to the BlueROV2. All ArduSub arming" + " and failsafe procedures will be disabled upon successful connection." + ) + if not self.thruster_params_backed_up: + # Get the IDs of the parameters that weren't successfully backed up on + # initialization + unsaved_params = [ + param_id + for param_id in self.thruster_params_backup.keys() + if self.thruster_params_backup[param_id] is None + ] - def backup_motor_params(self) -> bool: - ... + self.request_thruster_params(unsaved_params) - def disconnect(self) -> bool: - ... + if not self.thruster_params_backed_up: + self.get_logger().error( + "Connection attempt failed. The system has not finished backing" + " up the thruster parameters or the backup process failed." + ) + return False - def restore_motor_param_backup(self) -> bool: - ... + # Set the motors to PWM passthrough mode + return self.set_pwm_passthrough_mode() - def _manage_arming_cb(self, arm: bool) -> None: - ... + def set_pwm_passthrough_mode(self) -> bool: + if not self.thruster_params_backed_up: + self.get_logger().error( + "The thrusters cannot be set to PWM Pass-through mode without first" + " being backed up." + ) + return False - def arm(self) -> bool: - ... + passthrough_params = deepcopy(self.thruster_params_backup) + + # Set the servo mode to "DISABLED" + # 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 = 0 # type: ignore + param.value.type = ParameterType.PARAMETER_INTEGER # type: ignore + + # We would actually prefer to set all of the parameters atomically; however, + # this functionality is not supported by MAVROS + set_param_request = SetParameters.Request( + parameters=list(passthrough_params.values()) + ) + + self.set_param_srv_client.call_async(set_param_request).add_done_callback( + self.this_is_a_test + ) - def disarm(self) -> bool: + return True + + def this_is_a_test(self, future): + try: + response = future.result() + except Exception as e: + self.get_logger().info("Service call failed %r" % (e,)) + else: + self.get_logger().info(f"Parameter set succeed..maybe?: {response}") + + def disconnect(self) -> bool: + return False + + def restore_motor_param_backup(self) -> bool: ... diff --git a/blue_manager/setup.py b/blue_manager/setup.py index 9ef2bf98..104d74c1 100644 --- a/blue_manager/setup.py +++ b/blue_manager/setup.py @@ -1,25 +1,25 @@ from setuptools import setup -package_name = 'blue_manager' +package_name = "blue_manager" setup( name=package_name, - version='0.0.0', + version="0.0.0", packages=[package_name], data_files=[ - ('share/ament_index/resource_index/packages', - ['resource/' + package_name]), - ('share/' + package_name, ['package.xml']), + ("share/ament_index/resource_index/packages", ["resource/" + package_name]), + ("share/" + package_name, ["package.xml"]), ], - install_requires=['setuptools'], + install_requires=["setuptools"], zip_safe=True, - maintainer='Evan Palmer', - maintainer_email='evanp922@gmail.com', - description='TODO: Package description', - license='MIT', - tests_require=['pytest'], + maintainer="Evan Palmer", + maintainer_email="evanp922@gmail.com", + description="TODO: Package description", + license="MIT", + tests_require=["pytest"], entry_points={ - 'console_scripts': [ + "console_scripts": [ + "blue_manager = blue_manager.manager:main", ], }, ) diff --git a/blue_msgs/CMakeLists.txt b/blue_msgs/CMakeLists.txt deleted file mode 100644 index ec036f0c..00000000 --- a/blue_msgs/CMakeLists.txt +++ /dev/null @@ -1,30 +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(action_msgs REQUIRED) -find_package(rosidl_default_generators REQUIRED) - -set(action_files - "action/Arm.action" - "action/Connect.action" -) - -rosidl_generate_interfaces(${PROJECT_NAME} - ${action_files} - DEPENDENCIES builtin_interfaces std_msgs action_msgs -) - -if(BUILD_TESTING) - find_package(ament_lint_auto REQUIRED) - ament_lint_auto_find_test_dependencies() -endif() - -ament_export_dependencies(rosidl_default_runtime) - -ament_package() diff --git a/blue_msgs/LICENSE b/blue_msgs/LICENSE deleted file mode 100644 index 30e8e2ec..00000000 --- a/blue_msgs/LICENSE +++ /dev/null @@ -1,17 +0,0 @@ -Permission is hereby granted, free of charge, to any person obtaining a copy -of this software and associated documentation files (the "Software"), to deal -in the Software without restriction, including without limitation the rights -to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -copies of the Software, and to permit persons to whom the Software is -furnished to do so, subject to the following conditions: - -The above copyright notice and this permission notice shall be included in -all copies or substantial portions of the Software. - -THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL -THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN -THE SOFTWARE. diff --git a/blue_msgs/action/Arm.action b/blue_msgs/action/Arm.action deleted file mode 100644 index 43addb5d..00000000 --- a/blue_msgs/action/Arm.action +++ /dev/null @@ -1,11 +0,0 @@ -# Arm/disarm the BlueROV2 thrusters. - -# Set to 'True' to arm the BlueROV2 thrusters. Set to 'False' -# to disarm the thrusters. -# bool arm ---- -# Returns 'True' if the arm/disarm attempt was successful. Returns -# 'False' otherwise. -bool success ---- -# No feedback is required. diff --git a/blue_msgs/action/Connect.action b/blue_msgs/action/Connect.action deleted file mode 100644 index c95ff58c..00000000 --- a/blue_msgs/action/Connect.action +++ /dev/null @@ -1,11 +0,0 @@ -# Connect/disconnect the control interface. - -# Set to 'True' to attempt to establish a connection. Set to -# 'False' otherwise. -bool connect ---- -# Returns 'True' if the connect/disconnect was successful. Returns -# 'False' otherwise. -bool success ---- -# No feedback is provided. diff --git a/blue_msgs/package.xml b/blue_msgs/package.xml deleted file mode 100644 index 92fb92bb..00000000 --- a/blue_msgs/package.xml +++ /dev/null @@ -1,26 +0,0 @@ - - - - blue_msgs - 0.0.0 - TODO: Package description - Evan Palmer - MIT - - ament_cmake - - ament_lint_auto - ament_lint_common - - action_msgs - std_msgs - geometry_msgs - - rosidl_default_generators - rosidl_default_runtime - rosidl_interface_packages - - - ament_cmake - - diff --git a/cspell.json b/cspell.json new file mode 100644 index 00000000..baa1a5c6 --- /dev/null +++ b/cspell.json @@ -0,0 +1,14 @@ +{ + "version": "0.2", + "ignorePaths": [], + "dictionaryDefinitions": [], + "dictionaries": [], + "words": [ + "ArduSub", + "mavros", + "rclpy", + "srvs" + ], + "ignoreWords": [], + "import": [] +} From 980596100ef82eec65fd81f041dcc81f12fdc4c0 Mon Sep 17 00:00:00 2001 From: Evan Palmer Date: Fri, 5 May 2023 13:00:12 -0700 Subject: [PATCH 26/34] Cleaned up implementation and started param backup service --- blue_manager/blue_manager/manager.py | 231 +++++++++++++++------------ 1 file changed, 129 insertions(+), 102 deletions(-) diff --git a/blue_manager/blue_manager/manager.py b/blue_manager/blue_manager/manager.py index f9e7222d..a2d0e1cb 100644 --- a/blue_manager/blue_manager/manager.py +++ b/blue_manager/blue_manager/manager.py @@ -18,6 +18,7 @@ # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN # THE SOFTWARE. +import time from copy import deepcopy from functools import partial @@ -27,7 +28,7 @@ from rcl_interfaces.srv import GetParameters, SetParameters from rclpy.node import Node from rclpy.task import Future -from std_srvs.srv import SetBool +from std_srvs.srv import SetBool, Trigger class Manager(Node): @@ -42,9 +43,12 @@ def __init__(self) -> None: self.connected = False self.armed = False - self.declare_parameters("", [("rov_config", "heavy")]) + self.declare_parameters("", [("rov_config", "heavy"), ("backup_timeout", 5.0)]) rov_config = self.get_parameter("rov_config").get_parameter_value().string_value + self.backup_timeout = ( + self.get_parameter("backup_timeout").get_parameter_value().double_value + ) # The BlueROV2 has two configurations: the heavy configuration with 8 thrusters # and the standard configuration with 6 thrusters. @@ -55,7 +59,7 @@ def __init__(self) -> None: num_thrusters = 6 # Maintain a backup of the thruster parameters so that we can restore them when - # disconnecting + # switching modes self.thruster_params_backup: dict[str, Parameter | None] = { f"SERVO{i}_FUNCTION": None for i in range(1, num_thrusters + 1) } @@ -66,8 +70,14 @@ def __init__(self) -> None: ) # Services - self.connect_srv = self.create_service( - SetBool, "/blue/connect", self.manage_connection_cb + self.backup_params_srv = self.create_service( + Trigger, "/blue/param/backup", self.backup_thruster_params_cb + ) + self.restore_backup_params_srv = self.create_service( + Trigger, "/blue/param/restore_backup", self.restore_backup_params_cb + ) + self.set_pwm_passthrough_srv = self.create_service( + SetBool, "/blue/mode/set_pwm_passthrough", self.set_pwm_passthrough_mode_cb ) def wait_for_service(client): @@ -113,13 +123,8 @@ def save_default_thruster_params_cb(self, event: ParamEvent) -> None: name=event.param_id, value=event.value ) - if self.thruster_params_backed_up: - self.get_logger().info( - "Successfully backed up the ArduSub thruster parameters." - ) - - def request_ardusub_params(self, param_ids: list[str]) -> Future: - """Request parameter values from ArduSub. + def _request_ardusub_params(self, param_ids: list[str]) -> Future: + """Request ArduSub parameter values from MAVROS. Args: param_ids: The names of the parameters whose values should be requested. @@ -131,126 +136,148 @@ def request_ardusub_params(self, param_ids: list[str]) -> Future: GetParameters.Request(names=param_ids) ) - def request_thruster_params(self, thruster_param_ids: list[str]) -> None: - """Request the thruster parameters which will be overwritten. - - The thruster parameters requested include all MOTORn_FUNCTION parameters - assigned to be motors. These are modified by the manager to support the PWM - Pass-through mode, but need to be restored when the manager is shutdown. + def _set_ardusub_params(self, params: list[Parameter]) -> Future: + """Set ArduSub parameters using MAVROS. Args: - thruster_param_ids: The names of the thruster parameters whose values - should be backed up. - """ + params: The parameter values that should be set. - def handle_request_future_cb(future: Future, param_ids: list[str]): - try: - response = future.result() - except Exception: - ... - else: - # The parameter values are provided in the same order as the names - # were provided in - for param_value, param_id in zip(response.values, param_ids): - self.thruster_params_backup[param_id] = Parameter( - name=param_id, value=param_value - ) - - future = self.request_ardusub_params(thruster_param_ids) - future.add_done_callback( - partial(handle_request_future_cb, param_ids=thruster_param_ids) + Returns: + A future object which provides access to the parameter setting result. + """ + return self.set_param_srv_client.call_async( + SetParameters.Request(parameters=params) ) - def manage_connection_cb( - self, request: SetBool.Request, response: SetBool.Response - ) -> SetBool.Response: - """Connect/disconnect the manager. + def backup_thruster_params_cb( + self, request: Trigger.Request, response: Trigger.Response + ) -> Trigger.Response: + """Create a backup of the thruster function parameters. + + The thruster parameters that get backed up include all SERVOn_FUNCTION + parameters assigned to be motors (determined by the ROV configuration). + These are modified by the manager to support the PWM + Passthrough mode, but need to be restored when the manager is shutdown. Args: - request: The connection service request. - response: The connection service response. + request: The request to backup the thruster parameters. + response: The result of the backup attempt. Returns: - The connection service response. + The result of the backup attempt. """ - if request.data: - response.success = self.connect() - else: - response.success = True - - # TODO(Evan): This is wrong - self.connected = response.success - - return response - - def connect(self) -> bool: - self.get_logger().warning( - "Attempting to connect the manager to the BlueROV2. All ArduSub arming" - " and failsafe procedures will be disabled upon successful connection." - ) if not self.thruster_params_backed_up: - # Get the IDs of the parameters that weren't successfully backed up on - # initialization + # All thruster parameters should be received when MAVROS is populating the + # parameter server, but we make sure just in case that failed unsaved_params = [ param_id for param_id in self.thruster_params_backup.keys() if self.thruster_params_backup[param_id] is None ] - self.request_thruster_params(unsaved_params) + def handle_request_future_cb(future: Future, param_ids: list[str]): + try: + response = future.result() + except Exception: + ... + else: + # The parameter values are provided in the same order as the names + # were provided in + for param_value, param_id in zip(response.values, param_ids): + self.thruster_params_backup[param_id] = Parameter( + name=param_id, value=param_value + ) + + future = self._request_ardusub_params(unsaved_params) + future.add_done_callback( + partial(handle_request_future_cb, param_ids=unsaved_params) + ) - if not self.thruster_params_backed_up: - self.get_logger().error( - "Connection attempt failed. The system has not finished backing" - " up the thruster parameters or the backup process failed." - ) - return False + # Wait for the request to finish + # NOTE: There are a few methods that have been recommended/documented as + # ways to block until the request is finished including using + # `spin_until_future_complete` and `while not future.done()`. Unfortunately + # both of these methods block indefinitely in this scenario, so we just use + # a timeout instead + end_t = time.time() + self.backup_timeout + while time.time() < end_t: + if self.thruster_params_backed_up: + break + + time.sleep(0.1) + + response.success = self.thruster_params_backed_up + + if not response.success: + response.message = ( + "Failed to backup the thruster parameters. Please verify" + " that MAVROS has finished populating the parameter server." + ) + else: + response.message = "Successfully backed up all thruster parameters!" - # Set the motors to PWM passthrough mode - return self.set_pwm_passthrough_mode() + return response - def set_pwm_passthrough_mode(self) -> bool: + def restore_backup_params_cb( + self, request: Trigger.Request, response: Trigger.Response + ) -> Trigger.Response: if not self.thruster_params_backed_up: - self.get_logger().error( - "The thrusters cannot be set to PWM Pass-through mode without first" - " being backed up." + response.success = False + response.message = ( + "The thruster parameters have not yet been successfully backed up." + " No backup to restore." ) - return False - passthrough_params = deepcopy(self.thruster_params_backup) + return response - # Set the servo mode to "DISABLED" - # 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 = 0 # type: ignore - param.value.type = ParameterType.PARAMETER_INTEGER # type: ignore + # TODO(evan): Send a set request and record the result - # We would actually prefer to set all of the parameters atomically; however, - # this functionality is not supported by MAVROS - set_param_request = SetParameters.Request( - parameters=list(passthrough_params.values()) - ) + def set_pwm_passthrough_mode_cb( + self, request: SetBool.Request, response: SetBool.Response + ) -> SetBool.Response: + if request.data: + if not self.thruster_params_backed_up: + response.success = False + response.message = ( + "The thrusters cannot be set to PWM Passthrough mode without first" + " being successfully backed up!" + ) + return response - self.set_param_srv_client.call_async(set_param_request).add_done_callback( - self.this_is_a_test - ) + self.get_logger().warning( + "Attempting to switch to the PWM Passthrough flight mode. All ArduSub" + " arming and failsafe procedures will be disabled upon successful" + " connection." + ) - return True + passthrough_params = deepcopy(self.thruster_params_backup) - def this_is_a_test(self, future): - try: - response = future.result() - except Exception as e: - self.get_logger().info("Service call failed %r" % (e,)) - else: - self.get_logger().info(f"Parameter set succeed..maybe?: {response}") + # Set the servo mode to "DISABLED" + # 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.type = ParameterType.PARAMETER_INTEGER # type: ignore + param.value.integer_value = 0 # type: ignore - def disconnect(self) -> bool: - return False + def get_mode_change_result(future): + try: + response = future.result() + except Exception: + ... + else: + if all([result.successful for result in response.results]): + self.get_logger().info("whooooooooooooooooooooooooo") - def restore_motor_param_backup(self) -> bool: - ... + # We would actually prefer to set all of the parameters atomically, but + # this functionality is not currently supported by MAVROS + future = self._set_ardusub_params(list(passthrough_params.values())) + future.add_done_callback(get_mode_change_result) + + # TODO(evan): Find a better way to get the result of the parameter setting + + response.success = True + + return response def main(args: list[str] | None = None): From 4551bebede793697927978f73508ca375c15282c Mon Sep 17 00:00:00 2001 From: Evan Palmer Date: Fri, 5 May 2023 19:26:05 -0700 Subject: [PATCH 27/34] Time for some video games baby --- blue_manager/blue_manager/manager.py | 40 ++++++++++++++++++++++++---- 1 file changed, 35 insertions(+), 5 deletions(-) diff --git a/blue_manager/blue_manager/manager.py b/blue_manager/blue_manager/manager.py index a2d0e1cb..a6824254 100644 --- a/blue_manager/blue_manager/manager.py +++ b/blue_manager/blue_manager/manager.py @@ -70,6 +70,10 @@ def __init__(self) -> None: ) # Services + # TODO(evan): Is exposing this really the most usable interface? + # this makes it really easy to accidentally override your original parameters + # It might be better to instead bring back the "connect" functionality or + # something similar that protects the original backup self.backup_params_srv = self.create_service( Trigger, "/blue/param/backup", self.backup_thruster_params_cb ) @@ -185,7 +189,8 @@ def handle_request_future_cb(future: Future, param_ids: list[str]): # were provided in for param_value, param_id in zip(response.values, param_ids): self.thruster_params_backup[param_id] = Parameter( - name=param_id, value=param_value + name=param_id, + value=param_value, ) future = self._request_ardusub_params(unsaved_params) @@ -230,7 +235,30 @@ def restore_backup_params_cb( return response - # TODO(evan): Send a set request and record the result + self.get_logger().info( + "Attempting to restore the BlueROV2 backup thruster parameters." + ) + + def handle_restore_backup_result(future): + try: + response = future.result() + except Exception: + ... + else: + if all([result.successful for result in response.results]): + # TODO(evan): Do something smarter here + self.get_logger().info("whooooooooooooooooooooooooo") + + # We would actually prefer to set all of the parameters atomically, but + # this functionality is not currently supported by MAVROS + future = self._set_ardusub_params(list(self.thruster_params_backup.values())) + future.add_done_callback(handle_restore_backup_result) + + # TODO(evan): Delay and then update the response + + response.success = True + + return response def set_pwm_passthrough_mode_cb( self, request: SetBool.Request, response: SetBool.Response @@ -246,8 +274,7 @@ def set_pwm_passthrough_mode_cb( self.get_logger().warning( "Attempting to switch to the PWM Passthrough flight mode. All ArduSub" - " arming and failsafe procedures will be disabled upon successful" - " connection." + " arming and failsafe procedures will be disabled upon success." ) passthrough_params = deepcopy(self.thruster_params_backup) @@ -266,6 +293,7 @@ def get_mode_change_result(future): ... else: if all([result.successful for result in response.results]): + # TODO(evan): Do something smarter here self.get_logger().info("whooooooooooooooooooooooooo") # We would actually prefer to set all of the parameters atomically, but @@ -273,7 +301,9 @@ def get_mode_change_result(future): future = self._set_ardusub_params(list(passthrough_params.values())) future.add_done_callback(get_mode_change_result) - # TODO(evan): Find a better way to get the result of the parameter setting + # TODO(evan): Find a better way to get the result of the parameter setting + else: + self.restore_backup_params_cb(Trigger.Request(), Trigger.Response()) response.success = True From 17a5cf8dee6a5063cbad151caf17db7b2e2962db Mon Sep 17 00:00:00 2001 From: Evan Palmer Date: Sat, 6 May 2023 01:43:06 -0700 Subject: [PATCH 28/34] Removed unnecessary methods --- blue_manager/blue_manager/manager.py | 220 +++++++-------------------- 1 file changed, 52 insertions(+), 168 deletions(-) diff --git a/blue_manager/blue_manager/manager.py b/blue_manager/blue_manager/manager.py index a6824254..bf665742 100644 --- a/blue_manager/blue_manager/manager.py +++ b/blue_manager/blue_manager/manager.py @@ -25,7 +25,7 @@ import rclpy from mavros_msgs.msg import ParamEvent from rcl_interfaces.msg import Parameter, ParameterType -from rcl_interfaces.srv import GetParameters, SetParameters +from rcl_interfaces.srv import SetParameters from rclpy.node import Node from rclpy.task import Future from std_srvs.srv import SetBool, Trigger @@ -40,76 +40,43 @@ def __init__(self) -> None: """Create a new control manager.""" super().__init__("blue_manager") - self.connected = False - self.armed = False + self.passthrough_enabled = False - self.declare_parameters("", [("rov_config", "heavy"), ("backup_timeout", 5.0)]) - - rov_config = self.get_parameter("rov_config").get_parameter_value().string_value - self.backup_timeout = ( - self.get_parameter("backup_timeout").get_parameter_value().double_value - ) - - # The BlueROV2 has two configurations: the heavy configuration with 8 thrusters - # and the standard configuration with 6 thrusters. - match rov_config: - case "heavy": - num_thrusters = 8 - case _: - num_thrusters = 6 + self.declare_parameters("", [("num_thrusters", 8)]) # Maintain a backup of the thruster parameters so that we can restore them when # switching modes self.thruster_params_backup: dict[str, Parameter | None] = { - f"SERVO{i}_FUNCTION": None for i in range(1, num_thrusters + 1) + f"SERVO{i}_FUNCTION": None + for i in range( + 1, + self.get_parameter("num_thrusters").get_parameter_value().integer_value + + 1, + ) } # Subscribers self.param_event_sub = self.create_subscription( - ParamEvent, "/mavros/param/event", self.save_default_thruster_params_cb, 10 + ParamEvent, "/mavros/param/event", self.backup_thruster_params_cb, 50 ) # Services - # TODO(evan): Is exposing this really the most usable interface? - # this makes it really easy to accidentally override your original parameters - # It might be better to instead bring back the "connect" functionality or - # something similar that protects the original backup - self.backup_params_srv = self.create_service( - Trigger, "/blue/param/backup", self.backup_thruster_params_cb - ) - self.restore_backup_params_srv = self.create_service( - Trigger, "/blue/param/restore_backup", self.restore_backup_params_cb - ) self.set_pwm_passthrough_srv = self.create_service( - SetBool, "/blue/mode/set_pwm_passthrough", self.set_pwm_passthrough_mode_cb + SetBool, + "/blue/manager/enable_passthrough", + self.set_pwm_passthrough_mode_cb, ) - def wait_for_service(client): - while not client.wait_for_service(timeout_sec=1.0): - ... - # Service clients - self.get_param_srv_client = self.create_client( - GetParameters, "/mavros/param/get_parameters" - ) - wait_for_service(self.get_param_srv_client) - self.set_param_srv_client = self.create_client( SetParameters, "/mavros/param/set_parameters" ) - wait_for_service(self.set_param_srv_client) - @property - def thruster_params_backed_up(self) -> bool: - """All required thruster parameters have been successfully backed up. + while not self.set_param_srv_client.wait_for_service(timeout_sec=1.0): + ... - Returns: - Whether or not the thruster parameters have been backed up. - """ - return None not in self.thruster_params_backup.values() - - def save_default_thruster_params_cb(self, event: ParamEvent) -> None: - """Save the default thruster parameter values to the parameter backup. + def backup_thruster_params_cb(self, event: ParamEvent) -> None: + """Backup the default thruster parameter values. MAVROS publishes the parameter values of all ArduSub parameters when populating the parameter server. We subscribe to this topic to receive the @@ -127,19 +94,6 @@ def save_default_thruster_params_cb(self, event: ParamEvent) -> None: name=event.param_id, value=event.value ) - def _request_ardusub_params(self, param_ids: list[str]) -> Future: - """Request ArduSub parameter values from MAVROS. - - Args: - param_ids: The names of the parameters whose values should be requested. - - Returns: - A future object which provides access to the parameter request result. - """ - return self.get_param_srv_client.call_async( - GetParameters.Request(names=param_ids) - ) - def _set_ardusub_params(self, params: list[Parameter]) -> Future: """Set ArduSub parameters using MAVROS. @@ -153,73 +107,52 @@ def _set_ardusub_params(self, params: list[Parameter]) -> Future: SetParameters.Request(parameters=params) ) - def backup_thruster_params_cb( - self, request: Trigger.Request, response: Trigger.Response - ) -> Trigger.Response: - """Create a backup of the thruster function parameters. + def set_pwm_passthrough_mode_cb( + self, request: SetBool.Request, response: SetBool.Response + ) -> SetBool.Response: + if request.data: + if not self.thruster_params_backed_up: + response.success = False + response.message = ( + "The thrusters cannot be set to PWM Passthrough mode without first" + " being successfully backed up!" + ) + return response - The thruster parameters that get backed up include all SERVOn_FUNCTION - parameters assigned to be motors (determined by the ROV configuration). - These are modified by the manager to support the PWM - Passthrough mode, but need to be restored when the manager is shutdown. + self.get_logger().warning( + "Attempting to switch to the PWM Passthrough flight mode. All ArduSub" + " arming and failsafe procedures will be disabled upon success." + ) - Args: - request: The request to backup the thruster parameters. - response: The result of the backup attempt. + passthrough_params = deepcopy(self.thruster_params_backup) - Returns: - The result of the backup attempt. - """ - if not self.thruster_params_backed_up: - # All thruster parameters should be received when MAVROS is populating the - # parameter server, but we make sure just in case that failed - unsaved_params = [ - param_id - for param_id in self.thruster_params_backup.keys() - if self.thruster_params_backup[param_id] is None - ] - - def handle_request_future_cb(future: Future, param_ids: list[str]): + # Set the servo mode to "DISABLED" + # 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.type = ParameterType.PARAMETER_INTEGER # type: ignore + param.value.integer_value = 0 # type: ignore + + def get_mode_change_result(future): try: response = future.result() except Exception: ... else: - # The parameter values are provided in the same order as the names - # were provided in - for param_value, param_id in zip(response.values, param_ids): - self.thruster_params_backup[param_id] = Parameter( - name=param_id, - value=param_value, - ) - - future = self._request_ardusub_params(unsaved_params) - future.add_done_callback( - partial(handle_request_future_cb, param_ids=unsaved_params) - ) - - # Wait for the request to finish - # NOTE: There are a few methods that have been recommended/documented as - # ways to block until the request is finished including using - # `spin_until_future_complete` and `while not future.done()`. Unfortunately - # both of these methods block indefinitely in this scenario, so we just use - # a timeout instead - end_t = time.time() + self.backup_timeout - while time.time() < end_t: - if self.thruster_params_backed_up: - break - - time.sleep(0.1) + if all([result.successful for result in response.results]): + # TODO(evan): Do something smarter here + self.get_logger().info("whooooooooooooooooooooooooo") - response.success = self.thruster_params_backed_up + # We would actually prefer to set all of the parameters atomically, but + # this functionality is not currently supported by MAVROS + future = self._set_ardusub_params(list(passthrough_params.values())) + future.add_done_callback(get_mode_change_result) - if not response.success: - response.message = ( - "Failed to backup the thruster parameters. Please verify" - " that MAVROS has finished populating the parameter server." - ) + # TODO(evan): Find a better way to get the result of the parameter setting else: - response.message = "Successfully backed up all thruster parameters!" + self.restore_backup_params_cb(Trigger.Request(), Trigger.Response()) + + response.success = True return response @@ -260,55 +193,6 @@ def handle_restore_backup_result(future): return response - def set_pwm_passthrough_mode_cb( - self, request: SetBool.Request, response: SetBool.Response - ) -> SetBool.Response: - if request.data: - if not self.thruster_params_backed_up: - response.success = False - response.message = ( - "The thrusters cannot be set to PWM Passthrough mode without first" - " being successfully backed up!" - ) - return response - - self.get_logger().warning( - "Attempting to switch to the PWM Passthrough flight mode. All ArduSub" - " arming and failsafe procedures will be disabled upon success." - ) - - passthrough_params = deepcopy(self.thruster_params_backup) - - # Set the servo mode to "DISABLED" - # 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.type = ParameterType.PARAMETER_INTEGER # type: ignore - param.value.integer_value = 0 # type: ignore - - def get_mode_change_result(future): - try: - response = future.result() - except Exception: - ... - else: - if all([result.successful for result in response.results]): - # TODO(evan): Do something smarter here - self.get_logger().info("whooooooooooooooooooooooooo") - - # We would actually prefer to set all of the parameters atomically, but - # this functionality is not currently supported by MAVROS - future = self._set_ardusub_params(list(passthrough_params.values())) - future.add_done_callback(get_mode_change_result) - - # TODO(evan): Find a better way to get the result of the parameter setting - else: - self.restore_backup_params_cb(Trigger.Request(), Trigger.Response()) - - response.success = True - - return response - def main(args: list[str] | None = None): """Run the ROV manager.""" From 4e5d8ad84494a67dcf7bb041e2ddb7e18e7d9c4c Mon Sep 17 00:00:00 2001 From: Evan Palmer Date: Sat, 6 May 2023 01:55:22 -0700 Subject: [PATCH 29/34] Removed more unused methods --- blue_manager/blue_manager/manager.py | 61 ++++++++++------------------ blue_manager/package.xml | 4 +- blue_manager/setup.py | 4 +- 3 files changed, 25 insertions(+), 44 deletions(-) diff --git a/blue_manager/blue_manager/manager.py b/blue_manager/blue_manager/manager.py index bf665742..0d4b6e13 100644 --- a/blue_manager/blue_manager/manager.py +++ b/blue_manager/blue_manager/manager.py @@ -94,6 +94,17 @@ def backup_thruster_params_cb(self, event: ParamEvent) -> None: name=event.param_id, value=event.value ) + # Log this to the terminal in case the backup fails and the user didn't + # record the default values + self.get_logger().info( + f"Saved thruster parameter {event.param_id} with value {event.value}." + ) + + if None not in self.thruster_params_backup.values(): + self.get_logger().info( + "Successfully backed up the thruster parameters!" + ) + def _set_ardusub_params(self, params: list[Parameter]) -> Future: """Set ArduSub parameters using MAVROS. @@ -111,7 +122,7 @@ def set_pwm_passthrough_mode_cb( self, request: SetBool.Request, response: SetBool.Response ) -> SetBool.Response: if request.data: - if not self.thruster_params_backed_up: + if None in self.thruster_params_backup.values(): response.success = False response.message = ( "The thrusters cannot be set to PWM Passthrough mode without first" @@ -140,8 +151,15 @@ def get_mode_change_result(future): ... else: if all([result.successful for result in response.results]): - # TODO(evan): Do something smarter here - self.get_logger().info("whooooooooooooooooooooooooo") + self.get_logger().info( + "Successfully switched to PWM Passthrough mode!" + ) + self.passthrough_enabled = True + else: + self.get_logger().warning( + "Failed to switch to PWM Passthrough mode." + ) + self.passthrough_enabled = False # We would actually prefer to set all of the parameters atomically, but # this functionality is not currently supported by MAVROS @@ -156,43 +174,6 @@ def get_mode_change_result(future): return response - def restore_backup_params_cb( - self, request: Trigger.Request, response: Trigger.Response - ) -> Trigger.Response: - if not self.thruster_params_backed_up: - response.success = False - response.message = ( - "The thruster parameters have not yet been successfully backed up." - " No backup to restore." - ) - - return response - - self.get_logger().info( - "Attempting to restore the BlueROV2 backup thruster parameters." - ) - - def handle_restore_backup_result(future): - try: - response = future.result() - except Exception: - ... - else: - if all([result.successful for result in response.results]): - # TODO(evan): Do something smarter here - self.get_logger().info("whooooooooooooooooooooooooo") - - # We would actually prefer to set all of the parameters atomically, but - # this functionality is not currently supported by MAVROS - future = self._set_ardusub_params(list(self.thruster_params_backup.values())) - future.add_done_callback(handle_restore_backup_result) - - # TODO(evan): Delay and then update the response - - response.success = True - - return response - def main(args: list[str] | None = None): """Run the ROV manager.""" diff --git a/blue_manager/package.xml b/blue_manager/package.xml index f79f3510..881425fa 100644 --- a/blue_manager/package.xml +++ b/blue_manager/package.xml @@ -2,8 +2,8 @@ blue_manager - 0.0.0 - TODO: Package description + 0.0.1 + An interface for enabling PWM passthrough on the BlueROV2. Evan Palmer MIT diff --git a/blue_manager/setup.py b/blue_manager/setup.py index 104d74c1..b498c236 100644 --- a/blue_manager/setup.py +++ b/blue_manager/setup.py @@ -4,7 +4,7 @@ setup( name=package_name, - version="0.0.0", + version="0.0.1", packages=[package_name], data_files=[ ("share/ament_index/resource_index/packages", ["resource/" + package_name]), @@ -14,7 +14,7 @@ zip_safe=True, maintainer="Evan Palmer", maintainer_email="evanp922@gmail.com", - description="TODO: Package description", + description="An interface for enabling PWM passthrough on the BlueROV2.", license="MIT", tests_require=["pytest"], entry_points={ From fee7d3ab8a7a1f6eecc17034463ea6569e55e8fc Mon Sep 17 00:00:00 2001 From: Evan Palmer Date: Sun, 7 May 2023 21:27:11 -0700 Subject: [PATCH 30/34] Added interface to stop thrusters and improve success rate of mode change command --- blue_manager/blue_manager/manager.py | 256 ++++++++++++++++++++------- blue_manager/test/test_copyright.py | 10 +- blue_manager/test/test_flake8.py | 8 +- blue_manager/test/test_pep257.py | 6 +- 4 files changed, 201 insertions(+), 79 deletions(-) diff --git a/blue_manager/blue_manager/manager.py b/blue_manager/blue_manager/manager.py index 0d4b6e13..6cc300d6 100644 --- a/blue_manager/blue_manager/manager.py +++ b/blue_manager/blue_manager/manager.py @@ -20,19 +20,18 @@ import time from copy import deepcopy -from functools import partial import rclpy from mavros_msgs.msg import ParamEvent +from mavros_msgs.srv import CommandLong from rcl_interfaces.msg import Parameter, ParameterType from rcl_interfaces.srv import SetParameters from rclpy.node import Node -from rclpy.task import Future -from std_srvs.srv import SetBool, Trigger +from std_srvs.srv import SetBool class Manager(Node): - """Provides custom controllers with an interface to the BlueROV2 thrusters.""" + """Provides an interface between custom controllers and the BlueROV2.""" STOPPED_PWM = 1500 @@ -40,19 +39,29 @@ def __init__(self) -> None: """Create a new control manager.""" super().__init__("blue_manager") - self.passthrough_enabled = False - - self.declare_parameters("", [("num_thrusters", 8)]) + self.declare_parameters( + "", + [ + ("num_thrusters", 8), + ("mode_change_timeout", 1.0), + ("mode_change_retries", 3), + ], + ) - # Maintain a backup of the thruster parameters so that we can restore them when - # switching modes + self.passthrough_enabled = False + self.num_thrusters = ( + self.get_parameter("num_thrusters").get_parameter_value().integer_value + ) + self.timeout = ( + self.get_parameter("mode_change_timeout").get_parameter_value().double_value + ) + self.retries = ( + self.get_parameter("mode_change_retries") + .get_parameter_value() + .integer_value + ) self.thruster_params_backup: dict[str, Parameter | None] = { - f"SERVO{i}_FUNCTION": None - for i in range( - 1, - self.get_parameter("num_thrusters").get_parameter_value().integer_value - + 1, - ) + f"SERVO{i}_FUNCTION": None for i in range(1, self.num_thrusters + 1) } # Subscribers @@ -67,13 +76,20 @@ def __init__(self) -> None: self.set_pwm_passthrough_mode_cb, ) + def wait_for_service(client, timeout=1.0) -> None: + while not client.wait_for_service(timeout_sec=timeout): + ... + # Service clients self.set_param_srv_client = self.create_client( SetParameters, "/mavros/param/set_parameters" ) + wait_for_service(self.set_param_srv_client) - while not self.set_param_srv_client.wait_for_service(timeout_sec=1.0): - ... + self.command_long_srv_client = self.create_client( + CommandLong, "/mavros/cmd/long" + ) + wait_for_service(self.command_long_srv_client) def backup_thruster_params_cb(self, event: ParamEvent) -> None: """Backup the default thruster parameter values. @@ -94,83 +110,187 @@ def backup_thruster_params_cb(self, event: ParamEvent) -> None: name=event.param_id, value=event.value ) - # Log this to the terminal in case the backup fails and the user didn't - # record the default values - self.get_logger().info( - f"Saved thruster parameter {event.param_id} with value {event.value}." - ) - if None not in self.thruster_params_backup.values(): self.get_logger().info( - "Successfully backed up the thruster parameters!" + "Successfully backed up the thruster parameters." ) - def _set_ardusub_params(self, params: list[Parameter]) -> Future: - """Set ArduSub parameters using MAVROS. + def set_pwm(self, thruster: int, pwm: int) -> None: + """Set the PWM value of a thruster. + + Args: + thruster: The thruster number to set the PWM value of. + pwm: The PWM value to set the thruster to. + """ + self.command_long_srv_client.call_async( + CommandLong(command=183, confirmation=0, param1=thruster, param2=pwm) + ) + + def stop_thrusters(self) -> None: + """Stop all thrusters.""" + for i in range(1, self.num_thrusters + 1): + self.set_pwm(i, self.STOPPED_PWM) + + def enable_pwm_passthrough_mode(self, timeout: float) -> bool: + """Enable PWM Passthrough mode. + + Args: + timeout: The maximum amount of time to wait for the mode to be enabled. + + Returns: + True if the mode was successfully enabled, False otherwise. + """ + self.get_logger().warning( + "Attempting to switch to the PWM Passthrough flight mode. All ArduSub" + " arming and failsafe procedures will be disabled upon success." + ) + + passthrough_params = deepcopy(self.thruster_params_backup) + + # Set the servo mode to "DISABLED" + # 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 = 0 # type: ignore + + def get_enable_passthrough_result(future): + try: + response = future.result() + except Exception: + ... + else: + if all([result.successful for result in response.results]): + self.passthrough_enabled = True + else: + self.passthrough_enabled = False + + # We would actually prefer to set all of the parameters atomically, but + # this functionality is not currently supported by MAVROS + future = self.set_param_srv_client.call_async( + SetParameters.Request(parameters=list(passthrough_params.values())) + ) + future.add_done_callback(get_enable_passthrough_result) + + start_t = time.time() + + while time.time() - start_t < timeout: + if self.passthrough_enabled: + break + + time.sleep(0.1) + + return self.passthrough_enabled + + def disable_pwm_passthrough_mode(self, timeout: float) -> bool: + """Disable PWM Passthrough mode. + + NOTE: This fails when the original thruster parameters could not be restored. Args: - params: The parameter values that should be set. + timeout: The maximum amount of time to wait for the mode to be disabled. Returns: - A future object which provides access to the parameter setting result. + True if the mode was successfully disabled, False otherwise. """ - return self.set_param_srv_client.call_async( - SetParameters.Request(parameters=params) + self.get_logger().info("Attempting to disable PWM Passthrough mode.") + + def get_restore_backup_result(future): + try: + response = future.result() + except Exception: + ... + else: + if all([result.successful for result in response.results]): + self.passthrough_enabled = False + else: + self.get_logger().warning( + "Failed to leave the PWM Passthrough mode. If failure persists," + " the following backup parameters may be restored manually:" + f" {self.thruster_params_backup}" + ) + self.passthrough_enabled = False + + future = self.set_param_srv_client.call_async( + SetParameters.Request(parameters=list(self.thruster_params_backup.values())) ) + future.add_done_callback(get_restore_backup_result) + + start_t = time.time() + + while time.time() - start_t < timeout: + if not self.passthrough_enabled: + break + + time.sleep(0.1) + + return not self.passthrough_enabled def set_pwm_passthrough_mode_cb( self, request: SetBool.Request, response: SetBool.Response ) -> SetBool.Response: + """Set the PWM Passthrough mode. + + Args: + request: The request to enable/disable PWM passthrough mode. + response: The result of the request. + + Returns: + The result of the request. + """ if request.data: + if self.passthrough_enabled: + response.success = True + response.message = "The system is already in PWM Passthrough mode." + return response + if None in self.thruster_params_backup.values(): response.success = False response.message = ( "The thrusters cannot be set to PWM Passthrough mode without first" - " being successfully backed up!" + " being successfully backed up." ) return response - self.get_logger().warning( - "Attempting to switch to the PWM Passthrough flight mode. All ArduSub" - " arming and failsafe procedures will be disabled upon success." - ) - - passthrough_params = deepcopy(self.thruster_params_backup) + # Make sure to stop all of the thrusters before-hand to prevent damaging the + # thrusters when the custom controller takes over + self.stop_thrusters() - # Set the servo mode to "DISABLED" - # 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.type = ParameterType.PARAMETER_INTEGER # type: ignore - param.value.integer_value = 0 # type: ignore + for _ in range(self.retries): + response.success = self.enable_pwm_passthrough_mode( + timeout=self.timeout + ) + if response.success: + break - def get_mode_change_result(future): - try: - response = future.result() - except Exception: - ... - else: - if all([result.successful for result in response.results]): - self.get_logger().info( - "Successfully switched to PWM Passthrough mode!" - ) - self.passthrough_enabled = True - else: - self.get_logger().warning( - "Failed to switch to PWM Passthrough mode." - ) - self.passthrough_enabled = False - - # We would actually prefer to set all of the parameters atomically, but - # this functionality is not currently supported by MAVROS - future = self._set_ardusub_params(list(passthrough_params.values())) - future.add_done_callback(get_mode_change_result) - - # TODO(evan): Find a better way to get the result of the parameter setting + if response.success: + response.message = "Successfully switched to PWM Passthrough mode." + else: + response.message = "Failed to switch to PWM Passthrough mode." else: - self.restore_backup_params_cb(Trigger.Request(), Trigger.Response()) + if not self.passthrough_enabled: + response.success = True + response.message = ( + "The system was already not in the PWM Passthrough mode." + ) + return response - response.success = True + # Stop the thrusters to prevent any damage to the thrusters when ArduSub + # takes back control + self.stop_thrusters() + + for _ in range(self.retries): + response.success = self.disable_pwm_passthrough_mode( + timeout=self.timeout + ) + if response.success: + break + + if response.success: + response.message = "Successfully left PWM Passthrough mode." + else: + response.message = ( + "Failed to leave PWM Passthrough mode. Good luck soldier." + ) return response diff --git a/blue_manager/test/test_copyright.py b/blue_manager/test/test_copyright.py index 97a39196..8f18fa4b 100644 --- a/blue_manager/test/test_copyright.py +++ b/blue_manager/test/test_copyright.py @@ -12,14 +12,16 @@ # See the License for the specific language governing permissions and # limitations under the License. -from ament_copyright.main import main 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.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' + rc = main(argv=[".", "test"]) + assert rc == 0, "Found errors" diff --git a/blue_manager/test/test_flake8.py b/blue_manager/test/test_flake8.py index 27ee1078..f494570f 100644 --- a/blue_manager/test/test_flake8.py +++ b/blue_manager/test/test_flake8.py @@ -12,14 +12,14 @@ # See the License for the specific language governing permissions and # limitations under the License. -from ament_flake8.main import main_with_errors 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) + assert rc == 0, "Found %d code style errors / warnings:\n" % len( + errors + ) + "\n".join(errors) diff --git a/blue_manager/test/test_pep257.py b/blue_manager/test/test_pep257.py index b234a384..4eddb46e 100644 --- a/blue_manager/test/test_pep257.py +++ b/blue_manager/test/test_pep257.py @@ -12,12 +12,12 @@ # See the License for the specific language governing permissions and # limitations under the License. -from ament_pep257.main import main import pytest +from ament_pep257.main import main @pytest.mark.linter @pytest.mark.pep257 def test_pep257(): - rc = main(argv=['.', 'test']) - assert rc == 0, 'Found code style errors / warnings' + rc = main(argv=[".", "test"]) + assert rc == 0, "Found code style errors / warnings" From 6b6ab1c92d309d35aaf4ef43274fa87f925fd3b7 Mon Sep 17 00:00:00 2001 From: Evan Palmer Date: Mon, 8 May 2023 09:24:23 -0700 Subject: [PATCH 31/34] Refactored to use multithreadedexecutor --- blue_manager/blue_manager/manager.py | 162 ++++++++++----------------- 1 file changed, 60 insertions(+), 102 deletions(-) diff --git a/blue_manager/blue_manager/manager.py b/blue_manager/blue_manager/manager.py index 6cc300d6..a1b13ce9 100644 --- a/blue_manager/blue_manager/manager.py +++ b/blue_manager/blue_manager/manager.py @@ -18,14 +18,15 @@ # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN # THE SOFTWARE. -import time from copy import deepcopy import rclpy from mavros_msgs.msg import ParamEvent from mavros_msgs.srv import CommandLong -from rcl_interfaces.msg import Parameter, ParameterType +from rcl_interfaces.msg import Parameter from rcl_interfaces.srv import SetParameters +from rclpy.callback_groups import ReentrantCallbackGroup +from rclpy.executors import MultiThreadedExecutor from rclpy.node import Node from std_srvs.srv import SetBool @@ -39,6 +40,10 @@ def __init__(self) -> None: """Create a new control manager.""" super().__init__("blue_manager") + # We need a reentrant callback group to get synchronous calls to services from + # within services + reentrant_callback_group = ReentrantCallbackGroup() + self.declare_parameters( "", [ @@ -74,6 +79,7 @@ def __init__(self) -> None: SetBool, "/blue/manager/enable_passthrough", self.set_pwm_passthrough_mode_cb, + callback_group=reentrant_callback_group, ) def wait_for_service(client, timeout=1.0) -> None: @@ -82,12 +88,14 @@ def wait_for_service(client, timeout=1.0) -> None: # Service clients self.set_param_srv_client = self.create_client( - SetParameters, "/mavros/param/set_parameters" + SetParameters, + "/mavros/param/set_parameters", + callback_group=reentrant_callback_group, ) wait_for_service(self.set_param_srv_client) self.command_long_srv_client = self.create_client( - CommandLong, "/mavros/cmd/long" + CommandLong, "/mavros/cmd/long", callback_group=reentrant_callback_group ) wait_for_service(self.command_long_srv_client) @@ -115,115 +123,44 @@ def backup_thruster_params_cb(self, event: ParamEvent) -> None: "Successfully backed up the thruster parameters." ) - def set_pwm(self, thruster: int, pwm: int) -> None: + def set_pwm(self, thruster: int, pwm: int) -> bool: """Set the PWM value of a thruster. Args: thruster: The thruster number to set the PWM value of. pwm: The PWM value to set the thruster to. + + Returns: + The result of the PWM setting call. """ - self.command_long_srv_client.call_async( + result: CommandLong.Response = self.command_long_srv_client.call( CommandLong(command=183, confirmation=0, param1=thruster, param2=pwm) ) + return result.success def stop_thrusters(self) -> None: """Stop all thrusters.""" + self.get_logger().warning("Stopping all BlueROV2 thrusters.") for i in range(1, self.num_thrusters + 1): - self.set_pwm(i, self.STOPPED_PWM) + for _ in range(self.retries): + if self.set_pwm(i, self.STOPPED_PWM): + break - def enable_pwm_passthrough_mode(self, timeout: float) -> bool: - """Enable PWM Passthrough mode. + def set_thruster_params(self, parameters: list[Parameter]) -> bool: + """Set the thruster parameters. Args: - timeout: The maximum amount of time to wait for the mode to be enabled. + parameters: The parameters to set. Returns: - True if the mode was successfully enabled, False otherwise. + True if the parameters were successfully set, False otherwise. """ - self.get_logger().warning( - "Attempting to switch to the PWM Passthrough flight mode. All ArduSub" - " arming and failsafe procedures will be disabled upon success." - ) - - passthrough_params = deepcopy(self.thruster_params_backup) - - # Set the servo mode to "DISABLED" - # 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 = 0 # type: ignore - - def get_enable_passthrough_result(future): - try: - response = future.result() - except Exception: - ... - else: - if all([result.successful for result in response.results]): - self.passthrough_enabled = True - else: - self.passthrough_enabled = False - # We would actually prefer to set all of the parameters atomically, but # this functionality is not currently supported by MAVROS - future = self.set_param_srv_client.call_async( - SetParameters.Request(parameters=list(passthrough_params.values())) + response: SetParameters.Response = self.set_param_srv_client.call( + SetParameters.Request(parameters=parameters) ) - future.add_done_callback(get_enable_passthrough_result) - - start_t = time.time() - - while time.time() - start_t < timeout: - if self.passthrough_enabled: - break - - time.sleep(0.1) - - return self.passthrough_enabled - - def disable_pwm_passthrough_mode(self, timeout: float) -> bool: - """Disable PWM Passthrough mode. - - NOTE: This fails when the original thruster parameters could not be restored. - - Args: - timeout: The maximum amount of time to wait for the mode to be disabled. - - Returns: - True if the mode was successfully disabled, False otherwise. - """ - self.get_logger().info("Attempting to disable PWM Passthrough mode.") - - def get_restore_backup_result(future): - try: - response = future.result() - except Exception: - ... - else: - if all([result.successful for result in response.results]): - self.passthrough_enabled = False - else: - self.get_logger().warning( - "Failed to leave the PWM Passthrough mode. If failure persists," - " the following backup parameters may be restored manually:" - f" {self.thruster_params_backup}" - ) - self.passthrough_enabled = False - - future = self.set_param_srv_client.call_async( - SetParameters.Request(parameters=list(self.thruster_params_backup.values())) - ) - future.add_done_callback(get_restore_backup_result) - - start_t = time.time() - - while time.time() - start_t < timeout: - if not self.passthrough_enabled: - break - - time.sleep(0.1) - - return not self.passthrough_enabled + return all([result.successful for result in response.results]) def set_pwm_passthrough_mode_cb( self, request: SetBool.Request, response: SetBool.Response @@ -251,14 +188,27 @@ def set_pwm_passthrough_mode_cb( ) return response - # Make sure to stop all of the thrusters before-hand to prevent damaging the - # thrusters when the custom controller takes over self.stop_thrusters() + self.get_logger().warning( + "Attempting to switch to the PWM Passthrough flight mode. All ArduSub" + " arming and failsafe procedures will be disabled upon success." + ) + + passthrough_params = deepcopy(self.thruster_params_backup) + + # Set the servo mode to "DISABLED" + # 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 = 0 # type: ignore + for _ in range(self.retries): - response.success = self.enable_pwm_passthrough_mode( - timeout=self.timeout + self.passthrough_enabled = self.set_thruster_params( + list(passthrough_params.values()) ) + response.success = self.passthrough_enabled + if response.success: break @@ -270,24 +220,31 @@ def set_pwm_passthrough_mode_cb( if not self.passthrough_enabled: response.success = True response.message = ( - "The system was already not in the PWM Passthrough mode." + "The system was not in the PWM Passthrough mode to start with." ) return response - # Stop the thrusters to prevent any damage to the thrusters when ArduSub - # takes back control self.stop_thrusters() + self.get_logger().warning("Attempting to disable PWM Passthrough mode.") + for _ in range(self.retries): - response.success = self.disable_pwm_passthrough_mode( - timeout=self.timeout + self.passthrough_enabled = not self.set_thruster_params( + list(self.thruster_params_backup.values()) ) + response.success = not self.passthrough_enabled + if response.success: break if response.success: response.message = "Successfully left PWM Passthrough mode." else: + self.get_logger().warning( + "Failed to leave the PWM Passthrough mode. If failure persists," + " the following backup parameters may be restored manually using" + f" QGC: {self.thruster_params_backup}" + ) response.message = ( "Failed to leave PWM Passthrough mode. Good luck soldier." ) @@ -300,7 +257,8 @@ def main(args: list[str] | None = None): rclpy.init(args=args) node = Manager() - rclpy.spin(node) + executor = MultiThreadedExecutor() + rclpy.spin(node, executor) node.destroy_node() rclpy.shutdown() From 37313a38f139ef809e8c3fc889f756556ae62cc2 Mon Sep 17 00:00:00 2001 From: Evan Palmer Date: Mon, 8 May 2023 13:40:17 -0700 Subject: [PATCH 32/34] Save point for golf --- blue_manager/blue_manager/manager.py | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/blue_manager/blue_manager/manager.py b/blue_manager/blue_manager/manager.py index a1b13ce9..18fb1218 100644 --- a/blue_manager/blue_manager/manager.py +++ b/blue_manager/blue_manager/manager.py @@ -84,7 +84,7 @@ def __init__(self) -> None: def wait_for_service(client, timeout=1.0) -> None: while not client.wait_for_service(timeout_sec=timeout): - ... + self.get_logger().warning("Waiting for service to become available.") # Service clients self.set_param_srv_client = self.create_client( @@ -94,10 +94,10 @@ def wait_for_service(client, timeout=1.0) -> None: ) wait_for_service(self.set_param_srv_client) - self.command_long_srv_client = self.create_client( - CommandLong, "/mavros/cmd/long", callback_group=reentrant_callback_group - ) - wait_for_service(self.command_long_srv_client) + # self.command_long_srv_client = self.create_client( + # CommandLong, "/mavros/cmd/long", callback_group=reentrant_callback_group + # ) + # wait_for_service(self.command_long_srv_client) def backup_thruster_params_cb(self, event: ParamEvent) -> None: """Backup the default thruster parameter values. @@ -140,7 +140,7 @@ def set_pwm(self, thruster: int, pwm: int) -> bool: def stop_thrusters(self) -> None: """Stop all thrusters.""" - self.get_logger().warning("Stopping all BlueROV2 thrusters.") + self.get_logger().warning("Attempting to stop all BlueROV2 thrusters.") for i in range(1, self.num_thrusters + 1): for _ in range(self.retries): if self.set_pwm(i, self.STOPPED_PWM): @@ -188,7 +188,7 @@ def set_pwm_passthrough_mode_cb( ) return response - self.stop_thrusters() + # self.stop_thrusters() self.get_logger().warning( "Attempting to switch to the PWM Passthrough flight mode. All ArduSub" @@ -224,7 +224,7 @@ def set_pwm_passthrough_mode_cb( ) return response - self.stop_thrusters() + # self.stop_thrusters() self.get_logger().warning("Attempting to disable PWM Passthrough mode.") From adc56e5042372d0d4ca5287194c018c88daaf839 Mon Sep 17 00:00:00 2001 From: Evan Palmer Date: Mon, 8 May 2023 19:35:14 -0700 Subject: [PATCH 33/34] Finalized initial version --- .docker/Dockerfile | 3 +- blue_manager/blue_manager/manager.py | 152 +++++++++++++++------------ cspell.json | 1 + 3 files changed, 87 insertions(+), 69 deletions(-) diff --git a/.docker/Dockerfile b/.docker/Dockerfile index edf397c6..2dd764dd 100644 --- a/.docker/Dockerfile +++ b/.docker/Dockerfile @@ -102,7 +102,8 @@ RUN pip3 install \ mypy \ isort \ flake8 \ - black + black \ + setuptools==58.2.0 # Configure a new non-root user ARG USERNAME=dev diff --git a/blue_manager/blue_manager/manager.py b/blue_manager/blue_manager/manager.py index 18fb1218..1e822b10 100644 --- a/blue_manager/blue_manager/manager.py +++ b/blue_manager/blue_manager/manager.py @@ -21,8 +21,7 @@ from copy import deepcopy import rclpy -from mavros_msgs.msg import ParamEvent -from mavros_msgs.srv import CommandLong +from mavros_msgs.msg import OverrideRCIn, ParamEvent from rcl_interfaces.msg import Parameter from rcl_interfaces.srv import SetParameters from rclpy.callback_groups import ReentrantCallbackGroup @@ -40,35 +39,27 @@ def __init__(self) -> None: """Create a new control manager.""" super().__init__("blue_manager") + self.passthrough_enabled = False + self.num_thrusters = 8 + self.timeout = 1.0 + self.retries = 3 + + # Get the ROS parameters from the parameter server + self._get_ros_params() + # We need a reentrant callback group to get synchronous calls to services from - # within services + # within service callbacks. reentrant_callback_group = ReentrantCallbackGroup() - self.declare_parameters( - "", - [ - ("num_thrusters", 8), - ("mode_change_timeout", 1.0), - ("mode_change_retries", 3), - ], - ) - - self.passthrough_enabled = False - self.num_thrusters = ( - self.get_parameter("num_thrusters").get_parameter_value().integer_value - ) - self.timeout = ( - self.get_parameter("mode_change_timeout").get_parameter_value().double_value - ) - self.retries = ( - self.get_parameter("mode_change_retries") - .get_parameter_value() - .integer_value - ) self.thruster_params_backup: dict[str, Parameter | None] = { f"SERVO{i}_FUNCTION": None for i in range(1, self.num_thrusters + 1) } + # Publishers + self.override_rc_in_pub = self.create_publisher( + OverrideRCIn, "/mavros/rc/override", 1 + ) + # Subscribers self.param_event_sub = self.create_subscription( ParamEvent, "/mavros/param/event", self.backup_thruster_params_cb, 50 @@ -78,26 +69,42 @@ def __init__(self) -> None: self.set_pwm_passthrough_srv = self.create_service( SetBool, "/blue/manager/enable_passthrough", - self.set_pwm_passthrough_mode_cb, + self.set_rc_passthrough_mode_cb, callback_group=reentrant_callback_group, ) - def wait_for_service(client, timeout=1.0) -> None: - while not client.wait_for_service(timeout_sec=timeout): - self.get_logger().warning("Waiting for service to become available.") - # Service clients self.set_param_srv_client = self.create_client( SetParameters, "/mavros/param/set_parameters", callback_group=reentrant_callback_group, ) - wait_for_service(self.set_param_srv_client) - # self.command_long_srv_client = self.create_client( - # CommandLong, "/mavros/cmd/long", callback_group=reentrant_callback_group - # ) - # wait_for_service(self.command_long_srv_client) + while not self.set_param_srv_client.wait_for_service(timeout_sec=1.0): + ... + + def _get_ros_params(self) -> None: + """Get the ROS parameters from the parameter server.""" + self.declare_parameters( + "", + [ + ("num_thrusters", self.num_thrusters), + ("mode_change_timeout", self.timeout), + ("mode_change_retries", self.retries), + ], + ) + + self.num_thrusters = ( + self.get_parameter("num_thrusters").get_parameter_value().integer_value + ) + self.timeout = ( + self.get_parameter("mode_change_timeout").get_parameter_value().double_value + ) + self.retries = ( + self.get_parameter("mode_change_retries") + .get_parameter_value() + .integer_value + ) def backup_thruster_params_cb(self, event: ParamEvent) -> None: """Backup the default thruster parameter values. @@ -123,34 +130,36 @@ def backup_thruster_params_cb(self, event: ParamEvent) -> None: "Successfully backed up the thruster parameters." ) - def set_pwm(self, thruster: int, pwm: int) -> bool: - """Set the PWM value of a thruster. + def set_rc(self, pwm: list[int]) -> None: + """Set the PWM values of the thrusters. Args: - thruster: The thruster number to set the PWM value of. - pwm: The PWM value to set the thruster to. + pwm: The PWM values to set the thruster to. The length of the provided list + must be equal to the number of thrusters. Returns: The result of the PWM setting call. """ - result: CommandLong.Response = self.command_long_srv_client.call( - CommandLong(command=183, confirmation=0, param1=thruster, param2=pwm) - ) - return result.success + if len(pwm) != self.num_thrusters: + raise ValueError( + "The length of the PWM input must equal the number of thrusters." + ) + + # Change the values of only the thruster channels + channels = pwm + [OverrideRCIn.CHAN_NOCHANGE] * (18 - self.num_thrusters) + + self.override_rc_in_pub.publish(OverrideRCIn(channels=channels)) def stop_thrusters(self) -> None: """Stop all thrusters.""" - self.get_logger().warning("Attempting to stop all BlueROV2 thrusters.") - for i in range(1, self.num_thrusters + 1): - for _ in range(self.retries): - if self.set_pwm(i, self.STOPPED_PWM): - break + self.get_logger().warning("Stopping all BlueROV2 thrusters.") + self.set_rc([self.STOPPED_PWM] * self.num_thrusters) - def set_thruster_params(self, parameters: list[Parameter]) -> bool: + def set_thruster_params(self, params: list[Parameter]) -> bool: """Set the thruster parameters. Args: - parameters: The parameters to set. + params: The parameters to set. Returns: True if the parameters were successfully set, False otherwise. @@ -158,17 +167,22 @@ def set_thruster_params(self, parameters: list[Parameter]) -> bool: # We would actually prefer to set all of the parameters atomically, but # this functionality is not currently supported by MAVROS response: SetParameters.Response = self.set_param_srv_client.call( - SetParameters.Request(parameters=parameters) + SetParameters.Request(parameters=params) ) return all([result.successful for result in response.results]) - def set_pwm_passthrough_mode_cb( + def set_rc_passthrough_mode_cb( self, request: SetBool.Request, response: SetBool.Response ) -> SetBool.Response: - """Set the PWM Passthrough mode. + """Set the RC Passthrough mode. + + RC Passthrough mode enables users to control the BlueROV2 thrusters directly + using the RC channels. It is important that users disable their RC transmitter + prior to enabling RC Passthrough mode to avoid sending conflicting commands to + the thrusters. Args: - request: The request to enable/disable PWM passthrough mode. + request: The request to enable/disable RC passthrough mode. response: The result of the request. Returns: @@ -177,31 +191,29 @@ def set_pwm_passthrough_mode_cb( if request.data: if self.passthrough_enabled: response.success = True - response.message = "The system is already in PWM Passthrough mode." + response.message = "The system is already in RC Passthrough mode." return response if None in self.thruster_params_backup.values(): response.success = False response.message = ( - "The thrusters cannot be set to PWM Passthrough mode without first" + "The thrusters cannot be set to RC Passthrough mode without first" " being successfully backed up." ) return response - # self.stop_thrusters() - self.get_logger().warning( - "Attempting to switch to the PWM Passthrough flight mode. All ArduSub" + "Attempting to switch to the RC Passthrough flight mode. All ArduSub" " arming and failsafe procedures will be disabled upon success." ) passthrough_params = deepcopy(self.thruster_params_backup) - # Set the servo mode to "DISABLED" + # 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 = 0 # type: ignore + param.value.integer_value = 1 # type: ignore for _ in range(self.retries): self.passthrough_enabled = self.set_thruster_params( @@ -213,20 +225,22 @@ def set_pwm_passthrough_mode_cb( break if response.success: - response.message = "Successfully switched to PWM Passthrough mode." + response.message = "Successfully switched to RC Passthrough mode." + + self.stop_thrusters() else: - response.message = "Failed to switch to PWM Passthrough mode." + response.message = "Failed to switch to RC Passthrough mode." else: if not self.passthrough_enabled: response.success = True response.message = ( - "The system was not in the PWM Passthrough mode to start with." + "The system was not in the RC Passthrough mode to start with." ) return response - # self.stop_thrusters() + self.stop_thrusters() - self.get_logger().warning("Attempting to disable PWM Passthrough mode.") + self.get_logger().warning("Attempting to disable RC Passthrough mode.") for _ in range(self.retries): self.passthrough_enabled = not self.set_thruster_params( @@ -238,17 +252,19 @@ def set_pwm_passthrough_mode_cb( break if response.success: - response.message = "Successfully left PWM Passthrough mode." + response.message = "Successfully left RC Passthrough mode." else: self.get_logger().warning( - "Failed to leave the PWM Passthrough mode. If failure persists," + "Failed to leave the RC Passthrough mode. If failure persists," " the following backup parameters may be restored manually using" f" QGC: {self.thruster_params_backup}" ) response.message = ( - "Failed to leave PWM Passthrough mode. Good luck soldier." + "Failed to leave RC Passthrough mode. Good luck soldier." ) + self.get_logger().info(response.message) + return response diff --git a/cspell.json b/cspell.json index baa1a5c6..f73932ab 100644 --- a/cspell.json +++ b/cspell.json @@ -6,6 +6,7 @@ "words": [ "ArduSub", "mavros", + "NOCHANGE", "rclpy", "srvs" ], From 3e7ff222bc0cac3d5d5ad7c28fb2162c2adc93e0 Mon Sep 17 00:00:00 2001 From: Evan Palmer Date: Mon, 8 May 2023 20:05:11 -0700 Subject: [PATCH 34/34] Resolved PR comments --- blue_manager/blue_manager/manager.py | 21 ++++++++++++++++++--- blue_manager/package.xml | 2 +- blue_manager/setup.py | 4 +++- blue_manager/test/test_pep257.py | 23 ----------------------- 4 files changed, 22 insertions(+), 28 deletions(-) delete mode 100644 blue_manager/test/test_pep257.py diff --git a/blue_manager/blue_manager/manager.py b/blue_manager/blue_manager/manager.py index 1e822b10..580dcbfd 100644 --- a/blue_manager/blue_manager/manager.py +++ b/blue_manager/blue_manager/manager.py @@ -106,6 +106,15 @@ def _get_ros_params(self) -> None: .integer_value ) + @property + def params_successfully_backed_up(self) -> bool: + """Whether or not the thruster parameters are backed up. + + Returns: + Whether or not the parameters are backed up. + """ + return None not in self.thruster_params_backup.values() + def backup_thruster_params_cb(self, event: ParamEvent) -> None: """Backup the default thruster parameter values. @@ -125,7 +134,7 @@ def backup_thruster_params_cb(self, event: ParamEvent) -> None: name=event.param_id, value=event.value ) - if None not in self.thruster_params_backup.values(): + if self.params_successfully_backed_up: self.get_logger().info( "Successfully backed up the thruster parameters." ) @@ -159,7 +168,7 @@ def set_thruster_params(self, params: list[Parameter]) -> bool: """Set the thruster parameters. Args: - params: The parameters to set. + params: The ArduSub parameters to set. Returns: True if the parameters were successfully set, False otherwise. @@ -194,7 +203,7 @@ def set_rc_passthrough_mode_cb( response.message = "The system is already in RC Passthrough mode." return response - if None in self.thruster_params_backup.values(): + if not self.thruster_params_backup: response.success = False response.message = ( "The thrusters cannot be set to RC Passthrough mode without first" @@ -231,6 +240,12 @@ def set_rc_passthrough_mode_cb( else: response.message = "Failed to switch to RC Passthrough mode." else: + if not self.thruster_params_backup: + response.success = False + response.message = ( + "The thruster backup parameters have not yet been stored." + ) + if not self.passthrough_enabled: response.success = True response.message = ( diff --git a/blue_manager/package.xml b/blue_manager/package.xml index 881425fa..40717cf8 100644 --- a/blue_manager/package.xml +++ b/blue_manager/package.xml @@ -3,7 +3,7 @@ blue_manager 0.0.1 - An interface for enabling PWM passthrough on the BlueROV2. + An interface for enabling individual thruster control on the BlueROV2. Evan Palmer MIT diff --git a/blue_manager/setup.py b/blue_manager/setup.py index b498c236..372d6539 100644 --- a/blue_manager/setup.py +++ b/blue_manager/setup.py @@ -14,7 +14,9 @@ zip_safe=True, maintainer="Evan Palmer", maintainer_email="evanp922@gmail.com", - description="An interface for enabling PWM passthrough on the BlueROV2.", + description=( + "An interface for enabling individual thruster control on the BlueROV2." + ), license="MIT", tests_require=["pytest"], entry_points={ diff --git a/blue_manager/test/test_pep257.py b/blue_manager/test/test_pep257.py deleted file mode 100644 index 4eddb46e..00000000 --- a/blue_manager/test/test_pep257.py +++ /dev/null @@ -1,23 +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_pep257.main import main - - -@pytest.mark.linter -@pytest.mark.pep257 -def test_pep257(): - rc = main(argv=[".", "test"]) - assert rc == 0, "Found code style errors / warnings"