From 1d5d4dcef228aa011a87cc7dba84c5501000c2e8 Mon Sep 17 00:00:00 2001 From: Lovro Date: Wed, 23 Feb 2022 13:06:04 +0100 Subject: [PATCH 01/10] Introduce cartesian twist controller. --- cartesian_controllers/CMakeLists.txt | 85 +++++++++++ .../twist_controller.hpp | 72 ++++++++++ .../visibility_control.h | 49 +++++++ cartesian_controllers/package.xml | 27 ++++ .../src/twist_controller.cpp | 134 ++++++++++++++++++ .../twist_controller_plugin.xml | 7 + 6 files changed, 374 insertions(+) create mode 100644 cartesian_controllers/CMakeLists.txt create mode 100644 cartesian_controllers/include/cartesian_controllers/twist_controller.hpp create mode 100644 cartesian_controllers/include/cartesian_controllers/visibility_control.h create mode 100644 cartesian_controllers/package.xml create mode 100644 cartesian_controllers/src/twist_controller.cpp create mode 100644 cartesian_controllers/twist_controller_plugin.xml diff --git a/cartesian_controllers/CMakeLists.txt b/cartesian_controllers/CMakeLists.txt new file mode 100644 index 0000000000..077c3c4c03 --- /dev/null +++ b/cartesian_controllers/CMakeLists.txt @@ -0,0 +1,85 @@ +cmake_minimum_required(VERSION 3.8) +project(cartesian_controllers) + +# Default to C++14 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) +endif() + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +find_package(ament_cmake_ros REQUIRED) +find_package(controller_interface REQUIRED) +find_package(hardware_interface REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rclcpp_lifecycle REQUIRED) +find_package(realtime_tools REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(std_msgs REQUIRED) +find_package(pluginlib REQUIRED) + +add_library(twist_controller src/twist_controller.cpp) +target_include_directories(twist_controller PUBLIC + $ + $) +ament_target_dependencies( + twist_controller + "controller_interface" + "hardware_interface" + "rclcpp" + "rclcpp_lifecycle" + "realtime_tools" + "geometry_msgs" + "std_msgs" + "pluginlib" +) + +# Causes the visibility macros to use dllexport rather than dllimport, +# which is appropriate when building the dll but not consuming it. +target_compile_definitions(twist_controller PRIVATE "CARTESIAN_CONTROLLERS_BUILDING_LIBRARY") +pluginlib_export_plugin_description_file(controller_interface twist_controller_plugin.xml) + +install( + DIRECTORY include/ + DESTINATION include +) +install( + TARGETS twist_controller + EXPORT export_${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin +) + +if(BUILD_TESTING) + find_package(ament_cmake_gmock REQUIRED) + find_package(controller_manager REQUIRED) + find_package(hardware_interface REQUIRED) + find_package(ros2_control_test_assets REQUIRED) +endif() + +ament_export_dependencies( + controller_interface + hardware_interface + rclcpp + rclcpp_lifecycle + realtime_tools + std_msgs + geometry_msgs +) + +ament_export_include_directories( + include +) +ament_export_libraries( + twist_controller +) +ament_export_targets( + export_${PROJECT_NAME} +) + +ament_package() diff --git a/cartesian_controllers/include/cartesian_controllers/twist_controller.hpp b/cartesian_controllers/include/cartesian_controllers/twist_controller.hpp new file mode 100644 index 0000000000..0bc836a001 --- /dev/null +++ b/cartesian_controllers/include/cartesian_controllers/twist_controller.hpp @@ -0,0 +1,72 @@ +// Copyright 2022 VoodooIT, sole proprietorship +// +// 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. + +#ifndef CARTESIAN_CONTROLLERS__TWIST_CONTROLLER_HPP_ +#define CARTESIAN_CONTROLLERS__TWIST_CONTROLLER_HPP_ + +#include +#include +#include + +#include "controller_interface/controller_interface.hpp" +#include "geometry_msgs/msg/twist_stamped.hpp" +#include "realtime_tools/realtime_buffer.h" + +#include "cartesian_controllers/visibility_control.h" + +namespace cartesian_controllers +{ +using CmdType = geometry_msgs::msg::TwistStamped; +using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; +class TwistController : public controller_interface::ControllerInterface +{ +public: + CARTESIAN_CONTROLLERS_PUBLIC + TwistController(); + + CARTESIAN_CONTROLLERS_PUBLIC + controller_interface::InterfaceConfiguration command_interface_configuration() const override; + + CARTESIAN_CONTROLLERS_PUBLIC + controller_interface::InterfaceConfiguration state_interface_configuration() const override; + + CARTESIAN_CONTROLLERS_PUBLIC + CallbackReturn on_init() override; + + CARTESIAN_CONTROLLERS_PUBLIC + controller_interface::return_type update( + const rclcpp::Time & time, const rclcpp::Duration & period) override; + + CARTESIAN_CONTROLLERS_PUBLIC + CallbackReturn on_configure(const rclcpp_lifecycle::State & previous_state) override; + + CARTESIAN_CONTROLLERS_PUBLIC + CallbackReturn on_activate(const rclcpp_lifecycle::State & previous_state) override; + + CARTESIAN_CONTROLLERS_PUBLIC + CallbackReturn on_deactivate(const rclcpp_lifecycle::State & previous_state) override; + +protected: + std::string joint_name_; + std::vector interface_names_; + + realtime_tools::RealtimeBuffer> rt_command_ptr_; + rclcpp::Subscription::SharedPtr twist_command_subscriber_; + + std::string logger_name_; +}; + +} // namespace cartesian_controllers + +#endif // CARTESIAN_CONTROLLERS__TWIST_CONTROLLER_HPP_ diff --git a/cartesian_controllers/include/cartesian_controllers/visibility_control.h b/cartesian_controllers/include/cartesian_controllers/visibility_control.h new file mode 100644 index 0000000000..d88af9c751 --- /dev/null +++ b/cartesian_controllers/include/cartesian_controllers/visibility_control.h @@ -0,0 +1,49 @@ +// Copyright 2022 VoodooIT, sole proprietorship +// +// 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. + +#ifndef CARTESIAN_CONTROLLERS__VISIBILITY_CONTROL_H_ +#define CARTESIAN_CONTROLLERS__VISIBILITY_CONTROL_H_ + +// This logic was borrowed (then namespaced) from the examples on the gcc wiki: +// https://gcc.gnu.org/wiki/Visibility + +#if defined _WIN32 || defined __CYGWIN__ +#ifdef __GNUC__ +#define CARTESIAN_CONTROLLERS_EXPORT __attribute__((dllexport)) +#define CARTESIAN_CONTROLLERS_IMPORT __attribute__((dllimport)) +#else +#define CARTESIAN_CONTROLLERS_EXPORT __declspec(dllexport) +#define CARTESIAN_CONTROLLERS_IMPORT __declspec(dllimport) +#endif +#ifdef CARTESIAN_CONTROLLERS_BUILDING_LIBRARY +#define CARTESIAN_CONTROLLERS_PUBLIC CARTESIAN_CONTROLLERS_EXPORT +#else +#define CARTESIAN_CONTROLLERS_PUBLIC CARTESIAN_CONTROLLERS_IMPORT +#endif +#define CARTESIAN_CONTROLLERS_PUBLIC_TYPE CARTESIAN_CONTROLLERS_PUBLIC +#define CARTESIAN_CONTROLLERS_LOCAL +#else +#define CARTESIAN_CONTROLLERS_EXPORT __attribute__((visibility("default"))) +#define CARTESIAN_CONTROLLERS_IMPORT +#if __GNUC__ >= 4 +#define CARTESIAN_CONTROLLERS_PUBLIC __attribute__((visibility("default"))) +#define CARTESIAN_CONTROLLERS_LOCAL __attribute__((visibility("hidden"))) +#else +#define CARTESIAN_CONTROLLERS_PUBLIC +#define CARTESIAN_CONTROLLERS_LOCAL +#endif +#define CARTESIAN_CONTROLLERS_PUBLIC_TYPE +#endif + +#endif // CARTESIAN_CONTROLLERS__VISIBILITY_CONTROL_H_ diff --git a/cartesian_controllers/package.xml b/cartesian_controllers/package.xml new file mode 100644 index 0000000000..0a6fcc23a9 --- /dev/null +++ b/cartesian_controllers/package.xml @@ -0,0 +1,27 @@ + + + + cartesian_controllers + 0.0.0 + Cartesain controllers for ROS2. + Lovro Ivanov + Apache-2.0 + + ament_cmake_ros + + controller_interface + hardware_interface + rclcpp + rclcpp_lifecycle + realtime_tools + geometry_msgs + std_msgs + pluginlib + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/cartesian_controllers/src/twist_controller.cpp b/cartesian_controllers/src/twist_controller.cpp new file mode 100644 index 0000000000..10a8f2bca1 --- /dev/null +++ b/cartesian_controllers/src/twist_controller.cpp @@ -0,0 +1,134 @@ +// Copyright 2022 VoodooIT, sole proprietorship +// +// 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 "cartesian_controllers/twist_controller.hpp" + +namespace cartesian_controllers +{ +TwistController::TwistController() +: controller_interface::ControllerInterface(), + rt_command_ptr_(nullptr), + twist_command_subscriber_(nullptr) +{ +} + +controller_interface::InterfaceConfiguration TwistController::command_interface_configuration() + const +{ + controller_interface::InterfaceConfiguration command_interfaces_config; + command_interfaces_config.type = controller_interface::interface_configuration_type::INDIVIDUAL; + + for (const auto & interface : interface_names_) + { + command_interfaces_config.names.push_back(joint_name_ + "/" + interface); + } +} + +controller_interface::InterfaceConfiguration TwistController::state_interface_configuration() const +{ + return controller_interface::InterfaceConfiguration{ + controller_interface::interface_configuration_type::NONE}; +} + +CallbackReturn TwistController::on_init() +{ + try + { + auto_declare>("interface_names", std::vector()); + + auto_declare("joint", ""); + } + catch (const std::exception & e) + { + fprintf(stderr, "Exception thrown during init stage with message: %s \n", e.what()); + return CallbackReturn::ERROR; + } + + return CallbackReturn::SUCCESS; +} + +controller_interface::return_type TwistController::update( + const rclcpp::Time & time, const rclcpp::Duration & period) +{ + auto twist_commands = rt_command_ptr_.readFromRT(); + + // no command received yet + if (!twist_commands || !(*twist_commands)) + { + return controller_interface::return_type::OK; + } + + if (command_interfaces_.size() != 6) + { + RCLCPP_ERROR_THROTTLE( + get_node()->get_logger(), *node_->get_clock(), 1000, + "Twist controller needs does not match number of interfaces needed 6, given (%zu) interfaces", + command_interfaces_.size()); + return controller_interface::return_type::ERROR; + } + command_interfaces_[0].set_value((*twist_commands)->twist.linear.x); + command_interfaces_[1].set_value((*twist_commands)->twist.linear.y); + command_interfaces_[2].set_value((*twist_commands)->twist.linear.z); + command_interfaces_[3].set_value((*twist_commands)->twist.angular.x); + command_interfaces_[4].set_value((*twist_commands)->twist.angular.y); + command_interfaces_[5].set_value((*twist_commands)->twist.angular.z); + + return controller_interface::return_type::OK; +} + +CallbackReturn TwistController::on_configure(const rclcpp_lifecycle::State & previous_state) +{ + joint_name_ = node_->get_parameter("joint").as_string(); + + if (joint_name_.empty()) + { + RCLCPP_ERROR(get_node()->get_logger(), "'joint' parameter was empty"); + return CallbackReturn::ERROR; + } + + // Specialized, child controllers set interfaces before calling configure function. + if (interface_names_.empty()) + { + interface_names_ = node_->get_parameter("interface_names").as_string_array(); + } + + if (interface_names_.empty()) + { + RCLCPP_ERROR(get_node()->get_logger(), "'interface_names' parameter was empty"); + return CallbackReturn::ERROR; + } + + twist_command_subscriber_ = get_node()->create_subscription( + "~/commands", rclcpp::SystemDefaultsQoS(), + [this](const CmdType::SharedPtr msg) { rt_command_ptr_.writeFromNonRT(msg); }); + + RCLCPP_INFO(get_node()->get_logger(), "configure successful"); + return CallbackReturn::SUCCESS; +} + +CallbackReturn TwistController::on_activate(const rclcpp_lifecycle::State & previous_state) +{ + // reset command buffer if a command came through callback when controller was inactive + rt_command_ptr_ = realtime_tools::RealtimeBuffer>(nullptr); + return CallbackReturn::SUCCESS; +} + +CallbackReturn TwistController::on_deactivate(const rclcpp_lifecycle::State & previous_state) +{ + // reset command buffer + rt_command_ptr_ = realtime_tools::RealtimeBuffer>(nullptr); + return CallbackReturn::SUCCESS; +} + +} // namespace cartesian_controllers diff --git a/cartesian_controllers/twist_controller_plugin.xml b/cartesian_controllers/twist_controller_plugin.xml new file mode 100644 index 0000000000..0bd1d36c5f --- /dev/null +++ b/cartesian_controllers/twist_controller_plugin.xml @@ -0,0 +1,7 @@ + + + + 6dof pure forwarding twist controller. + + + From 9c6d2c196808f2a0e31b0423aa6234ac2edb8838 Mon Sep 17 00:00:00 2001 From: Lovro Date: Wed, 23 Feb 2022 13:16:09 +0100 Subject: [PATCH 02/10] Remove unused-parameter and return-type warnings. --- cartesian_controllers/src/twist_controller.cpp | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) diff --git a/cartesian_controllers/src/twist_controller.cpp b/cartesian_controllers/src/twist_controller.cpp index 10a8f2bca1..bcf6c1469f 100644 --- a/cartesian_controllers/src/twist_controller.cpp +++ b/cartesian_controllers/src/twist_controller.cpp @@ -33,6 +33,8 @@ controller_interface::InterfaceConfiguration TwistController::command_interface_ { command_interfaces_config.names.push_back(joint_name_ + "/" + interface); } + + return command_interfaces_config; } controller_interface::InterfaceConfiguration TwistController::state_interface_configuration() const @@ -59,7 +61,7 @@ CallbackReturn TwistController::on_init() } controller_interface::return_type TwistController::update( - const rclcpp::Time & time, const rclcpp::Duration & period) + const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) { auto twist_commands = rt_command_ptr_.readFromRT(); @@ -87,7 +89,7 @@ controller_interface::return_type TwistController::update( return controller_interface::return_type::OK; } -CallbackReturn TwistController::on_configure(const rclcpp_lifecycle::State & previous_state) +CallbackReturn TwistController::on_configure(const rclcpp_lifecycle::State & /*previous_state*/) { joint_name_ = node_->get_parameter("joint").as_string(); @@ -117,14 +119,14 @@ CallbackReturn TwistController::on_configure(const rclcpp_lifecycle::State & pre return CallbackReturn::SUCCESS; } -CallbackReturn TwistController::on_activate(const rclcpp_lifecycle::State & previous_state) +CallbackReturn TwistController::on_activate(const rclcpp_lifecycle::State & /*previous_state*/) { // reset command buffer if a command came through callback when controller was inactive rt_command_ptr_ = realtime_tools::RealtimeBuffer>(nullptr); return CallbackReturn::SUCCESS; } -CallbackReturn TwistController::on_deactivate(const rclcpp_lifecycle::State & previous_state) +CallbackReturn TwistController::on_deactivate(const rclcpp_lifecycle::State & /*previous_state*/) { // reset command buffer rt_command_ptr_ = realtime_tools::RealtimeBuffer>(nullptr); From b7187c96cdbd7f97dfeb68855c9cd61424819599 Mon Sep 17 00:00:00 2001 From: Lovro Date: Wed, 23 Feb 2022 13:19:29 +0100 Subject: [PATCH 03/10] Add pluginlib export based on ControllerInterface. --- cartesian_controllers/src/twist_controller.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/cartesian_controllers/src/twist_controller.cpp b/cartesian_controllers/src/twist_controller.cpp index bcf6c1469f..e471f7ad81 100644 --- a/cartesian_controllers/src/twist_controller.cpp +++ b/cartesian_controllers/src/twist_controller.cpp @@ -134,3 +134,7 @@ CallbackReturn TwistController::on_deactivate(const rclcpp_lifecycle::State & /* } } // namespace cartesian_controllers +#include "pluginlib/class_list_macros.hpp" + +PLUGINLIB_EXPORT_CLASS( + cartesian_controllers::TwistController, controller_interface::ControllerInterface) From 4bf2cc52444f02f16d9f354c018120403c3e1ffa Mon Sep 17 00:00:00 2001 From: Lovro Date: Wed, 23 Feb 2022 14:54:06 +0100 Subject: [PATCH 04/10] Add cartesian controllers test. --- cartesian_controllers/CMakeLists.txt | 18 ++ .../test/test_load_twist_controller.cpp | 41 ++++ .../test/test_twist_controller.cpp | 179 +++++++++++++++ .../test/test_twist_controller.hpp | 208 ++++++++++++++++++ 4 files changed, 446 insertions(+) create mode 100644 cartesian_controllers/test/test_load_twist_controller.cpp create mode 100644 cartesian_controllers/test/test_twist_controller.cpp create mode 100644 cartesian_controllers/test/test_twist_controller.hpp diff --git a/cartesian_controllers/CMakeLists.txt b/cartesian_controllers/CMakeLists.txt index 077c3c4c03..faaafa57ab 100644 --- a/cartesian_controllers/CMakeLists.txt +++ b/cartesian_controllers/CMakeLists.txt @@ -60,6 +60,24 @@ if(BUILD_TESTING) find_package(controller_manager REQUIRED) find_package(hardware_interface REQUIRED) find_package(ros2_control_test_assets REQUIRED) + + ament_add_gmock(test_load_twist_controller test/test_load_twist_controller.cpp) + target_include_directories(test_load_twist_controller PRIVATE include) + ament_target_dependencies( + test_load_twist_controller + controller_manager + hardware_interface + ros2_control_test_assets + ) + + ament_add_gmock(test_twist_controller test/test_twist_controller.cpp) + target_include_directories(test_twist_controller PRIVATE include) + target_link_libraries(test_twist_controller twist_controller) + ament_target_dependencies( + test_twist_controller + controller_interface + hardware_interface + ) endif() ament_export_dependencies( diff --git a/cartesian_controllers/test/test_load_twist_controller.cpp b/cartesian_controllers/test/test_load_twist_controller.cpp new file mode 100644 index 0000000000..d1e15272a4 --- /dev/null +++ b/cartesian_controllers/test/test_load_twist_controller.cpp @@ -0,0 +1,41 @@ +// Copyright 2022 VoodooIT, sole proprietorship +// +// 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 "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(TestLoadTwistController, load_controller) +{ + rclcpp::init(0, nullptr); + + 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"); + + ASSERT_NO_THROW(cm.load_controller("test_twist_controller", "cartesian_controllers/TwistController")); + + rclcpp::shutdown(); +} diff --git a/cartesian_controllers/test/test_twist_controller.cpp b/cartesian_controllers/test/test_twist_controller.cpp new file mode 100644 index 0000000000..47633306e2 --- /dev/null +++ b/cartesian_controllers/test/test_twist_controller.cpp @@ -0,0 +1,179 @@ +// Copyright 2022 VoodooIT, sole proprietorship +// +// 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 "test_twist_controller.hpp" + +#include +#include +#include +#include + +// When there are many mandatory parameters, set all by default and remove one by one in a +// parameterized test +TEST_P(TwistControllerTestParameterizedParameters, one_parameter_is_missing) +{ + SetUpController(); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_ERROR); +} + +// TODO(anyone): the new gtest version after 1.8.0 uses INSTANTIATE_TEST_SUITE_P +INSTANTIATE_TEST_SUITE_P( + MissingMandatoryParameterDuringConfiguration, TwistControllerTestParameterizedParameters, + ::testing::Values( + std::make_tuple(std::string("joint"), rclcpp::ParameterValue(std::string())), + std::make_tuple(std::string("interface_names"), rclcpp::ParameterValue(std::vector({}))))); + +TEST_F(TwistControllerTest, joint_names_parameter_not_set) +{ + SetUpController(false); + + ASSERT_TRUE(controller_->joint_name_.empty()); + ASSERT_TRUE(controller_->interface_names_.empty()); + + controller_->get_node()->set_parameter({"joint", joint_name_}); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_ERROR); + + ASSERT_TRUE(controller_->joint_name_.empty()); + ASSERT_TRUE(controller_->interface_names_.empty()); +} + +TEST_F(TwistControllerTest, interface_parameter_not_set) +{ + SetUpController(false); + + controller_->get_node()->set_parameter({"interface_names", interface_names_}); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_ERROR); + + ASSERT_TRUE(!controller_->joint_name_.empty()); + ASSERT_TRUE(controller_->interface_names_.empty()); +} + +TEST_F(TwistControllerTest, all_parameters_set_configure_success) +{ + SetUpController(); + + ASSERT_TRUE(controller_->joint_name_.empty()); + ASSERT_TRUE(controller_->interface_names_.empty()); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + + ASSERT_TRUE(!controller_->joint_name_.empty()); + ASSERT_TRUE(controller_->joint_name_.size() == joint_name_.size()); + ASSERT_TRUE(controller_->joint_name_== joint_name_); + + ASSERT_TRUE(!controller_->interface_names_.empty()); + ASSERT_TRUE(std::equal(controller_->interface_names_.begin(), controller_->interface_names_.end(), + interface_names_.begin(), interface_names_.end())); +} + +TEST_F(TwistControllerTest, check_intefaces) +{ + SetUpController(); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + + auto command_intefaces = controller_->command_interface_configuration(); + ASSERT_EQ(command_intefaces.names.size(), joint_command_values_.size()); + +} + +TEST_F(TwistControllerTest, activate_success) +{ + SetUpController(); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); +} + +TEST_F(TwistControllerTest, update_success) +{ + SetUpController(); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + + rclcpp::Time node_time = controller_->get_node()->now(); + rclcpp::Duration duration = rclcpp::Duration::from_nanoseconds(1000000); // 1ms + ASSERT_EQ(controller_->update(node_time, duration), controller_interface::return_type::OK); +} + +TEST_F(TwistControllerTest, 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(TwistControllerTest, 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); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + + rclcpp::Time node_time = controller_->get_node()->now(); + rclcpp::Duration duration = rclcpp::Duration::from_nanoseconds(1000000); // 1ms + ASSERT_EQ(controller_->update(node_time, duration), controller_interface::return_type::OK); +} + +TEST_F(TwistControllerTest, command_callback_test) +{ + SetUpController(); + + // default values + ASSERT_EQ(command_itfs_.size(), 6u); + + ASSERT_EQ(command_itfs_[0].get_value(), 0.0); + ASSERT_EQ(command_itfs_[1].get_value(), 0.0); + ASSERT_EQ(command_itfs_[2].get_value(), 0.1); + ASSERT_EQ(command_itfs_[3].get_value(), 0.0); + ASSERT_EQ(command_itfs_[4].get_value(), 0.0); + ASSERT_EQ(command_itfs_[5].get_value(), 0.0); + + auto node_state = controller_->configure(); + ASSERT_EQ(node_state.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); + + node_state = controller_->activate(); + ASSERT_EQ(node_state.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); + + // send a new command + publish_commands(); + + // wait for command message to be passed + ASSERT_EQ(wait_for(controller_->twist_command_subscriber_), rclcpp::WaitResultKind::Ready); + + // process callbacks + rclcpp::spin_some(controller_->get_node()->get_node_base_interface()); + + // update successful + ASSERT_EQ( + controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + // check command in handle was set + ASSERT_EQ(command_itfs_[0].get_value(), 10.0); + ASSERT_EQ(command_itfs_[1].get_value(), 10.0); + ASSERT_EQ(command_itfs_[2].get_value(), 10.0); + ASSERT_EQ(command_itfs_[3].get_value(), 0.1); + ASSERT_EQ(command_itfs_[4].get_value(), 0.1); + ASSERT_EQ(command_itfs_[5].get_value(), 0.1); +} + diff --git a/cartesian_controllers/test/test_twist_controller.hpp b/cartesian_controllers/test/test_twist_controller.hpp new file mode 100644 index 0000000000..cf508563fd --- /dev/null +++ b/cartesian_controllers/test/test_twist_controller.hpp @@ -0,0 +1,208 @@ +// Copyright 2022 VoodooIT, sole proprietorship +// +// 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. + +#ifndef TEST_TWIST_CONTROLLER_HPP_ +#define TEST_TWIST_CONTROLLER_HPP_ + +#include +#include +#include +#include +#include +#include + +#include "cartesian_controllers/twist_controller.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/time.hpp" +#include "rclcpp/utilities.hpp" +#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" + + +using ControllerCommandMsg = geometry_msgs::msg::TwistStamped; + +namespace +{ +constexpr auto NODE_SUCCESS = + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; +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 TestableTwistController : public cartesian_controllers::TwistController +{ + FRIEND_TEST(TwistControllerTest, joint_names_parameter_not_set); + FRIEND_TEST(TwistControllerTest, interface_parameter_not_set); + FRIEND_TEST(TwistControllerTest, all_parameters_set_configure_success); + FRIEND_TEST(TwistControllerTest, check_intefaces); + FRIEND_TEST(TwistControllerTest, activate_success); + FRIEND_TEST(TwistControllerTest, update_success); + FRIEND_TEST(TwistControllerTest, deactivate_success); + FRIEND_TEST(TwistControllerTest, reactivate_success); + FRIEND_TEST(TwistControllerTest, command_callback_test); + +public: + CallbackReturn on_configure(const rclcpp_lifecycle::State & previous_state) override + { + auto ret = cartesian_controllers::TwistController::on_configure(previous_state); + // Only if on_configure is successful create subscription + if (ret == CallbackReturn::SUCCESS) { + command_subscriber_wait_set_.add_subscription(twist_command_subscriber_); + } + return ret; + } + + /** + * @brief wait_for_command 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 = + command_subscriber_wait_set_.wait(timeout).kind() == rclcpp::WaitResultKind::Ready; + if (success) { + executor.spin_some(); + } + return success; + } + +private: + rclcpp::WaitSet command_subscriber_wait_set_; +}; + +class TwistControllerTest : 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"); + command_publisher_ = command_publisher_node_->create_publisher( + "/test_twist_controller/commands", rclcpp::SystemDefaultsQoS()); + } + + static void TearDownTestCase() { rclcpp::shutdown(); } + + void TearDown() { controller_.reset(nullptr); } + +protected: + void SetUpController(bool set_parameters = true) + { + const auto result = controller_->init("test_twist_controller"); + ASSERT_EQ(result, controller_interface::return_type::OK); + + 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_name_, interface_names_[i], &joint_command_values_[i])); + command_ifs.emplace_back(command_itfs_.back()); + } + // TODO(anyone): Add other command interfaces, if any + + std::vector state_ifs; + + controller_->assign_interfaces(std::move(command_ifs), std::move(state_ifs)); + + if (set_parameters) { + controller_->get_node()->set_parameter({"joint", joint_name_}); + controller_->get_node()->set_parameter({"interface_name", interface_names_}); + } + } + + 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; + } + }; + + wait_for_topic(command_publisher_->get_topic_name()); + + ControllerCommandMsg msg; + msg.header.frame_id = "twist_frame_id"; + msg.header.stamp = controller_->get_node()->get_clock()->now(); + msg.twist.linear.x = msg.twist.linear.y = msg.twist.linear.z = 10.0; + msg.twist.angular.x = msg.twist.angular.y = msg.twist.angular.z = 0.1; + + command_publisher_->publish(msg); + } + +protected: + // TODO(anyone): adjust the members as needed + + // Controller-related parameters + std::string joint_name_ = "tcp"; + std::vector interface_names_ = {"linear.x", "linear.y", "linear.z", "angular.x", "angular.y", "angular.z" }; + std::array joint_command_values_ = {0.0, 0.0, 0.1, 0.0, 0.0, 0.0}; + + std::vector command_itfs_; + + // Test related parameters + std::unique_ptr controller_; + rclcpp::Node::SharedPtr command_publisher_node_; + rclcpp::Publisher::SharedPtr command_publisher_; +}; + +// From the tutorial: https://www.sandordargo.com/blog/2019/04/24/parameterized-testing-with-gtest +class TwistControllerTestParameterizedParameters +: public TwistControllerTest, + public ::testing::WithParamInterface> +{ +public: + virtual void SetUp() { TwistControllerTest::SetUp(); } + + static void TearDownTestCase() { TwistControllerTest::TearDownTestCase(); } + +protected: + void SetUpController(bool set_parameters = true) + { + TwistControllerTest::SetUpController(set_parameters); + controller_->get_node()->set_parameter({std::get<0>(GetParam()), std::get<1>(GetParam())}); + } +}; + +#endif // TEST_TWIST_CONTROLLER_HPP_ From 8f31cf7355b672802c963a5123c5ebcfb2cfe722 Mon Sep 17 00:00:00 2001 From: Lovro Date: Wed, 23 Feb 2022 17:52:50 +0100 Subject: [PATCH 05/10] Correct package.xml/=. --- cartesian_controllers/package.xml | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/cartesian_controllers/package.xml b/cartesian_controllers/package.xml index 0a6fcc23a9..86092e2b00 100644 --- a/cartesian_controllers/package.xml +++ b/cartesian_controllers/package.xml @@ -7,7 +7,7 @@ Lovro Ivanov Apache-2.0 - ament_cmake_ros + ament_cmake controller_interface hardware_interface @@ -18,8 +18,9 @@ std_msgs pluginlib - ament_lint_auto - ament_lint_common + ament_cmake_gmock + controller_manager + ros2_control_test_assets ament_cmake From 5bda0a833b01b823c6bcf807196fcd5cd01bab35 Mon Sep 17 00:00:00 2001 From: Alex Moriarty Date: Mon, 1 May 2023 15:26:25 -0300 Subject: [PATCH 06/10] Twist Controller sync for open sourcing This Commit builds on a rebased PR https://github.com/ros-controls/ros2_controllers/pull/300 The Original author @livanov93 was in 2022 open sourcing part of a PickNik package form 2021. This commit brings the internal picknik and previously open PR #300 back in sync and rebases the open PR onto humble. --- .../twist_controller.hpp | 14 ++-- .../visibility_control.h | 2 +- .../src/twist_controller.cpp | 73 +++++++++++-------- .../test/test_twist_controller.cpp | 3 +- 4 files changed, 51 insertions(+), 41 deletions(-) diff --git a/cartesian_controllers/include/cartesian_controllers/twist_controller.hpp b/cartesian_controllers/include/cartesian_controllers/twist_controller.hpp index 0bc836a001..158ddebd03 100644 --- a/cartesian_controllers/include/cartesian_controllers/twist_controller.hpp +++ b/cartesian_controllers/include/cartesian_controllers/twist_controller.hpp @@ -1,4 +1,4 @@ -// Copyright 2022 VoodooIT, sole proprietorship +// Copyright 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. @@ -21,14 +21,14 @@ #include "controller_interface/controller_interface.hpp" #include "geometry_msgs/msg/twist_stamped.hpp" -#include "realtime_tools/realtime_buffer.h" - #include "cartesian_controllers/visibility_control.h" +#include "realtime_tools/realtime_buffer.h" namespace cartesian_controllers { using CmdType = geometry_msgs::msg::TwistStamped; using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; + class TwistController : public controller_interface::ControllerInterface { public: @@ -44,10 +44,6 @@ class TwistController : public controller_interface::ControllerInterface CARTESIAN_CONTROLLERS_PUBLIC CallbackReturn on_init() override; - CARTESIAN_CONTROLLERS_PUBLIC - controller_interface::return_type update( - const rclcpp::Time & time, const rclcpp::Duration & period) override; - CARTESIAN_CONTROLLERS_PUBLIC CallbackReturn on_configure(const rclcpp_lifecycle::State & previous_state) override; @@ -57,6 +53,10 @@ class TwistController : public controller_interface::ControllerInterface CARTESIAN_CONTROLLERS_PUBLIC CallbackReturn on_deactivate(const rclcpp_lifecycle::State & previous_state) override; + CARTESIAN_CONTROLLERS_PUBLIC + controller_interface::return_type update( + const rclcpp::Time & time, const rclcpp::Duration & period) override; + protected: std::string joint_name_; std::vector interface_names_; diff --git a/cartesian_controllers/include/cartesian_controllers/visibility_control.h b/cartesian_controllers/include/cartesian_controllers/visibility_control.h index d88af9c751..e8caed36d8 100644 --- a/cartesian_controllers/include/cartesian_controllers/visibility_control.h +++ b/cartesian_controllers/include/cartesian_controllers/visibility_control.h @@ -1,4 +1,4 @@ -// Copyright 2022 VoodooIT, sole proprietorship +// Copyright 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. diff --git a/cartesian_controllers/src/twist_controller.cpp b/cartesian_controllers/src/twist_controller.cpp index e471f7ad81..9fef84c349 100644 --- a/cartesian_controllers/src/twist_controller.cpp +++ b/cartesian_controllers/src/twist_controller.cpp @@ -1,4 +1,4 @@ -// Copyright 2022 VoodooIT, sole proprietorship +// Copyright 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. @@ -14,8 +14,17 @@ #include "cartesian_controllers/twist_controller.hpp" +#include +#include +#include + +#include "controller_interface/helpers.hpp" +#include "hardware_interface/loaned_command_interface.hpp" + namespace cartesian_controllers { +using hardware_interface::LoanedCommandInterface; + TwistController::TwistController() : controller_interface::ControllerInterface(), rt_command_ptr_(nullptr), @@ -60,38 +69,9 @@ CallbackReturn TwistController::on_init() return CallbackReturn::SUCCESS; } -controller_interface::return_type TwistController::update( - const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) -{ - auto twist_commands = rt_command_ptr_.readFromRT(); - - // no command received yet - if (!twist_commands || !(*twist_commands)) - { - return controller_interface::return_type::OK; - } - - if (command_interfaces_.size() != 6) - { - RCLCPP_ERROR_THROTTLE( - get_node()->get_logger(), *node_->get_clock(), 1000, - "Twist controller needs does not match number of interfaces needed 6, given (%zu) interfaces", - command_interfaces_.size()); - return controller_interface::return_type::ERROR; - } - command_interfaces_[0].set_value((*twist_commands)->twist.linear.x); - command_interfaces_[1].set_value((*twist_commands)->twist.linear.y); - command_interfaces_[2].set_value((*twist_commands)->twist.linear.z); - command_interfaces_[3].set_value((*twist_commands)->twist.angular.x); - command_interfaces_[4].set_value((*twist_commands)->twist.angular.y); - command_interfaces_[5].set_value((*twist_commands)->twist.angular.z); - - return controller_interface::return_type::OK; -} - CallbackReturn TwistController::on_configure(const rclcpp_lifecycle::State & /*previous_state*/) { - joint_name_ = node_->get_parameter("joint").as_string(); + joint_name_ = get_node()->get_parameter("joint").as_string(); if (joint_name_.empty()) { @@ -102,7 +82,7 @@ CallbackReturn TwistController::on_configure(const rclcpp_lifecycle::State & /*p // Specialized, child controllers set interfaces before calling configure function. if (interface_names_.empty()) { - interface_names_ = node_->get_parameter("interface_names").as_string_array(); + interface_names_ = get_node()->get_parameter("interface_names").as_string_array(); } if (interface_names_.empty()) @@ -133,7 +113,36 @@ CallbackReturn TwistController::on_deactivate(const rclcpp_lifecycle::State & /* return CallbackReturn::SUCCESS; } +controller_interface::return_type TwistController::update( + const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) +{ + auto twist_commands = rt_command_ptr_.readFromRT(); + + // no command received yet + if (!twist_commands || !(*twist_commands)) + { + return controller_interface::return_type::OK; + } + + if (command_interfaces_.size() != 6) + { + RCLCPP_ERROR_THROTTLE( + get_node()->get_logger(), *get_node()->get_clock(), 1000, + "Twist controller needs does not match number of interfaces needed 6, given (%zu) interfaces", + command_interfaces_.size()); + return controller_interface::return_type::ERROR; + } + command_interfaces_[0].set_value((*twist_commands)->twist.linear.x); + command_interfaces_[1].set_value((*twist_commands)->twist.linear.y); + command_interfaces_[2].set_value((*twist_commands)->twist.linear.z); + command_interfaces_[3].set_value((*twist_commands)->twist.angular.x); + command_interfaces_[4].set_value((*twist_commands)->twist.angular.y); + command_interfaces_[5].set_value((*twist_commands)->twist.angular.z); + + return controller_interface::return_type::OK; +} } // namespace cartesian_controllers + #include "pluginlib/class_list_macros.hpp" PLUGINLIB_EXPORT_CLASS( diff --git a/cartesian_controllers/test/test_twist_controller.cpp b/cartesian_controllers/test/test_twist_controller.cpp index 47633306e2..860ea3f3df 100644 --- a/cartesian_controllers/test/test_twist_controller.cpp +++ b/cartesian_controllers/test/test_twist_controller.cpp @@ -14,6 +14,7 @@ #include "test_twist_controller.hpp" +#include #include #include #include @@ -151,7 +152,7 @@ TEST_F(TwistControllerTest, command_callback_test) auto node_state = controller_->configure(); ASSERT_EQ(node_state.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); - node_state = controller_->activate(); + node_state = controller_->get_node()->activate(); ASSERT_EQ(node_state.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); // send a new command From c312b69b78fcc641d6610a96603d276d17a7d48a Mon Sep 17 00:00:00 2001 From: Alex Moriarty Date: Tue, 2 May 2023 15:00:42 -0300 Subject: [PATCH 07/10] fix typo in Twist Controller Tests Signed-off-by: Alex Moriarty --- cartesian_controllers/test/test_twist_controller.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/cartesian_controllers/test/test_twist_controller.hpp b/cartesian_controllers/test/test_twist_controller.hpp index cf508563fd..49242b500c 100644 --- a/cartesian_controllers/test/test_twist_controller.hpp +++ b/cartesian_controllers/test/test_twist_controller.hpp @@ -141,7 +141,7 @@ class TwistControllerTest : public ::testing::Test if (set_parameters) { controller_->get_node()->set_parameter({"joint", joint_name_}); - controller_->get_node()->set_parameter({"interface_name", interface_names_}); + controller_->get_node()->set_parameter({"interface_names", interface_names_}); } } From 5c392252565cb95dd690b3fc4e5e02edde6016c6 Mon Sep 17 00:00:00 2001 From: Alex Moriarty Date: Tue, 2 May 2023 22:35:32 -0300 Subject: [PATCH 08/10] Twist Controller fixup old tests Unclear what the state of these tests were from original PR and if they were finished because it was marked as Work in Progress. This change makes the tests pass Signed-off-by: Alex Moriarty --- .../test/test_twist_controller.cpp | 17 +++++++---------- 1 file changed, 7 insertions(+), 10 deletions(-) diff --git a/cartesian_controllers/test/test_twist_controller.cpp b/cartesian_controllers/test/test_twist_controller.cpp index 860ea3f3df..fae69fee62 100644 --- a/cartesian_controllers/test/test_twist_controller.cpp +++ b/cartesian_controllers/test/test_twist_controller.cpp @@ -43,7 +43,7 @@ TEST_F(TwistControllerTest, joint_names_parameter_not_set) ASSERT_TRUE(controller_->joint_name_.empty()); ASSERT_TRUE(controller_->interface_names_.empty()); - controller_->get_node()->set_parameter({"joint", joint_name_}); + controller_->get_node()->set_parameter({"interface_names", interface_names_}); ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_ERROR); @@ -55,7 +55,7 @@ TEST_F(TwistControllerTest, interface_parameter_not_set) { SetUpController(false); - controller_->get_node()->set_parameter({"interface_names", interface_names_}); + controller_->get_node()->set_parameter({"joint", joint_name_}); ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_ERROR); @@ -138,6 +138,8 @@ TEST_F(TwistControllerTest, reactivate_success) TEST_F(TwistControllerTest, command_callback_test) { SetUpController(); + rclcpp::executors::MultiThreadedExecutor executor; + executor.add_node(controller_->get_node()->get_node_base_interface()); // default values ASSERT_EQ(command_itfs_.size(), 6u); @@ -149,20 +151,15 @@ TEST_F(TwistControllerTest, command_callback_test) ASSERT_EQ(command_itfs_[4].get_value(), 0.0); ASSERT_EQ(command_itfs_[5].get_value(), 0.0); - auto node_state = controller_->configure(); + auto node_state = controller_->get_node()->configure(); ASSERT_EQ(node_state.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); node_state = controller_->get_node()->activate(); ASSERT_EQ(node_state.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); - // send a new command + // send a new command and wait publish_commands(); - - // wait for command message to be passed - ASSERT_EQ(wait_for(controller_->twist_command_subscriber_), rclcpp::WaitResultKind::Ready); - - // process callbacks - rclcpp::spin_some(controller_->get_node()->get_node_base_interface()); + ASSERT_TRUE(controller_->wait_for_commands(executor)); // update successful ASSERT_EQ( From 87717e74e2610fc7d0d0139d39700d8e2b0ca133 Mon Sep 17 00:00:00 2001 From: Alex Moriarty Date: Wed, 3 May 2023 10:45:05 -0300 Subject: [PATCH 09/10] remove unused ament_cmake_ros Signed-off-by: Alex Moriarty --- cartesian_controllers/CMakeLists.txt | 1 - 1 file changed, 1 deletion(-) diff --git a/cartesian_controllers/CMakeLists.txt b/cartesian_controllers/CMakeLists.txt index faaafa57ab..51fa452548 100644 --- a/cartesian_controllers/CMakeLists.txt +++ b/cartesian_controllers/CMakeLists.txt @@ -12,7 +12,6 @@ endif() # find dependencies find_package(ament_cmake REQUIRED) -find_package(ament_cmake_ros REQUIRED) find_package(controller_interface REQUIRED) find_package(hardware_interface REQUIRED) find_package(rclcpp REQUIRED) From d99b3afde0c2ae6b8f079524f030bbfb1abc73a3 Mon Sep 17 00:00:00 2001 From: Alex Moriarty Date: Wed, 3 May 2023 15:09:38 -0300 Subject: [PATCH 10/10] run pre-commit to fixup formatting Signed-off-by: Alex Moriarty --- .../twist_controller.hpp | 2 +- .../test/test_load_twist_controller.cpp | 3 +- .../test/test_twist_controller.cpp | 84 +++++++++---------- .../test/test_twist_controller.hpp | 25 ++++-- 4 files changed, 61 insertions(+), 53 deletions(-) diff --git a/cartesian_controllers/include/cartesian_controllers/twist_controller.hpp b/cartesian_controllers/include/cartesian_controllers/twist_controller.hpp index 158ddebd03..b179746f0a 100644 --- a/cartesian_controllers/include/cartesian_controllers/twist_controller.hpp +++ b/cartesian_controllers/include/cartesian_controllers/twist_controller.hpp @@ -19,9 +19,9 @@ #include #include +#include "cartesian_controllers/visibility_control.h" #include "controller_interface/controller_interface.hpp" #include "geometry_msgs/msg/twist_stamped.hpp" -#include "cartesian_controllers/visibility_control.h" #include "realtime_tools/realtime_buffer.h" namespace cartesian_controllers diff --git a/cartesian_controllers/test/test_load_twist_controller.cpp b/cartesian_controllers/test/test_load_twist_controller.cpp index d1e15272a4..b0a4f21ce7 100644 --- a/cartesian_controllers/test/test_load_twist_controller.cpp +++ b/cartesian_controllers/test/test_load_twist_controller.cpp @@ -35,7 +35,8 @@ TEST(TestLoadTwistController, load_controller) ros2_control_test_assets::minimal_robot_urdf), executor, "test_controller_manager"); - ASSERT_NO_THROW(cm.load_controller("test_twist_controller", "cartesian_controllers/TwistController")); + ASSERT_NO_THROW( + cm.load_controller("test_twist_controller", "cartesian_controllers/TwistController")); rclcpp::shutdown(); } diff --git a/cartesian_controllers/test/test_twist_controller.cpp b/cartesian_controllers/test/test_twist_controller.cpp index fae69fee62..6b6fc4f15d 100644 --- a/cartesian_controllers/test/test_twist_controller.cpp +++ b/cartesian_controllers/test/test_twist_controller.cpp @@ -34,7 +34,8 @@ INSTANTIATE_TEST_SUITE_P( MissingMandatoryParameterDuringConfiguration, TwistControllerTestParameterizedParameters, ::testing::Values( std::make_tuple(std::string("joint"), rclcpp::ParameterValue(std::string())), - std::make_tuple(std::string("interface_names"), rclcpp::ParameterValue(std::vector({}))))); + std::make_tuple( + std::string("interface_names"), rclcpp::ParameterValue(std::vector({}))))); TEST_F(TwistControllerTest, joint_names_parameter_not_set) { @@ -74,11 +75,12 @@ TEST_F(TwistControllerTest, all_parameters_set_configure_success) ASSERT_TRUE(!controller_->joint_name_.empty()); ASSERT_TRUE(controller_->joint_name_.size() == joint_name_.size()); - ASSERT_TRUE(controller_->joint_name_== joint_name_); + ASSERT_TRUE(controller_->joint_name_ == joint_name_); ASSERT_TRUE(!controller_->interface_names_.empty()); - ASSERT_TRUE(std::equal(controller_->interface_names_.begin(), controller_->interface_names_.end(), - interface_names_.begin(), interface_names_.end())); + ASSERT_TRUE(std::equal( + controller_->interface_names_.begin(), controller_->interface_names_.end(), + interface_names_.begin(), interface_names_.end())); } TEST_F(TwistControllerTest, check_intefaces) @@ -89,7 +91,6 @@ TEST_F(TwistControllerTest, check_intefaces) auto command_intefaces = controller_->command_interface_configuration(); ASSERT_EQ(command_intefaces.names.size(), joint_command_values_.size()); - } TEST_F(TwistControllerTest, activate_success) @@ -137,41 +138,40 @@ TEST_F(TwistControllerTest, reactivate_success) TEST_F(TwistControllerTest, command_callback_test) { - SetUpController(); - rclcpp::executors::MultiThreadedExecutor executor; - executor.add_node(controller_->get_node()->get_node_base_interface()); - - // default values - ASSERT_EQ(command_itfs_.size(), 6u); - - ASSERT_EQ(command_itfs_[0].get_value(), 0.0); - ASSERT_EQ(command_itfs_[1].get_value(), 0.0); - ASSERT_EQ(command_itfs_[2].get_value(), 0.1); - ASSERT_EQ(command_itfs_[3].get_value(), 0.0); - ASSERT_EQ(command_itfs_[4].get_value(), 0.0); - ASSERT_EQ(command_itfs_[5].get_value(), 0.0); - - auto node_state = controller_->get_node()->configure(); - ASSERT_EQ(node_state.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); - - node_state = controller_->get_node()->activate(); - ASSERT_EQ(node_state.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); - - // send a new command and wait - publish_commands(); - ASSERT_TRUE(controller_->wait_for_commands(executor)); - - // update successful - ASSERT_EQ( - controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), - controller_interface::return_type::OK); - - // check command in handle was set - ASSERT_EQ(command_itfs_[0].get_value(), 10.0); - ASSERT_EQ(command_itfs_[1].get_value(), 10.0); - ASSERT_EQ(command_itfs_[2].get_value(), 10.0); - ASSERT_EQ(command_itfs_[3].get_value(), 0.1); - ASSERT_EQ(command_itfs_[4].get_value(), 0.1); - ASSERT_EQ(command_itfs_[5].get_value(), 0.1); + SetUpController(); + rclcpp::executors::MultiThreadedExecutor executor; + executor.add_node(controller_->get_node()->get_node_base_interface()); + + // default values + ASSERT_EQ(command_itfs_.size(), 6u); + + ASSERT_EQ(command_itfs_[0].get_value(), 0.0); + ASSERT_EQ(command_itfs_[1].get_value(), 0.0); + ASSERT_EQ(command_itfs_[2].get_value(), 0.1); + ASSERT_EQ(command_itfs_[3].get_value(), 0.0); + ASSERT_EQ(command_itfs_[4].get_value(), 0.0); + ASSERT_EQ(command_itfs_[5].get_value(), 0.0); + + auto node_state = controller_->get_node()->configure(); + ASSERT_EQ(node_state.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); + + node_state = controller_->get_node()->activate(); + ASSERT_EQ(node_state.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); + + // send a new command and wait + publish_commands(); + ASSERT_TRUE(controller_->wait_for_commands(executor)); + + // update successful + ASSERT_EQ( + controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + // check command in handle was set + ASSERT_EQ(command_itfs_[0].get_value(), 10.0); + ASSERT_EQ(command_itfs_[1].get_value(), 10.0); + ASSERT_EQ(command_itfs_[2].get_value(), 10.0); + ASSERT_EQ(command_itfs_[3].get_value(), 0.1); + ASSERT_EQ(command_itfs_[4].get_value(), 0.1); + ASSERT_EQ(command_itfs_[5].get_value(), 0.1); } - diff --git a/cartesian_controllers/test/test_twist_controller.hpp b/cartesian_controllers/test/test_twist_controller.hpp index 49242b500c..6b0fc9a9e3 100644 --- a/cartesian_controllers/test/test_twist_controller.hpp +++ b/cartesian_controllers/test/test_twist_controller.hpp @@ -32,7 +32,6 @@ #include "rclcpp/utilities.hpp" #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" - using ControllerCommandMsg = geometry_msgs::msg::TwistStamped; namespace @@ -70,7 +69,8 @@ class TestableTwistController : public cartesian_controllers::TwistController { auto ret = cartesian_controllers::TwistController::on_configure(previous_state); // Only if on_configure is successful create subscription - if (ret == CallbackReturn::SUCCESS) { + if (ret == CallbackReturn::SUCCESS) + { command_subscriber_wait_set_.add_subscription(twist_command_subscriber_); } return ret; @@ -89,7 +89,8 @@ class TestableTwistController : public cartesian_controllers::TwistController { bool success = command_subscriber_wait_set_.wait(timeout).kind() == rclcpp::WaitResultKind::Ready; - if (success) { + if (success) + { executor.spin_some(); } return success; @@ -128,7 +129,8 @@ class TwistControllerTest : public ::testing::Test command_itfs_.reserve(joint_command_values_.size()); command_ifs.reserve(joint_command_values_.size()); - for (auto i = 0u; i < joint_command_values_.size(); ++i) { + for (auto i = 0u; i < joint_command_values_.size(); ++i) + { command_itfs_.emplace_back(hardware_interface::CommandInterface( joint_name_, interface_names_[i], &joint_command_values_[i])); command_ifs.emplace_back(command_itfs_.back()); @@ -139,7 +141,8 @@ class TwistControllerTest : public ::testing::Test controller_->assign_interfaces(std::move(command_ifs), std::move(state_ifs)); - if (set_parameters) { + if (set_parameters) + { controller_->get_node()->set_parameter({"joint", joint_name_}); controller_->get_node()->set_parameter({"interface_names", interface_names_}); } @@ -147,10 +150,13 @@ class TwistControllerTest : public ::testing::Test void publish_commands() { - auto wait_for_topic = [&](const auto topic_name) { + 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) { + 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); @@ -176,7 +182,8 @@ class TwistControllerTest : public ::testing::Test // Controller-related parameters std::string joint_name_ = "tcp"; - std::vector interface_names_ = {"linear.x", "linear.y", "linear.z", "angular.x", "angular.y", "angular.z" }; + std::vector interface_names_ = {"linear.x", "linear.y", "linear.z", + "angular.x", "angular.y", "angular.z"}; std::array joint_command_values_ = {0.0, 0.0, 0.1, 0.0, 0.0, 0.0}; std::vector command_itfs_;