From 3f128440d00f1c3561ce7ff4e035172f976ba009 Mon Sep 17 00:00:00 2001 From: unknown Date: Sun, 25 Feb 2024 20:04:13 -0800 Subject: [PATCH 1/5] 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. + + + + From d04a3053bb33dceebb4935b706139985d33e6ce3 Mon Sep 17 00:00:00 2001 From: EverardoG Date: Thu, 29 Feb 2024 21:48:21 -0800 Subject: [PATCH 2/5] changes according to PR comments --- .../CMakeLists.txt | 9 +- .../thruster_allocation_matrix_controller.hpp | 16 +- .../visibility_control.h | 5 +- .../package.xml | 3 +- .../thruster_allocation_matrix_controller.cpp | 4 +- ...ocation_matrix_controller_parameters.yaml} | 0 ..._thruster_allocation_matrix_controller.cpp | 60 --- ..._thruster_allocation_matrix_controller.cpp | 297 ------------ ..._thruster_allocation_matrix_controller.hpp | 449 ------------------ .../thruster_allocation_matrix_controller.xml | 2 +- 10 files changed, 14 insertions(+), 831 deletions(-) rename thruster_allocation_matrix_controller/src/{thruster_allocation_matrix_parameters.yaml => thruster_allocation_matrix_controller_parameters.yaml} (100%) diff --git a/thruster_allocation_matrix_controller/CMakeLists.txt b/thruster_allocation_matrix_controller/CMakeLists.txt index cc927e4..3c8ec01 100644 --- a/thruster_allocation_matrix_controller/CMakeLists.txt +++ b/thruster_allocation_matrix_controller/CMakeLists.txt @@ -17,6 +17,7 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS Eigen3 geometry_msgs control_msgs + hydrodynamics ) find_package(ament_cmake REQUIRED) @@ -24,8 +25,8 @@ 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 +generate_parameter_library(thruster_allocation_matrix_controller_parameters + src/thruster_allocation_matrix_controller_parameters.yaml ) add_library(thruster_allocation_matrix_controller SHARED @@ -44,7 +45,7 @@ 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 + thruster_allocation_matrix_controller_parameters ) # Use dllexport instead of dllimport @@ -72,7 +73,7 @@ install( install( TARGETS thruster_allocation_matrix_controller - thruster_allocation_matrix_parameters + thruster_allocation_matrix_controller_parameters EXPORT export_thruster_allocation_matrix_controller RUNTIME DESTINATION bin 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 88dec7b..65f2212 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 @@ -1,4 +1,4 @@ -// Copyright 2024, Evan Palmer +// Copyright 2024, Colin Mitchell, Everardo Gonzalez, Rakesh Vivekanandan // // Permission is hereby granted, free of charge, to any person obtaining a copy // of this software and associated documentation files (the "Software"), to deal @@ -32,22 +32,14 @@ #include "controller_interface/chainable_controller_interface.hpp" #include "controller_interface/controller_interface.hpp" #include "geometry_msgs/msg/wrench.hpp" +#include "hydrodynamics/eigen.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 +#include "thruster_allocation_matrix_controller_parameters.hpp" namespace thruster_allocation_matrix_controller { @@ -85,7 +77,6 @@ class ThrusterAllocationMatrixController : public controller_interface::Chainabl 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; @@ -133,7 +124,6 @@ class ThrusterAllocationMatrixController : public controller_interface::Chainabl private: THRUSTER_ALLOCATION_MATRIX_CONTROLLER_LOCAL - // TODO: replace message type with something for the Wrench void reference_callback(std::shared_ptr msg); }; diff --git a/thruster_allocation_matrix_controller/include/thruster_allocation_matrix_controller/visibility_control.h b/thruster_allocation_matrix_controller/include/thruster_allocation_matrix_controller/visibility_control.h index 75d92e8..3be244e 100644 --- a/thruster_allocation_matrix_controller/include/thruster_allocation_matrix_controller/visibility_control.h +++ b/thruster_allocation_matrix_controller/include/thruster_allocation_matrix_controller/visibility_control.h @@ -1,4 +1,4 @@ -// Copyright 2024, Evan Palmer +// Copyright 2024, Colin Mitchell, Everardo Gonzalez, Rakesh Vivekanandan // // Permission is hereby granted, free of charge, to any person obtaining a copy // of this software and associated documentation files (the "Software"), to deal @@ -37,8 +37,7 @@ #else #define THRUSTER_ALLOCATION_MATRIX_CONTROLLER_PUBLIC THRUSTER_ALLOCATION_MATRIX_CONTROLLER_IMPORT #endif -#define THRUSTER_ALLOCATION_MATRIX_CONTROLLER_PUBLIC_TYPE \ - THRUSTER_ALLOCATION_MATRIX_CONTROLLER_PUBLIC +#define THRUSTER_ALLOCATION_MATRIX_CONTROLLER_PUBLIC_TYPE THRUSTER_ALLOCATION_MATRIX_CONTROLLER_PUBLIC #define THRUSTER_ALLOCATION_MATRIX_CONTROLLER_LOCAL #else #define THRUSTER_ALLOCATION_MATRIX_CONTROLLER_EXPORT __attribute__((visibility("default"))) diff --git a/thruster_allocation_matrix_controller/package.xml b/thruster_allocation_matrix_controller/package.xml index 6e8bb34..534b243 100644 --- a/thruster_allocation_matrix_controller/package.xml +++ b/thruster_allocation_matrix_controller/package.xml @@ -4,11 +4,10 @@ thruster_allocation_matrix_controller 0.0.0 TODO: Package description - ros + ros MIT ament_cmake - eigen3_cmake_module eigen rclcpp 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 003d474..c0c112c 100644 --- a/thruster_allocation_matrix_controller/src/thruster_allocation_matrix_controller.cpp +++ b/thruster_allocation_matrix_controller/src/thruster_allocation_matrix_controller.cpp @@ -1,4 +1,4 @@ -// Copyright 2024, Evan Palmer +// Copyright 2024, Colin Mitchell, Everardo Gonzalez, Rakesh Vivekanandan // // Permission is hereby granted, free of charge, to any person obtaining a copy // of this software and associated documentation files (the "Software"), to deal @@ -134,7 +134,7 @@ controller_interface::return_type ThrusterAllocationMatrixController::update_and 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()); + const std::vector reference_forces_values(reference_interfaces_.begin(), 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; diff --git a/thruster_allocation_matrix_controller/src/thruster_allocation_matrix_parameters.yaml b/thruster_allocation_matrix_controller/src/thruster_allocation_matrix_controller_parameters.yaml similarity index 100% rename from thruster_allocation_matrix_controller/src/thruster_allocation_matrix_parameters.yaml rename to thruster_allocation_matrix_controller/src/thruster_allocation_matrix_controller_parameters.yaml 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 index adea31c..e69de29 100644 --- 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 @@ -1,60 +0,0 @@ -// 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 index c15ec26..e69de29 100644 --- a/thruster_allocation_matrix_controller/test/test_thruster_allocation_matrix_controller.cpp +++ b/thruster_allocation_matrix_controller/test/test_thruster_allocation_matrix_controller.cpp @@ -1,297 +0,0 @@ -// // 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 index 0be2d8f..e69de29 100644 --- a/thruster_allocation_matrix_controller/test/test_thruster_allocation_matrix_controller.hpp +++ b/thruster_allocation_matrix_controller/test/test_thruster_allocation_matrix_controller.hpp @@ -1,449 +0,0 @@ -// 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 index a8a1dac..40fccb0 100644 --- a/thruster_allocation_matrix_controller/thruster_allocation_matrix_controller.xml +++ b/thruster_allocation_matrix_controller/thruster_allocation_matrix_controller.xml @@ -4,7 +4,7 @@ type="thruster_allocation_matrix_controller::ThrusterAllocationMatrixController" base_class_type="controller_interface::ChainableControllerInterface"> - Thruster allocation matrix controller for wrench to thrust control. + Thruster allocation matrix controller for wrench-to-thruster. From 6bacb5890244580a232f002c669c4339aa08f96c Mon Sep 17 00:00:00 2001 From: Rakesh Vivekanandan Date: Fri, 1 Mar 2024 12:27:33 -0800 Subject: [PATCH 3/5] Fixed pluginlib issue. Able to load both controllers but need to address why ros2_control node dies --- controller_testing/config/controllers.yaml | 3 ++ controller_testing/launch/testing.launch.py | 39 ++++++++++++++----- .../thruster_allocation_matrix_controller.cpp | 8 ++++ 3 files changed, 41 insertions(+), 9 deletions(-) diff --git a/controller_testing/config/controllers.yaml b/controller_testing/config/controllers.yaml index f730a31..53b794b 100644 --- a/controller_testing/config/controllers.yaml +++ b/controller_testing/config/controllers.yaml @@ -5,6 +5,9 @@ controller_manager: integral_sliding_mode_controller: type: velocity_controllers/IntegralSlidingModeController + thruster_allocation_matrix_controller: + type: thruster_allocation_matrix_controller/ThrusterAllocationMatrixController + integral_sliding_mode_controller: ros__parameters: dof_names: diff --git a/controller_testing/launch/testing.launch.py b/controller_testing/launch/testing.launch.py index 415500d..da20db0 100644 --- a/controller_testing/launch/testing.launch.py +++ b/controller_testing/launch/testing.launch.py @@ -1,4 +1,6 @@ from launch import LaunchDescription +from launch.actions import RegisterEventHandler +from launch.event_handlers import OnProcessExit from launch.substitutions import Command, FindExecutable, PathJoinSubstitution from launch_ros.actions import Node from launch_ros.substitutions import FindPackageShare @@ -20,6 +22,32 @@ def generate_launch_description(): ] ), } + velocity_controller_spawner = Node( + package="controller_manager", + executable="spawner", + arguments=[ + "integral_sliding_mode_controller", + "--controller-manager", + ["", "controller_manager"], + ], + ) + tam_controller_spawner = Node( + package="controller_manager", + executable="spawner", + arguments=[ + "thruster_allocation_matrix_controller", + "--controller-manager", + ["", "controller_manager"], + ], + ) + delay_velocity_controller_spawner_after_tam_controller_spawner = ( + RegisterEventHandler( + event_handler=OnProcessExit( + target_action=tam_controller_spawner, + on_exit=[velocity_controller_spawner], + ) + ) + ) nodes = [ Node( package="controller_manager", @@ -36,15 +64,8 @@ def generate_launch_description(): ), ], ), - Node( - package="controller_manager", - executable="spawner", - arguments=[ - "integral_sliding_mode_controller", - "--controller-manager", - ["", "controller_manager"], - ], - ), + tam_controller_spawner, + velocity_controller_spawner, ] return LaunchDescription(nodes) 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 c0c112c..f440471 100644 --- a/thruster_allocation_matrix_controller/src/thruster_allocation_matrix_controller.cpp +++ b/thruster_allocation_matrix_controller/src/thruster_allocation_matrix_controller.cpp @@ -159,6 +159,14 @@ controller_interface::return_type ThrusterAllocationMatrixController::update_and return controller_interface::return_type::OK; } +controller_interface::InterfaceConfiguration ThrusterAllocationMatrixController::state_interface_configuration() const +{ + controller_interface::InterfaceConfiguration state_interface_configuration; + + return state_interface_configuration; +} + + bool ThrusterAllocationMatrixController::on_set_chained_mode(bool /*chained_mode*/) { return true; } std::vector ThrusterAllocationMatrixController::on_export_reference_interfaces() From 3f9d3d6d259704a80fe3a528a99c5995d8c84f41 Mon Sep 17 00:00:00 2001 From: Rakesh Vivekanandan Date: Fri, 1 Mar 2024 13:03:24 -0800 Subject: [PATCH 4/5] WIP: Modified URDF --- controller_testing/config/controllers.yaml | 17 +++++ controller_testing/urdf/testing.urdf.xacro | 89 ++++++++++++++++++++++ 2 files changed, 106 insertions(+) diff --git a/controller_testing/config/controllers.yaml b/controller_testing/config/controllers.yaml index 53b794b..2d72ab9 100644 --- a/controller_testing/config/controllers.yaml +++ b/controller_testing/config/controllers.yaml @@ -19,3 +19,20 @@ integral_sliding_mode_controller: - "rz" hydrodynamics: mass: 1.0 + +thruster_allocation_matrix_controller: + ros__parameters: + dof_names: + - "x" + - "y" + - "z" + - "rx" + - "ry" + - "rz" + tam: + x: [-0.707, -0.707, 0.707, 0.707, 0.0, 0.0, 0.0, 0.0] + y: [0.707, -0.707, 0.707, -0.707, 0.0, 0.0, 0.0, 0.0] + z: [0.0, 0.0, 0.0, 0.0, 1.0, 1.0, 1.0, 1.0] + rx: [0.0, 0.0, 0.0, 0.0, -0.21805, 0.21805, -0.21805, 0.21805] + ry: [0.0, 0.0, 0.0, 0.0, -0.12, -0.12, 0.12, 0.12] + rz: [0.1888, -0.1888, -0.1888, 0.1888, 0.0, 0.0, 0.0 , 0.0] diff --git a/controller_testing/urdf/testing.urdf.xacro b/controller_testing/urdf/testing.urdf.xacro index 836d2be..34dd7b0 100644 --- a/controller_testing/urdf/testing.urdf.xacro +++ b/controller_testing/urdf/testing.urdf.xacro @@ -37,6 +37,70 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + mock_components/GenericSystem @@ -66,6 +130,31 @@ + + + + + + + + + + + + + + + + + + + + + + + + + From f330f1f885ec664daf2389afe66647b0536d014d Mon Sep 17 00:00:00 2001 From: Rakesh Vivekanandan Date: Fri, 1 Mar 2024 18:12:53 -0800 Subject: [PATCH 5/5] Partially works. Still need to fix TAM controller --- .vscode/settings.json | 73 ++++++++++++++++- controller_testing/launch/testing.launch.py | 3 +- controller_testing/urdf/testing.urdf.xacro | 80 +++++++++---------- .../thruster_allocation_matrix_controller.hpp | 6 +- .../thruster_allocation_matrix_controller.cpp | 60 +++++++------- 5 files changed, 147 insertions(+), 75 deletions(-) diff --git a/.vscode/settings.json b/.vscode/settings.json index 4860e8f..a7fa845 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -10,7 +10,78 @@ "dense": "cpp", "vector": "cpp", "memory": "cpp", - "cstddef": "cpp" + "cstddef": "cpp", + "cctype": "cpp", + "clocale": "cpp", + "cmath": "cpp", + "csignal": "cpp", + "cstdarg": "cpp", + "cstdio": "cpp", + "cstdlib": "cpp", + "cstring": "cpp", + "ctime": "cpp", + "cwchar": "cpp", + "cwctype": "cpp", + "array": "cpp", + "atomic": "cpp", + "strstream": "cpp", + "bit": "cpp", + "*.tcc": "cpp", + "bitset": "cpp", + "chrono": "cpp", + "codecvt": "cpp", + "compare": "cpp", + "complex": "cpp", + "concepts": "cpp", + "condition_variable": "cpp", + "cstdint": "cpp", + "deque": "cpp", + "forward_list": "cpp", + "list": "cpp", + "map": "cpp", + "set": "cpp", + "string": "cpp", + "unordered_map": "cpp", + "unordered_set": "cpp", + "exception": "cpp", + "algorithm": "cpp", + "functional": "cpp", + "iterator": "cpp", + "memory_resource": "cpp", + "numeric": "cpp", + "optional": "cpp", + "random": "cpp", + "ratio": "cpp", + "string_view": "cpp", + "system_error": "cpp", + "tuple": "cpp", + "type_traits": "cpp", + "utility": "cpp", + "fstream": "cpp", + "future": "cpp", + "initializer_list": "cpp", + "iomanip": "cpp", + "iosfwd": "cpp", + "iostream": "cpp", + "istream": "cpp", + "limits": "cpp", + "mutex": "cpp", + "new": "cpp", + "numbers": "cpp", + "ostream": "cpp", + "semaphore": "cpp", + "shared_mutex": "cpp", + "sstream": "cpp", + "stdexcept": "cpp", + "stop_token": "cpp", + "streambuf": "cpp", + "thread": "cpp", + "cfenv": "cpp", + "cinttypes": "cpp", + "typeindex": "cpp", + "typeinfo": "cpp", + "valarray": "cpp", + "variant": "cpp" }, "terminal.integrated.defaultProfile.linux": "bash", "terminal.integrated.profiles.linux": { diff --git a/controller_testing/launch/testing.launch.py b/controller_testing/launch/testing.launch.py index da20db0..ffc6431 100644 --- a/controller_testing/launch/testing.launch.py +++ b/controller_testing/launch/testing.launch.py @@ -64,8 +64,9 @@ def generate_launch_description(): ), ], ), + # velocity_controller_spawner, tam_controller_spawner, - velocity_controller_spawner, + delay_velocity_controller_spawner_after_tam_controller_spawner, ] return LaunchDescription(nodes) diff --git a/controller_testing/urdf/testing.urdf.xacro b/controller_testing/urdf/testing.urdf.xacro index 34dd7b0..88c82df 100644 --- a/controller_testing/urdf/testing.urdf.xacro +++ b/controller_testing/urdf/testing.urdf.xacro @@ -37,68 +37,60 @@ - + - + - - + - + - - + - + - - + - + - - + - + - - + - + - - + - + - - + - + - @@ -130,29 +122,37 @@ - - + + + - - + + + - - + + + - - + + + - - + + + - - + + + - - + + + - - + + + 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 65f2212..c48ca23 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 @@ -53,7 +53,8 @@ class ThrusterAllocationMatrixController : public controller_interface::Chainabl 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::CallbackReturn on_init() override; THRUSTER_ALLOCATION_MATRIX_CONTROLLER_PUBLIC controller_interface::InterfaceConfiguration command_interface_configuration() const override; @@ -122,9 +123,6 @@ class ThrusterAllocationMatrixController : public controller_interface::Chainabl std::int64_t num_thrusters_; -private: - THRUSTER_ALLOCATION_MATRIX_CONTROLLER_LOCAL - void reference_callback(std::shared_ptr msg); }; } // namespace thruster_allocation_matrix_controller 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 f440471..442d4a0 100644 --- a/thruster_allocation_matrix_controller/src/thruster_allocation_matrix_controller.cpp +++ b/thruster_allocation_matrix_controller/src/thruster_allocation_matrix_controller.cpp @@ -73,12 +73,15 @@ ThrusterAllocationMatrixController::CallbackReturn ThrusterAllocationMatrixContr controller_interface::InterfaceConfiguration ThrusterAllocationMatrixController::command_interface_configuration() const { controller_interface::InterfaceConfiguration command_interfaces_config; - command_interfaces_config.type = controller_interface::interface_configuration_type::INDIVIDUAL; + // 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); + // RCLCPP_INFO(get_node()->get_logger(), "command_interface_type: %d", command_interfaces_config.type); + + command_interfaces_config.names.reserve(num_thrusters_); + for (size_t i = 0; i < num_thrusters_; ++i) { + std::ostringstream thruster_name; + thruster_name << "thruster_" << i + 1; + command_interfaces_config.names.emplace_back(thruster_name.str() + "/" + hardware_interface::HW_IF_VELOCITY); } return command_interfaces_config; @@ -105,7 +108,7 @@ controller_interface::CallbackReturn ThrusterAllocationMatrixController::on_conf // Subscribe to the reference topic reference_sub_ = get_node()->create_subscription( "~/reference", rclcpp::SystemDefaultsQoS(), - [this](const std::shared_ptr msg) { reference_callback(msg); }); // NOLINT + [this](const std::shared_ptr msg) { reference_.writeFromNonRT(msg); }); // NOLINT // Setup the controller state publisher controller_state_pub_ = @@ -134,14 +137,23 @@ controller_interface::return_type ThrusterAllocationMatrixController::update_and 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(), reference_interfaces_.end()); - auto reference_forces = create_six_dof_eigen_from_named_vector(dof_names_, six_dof_names_, reference_forces_values); + // const std::vector reference_forces_values(reference_interfaces_.begin(), reference_interfaces_.end()); + // auto reference_forces = create_six_dof_eigen_from_named_vector(dof_names_, six_dof_names_, + // reference_forces_values); + + // RCLCPP_INFO(get_node()->get_logger(), "reference_forces size: %d", reference_forces.size()); - Eigen::VectorXd thruster_forces = tam_.completeOrthogonalDecomposition().pseudoInverse() * reference_forces; + // Eigen::VectorXd thruster_forces = Eigen::VectorXd::Zero(num_thrusters_); + // Eigen::VectorXd thruster_forces = tam_.completeOrthogonalDecomposition().pseudoInverse() * reference_forces; // setting command interfaces with calculated thruster forces + const double value = 0.0; + // RCLCPP_INFO(get_node()->get_logger(), "command_interfaces_ size: %d", command_interfaces_.size()); + // RCLCPP_INFO(get_node()->get_logger(), "thruster_forces size: %d", thruster_forces.size()); + for (std::int64_t i = 0; i < num_thrusters_; i++) { - command_interfaces_[i].set_value(thruster_forces[i]); + command_interfaces_[i].set_value(value); + // 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 @@ -166,7 +178,6 @@ controller_interface::InterfaceConfiguration ThrusterAllocationMatrixController: return state_interface_configuration; } - bool ThrusterAllocationMatrixController::on_set_chained_mode(bool /*chained_mode*/) { return true; } std::vector ThrusterAllocationMatrixController::on_export_reference_interfaces() @@ -178,7 +189,7 @@ std::vector ThrusterAllocationMatrixContro 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]); + get_node()->get_name(), dof_names_[i] + "/" + hardware_interface::HW_IF_VELOCITY, &reference_interfaces_[i]); } return reference_interfaces; @@ -224,12 +235,12 @@ controller_interface::CallbackReturn ThrusterAllocationMatrixController::configu 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()); + const Eigen::Map tam_x(params_.tam.x.data(), params_.tam.x.size()); + const Eigen::Map tam_y(params_.tam.y.data(), params_.tam.y.size()); + const Eigen::Map tam_z(params_.tam.z.data(), params_.tam.z.size()); + const Eigen::Map tam_rx(params_.tam.rx.data(), params_.tam.rx.size()); + const Eigen::Map tam_ry(params_.tam.ry.data(), params_.tam.ry.size()); + const 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(); @@ -243,21 +254,12 @@ controller_interface::CallbackReturn ThrusterAllocationMatrixController::configu // 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; + tam_ << tam_x.transpose(), tam_y.transpose(), tam_z.transpose(), tam_rx.transpose(), tam_ry.transpose(), + tam_rz.transpose(); 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"