From 3f128440d00f1c3561ce7ff4e035172f976ba009 Mon Sep 17 00:00:00 2001 From: unknown Date: Sun, 25 Feb 2024 20:04:13 -0800 Subject: [PATCH] thruster allocation matrix controller before testing --- .../CMakeLists.txt | 85 +++- .../thruster_allocation_matrix_controller.hpp | 140 ++++++ .../package.xml | 18 + .../thruster_allocation_matrix_controller.cpp | 259 ++++++++++ ...thruster_allocation_matrix_parameters.yaml | 50 ++ ..._thruster_allocation_matrix_controller.cpp | 60 +++ ..._thruster_allocation_matrix_controller.cpp | 297 ++++++++++++ ..._thruster_allocation_matrix_controller.hpp | 449 ++++++++++++++++++ .../thruster_allocation_matrix_controller.xml | 11 + 9 files changed, 1357 insertions(+), 12 deletions(-) create mode 100644 thruster_allocation_matrix_controller/test/test_load_thruster_allocation_matrix_controller.cpp create mode 100644 thruster_allocation_matrix_controller/test/test_thruster_allocation_matrix_controller.cpp create mode 100644 thruster_allocation_matrix_controller/test/test_thruster_allocation_matrix_controller.hpp create mode 100644 thruster_allocation_matrix_controller/thruster_allocation_matrix_controller.xml diff --git a/thruster_allocation_matrix_controller/CMakeLists.txt b/thruster_allocation_matrix_controller/CMakeLists.txt index fdd4e60..cc927e4 100644 --- a/thruster_allocation_matrix_controller/CMakeLists.txt +++ b/thruster_allocation_matrix_controller/CMakeLists.txt @@ -1,26 +1,87 @@ cmake_minimum_required(VERSION 3.8) project(thruster_allocation_matrix_controller) -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") +if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") add_compile_options(-Wall -Wextra -Wpedantic) endif() -# find dependencies +set(THIS_PACKAGE_INCLUDE_DEPENDS + controller_interface + generate_parameter_library + hardware_interface + pluginlib + rclcpp + rclcpp_lifecycle + realtime_tools + eigen3_cmake_module + Eigen3 + geometry_msgs + control_msgs +) + find_package(ament_cmake REQUIRED) -# uncomment the following section in order to fill in -# further dependencies manually. -# find_package( REQUIRED) +foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) + find_package(${Dependency} REQUIRED) +endforeach() + +generate_parameter_library(thruster_allocation_matrix_parameters + src/thruster_allocation_matrix_parameters.yaml +) + +add_library(thruster_allocation_matrix_controller SHARED + src/thruster_allocation_matrix_controller.cpp +) + +target_include_directories(thruster_allocation_matrix_controller + PUBLIC + $ + $ + ${EIGEN3_INCLUDE_DIR} + PRIVATE + ${PROJECT_SOURCE_DIR}/src +) +target_compile_features(thruster_allocation_matrix_controller PUBLIC cxx_std_17) +target_link_libraries(thruster_allocation_matrix_controller + PUBLIC + ${rclcpp_LIBRARIES} + thruster_allocation_matrix_parameters +) + +# Use dllexport instead of dllimport +target_compile_definitions(thruster_allocation_matrix_controller PRIVATE "THRUSTER_ALLOCATION_MATRIX_CONTROLLER_BUILDING_DLL") +ament_target_dependencies(thruster_allocation_matrix_controller + PUBLIC + ${THIS_PACKAGE_INCLUDE_DEPENDS} +) + +pluginlib_export_plugin_description_file(controller_interface thruster_allocation_matrix_controller.xml) if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) - # the following line skips the linter which checks for copyrights - # comment the line when a copyright and license is added to all source files - set(ament_cmake_copyright_FOUND TRUE) - # the following line skips cpplint (only works in a git repo) - # comment the line when this package is in a git repo and when - # a copyright and license is added to all source files - set(ament_cmake_cpplint_FOUND TRUE) + + set(ament_cmake_uncrustify_FOUND TRUE) + ament_lint_auto_find_test_dependencies() endif() +install( + DIRECTORY include/ + DESTINATION include/thruster_allocation_matrix_controller +) + +install( + TARGETS + thruster_allocation_matrix_controller + thruster_allocation_matrix_parameters + EXPORT + export_thruster_allocation_matrix_controller + RUNTIME DESTINATION bin + LIBRARY DESTINATION lib + ARCHIVE DESTINATION lib + INCLUDES DESTINATION include +) + +ament_export_targets(export_thruster_allocation_matrix_controller HAS_LIBRARY_TARGET) +ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS}) + ament_package() diff --git a/thruster_allocation_matrix_controller/include/thruster_allocation_matrix_controller/thruster_allocation_matrix_controller.hpp b/thruster_allocation_matrix_controller/include/thruster_allocation_matrix_controller/thruster_allocation_matrix_controller.hpp index e69de29..88dec7b 100644 --- a/thruster_allocation_matrix_controller/include/thruster_allocation_matrix_controller/thruster_allocation_matrix_controller.hpp +++ b/thruster_allocation_matrix_controller/include/thruster_allocation_matrix_controller/thruster_allocation_matrix_controller.hpp @@ -0,0 +1,140 @@ +// Copyright 2024, Evan Palmer +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in all +// copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. + +#pragma once + +#include +#include +#include +#include +#include +#include +#include + +#include "control_msgs/msg/multi_dof_state_stamped.hpp" +#include "controller_interface/chainable_controller_interface.hpp" +#include "controller_interface/controller_interface.hpp" +#include "geometry_msgs/msg/wrench.hpp" +#include "rclcpp/rclcpp.hpp" +#include "realtime_tools/realtime_buffer.h" +#include "realtime_tools/realtime_publisher.h" +#include "thruster_allocation_matrix_controller/visibility_control.h" + +// auto-generated by generate_parameter_library +#include "thruster_allocation_matrix_parameters.hpp" + +namespace Eigen +{ + +// Extend the Eigen namespace to include commonly used matrix types +using Matrix6d = Eigen::Matrix; +using Vector6d = Eigen::Matrix; + +} // namespace Eigen + +namespace thruster_allocation_matrix_controller +{ + +/** + * @brief Integral sliding mode controller (ISMC) for velocity control of an autonomous underwater vehicle. + */ +class ThrusterAllocationMatrixController : public controller_interface::ChainableControllerInterface +{ +public: + THRUSTER_ALLOCATION_MATRIX_CONTROLLER_PUBLIC + ThrusterAllocationMatrixController() = default; + + THRUSTER_ALLOCATION_MATRIX_CONTROLLER_PUBLIC controller_interface::CallbackReturn on_init() override; + + THRUSTER_ALLOCATION_MATRIX_CONTROLLER_PUBLIC + controller_interface::InterfaceConfiguration command_interface_configuration() const override; + + THRUSTER_ALLOCATION_MATRIX_CONTROLLER_PUBLIC + controller_interface::InterfaceConfiguration state_interface_configuration() const override; + + THRUSTER_ALLOCATION_MATRIX_CONTROLLER_PUBLIC + controller_interface::CallbackReturn on_cleanup(const rclcpp_lifecycle::State & previous_state) override; + + THRUSTER_ALLOCATION_MATRIX_CONTROLLER_PUBLIC + controller_interface::CallbackReturn on_configure(const rclcpp_lifecycle::State & previous_state) override; + + THRUSTER_ALLOCATION_MATRIX_CONTROLLER_PUBLIC + controller_interface::CallbackReturn on_activate(const rclcpp_lifecycle::State & previous_state) override; + + THRUSTER_ALLOCATION_MATRIX_CONTROLLER_PUBLIC + controller_interface::CallbackReturn on_deactivate(const rclcpp_lifecycle::State & previous_state) override; + + THRUSTER_ALLOCATION_MATRIX_CONTROLLER_PUBLIC + controller_interface::return_type update_and_write_commands( + const rclcpp::Time & time, const rclcpp::Duration & period) override; + + // TODO(evan): Might not need to override this + THRUSTER_ALLOCATION_MATRIX_CONTROLLER_PUBLIC + bool on_set_chained_mode(bool chained_mode) override; + +protected: + std::vector on_export_reference_interfaces() override; + + controller_interface::return_type update_reference_from_subscribers( + const rclcpp::Time & time, const rclcpp::Duration & period) override; + + /** + * @brief Update the controller parameters, including the dynamic parameters. + */ + void update_parameters(); + + /** + * @brief Configure the controller parameters. + * + * @note This can be called in a control loop to update the dynamic parameters for online controller tuning. + * + * @return controller_interface::CallbackReturn + */ + controller_interface::CallbackReturn configure_parameters(); + + // Reference signal to track + realtime_tools::RealtimeBuffer> reference_; + std::shared_ptr> reference_sub_; + + // Publish the controller state + std::shared_ptr> controller_state_pub_; + std::unique_ptr> rt_controller_state_pub_; + + // generate_parameter_library members + std::shared_ptr param_listener_; + thruster_allocation_matrix_controller::Params params_; + + // DOF information + const std::array six_dof_names_{"x", "y", "z", "rx", "ry", "rz"}; + std::vector dof_names_; + size_t dof_; + + // rows for tam matrix + Eigen::Matrix tam_; + + std::int64_t num_thrusters_; + +private: + THRUSTER_ALLOCATION_MATRIX_CONTROLLER_LOCAL + // TODO: replace message type with something for the Wrench + void reference_callback(std::shared_ptr msg); +}; + +} // namespace thruster_allocation_matrix_controller diff --git a/thruster_allocation_matrix_controller/package.xml b/thruster_allocation_matrix_controller/package.xml index 731b1c4..6e8bb34 100644 --- a/thruster_allocation_matrix_controller/package.xml +++ b/thruster_allocation_matrix_controller/package.xml @@ -8,9 +8,27 @@ MIT ament_cmake + eigen3_cmake_module + + eigen + rclcpp + ros2_control + ros2_controllers + pluginlib + controller_interface + hardware_interface + rclcpp_lifecycle + generate_parameter_library + control_msgs ament_lint_auto ament_lint_common + ament_add_gmock + controller_manager + ros2_control_test_assets + hardware_interface + + eigen3_cmake_module ament_cmake diff --git a/thruster_allocation_matrix_controller/src/thruster_allocation_matrix_controller.cpp b/thruster_allocation_matrix_controller/src/thruster_allocation_matrix_controller.cpp index e69de29..003d474 100644 --- a/thruster_allocation_matrix_controller/src/thruster_allocation_matrix_controller.cpp +++ b/thruster_allocation_matrix_controller/src/thruster_allocation_matrix_controller.cpp @@ -0,0 +1,259 @@ +// Copyright 2024, Evan Palmer +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in all +// copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. + +#include "thruster_allocation_matrix_controller/thruster_allocation_matrix_controller.hpp" + +#include + +#include "hardware_interface/types/hardware_interface_type_values.hpp" + +namespace thruster_allocation_matrix_controller +{ + +namespace +{ + +Eigen::Vector6d create_six_dof_eigen_from_named_vector( + const std::vector & dof_names, const std::array & six_dof_names, + const std::vector & values) +{ + if (dof_names.size() != values.size()) { + throw std::invalid_argument("The DoF names and values must have the same size."); + } + + Eigen::Vector6d vec = Eigen::Vector6d::Zero(); + + for (size_t i = 0; i < six_dof_names.size(); ++i) { + auto it = std::find(dof_names.begin(), dof_names.end(), six_dof_names[i]); + + if (it == dof_names.end()) { + vec[i] = std::numeric_limits::quiet_NaN(); + } else { + vec[i] = values[std::distance(dof_names.begin(), it)]; + } + } + + return vec; +} + +} // namespace + +ThrusterAllocationMatrixController::CallbackReturn ThrusterAllocationMatrixController::on_init() +{ + // Pulls parameters and updates them + try { + param_listener_ = std::make_shared(get_node()); + params_ = param_listener_->get_params(); + } + catch (const std::exception & e) { + fprintf(stderr, "An exception occurred while initializing the controller: %s\n", e.what()); + return controller_interface::CallbackReturn::ERROR; + } + + return controller_interface::CallbackReturn::SUCCESS; +} + +controller_interface::InterfaceConfiguration ThrusterAllocationMatrixController::command_interface_configuration() const +{ + controller_interface::InterfaceConfiguration command_interfaces_config; + command_interfaces_config.type = controller_interface::interface_configuration_type::INDIVIDUAL; + + for (std::int64_t i = 0; i < num_thrusters_; i++) { + std::ostringstream temp_name; + temp_name << "thruster_" << i; + command_interfaces_config.names.emplace_back(temp_name.str() + "/" + hardware_interface::HW_IF_EFFORT); + } + + return command_interfaces_config; +} + +controller_interface::CallbackReturn ThrusterAllocationMatrixController::on_cleanup( + const rclcpp_lifecycle::State & /*previous_state*/) +{ + return controller_interface::CallbackReturn::SUCCESS; +} + +controller_interface::CallbackReturn ThrusterAllocationMatrixController::on_configure( + const rclcpp_lifecycle::State & /*previous_state*/) +{ + auto ret = configure_parameters(); + if (ret != controller_interface::CallbackReturn::SUCCESS) { + return ret; + } + + // Initialize the reference and state realtime messages + const std::shared_ptr reference_msg = std::make_shared(); + reference_.writeFromNonRT(reference_msg); + + // Subscribe to the reference topic + reference_sub_ = get_node()->create_subscription( + "~/reference", rclcpp::SystemDefaultsQoS(), + [this](const std::shared_ptr msg) { reference_callback(msg); }); // NOLINT + + // Setup the controller state publisher + controller_state_pub_ = + get_node()->create_publisher("~/status", rclcpp::SystemDefaultsQoS()); + rt_controller_state_pub_ = + std::make_unique>(controller_state_pub_); + + return controller_interface::CallbackReturn::SUCCESS; +} + +controller_interface::CallbackReturn ThrusterAllocationMatrixController::on_activate( + const rclcpp_lifecycle::State & /*previous_state*/) +{ + reference_interfaces_.assign(reference_interfaces_.size(), std::numeric_limits::quiet_NaN()); + + return controller_interface::CallbackReturn::SUCCESS; +} + +controller_interface::CallbackReturn ThrusterAllocationMatrixController::on_deactivate( + const rclcpp_lifecycle::State & /*previous_state*/) +{ + return controller_interface::CallbackReturn::SUCCESS; +} + +controller_interface::return_type ThrusterAllocationMatrixController::update_and_write_commands( + const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) +{ + // getting the reference forces and calculating the thrust forces using the tam + const std::vector reference_forces_values(reference_interfaces_.begin() + dof_, reference_interfaces_.end()); + auto reference_forces = create_six_dof_eigen_from_named_vector(dof_names_, six_dof_names_, reference_forces_values); + + Eigen::VectorXd thruster_forces = tam_.completeOrthogonalDecomposition().pseudoInverse() * reference_forces; + + // setting command interfaces with calculated thruster forces + for (std::int64_t i = 0; i < num_thrusters_; i++) { + command_interfaces_[i].set_value(thruster_forces[i]); + } + + // TODO: we still aren't sure if this is the correct type/way of doing this. we may need to come back and do + // something that makes more sense. (edit: this doesn't work) + // if (rt_controller_state_pub_ && rt_controller_state_pub_->trylock()) { + // rt_controller_state_pub_->msg_.header.stamp = time; + + // rt_controller_state_pub_->msg_.dof_states[0].reference = reference_interfaces_; + // rt_controller_state_pub_->msg_.dof_states[0].time_step = period.seconds(); + // rt_controller_state_pub_->msg_.dof_states[0].output = thruster_forces; + + // rt_controller_state_pub_->unlockAndPublish(); + // } + + return controller_interface::return_type::OK; +} + +bool ThrusterAllocationMatrixController::on_set_chained_mode(bool /*chained_mode*/) { return true; } + +std::vector ThrusterAllocationMatrixController::on_export_reference_interfaces() +{ + reference_interfaces_.resize(dof_, std::numeric_limits::quiet_NaN()); + + std::vector reference_interfaces; + reference_interfaces.reserve(reference_interfaces_.size()); + + for (size_t i = 0; i < dof_; ++i) { + reference_interfaces.emplace_back( + get_node()->get_name(), dof_names_[i] + "/" + hardware_interface::HW_IF_EFFORT, &reference_interfaces_[i]); + } + + return reference_interfaces; +} + +controller_interface::return_type ThrusterAllocationMatrixController::update_reference_from_subscribers( + const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) +{ + auto * current_reference = reference_.readFromNonRT(); + + // define wrench list and iterate through to assign it to interfaces + std::vector wrench = {(*current_reference)->force.x, (*current_reference)->force.y, + (*current_reference)->force.z, (*current_reference)->torque.x, + (*current_reference)->torque.y, (*current_reference)->torque.z}; + + // Update the thrust reference + for (size_t i = 0; i < wrench.size(); i++) { + if (!std::isnan(wrench[i])) { + reference_interfaces_[i] = wrench[i]; + wrench[i] = std::numeric_limits::quiet_NaN(); + } + } + + return controller_interface::return_type::OK; +} + +void ThrusterAllocationMatrixController::update_parameters() +{ + // Need this + if (!param_listener_->is_old(params_)) { + return; + } + param_listener_->refresh_dynamic_parameters(); + params_ = param_listener_->get_params(); +} + +controller_interface::CallbackReturn ThrusterAllocationMatrixController::configure_parameters() +{ + update_parameters(); + + // These are just used to improve readability + dof_names_ = params_.dof_names; + dof_ = dof_names_.size(); + + // stroing each row of the tam matrix + Eigen::Map tam_x(params_.tam.x.data(), params_.tam.x.size()); + Eigen::Map tam_y(params_.tam.y.data(), params_.tam.y.size()); + Eigen::Map tam_z(params_.tam.z.data(), params_.tam.z.size()); + Eigen::Map tam_rx(params_.tam.rx.data(), params_.tam.rx.size()); + Eigen::Map tam_ry(params_.tam.ry.data(), params_.tam.ry.size()); + Eigen::Map tam_rz(params_.tam.rz.data(), params_.tam.rz.size()); + + // checking that each of the vectors is the same size + num_thrusters_ = tam_x.size(); + if ( + (tam_y.size() != num_thrusters_) || (tam_z.size() != num_thrusters_) || (tam_rx.size() != num_thrusters_) || + (tam_ry.size() != num_thrusters_) || (tam_rz.size() != num_thrusters_)) { + // if any of the tam vectors aren't the same size, then we need to notify the user and return failure + RCLCPP_ERROR(get_node()->get_logger(), "Received vectors for TAM with differing lengths."); + return controller_interface::CallbackReturn::ERROR; + } + + // otherwise, they are all the same length and we can pack them into an Eigen matrix + Eigen::MatrixXd tam_(dof_, num_thrusters_); + tam_ << tam_x, tam_y, tam_z, tam_rx, tam_ry, tam_rz; + + return controller_interface::CallbackReturn::SUCCESS; +} + +void ThrusterAllocationMatrixController::reference_callback(std::shared_ptr msg) +{ + try { + reference_.writeFromNonRT(msg); + } + catch (const std::invalid_argument & e) { + RCLCPP_ERROR(get_node()->get_logger(), "Received an invalid reference message: %s", e.what()); // NOLINT + } +} + +} // namespace thruster_allocation_matrix_controller + +#include "pluginlib/class_list_macros.hpp" + +PLUGINLIB_EXPORT_CLASS( + thruster_allocation_matrix_controller::ThrusterAllocationMatrixController, + controller_interface::ChainableControllerInterface) diff --git a/thruster_allocation_matrix_controller/src/thruster_allocation_matrix_parameters.yaml b/thruster_allocation_matrix_controller/src/thruster_allocation_matrix_parameters.yaml index e69de29..ac25b07 100644 --- a/thruster_allocation_matrix_controller/src/thruster_allocation_matrix_parameters.yaml +++ b/thruster_allocation_matrix_controller/src/thruster_allocation_matrix_parameters.yaml @@ -0,0 +1,50 @@ +thruster_allocation_matrix_controller: + dof_names: { + type: string_array, + read_only: true, + default_value: ["x", "y", "z", "rx", "ry", "rz"], + description: "Specifies the degrees-of-freedom used by the controller.", + validation: { + unique<>: null, + not_empty<>: null, + subset_of<>: [["x", "y", "z", "rx", "ry", "rz"]], + } + } + tam: { + x: { + type: double_array, + read_only: true, + default_value: [-0.707, -0.707, 0.707, 0.707, 0.0, 0.0, 0.0, 0.0], + description: "Thruster allocation matrix x." + }, + y: { + type: double_array, + read_only: true, + default_value: [0.707, -0.707, 0.707, -0.707, 0.0, 0.0, 0.0, 0.0], + description: "Thruster allocation matrix y." + }, + z: { + type: double_array, + read_only: true, + default_value: [0.0, 0.0, 0.0, 0.0, 1.0, 1.0, 1.0, 1.0], + description: "Thruster allocation matrix z." + }, + rx: { + type: double_array, + read_only: true, + default_value: [0.0, 0.0, 0.0, 0.0, -0.21805, 0.21805, -0.21805, 0.21805], + description: "Thruster allocation matrix rx." + }, + ry: { + type: double_array, + read_only: true, + default_value: [0.0, 0.0, 0.0, 0.0, -0.12, -0.12, 0.12, 0.12], + description: "Thruster allocation matrix ry." + }, + rz: { + type: double_array, + read_only: true, + default_value: [0.1888, -0.1888, -0.1888, 0.1888, 0.0, 0.0, 0.0 , 0.0], + description: "Trhuster allocation matrix rz." + } + } diff --git a/thruster_allocation_matrix_controller/test/test_load_thruster_allocation_matrix_controller.cpp b/thruster_allocation_matrix_controller/test/test_load_thruster_allocation_matrix_controller.cpp new file mode 100644 index 0000000..adea31c --- /dev/null +++ b/thruster_allocation_matrix_controller/test/test_load_thruster_allocation_matrix_controller.cpp @@ -0,0 +1,60 @@ +// Copyright (c) 2021, PickNik, 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. + +#include +// #include + +#include +#include + +#include "controller_manager/controller_manager.hpp" +#include "hardware_interface/resource_manager.hpp" +#include "rclcpp/executor.hpp" +#include "rclcpp/executors/single_threaded_executor.hpp" +#include "rclcpp/utilities.hpp" +#include "ros2_control_test_assets/descriptions.hpp" + +TEST(TestLoadThrusterAllocationMatrixController, load_controller) +{ + // TEST_LOG(INFO) << "This is an informational message."; + GTEST_LOG_(WARNING) << "This is a warning message."; + GTEST_LOG_(ERROR) << "This is an error message."; + GTEST_LOG_(FATAL) << "This is a fatal error message."; + rclcpp::init(0, nullptr); + std::cout << "Printing" << std::endl; + std::shared_ptr executor = std::make_shared(); + + controller_manager::ControllerManager cm( + std::make_unique(ros2_control_test_assets::minimal_robot_urdf), executor, + "test_controller_manager"); + + std::cout << "Printing" << std::endl; + + ASSERT_EQ( + cm.load_controller( + "load_thruster_allocation_matrix_controller", + "thruster_allocation_matrix_controller/ThrusterAllocationMatrixController"), + nullptr); + rclcpp::shutdown(); +} + +// int main(int argc, char ** argv) +// { +// std::cout << "Printing" << std::endl; +// ::testing::InitGoogleTest(&argc, argv); +// rclcpp::init(argc, argv); +// int result = RUN_ALL_TESTS(); +// rclcpp::shutdown(); +// return result; +// } diff --git a/thruster_allocation_matrix_controller/test/test_thruster_allocation_matrix_controller.cpp b/thruster_allocation_matrix_controller/test/test_thruster_allocation_matrix_controller.cpp new file mode 100644 index 0000000..c15ec26 --- /dev/null +++ b/thruster_allocation_matrix_controller/test/test_thruster_allocation_matrix_controller.cpp @@ -0,0 +1,297 @@ +// // Copyright (c) 2021, Stogl Robotics Consulting UG (haftungsbeschränkt) +// // +// // 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. +// // +// /// \author: Denis Stogl + +// #include "test_thruster_allocation_matrix_controller.hpp" + +// #include +// #include +// #include +// #include + +// // Test on_init returns ERROR when a required parameter is missing +// TEST_P(ThrusterAllocationMatrixControllerTestParameterizedMissingParameters, one_init_parameter_is_missing) +// { +// ASSERT_EQ(SetUpController(GetParam()), controller_interface::return_type::ERROR); +// } + +// INSTANTIATE_TEST_SUITE_P( +// MissingMandatoryParameterDuringInit, ThrusterAllocationMatrixControllerTestParameterizedMissingParameters, +// ::testing::Values( +// "thruster_allocation_matrix.mass", "thruster_allocation_matrix.selected_axes", +// "thruster_allocation_matrix.stiffness", "chainable_command_interfaces", "command_interfaces", "control.frame.id", +// "fixed_world_frame.frame.id", "ft_sensor.frame.id", "ft_sensor.name", "gravity_compensation.CoG.pos", +// "gravity_compensation.frame.id", "joints", "kinematics.base", "kinematics.plugin_name", +// "kinematics.plugin_package", "kinematics.tip", "state_interfaces")); + +// INSTANTIATE_TEST_SUITE_P( +// InvalidParameterDuringConfiguration, ThrusterAllocationMatrixControllerTestParameterizedInvalidParameters, +// ::testing::Values( +// // wrong length COG +// std::make_tuple( +// std::string("gravity_compensation.CoG.pos"), rclcpp::ParameterValue(std::vector() = {1, 2, 3, 4})), +// // wrong length stiffness +// std::make_tuple( +// std::string("thruster_allocation_matrix.stiffness"), rclcpp::ParameterValue(std::vector() = {1, 2, +// 3})), +// // negative stiffness +// std::make_tuple( +// std::string("thruster_allocation_matrix.stiffness"), +// rclcpp::ParameterValue(std::vector() = {-1, -2, 3, 4, 5, 6})), +// // wrong length mass +// std::make_tuple( +// std::string("thruster_allocation_matrix.mass"), rclcpp::ParameterValue(std::vector() = {1, 2, 3})), +// // negative mass +// std::make_tuple( +// std::string("thruster_allocation_matrix.mass"), +// rclcpp::ParameterValue(std::vector() = {-1, -2, 3, 4, 5, 6})), +// // wrong length damping ratio +// std::make_tuple( +// std::string("thruster_allocation_matrix.damping_ratio"), +// rclcpp::ParameterValue(std::vector() = {1, 2, 3})), +// // wrong length selected axes +// std::make_tuple( +// std::string("thruster_allocation_matrix.selected_axes"), +// rclcpp::ParameterValue(std::vector() = {1, 2, 3})) +// // invalid robot description. +// // TODO(anyone): deactivated, because SetUpController returns SUCCESS here? +// // ,std::make_tuple( +// // std::string("robot_description"), rclcpp::ParameterValue(std::string() = "bad_robot"))) +// )); + +// // Test on_init returns ERROR when a parameter is invalid +// TEST_P(ThrusterAllocationMatrixControllerTestParameterizedInvalidParameters, invalid_parameters) +// { +// ASSERT_EQ(SetUpController(), controller_interface::return_type::ERROR); +// } + +// TEST_F(ThrusterAllocationMatrixControllerTest, all_parameters_set_configure_success) +// { +// auto result = SetUpController(); + +// ASSERT_EQ(result, controller_interface::return_type::OK); + +// ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + +// ASSERT_TRUE(!controller_->thruster_allocation_matrix_->parameters_.joints.empty()); +// ASSERT_TRUE(controller_->thruster_allocation_matrix_->parameters_.joints.size() == joint_names_.size()); +// ASSERT_TRUE(std::equal( +// controller_->thruster_allocation_matrix_->parameters_.joints.begin(), +// controller_->thruster_allocation_matrix_->parameters_.joints.end(), joint_names_.begin(), joint_names_.end())); + +// ASSERT_TRUE(!controller_->thruster_allocation_matrix_->parameters_.command_interfaces.empty()); +// ASSERT_TRUE( +// controller_->thruster_allocation_matrix_->parameters_.command_interfaces.size() == +// command_interface_types_.size()); +// ASSERT_TRUE(std::equal( +// controller_->thruster_allocation_matrix_->parameters_.command_interfaces.begin(), +// controller_->thruster_allocation_matrix_->parameters_.command_interfaces.end(), command_interface_types_.begin(), +// command_interface_types_.end())); + +// ASSERT_TRUE(!controller_->thruster_allocation_matrix_->parameters_.state_interfaces.empty()); +// ASSERT_TRUE( +// controller_->thruster_allocation_matrix_->parameters_.state_interfaces.size() == state_interface_types_.size()); +// ASSERT_TRUE(std::equal( +// controller_->thruster_allocation_matrix_->parameters_.state_interfaces.begin(), +// controller_->thruster_allocation_matrix_->parameters_.state_interfaces.end(), state_interface_types_.begin(), +// state_interface_types_.end())); + +// ASSERT_EQ(controller_->thruster_allocation_matrix_->parameters_.ft_sensor.name, ft_sensor_name_); +// ASSERT_EQ(controller_->thruster_allocation_matrix_->parameters_.kinematics.base, ik_base_frame_); +// ASSERT_EQ(controller_->thruster_allocation_matrix_->parameters_.ft_sensor.frame.id, sensor_frame_); + +// ASSERT_TRUE(!controller_->thruster_allocation_matrix_->parameters_.thruster_allocation_matrix.selected_axes.empty()); +// ASSERT_TRUE( +// controller_->thruster_allocation_matrix_->parameters_.thruster_allocation_matrix.selected_axes.size() == +// thruster_allocation_matrix_selected_axes_.size()); +// ASSERT_TRUE(std::equal( +// controller_->thruster_allocation_matrix_->parameters_.thruster_allocation_matrix.selected_axes.begin(), +// controller_->thruster_allocation_matrix_->parameters_.thruster_allocation_matrix.selected_axes.end(), +// thruster_allocation_matrix_selected_axes_.begin(), thruster_allocation_matrix_selected_axes_.end())); + +// ASSERT_TRUE(!controller_->thruster_allocation_matrix_->parameters_.thruster_allocation_matrix.mass.empty()); +// ASSERT_TRUE( +// controller_->thruster_allocation_matrix_->parameters_.thruster_allocation_matrix.mass.size() == +// thruster_allocation_matrix_mass_.size()); +// ASSERT_TRUE(std::equal( +// controller_->thruster_allocation_matrix_->parameters_.thruster_allocation_matrix.mass.begin(), +// controller_->thruster_allocation_matrix_->parameters_.thruster_allocation_matrix.mass.end(), +// thruster_allocation_matrix_mass_.begin(), thruster_allocation_matrix_mass_.end())); + +// ASSERT_TRUE(!controller_->thruster_allocation_matrix_->parameters_.thruster_allocation_matrix.damping_ratio.empty()); +// ASSERT_TRUE( +// controller_->thruster_allocation_matrix_->parameters_.thruster_allocation_matrix.damping_ratio.size() == +// thruster_allocation_matrix_damping_ratio_.size()); +// ASSERT_TRUE(std::equal( +// controller_->thruster_allocation_matrix_->parameters_.thruster_allocation_matrix.damping_ratio.begin(), +// controller_->thruster_allocation_matrix_->parameters_.thruster_allocation_matrix.damping_ratio.end(), +// thruster_allocation_matrix_damping_ratio_.begin(), thruster_allocation_matrix_damping_ratio_.end())); + +// ASSERT_TRUE(!controller_->thruster_allocation_matrix_->parameters_.thruster_allocation_matrix.stiffness.empty()); +// ASSERT_TRUE( +// controller_->thruster_allocation_matrix_->parameters_.thruster_allocation_matrix.stiffness.size() == +// thruster_allocation_matrix_stiffness_.size()); +// ASSERT_TRUE(std::equal( +// controller_->thruster_allocation_matrix_->parameters_.thruster_allocation_matrix.stiffness.begin(), +// controller_->thruster_allocation_matrix_->parameters_.thruster_allocation_matrix.stiffness.end(), +// thruster_allocation_matrix_stiffness_.begin(), thruster_allocation_matrix_stiffness_.end())); +// } + +// TEST_F(ThrusterAllocationMatrixControllerTest, check_interfaces) +// { +// SetUpController(); + +// ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + +// auto command_interfaces = controller_->command_interface_configuration(); +// ASSERT_EQ(command_interfaces.names.size(), joint_command_values_.size()); +// EXPECT_EQ(command_interfaces.type, controller_interface::interface_configuration_type::INDIVIDUAL); + +// ASSERT_EQ(controller_->command_interfaces_.size(), command_interface_types_.size() * joint_names_.size()); + +// auto state_interfaces = controller_->state_interface_configuration(); +// ASSERT_EQ(state_interfaces.names.size(), joint_state_values_.size() + fts_state_values_.size()); +// EXPECT_EQ(state_interfaces.type, controller_interface::interface_configuration_type::INDIVIDUAL); + +// ASSERT_EQ( +// controller_->state_interfaces_.size(), +// state_interface_types_.size() * joint_names_.size() + fts_state_values_.size()); +// } + +// TEST_F(ThrusterAllocationMatrixControllerTest, activate_success) +// { +// SetUpController(); + +// ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); +// ASSERT_EQ(controller_->command_interfaces_.size(), command_interface_types_.size() * joint_names_.size()); +// ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); +// } + +// TEST_F(ThrusterAllocationMatrixControllerTest, update_success) +// { +// SetUpController(); + +// ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); +// ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); +// broadcast_tfs(); +// ASSERT_EQ( +// controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), +// controller_interface::return_type::OK); +// } + +// TEST_F(ThrusterAllocationMatrixControllerTest, deactivate_success) +// { +// SetUpController(); + +// ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); +// ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); +// ASSERT_EQ(controller_->on_deactivate(rclcpp_lifecycle::State()), NODE_SUCCESS); +// } + +// TEST_F(ThrusterAllocationMatrixControllerTest, reactivate_success) +// { +// SetUpController(); + +// ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); +// ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); +// ASSERT_EQ(controller_->on_deactivate(rclcpp_lifecycle::State()), NODE_SUCCESS); +// assign_interfaces(); +// ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); +// broadcast_tfs(); +// ASSERT_EQ( +// controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), +// controller_interface::return_type::OK); +// } + +// TEST_F(ThrusterAllocationMatrixControllerTest, publish_status_success) +// { +// SetUpController(); + +// ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); +// ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + +// broadcast_tfs(); +// ASSERT_EQ( +// controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), +// controller_interface::return_type::OK); + +// ControllerStateMsg msg; +// subscribe_and_get_messages(msg); + +// // // Check that wrench command are all zero since not used +// // ASSERT_EQ(msg.wrench_base.header.frame_id, ik_base_frame_); +// // ASSERT_EQ(msg.wrench_base.wrench.force.x, 0.0); +// // ASSERT_EQ(msg.wrench_base.wrench.force.y, 0.0); +// // ASSERT_TRUE(msg.wrench_base.wrench.force.z > 0.15); +// // ASSERT_TRUE(msg.wrench_base.wrench.torque.x != 0.0); +// // ASSERT_TRUE(msg.wrench_base.wrench.torque.y != 0.0); +// // ASSERT_EQ(msg.wrench_base.wrench.torque.z, 0.0); + +// // // Check joint command message +// // for (auto i = 0ul; i < joint_names_.size(); i++) +// // { +// // ASSERT_EQ(joint_names_[i], msg.joint_state.name[i]); +// // ASSERT_FALSE(std::isnan(msg.joint_state.position[i])); +// // ASSERT_FALSE(std::isnan(msg.joint_state.velocity[i])); +// // ASSERT_FALSE(std::isnan(msg.joint_state.effort[i])); +// // } +// } + +// TEST_F(ThrusterAllocationMatrixControllerTest, receive_message_and_publish_updated_status) +// { +// SetUpController(); +// rclcpp::executors::MultiThreadedExecutor executor; +// executor.add_node(controller_->get_node()->get_node_base_interface()); + +// ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); +// ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); +// broadcast_tfs(); +// ASSERT_EQ( +// controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), +// controller_interface::return_type::OK); + +// // After first update state, commanded position should be near the start state +// for (auto i = 0ul; i < joint_state_values_.size(); i++) { +// ASSERT_NEAR(joint_state_values_[i], joint_command_values_[i], COMMON_THRESHOLD); +// } + +// ControllerStateMsg msg; +// subscribe_and_get_messages(msg); +// // ASSERT_EQ(msg.wrench_base.header.frame_id, ik_base_frame_); +// // ASSERT_EQ(msg.wrench_base.header.frame_id, ik_base_frame_); + +// publish_commands(); +// ASSERT_TRUE(controller_->wait_for_commands(executor)); + +// broadcast_tfs(); +// ASSERT_EQ( +// controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), +// controller_interface::return_type::OK); + +// EXPECT_NEAR(joint_command_values_[0], joint_state_values_[0], COMMON_THRESHOLD); + +// subscribe_and_get_messages(msg); +// } + +// int main(int argc, char ** argv) +// { +// ::testing::InitGoogleTest(&argc, argv); +// rclcpp::init(argc, argv); +// int result = RUN_ALL_TESTS(); +// rclcpp::shutdown(); +// return result; +// } + +// // Add test, wrong interfaces diff --git a/thruster_allocation_matrix_controller/test/test_thruster_allocation_matrix_controller.hpp b/thruster_allocation_matrix_controller/test/test_thruster_allocation_matrix_controller.hpp new file mode 100644 index 0000000..0be2d8f --- /dev/null +++ b/thruster_allocation_matrix_controller/test/test_thruster_allocation_matrix_controller.hpp @@ -0,0 +1,449 @@ +// Copyright (c) 2021, Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// 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. +// +/// \author: Denis Stogl + +#ifndef TEST_THRUSTERALLOCATIONMATRIX_CONTROLLER_HPP_ +#define TEST_THRUSTERALLOCATIONMATRIX_CONTROLLER_HPP_ + +#include +#include +#include +#include +#include +#include +#include + +// #include "control_msgs/msg/thruster_allocation_matrix_controller_state.hpp" +#include "geometry_msgs/msg/transform_stamped.hpp" +#include "gmock/gmock.h" +#include "hardware_interface/loaned_command_interface.hpp" +#include "hardware_interface/loaned_state_interface.hpp" +#include "hardware_interface/types/hardware_interface_return_values.hpp" +#include "rclcpp/parameter_value.hpp" +#include "rclcpp/utilities.hpp" +#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" +#include "semantic_components/force_torque_sensor.hpp" +// #include "test_asset_6d_robot_description.hpp" +// #include "tf2_ros/transform_broadcaster.h" +#include "thruster_allocation_matrix_controller/thruster_allocation_matrix_controller.hpp" +#include "trajectory_msgs/msg/joint_trajectory.hpp" + +// TODO(anyone): replace the state and command message types +using ControllerCommandWrenchMsg = geometry_msgs::msg::WrenchStamped; +using ControllerCommandPoseMsg = geometry_msgs::msg::PoseStamped; +using ControllerCommandJointMsg = trajectory_msgs::msg::JointTrajectoryPoint; +using ControllerStateMsg = control_msgs::msg::ThrusterAllocationMatrixControllerState; + +namespace +{ +const double COMMON_THRESHOLD = 0.001; + +constexpr auto NODE_SUCCESS = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; +constexpr auto NODE_FAILURE = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::FAILURE; +constexpr auto NODE_ERROR = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR; + +rclcpp::WaitResultKind wait_for(rclcpp::SubscriptionBase::SharedPtr subscription) +{ + rclcpp::WaitSet wait_set; + wait_set.add_subscription(subscription); + const auto timeout = std::chrono::seconds(10); + return wait_set.wait(timeout).kind(); +} + +} // namespace + +// subclassing and friending so we can access member variables +class TestableThrusterAllocationMatrixController +: public thruster_allocation_matrix_controller::ThrusterAllocationMatrixController +{ + FRIEND_TEST(ThrusterAllocationMatrixControllerTest, joint_names_parameter_not_set); + FRIEND_TEST(ThrusterAllocationMatrixControllerTest, interface_parameter_not_set); + FRIEND_TEST(ThrusterAllocationMatrixControllerTest, all_parameters_set_configure_success); + FRIEND_TEST(ThrusterAllocationMatrixControllerTest, check_interfaces); + FRIEND_TEST(ThrusterAllocationMatrixControllerTest, activate_success); + FRIEND_TEST(ThrusterAllocationMatrixControllerTest, receive_message_and_publish_updated_status); + +public: + CallbackReturn on_init() override + { + get_node()->declare_parameter("robot_description", rclcpp::ParameterType::PARAMETER_STRING); + get_node()->declare_parameter("robot_description_semantic", rclcpp::ParameterType::PARAMETER_STRING); + get_node()->set_parameter({"robot_description", robot_description_}); + get_node()->set_parameter({"robot_description_semantic", robot_description_semantic_}); + + return thruster_allocation_matrix_controller::ThrusterAllocationMatrixController::on_init(); + } + + CallbackReturn on_configure(const rclcpp_lifecycle::State & previous_state) override + { + auto ret = thruster_allocation_matrix_controller::ThrusterAllocationMatrixController::on_configure(previous_state); + // Only if on_configure is successful create subscription + if (ret == CallbackReturn::SUCCESS) { + input_pose_command_subscriber_wait_set_.add_subscription(input_joint_command_subscriber_); + } + return ret; + } + + /** + * @brief wait_for_commands blocks until a new ControllerCommandMsg is received. + * Requires that the executor is not spinned elsewhere between the + * message publication and the call to this function. + * + * @return true if new ControllerCommandMsg msg was received, false if timeout. + */ + bool wait_for_commands( + rclcpp::Executor & executor, const std::chrono::milliseconds & timeout = std::chrono::milliseconds{500}) + { + bool success = input_pose_command_subscriber_wait_set_.wait(timeout).kind() == rclcpp::WaitResultKind::Ready; + + if (success) { + executor.spin_some(); + } + return success; + } + +private: + rclcpp::WaitSet input_wrench_command_subscriber_wait_set_; + rclcpp::WaitSet input_pose_command_subscriber_wait_set_; + const std::string robot_description_ = ros2_control_test_assets::valid_6d_robot_urdf; + const std::string robot_description_semantic_ = ros2_control_test_assets::valid_6d_robot_srdf; +}; + +class ThrusterAllocationMatrixControllerTest : public ::testing::Test +{ +public: + static void SetUpTestCase() + { + // rclcpp::init(0, nullptr); + } + + void SetUp() + { + // initialize controller + controller_ = std::make_unique(); + + command_publisher_node_ = std::make_shared("command_publisher"); + force_command_publisher_ = command_publisher_node_->create_publisher( + "/test_thruster_allocation_matrix_controller/force_references", rclcpp::SystemDefaultsQoS()); + // pose_command_publisher_ =command_publisher_node_->create_publisher( + // "/test_thruster_allocation_matrix_controller/pose_commands", rclcpp::SystemDefaultsQoS()); + joint_command_publisher_ = command_publisher_node_->create_publisher( + "/test_thruster_allocation_matrix_controller/joint_references", rclcpp::SystemDefaultsQoS()); + + test_subscription_node_ = std::make_shared("test_subscription_node"); + test_broadcaster_node_ = std::make_shared("test_broadcaster_node"); + } + + static void TearDownTestCase() + { + // rclcpp::shutdown(); + } + + void TearDown() { controller_.reset(nullptr); } + +protected: + controller_interface::return_type SetUpController( + const std::string & controller_name, const std::vector & parameter_overrides) + { + auto options = rclcpp::NodeOptions() + .allow_undeclared_parameters(false) + .parameter_overrides(parameter_overrides) + .automatically_declare_parameters_from_overrides(false); + return SetUpControllerCommon(controller_name, options); + } + + controller_interface::return_type SetUpController( + const std::string & controller_name = "test_thruster_allocation_matrix_controller") + { + auto options = + rclcpp::NodeOptions().allow_undeclared_parameters(false).automatically_declare_parameters_from_overrides(false); + return SetUpControllerCommon(controller_name, options); + } + + controller_interface::return_type SetUpControllerCommon( + const std::string & controller_name, const rclcpp::NodeOptions & options) + { + auto result = controller_->init(controller_name, "", 0, "", options); + + controller_->export_reference_interfaces(); + assign_interfaces(); + + return result; + } + + void assign_interfaces() + { + std::vector command_ifs; + command_itfs_.reserve(joint_command_values_.size()); + command_ifs.reserve(joint_command_values_.size()); + + for (auto i = 0u; i < joint_command_values_.size(); ++i) { + command_itfs_.emplace_back( + hardware_interface::CommandInterface(joint_names_[i], command_interface_types_[0], &joint_command_values_[i])); + command_ifs.emplace_back(command_itfs_.back()); + } + + auto sc_fts = semantic_components::ForceTorqueSensor(ft_sensor_name_); + fts_state_names_ = sc_fts.get_state_interface_names(); + std::vector state_ifs; + + const size_t num_state_ifs = joint_state_values_.size() + fts_state_names_.size(); + state_itfs_.reserve(num_state_ifs); + state_ifs.reserve(num_state_ifs); + + for (auto i = 0u; i < joint_state_values_.size(); ++i) { + state_itfs_.emplace_back( + hardware_interface::StateInterface(joint_names_[i], state_interface_types_[0], &joint_state_values_[i])); + state_ifs.emplace_back(state_itfs_.back()); + } + + std::vector fts_itf_names = {"force.x", "force.y", "force.z", "torque.x", "torque.y", "torque.z"}; + + for (auto i = 0u; i < fts_state_names_.size(); ++i) { + state_itfs_.emplace_back( + hardware_interface::StateInterface(ft_sensor_name_, fts_itf_names[i], &fts_state_values_[i])); + state_ifs.emplace_back(state_itfs_.back()); + } + + controller_->assign_interfaces(std::move(command_ifs), std::move(state_ifs)); + } + + void broadcast_tfs() + { + static tf2_ros::TransformBroadcaster br(test_broadcaster_node_); + geometry_msgs::msg::TransformStamped transform_stamped; + + transform_stamped.header.stamp = test_broadcaster_node_->now(); + transform_stamped.header.frame_id = fixed_world_frame_; + transform_stamped.transform.translation.x = 1.3; + transform_stamped.transform.translation.y = 0.5; + transform_stamped.transform.translation.z = 0.5; + transform_stamped.transform.rotation.x = 0; + transform_stamped.transform.rotation.y = 0; + transform_stamped.transform.rotation.z = 0; + transform_stamped.transform.rotation.w = 1; + + transform_stamped.child_frame_id = ik_base_frame_; + br.sendTransform(transform_stamped); + + transform_stamped.child_frame_id = ik_tip_frame_; + br.sendTransform(transform_stamped); + + transform_stamped.header.frame_id = ik_tip_frame_; + transform_stamped.transform.translation.x = 0; + transform_stamped.transform.translation.y = 0; + transform_stamped.transform.translation.z = 0; + transform_stamped.transform.rotation.x = 0; + transform_stamped.transform.rotation.y = 0; + transform_stamped.transform.rotation.z = 0; + transform_stamped.transform.rotation.w = 1; + + transform_stamped.child_frame_id = control_frame_; + br.sendTransform(transform_stamped); + + transform_stamped.transform.translation.z = 0.05; + transform_stamped.child_frame_id = sensor_frame_; + br.sendTransform(transform_stamped); + + transform_stamped.transform.translation.z = 0.2; + transform_stamped.child_frame_id = endeffector_frame_; + br.sendTransform(transform_stamped); + } + + void subscribe_and_get_messages(ControllerStateMsg & msg) + { + // create a new subscriber + auto subs_callback = [&](const ControllerStateMsg::SharedPtr) {}; + auto subscription = test_subscription_node_->create_subscription( + "/test_thruster_allocation_matrix_controller/status", 10, subs_callback); + + // call update to publish the test value + ASSERT_EQ( + controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + // wait for message to be passed + ASSERT_EQ(wait_for(subscription), rclcpp::WaitResultKind::Ready); + + // take message from subscription + rclcpp::MessageInfo msg_info; + ASSERT_TRUE(subscription->take(msg, msg_info)); + } + + void publish_commands() + { + auto wait_for_topic = [&](const auto topic_name) { + size_t wait_count = 0; + while (command_publisher_node_->count_subscribers(topic_name) == 0) { + if (wait_count >= 5) { + auto error_msg = std::string("publishing to ") + topic_name + " but no node subscribes to it"; + throw std::runtime_error(error_msg); + } + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + ++wait_count; + } + }; + + // TODO(destogl): comment in when using unified mode + // if (controller_->thruster_allocation_matrix_->unified_mode_) { + // wait_for_topic(force_command_publisher_->get_topic_name()); + // } + + // wait_for_topic(pose_command_publisher_->get_topic_name()); + + ControllerCommandWrenchMsg force_msg; + force_msg.header.frame_id = sensor_frame_; + force_msg.wrench.force.x = 0.1; + force_msg.wrench.force.y = 0.2; + force_msg.wrench.force.z = 0.3; + force_msg.wrench.torque.x = 3.1; + force_msg.wrench.torque.y = 3.2; + force_msg.wrench.torque.z = 3.3; + + ControllerCommandPoseMsg pose_msg; + pose_msg.header.frame_id = endeffector_frame_; + pose_msg.pose.position.x = 0.1; + pose_msg.pose.position.y = 0.2; + pose_msg.pose.position.z = 0.3; + pose_msg.pose.orientation.x = 0; + pose_msg.pose.orientation.y = 0; + pose_msg.pose.orientation.z = 0; + pose_msg.pose.orientation.w = 1; + + // TODO(destogl): comment in when using unified mode + // if (controller_->thruster_allocation_matrix_->unified_mode_) { + // force_command_publisher_->publish(force_msg); + // } + // pose_command_publisher_->publish(pose_msg); + + wait_for_topic(joint_command_publisher_->get_topic_name()); + + ControllerCommandJointMsg joint_msg; + // joint_msg.joint_names = joint_names_; + trajectory_msgs::msg::JointTrajectoryPoint trajectory_point; + auto num_joints = joint_names_.size(); + trajectory_point.positions.reserve(num_joints); + trajectory_point.velocities.resize(num_joints, 0.0); + for (auto index = 0u; index < num_joints; ++index) { + trajectory_point.positions.emplace_back(joint_state_values_[index]); + } + joint_msg = trajectory_point; + + joint_command_publisher_->publish(joint_msg); + } + +protected: + // TODO(anyone): adjust the members as needed + + // Controller-related parameters + const std::vector joint_names_ = {"joint1", "joint2", "joint3", "joint4", "joint5", "joint6"}; + const std::vector command_interface_types_ = {"position"}; + const std::vector state_interface_types_ = {"position"}; + const std::string ft_sensor_name_ = "ft_sensor_name"; + + bool hardware_state_has_offset_ = false; + + const std::string ik_base_frame_ = "base_link"; + const std::string ik_tip_frame_ = "tool0"; + const std::string ik_group_name_ = "arm"; + // const std::string robot_description_ = ros2_control_test_assets::valid_6d_robot_urdf; + // const std::string robot_description_semantic_ = ros2_control_test_assets::valid_6d_robot_srdf; + + const std::string control_frame_ = "tool0"; + const std::string endeffector_frame_ = "endeffector_frame"; + const std::string fixed_world_frame_ = "fixed_world_frame"; + const std::string sensor_frame_ = "link_6"; + + std::array thruster_allocation_matrix_selected_axes_ = {true, true, true, true, true, true}; + std::array thruster_allocation_matrix_mass_ = {5.5, 6.6, 7.7, 8.8, 9.9, 10.10}; + std::array thruster_allocation_matrix_damping_ratio_ = {2.828427, 2.828427, 2.828427, + 2.828427, 2.828427, 2.828427}; + std::array thruster_allocation_matrix_stiffness_ = {214.1, 214.2, 214.3, 214.4, 214.5, 214.6}; + + std::array joint_command_values_ = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0}; + std::array joint_state_values_ = {1.1, 2.2, 3.3, 4.4, 5.5, 6.6}; + std::array fts_state_values_ = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0}; + std::vector fts_state_names_; + + std::vector state_itfs_; + std::vector command_itfs_; + + // Test related parameters + std::unique_ptr controller_; + rclcpp::Node::SharedPtr command_publisher_node_; + rclcpp::Publisher::SharedPtr force_command_publisher_; + // rclcpp::Publisher::SharedPtr pose_command_publisher_; + rclcpp::Publisher::SharedPtr joint_command_publisher_; + rclcpp::Node::SharedPtr test_subscription_node_; + rclcpp::Node::SharedPtr test_broadcaster_node_; +}; + +// From the tutorial: https://www.sandordargo.com/blog/2019/04/24/parameterized-testing-with-gtest +class ThrusterAllocationMatrixControllerTestParameterizedMissingParameters +: public ThrusterAllocationMatrixControllerTest, + public ::testing::WithParamInterface +{ +public: + virtual void SetUp() + { + ThrusterAllocationMatrixControllerTest::SetUp(); + auto node = std::make_shared("test_thruster_allocation_matrix_controller"); + overrides_ = node->get_node_parameters_interface()->get_parameter_overrides(); + } + + static void TearDownTestCase() { ThrusterAllocationMatrixControllerTest::TearDownTestCase(); } + +protected: + controller_interface::return_type SetUpController(const std::string & remove_name) + { + std::vector parameter_overrides; + for (const auto & override : overrides_) { + if (override.first != remove_name) { + rclcpp::Parameter param(override.first, override.second); + parameter_overrides.push_back(param); + } + } + + return ThrusterAllocationMatrixControllerTest::SetUpController( + "test_thruster_allocation_matrix_controller_no_overrides", parameter_overrides); + } + + std::map overrides_; +}; + +// From the tutorial: https://www.sandordargo.com/blog/2019/04/24/parameterized-testing-with-gtest +class ThrusterAllocationMatrixControllerTestParameterizedInvalidParameters +: public ThrusterAllocationMatrixControllerTest, + public ::testing::WithParamInterface> +{ +public: + virtual void SetUp() { ThrusterAllocationMatrixControllerTest::SetUp(); } + + static void TearDownTestCase() { ThrusterAllocationMatrixControllerTest::TearDownTestCase(); } + +protected: + controller_interface::return_type SetUpController() + { + auto param_name = std::get<0>(GetParam()); + auto param_value = std::get<1>(GetParam()); + std::vector parameter_overrides; + rclcpp::Parameter param(param_name, param_value); + parameter_overrides.push_back(param); + return ThrusterAllocationMatrixControllerTest::SetUpController( + "test_thruster_allocation_matrix_controller", parameter_overrides); + } +}; + +#endif // TEST_THRUSTERALLOCATIONMATRIX_CONTROLLER_HPP_ diff --git a/thruster_allocation_matrix_controller/thruster_allocation_matrix_controller.xml b/thruster_allocation_matrix_controller/thruster_allocation_matrix_controller.xml new file mode 100644 index 0000000..a8a1dac --- /dev/null +++ b/thruster_allocation_matrix_controller/thruster_allocation_matrix_controller.xml @@ -0,0 +1,11 @@ + + + + + Thruster allocation matrix controller for wrench to thrust control. + + + +