diff --git a/.github/ISSUE_TEMPLATE/good-first-issue.md b/.github/ISSUE_TEMPLATE/good-first-issue.md index ececdae62c..df11022c50 100644 --- a/.github/ISSUE_TEMPLATE/good-first-issue.md +++ b/.github/ISSUE_TEMPLATE/good-first-issue.md @@ -2,7 +2,7 @@ name: Good first issue about: Create an issue to welcome a new contributor into the community. title: '' -labels: good-first-issue +labels: ["good first issue"] assignees: '' --- diff --git a/controller_interface/CHANGELOG.rst b/controller_interface/CHANGELOG.rst index 9e77ef15b1..32795abf9d 100644 --- a/controller_interface/CHANGELOG.rst +++ b/controller_interface/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package controller_interface ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +4.22.0 (2024-12-20) +------------------- +* Fixed typo. Added s to state_interfaces\_ (`#1930 `_) +* [CI] Add clang job, setup concurrency, use rt_tools humble branch (`#1910 `_) +* Contributors: Christoph Fröhlich, louietouie + 4.21.0 (2024-12-06) ------------------- * [Diagnostics] Add diagnostics of execution time and periodicity of the controllers and controller_manager (`#1871 `_) diff --git a/controller_interface/package.xml b/controller_interface/package.xml index c0ebc15b08..d3a40a227c 100644 --- a/controller_interface/package.xml +++ b/controller_interface/package.xml @@ -2,7 +2,7 @@ controller_interface - 4.21.0 + 4.22.0 Description of controller_interface Bence Magyar Denis Štogl diff --git a/controller_manager/CHANGELOG.rst b/controller_manager/CHANGELOG.rst index 26e56883b8..64b1567dd2 100644 --- a/controller_manager/CHANGELOG.rst +++ b/controller_manager/CHANGELOG.rst @@ -2,6 +2,19 @@ Changelog for package controller_manager ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +4.22.0 (2024-12-20) +------------------- +* Async Hardware Components (`#1567 `_) +* Add controller node options args to be able to set controller specific node arguments (`#1713 `_) +* Use singleton approach to store and reuse the service clients (`#1949 `_) +* Increase the max and min periodicity tolerances to fix flaky tests (`#1937 `_) +* Fix the spawner to support full wildcard parameter entries (`#1933 `_) +* Suppress unnecessary warnings in clock received validation (`#1935 `_) +* Optimize the valid time check in the update loop (`#1923 `_) +* [CI] Add clang job, setup concurrency, use rt_tools humble branch (`#1910 `_) +* Update CPU affinity parameter to be able to set multiple CPUs (`#1915 `_) +* Contributors: Christoph Fröhlich, Sai Kishor Kothakota, Takashi Sato + 4.21.0 (2024-12-06) ------------------- * Use the .hpp headers from realtime_tools package (`#1916 `_) diff --git a/controller_manager/CMakeLists.txt b/controller_manager/CMakeLists.txt index 1b0f308613..69f6ef24df 100644 --- a/controller_manager/CMakeLists.txt +++ b/controller_manager/CMakeLists.txt @@ -58,10 +58,12 @@ target_link_libraries(ros2_control_node PRIVATE if(BUILD_TESTING) find_package(ament_cmake_gmock REQUIRED) find_package(ros2_control_test_assets REQUIRED) + find_package(example_interfaces REQUIRED) # Plugin Libraries that are built and installed for use in testing add_library(test_controller SHARED test/test_controller/test_controller.cpp) target_link_libraries(test_controller PUBLIC controller_manager) + ament_target_dependencies(test_controller PUBLIC example_interfaces) target_compile_definitions(test_controller PRIVATE "CONTROLLER_MANAGER_BUILDING_DLL") pluginlib_export_plugin_description_file(controller_interface test/test_controller/test_controller.xml) install( @@ -210,20 +212,17 @@ if(BUILD_TESTING) ) target_link_libraries(test_hardware_spawner controller_manager + test_controller ros2_control_test_assets::ros2_control_test_assets ) - install(FILES test/test_controller_spawner_with_fallback_controllers.yaml - DESTINATION test) - - install(FILES test/test_controller_spawner_with_type.yaml - DESTINATION test) - - install(FILES test/test_controller_overriding_parameters.yaml - DESTINATION test) - - install(FILES test/test_controller_spawner_wildcard_entries.yaml - DESTINATION test) + install(FILES + test/test_controller_spawner_with_type.yaml + test/test_controller_overriding_parameters.yaml + test/test_controller_spawner_with_fallback_controllers.yaml + test/test_controller_spawner_wildcard_entries.yaml + test/test_controller_spawner_with_interfaces.yaml + DESTINATION test) ament_add_gmock(test_hardware_management_srvs test/test_hardware_management_srvs.cpp diff --git a/controller_manager/controller_manager/spawner.py b/controller_manager/controller_manager/spawner.py index b8abe8469f..58eb7be198 100644 --- a/controller_manager/controller_manager/spawner.py +++ b/controller_manager/controller_manager/spawner.py @@ -26,6 +26,7 @@ load_controller, switch_controllers, unload_controller, + set_controller_parameters, set_controller_parameters_from_param_files, bcolors, ) @@ -145,6 +146,12 @@ def main(args=None): action="store_true", required=False, ) + parser.add_argument( + "--controller-ros-args", + help="The --ros-args to be passed to the controller node for remapping topics etc", + default=None, + required=False, + ) command_line_args = rclpy.utilities.remove_ros_args(args=sys.argv)[1:] args = parser.parse_args(command_line_args) @@ -203,6 +210,15 @@ def main(args=None): + bcolors.ENDC ) else: + if controller_ros_args := args.controller_ros_args: + if not set_controller_parameters( + node, + controller_manager_name, + controller_name, + "node_options_args", + controller_ros_args.split(), + ): + return 1 if param_files: if not set_controller_parameters_from_param_files( node, @@ -254,7 +270,7 @@ def main(args=None): ) if not ret.ok: node.get_logger().error( - bcolors.FAIL + "Failed to activate controller" + bcolors.ENDC + f"{bcolors.FAIL}Failed to activate controller : {controller_name}{bcolors.ENDC}" ) return 1 @@ -279,7 +295,7 @@ def main(args=None): ) if not ret.ok: node.get_logger().error( - bcolors.FAIL + "Failed to activate the parsed controllers list" + bcolors.ENDC + f"{bcolors.FAIL}Failed to activate the parsed controllers list : {controller_names}{bcolors.ENDC}" ) return 1 diff --git a/controller_manager/doc/userdoc.rst b/controller_manager/doc/userdoc.rst index 4cbc7a73a1..f6967930eb 100644 --- a/controller_manager/doc/userdoc.rst +++ b/controller_manager/doc/userdoc.rst @@ -142,7 +142,7 @@ There are two scripts to interact with controller manager from launch files: $ ros2 run controller_manager spawner -h usage: spawner [-h] [-c CONTROLLER_MANAGER] [-p PARAM_FILE] [-n NAMESPACE] [--load-only] [--inactive] [-u] [--controller-manager-timeout CONTROLLER_MANAGER_TIMEOUT] - [--switch-timeout SWITCH_TIMEOUT] [--activate-as-group] [--service-call-timeout SERVICE_CALL_TIMEOUT] + [--switch-timeout SWITCH_TIMEOUT] [--activate-as-group] [--service-call-timeout SERVICE_CALL_TIMEOUT] [--controller-ros-args CONTROLLER_ROS_ARGS] controller_names [controller_names ...] positional arguments: @@ -167,6 +167,8 @@ There are two scripts to interact with controller manager from launch files: Time to wait for a successful state switch of controllers. Useful if controllers cannot be switched immediately, e.g., paused simulations at startup --activate-as-group Activates all the parsed controllers list together instead of one by one. Useful for activating all chainable controllers altogether + --controller-ros-args CONTROLLER_ROS_ARGS + The --ros-args to be passed to the controller node for remapping topics etc The parsed controller config file can follow the same conventions as the typical ROS 2 parameter file format. Now, the spawner can handle config files with wildcard entries and also the controller name in the absolute namespace. See the following examples on the config files: diff --git a/controller_manager/include/controller_manager/controller_manager.hpp b/controller_manager/include/controller_manager/controller_manager.hpp index 332d565238..d33b167997 100644 --- a/controller_manager/include/controller_manager/controller_manager.hpp +++ b/controller_manager/include/controller_manager/controller_manager.hpp @@ -222,15 +222,6 @@ class ControllerManager : public rclcpp::Node CONTROLLER_MANAGER_PUBLIC unsigned int get_update_rate() const; - /// Deletes all async controllers and components. - /** - * Needed to join the threads immediately after the control loop is ended - * to avoid unnecessary iterations. Otherwise - * the threads will be joined only when the controller manager gets destroyed. - */ - CONTROLLER_MANAGER_PUBLIC - void shutdown_async_controllers_and_components(); - protected: CONTROLLER_MANAGER_PUBLIC void init_services(); diff --git a/controller_manager/package.xml b/controller_manager/package.xml index 5447040537..b23ca17d4b 100644 --- a/controller_manager/package.xml +++ b/controller_manager/package.xml @@ -2,7 +2,7 @@ controller_manager - 4.21.0 + 4.22.0 Description of controller_manager Bence Magyar Denis Štogl @@ -36,6 +36,7 @@ python3-coverage hardware_interface_testing ros2_control_test_assets + example_interfaces ament_cmake diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 9f20e2b584..34566f95b3 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -616,6 +616,17 @@ controller_interface::ControllerInterfaceBaseSharedPtr ControllerManager::load_c controller_spec.info.fallback_controllers_names = fallback_controllers; } + const std::string node_options_args_param = controller_name + ".node_options_args"; + std::vector node_options_args; + if (!has_parameter(node_options_args_param)) + { + declare_parameter(node_options_args_param, rclcpp::ParameterType::PARAMETER_STRING_ARRAY); + } + if (get_parameter(node_options_args_param, node_options_args) && !node_options_args.empty()) + { + controller_spec.info.node_options_args = node_options_args; + } + return add_controller_impl(controller_spec); } @@ -1468,6 +1479,7 @@ controller_interface::return_type ControllerManager::switch_controller( to = controllers; // update the claimed interface controller info + auto switch_result = controller_interface::return_type::OK; for (auto & controller : to) { if (is_controller_active(controller.c)) @@ -1488,6 +1500,32 @@ controller_interface::return_type ControllerManager::switch_controller( { controller.info.claimed_interfaces.clear(); } + if ( + std::find(activate_request_.begin(), activate_request_.end(), controller.info.name) != + activate_request_.end()) + { + if (!is_controller_active(controller.c)) + { + RCLCPP_ERROR( + get_logger(), "Could not activate controller : '%s'", controller.info.name.c_str()); + switch_result = controller_interface::return_type::ERROR; + } + } + /// @note The following is the case of the real controllers that are deactivated and doesn't + /// include the chained controllers that are deactivated and activated + if ( + std::find(deactivate_request_.begin(), deactivate_request_.end(), controller.info.name) != + deactivate_request_.end() && + std::find(activate_request_.begin(), activate_request_.end(), controller.info.name) == + activate_request_.end()) + { + if (is_controller_active(controller.c)) + { + RCLCPP_ERROR( + get_logger(), "Could not deactivate controller : '%s'", controller.info.name.c_str()); + switch_result = controller_interface::return_type::ERROR; + } + } } // switch lists @@ -1497,8 +1535,10 @@ controller_interface::return_type ControllerManager::switch_controller( clear_requests(); - RCLCPP_DEBUG(get_logger(), "Successfully switched controllers"); - return controller_interface::return_type::OK; + RCLCPP_DEBUG_EXPRESSION( + get_logger(), switch_result == controller_interface::return_type::OK, + "Successfully switched controllers"); + return switch_result; } controller_interface::ControllerInterfaceBaseSharedPtr ControllerManager::add_controller_impl( @@ -1730,6 +1770,7 @@ void ControllerManager::activate_controllers( get_logger(), "Resource conflict for controller '%s'. Command interface '%s' is already claimed.", controller_name.c_str(), command_interface.c_str()); + command_loans.clear(); assignment_successful = false; break; } @@ -1744,6 +1785,7 @@ void ControllerManager::activate_controllers( "Caught exception of type : %s while claiming the command interfaces. Can't activate " "controller '%s': %s", typeid(e).name(), controller_name.c_str(), e.what()); + command_loans.clear(); assignment_successful = false; break; } @@ -1813,6 +1855,7 @@ void ControllerManager::activate_controllers( RCLCPP_ERROR( get_logger(), "Caught exception of type : %s while activating the controller '%s': %s", typeid(e).name(), controller_name.c_str(), e.what()); + controller->release_interfaces(); continue; } catch (...) @@ -1820,6 +1863,7 @@ void ControllerManager::activate_controllers( RCLCPP_ERROR( get_logger(), "Caught unknown exception while activating the controller '%s'", controller_name.c_str()); + controller->release_interfaces(); continue; } @@ -2681,11 +2725,6 @@ std::pair ControllerManager::split_command_interface( unsigned int ControllerManager::get_update_rate() const { return update_rate_; } -void ControllerManager::shutdown_async_controllers_and_components() -{ - resource_manager_->shutdown_async_components(); -} - void ControllerManager::propagate_deactivation_of_chained_mode( const std::vector & controllers) { @@ -3453,6 +3492,18 @@ rclcpp::NodeOptions ControllerManager::determine_controller_node_options( node_options_arguments.push_back(arg); } + // Add deprecation notice if the arguments are from the controller_manager node + if ( + check_for_element(node_options_arguments, RCL_REMAP_FLAG) || + check_for_element(node_options_arguments, RCL_SHORT_REMAP_FLAG)) + { + RCLCPP_WARN( + get_logger(), + "The use of remapping arguments to the controller_manager node is deprecated. Please use the " + "'--controller-ros-args' argument of the spawner to pass remapping arguments to the " + "controller node."); + } + for (const auto & parameters_file : controller.info.parameters_files) { if (!check_for_element(node_options_arguments, RCL_ROS_ARGS_FLAG)) @@ -3475,6 +3526,18 @@ rclcpp::NodeOptions ControllerManager::determine_controller_node_options( node_options_arguments.push_back("use_sim_time:=true"); } + // Add options parsed through the spawner + if ( + !controller.info.node_options_args.empty() && + !check_for_element(controller.info.node_options_args, RCL_ROS_ARGS_FLAG)) + { + node_options_arguments.push_back(RCL_ROS_ARGS_FLAG); + } + for (const auto & arg : controller.info.node_options_args) + { + node_options_arguments.push_back(arg); + } + std::string arguments; arguments.reserve(1000); for (const auto & arg : node_options_arguments) diff --git a/controller_manager/src/ros2_control_node.cpp b/controller_manager/src/ros2_control_node.cpp index 6bff653044..a8f8c079c3 100644 --- a/controller_manager/src/ros2_control_node.cpp +++ b/controller_manager/src/ros2_control_node.cpp @@ -147,8 +147,6 @@ int main(int argc, char ** argv) std::this_thread::sleep_until(next_iteration_time); } } - - cm->shutdown_async_controllers_and_components(); }); executor->add_node(cm); diff --git a/controller_manager/test/test_controller/test_controller.cpp b/controller_manager/test/test_controller/test_controller.cpp index 04ae8c02c2..ee8377f2a3 100644 --- a/controller_manager/test/test_controller/test_controller.cpp +++ b/controller_manager/test/test_controller/test_controller.cpp @@ -87,7 +87,7 @@ controller_interface::return_type TestController::update( command_interfaces_[i].get_name().c_str()); return controller_interface::return_type::ERROR; } - RCLCPP_INFO( + RCLCPP_DEBUG( get_node()->get_logger(), "Setting value of command interface '%s' to %f", command_interfaces_[i].get_name().c_str(), external_commands_for_testing_[i]); command_interfaces_[i].set_value(external_commands_for_testing_[i]); @@ -101,6 +101,48 @@ CallbackReturn TestController::on_init() { return CallbackReturn::SUCCESS; } CallbackReturn TestController::on_configure(const rclcpp_lifecycle::State & /*previous_state*/) { + auto ctrl_node = get_node(); + if (!ctrl_node->has_parameter("command_interfaces")) + { + ctrl_node->declare_parameter("command_interfaces", std::vector({})); + } + if (!ctrl_node->has_parameter("state_interfaces")) + { + ctrl_node->declare_parameter("state_interfaces", std::vector({})); + } + const std::vector command_interfaces = + ctrl_node->get_parameter("command_interfaces").as_string_array(); + const std::vector state_interfaces = + ctrl_node->get_parameter("state_interfaces").as_string_array(); + if (!command_interfaces.empty() || !state_interfaces.empty()) + { + cmd_iface_cfg_.names.clear(); + state_iface_cfg_.names.clear(); + for (const auto & cmd_itf : command_interfaces) + { + cmd_iface_cfg_.names.push_back(cmd_itf); + } + cmd_iface_cfg_.type = controller_interface::interface_configuration_type::INDIVIDUAL; + external_commands_for_testing_.resize(command_interfaces.size(), 0.0); + for (const auto & state_itf : state_interfaces) + { + state_iface_cfg_.names.push_back(state_itf); + } + state_iface_cfg_.type = controller_interface::interface_configuration_type::INDIVIDUAL; + } + + const std::string service_name = get_node()->get_name() + std::string("/set_bool"); + service_ = get_node()->create_service( + service_name, + [this]( + const std::shared_ptr request, + std::shared_ptr response) + { + RCLCPP_INFO_STREAM( + get_node()->get_logger(), "Setting response to " << std::boolalpha << request->data); + response->success = request->data; + }); + return CallbackReturn::SUCCESS; } diff --git a/controller_manager/test/test_controller/test_controller.hpp b/controller_manager/test/test_controller/test_controller.hpp index d57fd9ddd9..ee9e668cfa 100644 --- a/controller_manager/test/test_controller/test_controller.hpp +++ b/controller_manager/test/test_controller/test_controller.hpp @@ -15,11 +15,14 @@ #ifndef TEST_CONTROLLER__TEST_CONTROLLER_HPP_ #define TEST_CONTROLLER__TEST_CONTROLLER_HPP_ +#include #include #include #include "controller_interface/controller_interface.hpp" #include "controller_manager/visibility_control.h" +#include "example_interfaces/srv/set_bool.hpp" +#include "rclcpp/rclcpp.hpp" #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" namespace test_controller @@ -68,10 +71,9 @@ class TestController : public controller_interface::ControllerInterface CONTROLLER_MANAGER_PUBLIC std::vector get_state_interface_data() const; - const std::string & getRobotDescription() const; - void set_external_commands_for_testing(const std::vector & commands); + rclcpp::Service::SharedPtr service_; unsigned int internal_counter = 0; bool simulate_cleanup_failure = false; // Variable where we store when cleanup was called, pointer because the controller diff --git a/controller_manager/test/test_controller_spawner_with_interfaces.yaml b/controller_manager/test/test_controller_spawner_with_interfaces.yaml new file mode 100644 index 0000000000..05b650c232 --- /dev/null +++ b/controller_manager/test/test_controller_spawner_with_interfaces.yaml @@ -0,0 +1,25 @@ +ctrl_with_joint2_command_interface: + ros__parameters: + type: "controller_manager/test_controller" + command_interfaces: + - "joint2/velocity" + +ctrl_with_joint1_command_interface: + ros__parameters: + type: "controller_manager/test_controller" + command_interfaces: + - "joint1/position" + +ctrl_with_joint1_and_joint2_command_interfaces: + ros__parameters: + type: "controller_manager/test_controller" + command_interfaces: + - "joint1/position" + - "joint2/velocity" + +ctrl_with_state_interfaces: + ros__parameters: + type: "controller_manager/test_controller" + state_interfaces: + - "joint1/position" + - "joint2/position" diff --git a/controller_manager/test/test_release_interfaces.cpp b/controller_manager/test/test_release_interfaces.cpp index 9caef761ab..4dedb47241 100644 --- a/controller_manager/test/test_release_interfaces.cpp +++ b/controller_manager/test/test_release_interfaces.cpp @@ -84,7 +84,7 @@ TEST_F(TestReleaseInterfaces, switch_controllers_same_interface) ASSERT_EQ(std::future_status::timeout, switch_future.wait_for(std::chrono::milliseconds(100))) << "switch_controller should be blocking until next update cycle"; ControllerManagerRunner cm_runner(this); - EXPECT_EQ(controller_interface::return_type::OK, switch_future.get()); + EXPECT_EQ(controller_interface::return_type::ERROR, switch_future.get()); ASSERT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, abstract_test_controller1.c->get_lifecycle_state().id()); @@ -188,7 +188,7 @@ TEST_F(TestReleaseInterfaces, switch_controllers_same_interface) ASSERT_EQ(std::future_status::timeout, switch_future.wait_for(std::chrono::milliseconds(100))) << "switch_controller should be blocking until next update cycle"; ControllerManagerRunner cm_runner(this); - EXPECT_EQ(controller_interface::return_type::OK, switch_future.get()); + EXPECT_EQ(controller_interface::return_type::ERROR, switch_future.get()); ASSERT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, abstract_test_controller1.c->get_lifecycle_state().id()); diff --git a/controller_manager/test/test_spawner_unspawner.cpp b/controller_manager/test/test_spawner_unspawner.cpp index 107c557dbb..ff39b69e8c 100644 --- a/controller_manager/test/test_spawner_unspawner.cpp +++ b/controller_manager/test/test_spawner_unspawner.cpp @@ -437,6 +437,106 @@ TEST_F(TestLoadController, spawner_test_with_wildcard_entries_with_no_ctrl_name) verify_ctrl_parameter(wildcard_ctrl_1.c->get_node(), false); } +TEST_F(TestLoadController, spawner_test_failed_activation_of_controllers) +{ + const std::string test_file_path = ament_index_cpp::get_package_prefix("controller_manager") + + "/test/test_controller_spawner_with_interfaces.yaml"; + + ControllerManagerRunner cm_runner(this); + // Provide controller type via the parsed file + EXPECT_EQ( + call_spawner( + "ctrl_with_joint1_command_interface ctrl_with_joint2_command_interface -c " + "test_controller_manager " + "--controller-manager-timeout 1.0 " + "-p " + + test_file_path), + 0); + + ASSERT_EQ(cm_->get_loaded_controllers().size(), 2ul); + + auto ctrl_with_joint2_command_interface = cm_->get_loaded_controllers()[0]; + ASSERT_EQ(ctrl_with_joint2_command_interface.info.name, "ctrl_with_joint2_command_interface"); + ASSERT_EQ( + ctrl_with_joint2_command_interface.info.type, test_controller::TEST_CONTROLLER_CLASS_NAME); + EXPECT_EQ( + ctrl_with_joint2_command_interface.c->get_lifecycle_state().id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); + ASSERT_EQ( + ctrl_with_joint2_command_interface.c->command_interface_configuration().names.size(), 1ul); + ASSERT_THAT( + ctrl_with_joint2_command_interface.c->command_interface_configuration().names, + std::vector({"joint2/velocity"})); + + auto ctrl_with_joint1_command_interface = cm_->get_loaded_controllers()[1]; + ASSERT_EQ(ctrl_with_joint1_command_interface.info.name, "ctrl_with_joint1_command_interface"); + ASSERT_EQ( + ctrl_with_joint1_command_interface.info.type, test_controller::TEST_CONTROLLER_CLASS_NAME); + EXPECT_EQ( + ctrl_with_joint1_command_interface.c->get_lifecycle_state().id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); + ASSERT_EQ( + ctrl_with_joint1_command_interface.c->command_interface_configuration().names.size(), 1ul); + ASSERT_THAT( + ctrl_with_joint1_command_interface.c->command_interface_configuration().names, + std::vector({"joint1/position"})); + + EXPECT_EQ( + call_spawner( + "ctrl_with_joint1_and_joint2_command_interfaces -c test_controller_manager " + "--controller-manager-timeout 1.0 " + "-p " + + test_file_path), + 256) + << "Should fail as the ctrl_with_joint1_command_interface and " + "ctrl_with_joint2_command_interface are active"; + + ASSERT_EQ(cm_->get_loaded_controllers().size(), 3ul); + + auto ctrl_with_joint1_and_joint2_command_interfaces = cm_->get_loaded_controllers()[0]; + ASSERT_EQ( + ctrl_with_joint1_and_joint2_command_interfaces.info.name, + "ctrl_with_joint1_and_joint2_command_interfaces"); + ASSERT_EQ( + ctrl_with_joint1_and_joint2_command_interfaces.info.type, + test_controller::TEST_CONTROLLER_CLASS_NAME); + ASSERT_EQ( + ctrl_with_joint1_and_joint2_command_interfaces.c->get_lifecycle_state().id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); + ASSERT_EQ( + ctrl_with_joint1_and_joint2_command_interfaces.c->command_interface_configuration() + .names.size(), + 2ul); + ASSERT_THAT( + ctrl_with_joint1_and_joint2_command_interfaces.c->command_interface_configuration().names, + std::vector({"joint1/position", "joint2/velocity"})); + + EXPECT_EQ(call_unspawner("ctrl_with_joint1_command_interface -c test_controller_manager"), 0); + + ASSERT_EQ(cm_->get_loaded_controllers().size(), 2ul); + EXPECT_EQ( + call_spawner( + "ctrl_with_joint1_and_joint2_command_interfaces -c test_controller_manager " + "--controller-manager-timeout 1.0 " + "-p " + + test_file_path), + 256) + << "Should fail as the ctrl_with_joint2_command_interface is still active"; + + EXPECT_EQ(call_unspawner("ctrl_with_joint2_command_interface -c test_controller_manager"), 0); + + ASSERT_EQ(cm_->get_loaded_controllers().size(), 1ul); + EXPECT_EQ( + call_spawner( + "ctrl_with_joint1_and_joint2_command_interfaces -c test_controller_manager " + "--controller-manager-timeout 1.0 " + "-p " + + test_file_path), + 0) + << "Should pass as the ctrl_with_joint1_command_interface and " + "ctrl_with_joint2_command_interface are inactive"; +} + TEST_F(TestLoadController, unload_on_kill) { // Launch spawner with unload on kill @@ -664,6 +764,45 @@ TEST_F(TestLoadController, spawner_with_many_controllers) } } +TEST_F(TestLoadController, test_spawner_parsed_controller_ros_args) +{ + ControllerManagerRunner cm_runner(this); + cm_->set_parameter(rclcpp::Parameter("ctrl_1.type", test_controller::TEST_CONTROLLER_CLASS_NAME)); + cm_->set_parameter(rclcpp::Parameter("ctrl_2.type", test_controller::TEST_CONTROLLER_CLASS_NAME)); + std::stringstream ss; + + EXPECT_EQ(call_spawner("ctrl_1 -c test_controller_manager"), 0); + ASSERT_EQ(cm_->get_loaded_controllers().size(), 1ul); + + // Now as the controller is active, we can call check for the service + std::shared_ptr node = rclcpp::Node::make_shared("set_bool_client"); + auto set_bool_service = node->create_client("/set_bool"); + ASSERT_FALSE(set_bool_service->wait_for_service(std::chrono::seconds(2))); + ASSERT_FALSE(set_bool_service->service_is_ready()); + // Now check the service availability in the controller namespace + auto ctrl_1_set_bool_service = + node->create_client("/ctrl_1/set_bool"); + ASSERT_TRUE(ctrl_1_set_bool_service->wait_for_service(std::chrono::seconds(2))); + ASSERT_TRUE(ctrl_1_set_bool_service->service_is_ready()); + + // Now test the remapping of the service name with the controller_ros_args + EXPECT_EQ( + call_spawner( + "ctrl_2 -c test_controller_manager --controller-ros-args '-r /ctrl_2/set_bool:=/set_bool'"), + 0); + + ASSERT_EQ(cm_->get_loaded_controllers().size(), 2ul); + + // Now as the controller is active, we can call check for the remapped service + ASSERT_TRUE(set_bool_service->wait_for_service(std::chrono::seconds(2))); + ASSERT_TRUE(set_bool_service->service_is_ready()); + // Now check the service availability in the controller namespace + auto ctrl_2_set_bool_service = + node->create_client("/ctrl_2/set_bool"); + ASSERT_FALSE(ctrl_2_set_bool_service->wait_for_service(std::chrono::seconds(2))); + ASSERT_FALSE(ctrl_2_set_bool_service->service_is_ready()); +} + class TestLoadControllerWithoutRobotDescription : public ControllerManagerFixture { diff --git a/controller_manager_msgs/CHANGELOG.rst b/controller_manager_msgs/CHANGELOG.rst index 79b2ca739a..fc92596273 100644 --- a/controller_manager_msgs/CHANGELOG.rst +++ b/controller_manager_msgs/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package controller_manager_msgs ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +4.22.0 (2024-12-20) +------------------- + 4.21.0 (2024-12-06) ------------------- diff --git a/controller_manager_msgs/package.xml b/controller_manager_msgs/package.xml index 1d9a26d3d1..703feeccc7 100644 --- a/controller_manager_msgs/package.xml +++ b/controller_manager_msgs/package.xml @@ -2,7 +2,7 @@ controller_manager_msgs - 4.21.0 + 4.22.0 Messages and services for the controller manager. Bence Magyar Denis Štogl diff --git a/doc/release_notes.rst b/doc/release_notes.rst index a187e62437..69c282b051 100644 --- a/doc/release_notes.rst +++ b/doc/release_notes.rst @@ -76,6 +76,7 @@ controller_manager * The ``--controller-type`` or ``-t`` spawner arg is removed. Now the controller type is defined in the controller configuration file with ``type`` field (`#1639 `_). * The ``--namespace`` or ``-n`` spawner arg is deprecated. Now the spawner namespace can be defined using the ROS 2 standard way (`#1640 `_). * Added support for the wildcard entries for the controller configuration files (`#1724 `_). +* The spawner now supports the ``--controller-ros-args`` argument to pass the ROS 2 node arguments to the controller node to be able to remap topics, services and etc (`#1713 `_). * The spawner now supports parsing multiple ``-p`` or ``--param-file`` arguments, this should help in loading multiple parameter files for a controller or for multiple controllers (`#1805 `_). * ``--switch-timeout`` was added as parameter to the helper scripts ``spawner.py`` and ``unspawner.py``. Useful if controllers cannot be switched immediately, e.g., paused simulations at startup (`#1790 `_). * ``ros2_control_node`` can now handle the sim time used by different simulators, when ``use_sim_time`` is set to true (`#1810 `_). @@ -156,6 +157,7 @@ hardware_interface * With (`#1683 `_) the ``rclcpp_lifecycle::State & get_state()`` and ``void set_state(const rclcpp_lifecycle::State & new_state)`` are replaced by ``rclcpp_lifecycle::State & get_lifecycle_state()`` and ``void set_lifecycle_state(const rclcpp_lifecycle::State & new_state)``. This change affects controllers and hardware. This is related to (`#1240 `_) as variant support introduces ``get_state`` and ``set_state`` methods for setting/getting state of handles. * With (`#1421 `_) a key-value storage is added to InterfaceInfo. This allows to define extra params with per Command-/StateInterface in the ``.ros2_control.xacro`` file. * With (`#1763 `_) parsing for SDF published to ``robot_description`` topic is now also supported. +* With (`#1567 `_) all the Hardware components now have a fully functional asynchronous functionality, by simply adding ``is_async`` tag to the ros2_control tag in the URDF. This will allow the hardware components to run in a separate thread, and the controller manager will be able to run the controllers in parallel with the hardware components. joint_limits ************ diff --git a/hardware_interface/CHANGELOG.rst b/hardware_interface/CHANGELOG.rst index 41152d6eaa..df05cb39e3 100644 --- a/hardware_interface/CHANGELOG.rst +++ b/hardware_interface/CHANGELOG.rst @@ -2,6 +2,16 @@ Changelog for package hardware_interface ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +4.22.0 (2024-12-20) +------------------- +* Propagate read/write rate to the HardwareInfo properly (`#1928 `_) +* Async Hardware Components (`#1567 `_) +* Add controller node options args to be able to set controller specific node arguments (`#1713 `_) +* Make get_name() return a const reference (`#1952 `_) +* Let sensors also export state interfaces of joints (`#1885 `_) +* [CI] Add clang job, setup concurrency, use rt_tools humble branch (`#1910 `_) +* Contributors: Christoph Fröhlich, Sai Kishor Kothakota, Takashi Sato + 4.21.0 (2024-12-06) ------------------- * [Feature] Choose different read and write rate for the hardware components (`#1570 `_) diff --git a/hardware_interface/CMakeLists.txt b/hardware_interface/CMakeLists.txt index f9637b4f07..4e3fe6a9ae 100644 --- a/hardware_interface/CMakeLists.txt +++ b/hardware_interface/CMakeLists.txt @@ -13,6 +13,7 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS rclcpp_lifecycle rcpputils rcutils + realtime_tools TinyXML2 tinyxml2_vendor joint_limits diff --git a/hardware_interface/doc/asynchronous_components.rst b/hardware_interface/doc/asynchronous_components.rst new file mode 100644 index 0000000000..c73281c3fe --- /dev/null +++ b/hardware_interface/doc/asynchronous_components.rst @@ -0,0 +1,99 @@ +:github_url: https://github.com/ros-controls/ros2_control/blob/{REPOS_FILE_BRANCH}/hardware_interface/doc/different_update_rates_userdoc.rst + +.. _asynchronous_components: + +Running Hardware Components Asynchronously +============================================ + +The ``ros2_control`` framework allows to run hardware components asynchronously. This is useful when some of the hardware components need to run in a separate thread or executor. For example, a sensor takes longer to read data that affects the periodicity of the ``controller_manager`` control loop. In this case, the sensor can be run in a separate thread or executor to avoid blocking the control loop. + +Parameters +----------- + +The following parameters can be set in the ``ros2_control`` hardware configuration to run the hardware component asynchronously: + +* ``is_async``: (optional) If set to ``true``, the hardware component will run asynchronously. Default is ``false``. +* ``thread_priority``: (optional) The priority of the thread that runs the hardware component. The priority is an integer value between 0 and 99. The default value is 50. + +.. note:: + The thread priority is only used when the hardware component is run asynchronously. + When the hardware component is run asynchronously, it uses the FIFO scheduling policy. + +Examples +--------- + +The following examples show how to use the different hardware interface types synchronously and asynchronously with ``ros2_control`` URDF. +They can be combined together within the different hardware component types (system, actuator, sensor) (:ref:`see detailed documentation `) as follows + +For a RRBot with multimodal gripper and external sensor: + +.. code-block:: xml + + + + ros2_control_demo_hardware/RRBotSystemPositionOnlyHardware + 2.0 + 3.0 + 2.0 + + + + -1 + 1 + + + + + + -1 + 1 + + + + + + + + + + + + + + + ros2_control_demo_hardware/MultimodalGripper + + + + 0 + 100 + + + + + + + ros2_control_demo_hardware/ForceTorqueSensor2DHardware + 0.43 + + + + + kuka_tcp + 100 + 100 + + + + + + + + + + +In the above example, the following components are defined: + +* A system hardware component named ``RRBotSystemMutipleGPIOs`` with two joints and a GPIO component that runs synchronously. +* An actuator hardware component named ``MultimodalGripper`` with a joint that runs asynchronously with a thread priority of 30. +* A sensor hardware component named ``RRBotForceTorqueSensor2D`` with two sensors and a GPIO component that runs asynchronously with the default thread priority of 50. diff --git a/hardware_interface/doc/hardware_components_userdoc.rst b/hardware_interface/doc/hardware_components_userdoc.rst index 0a980cad4e..3870433ef0 100644 --- a/hardware_interface/doc/hardware_components_userdoc.rst +++ b/hardware_interface/doc/hardware_components_userdoc.rst @@ -17,6 +17,7 @@ Guidelines and Best Practices Hardware Interface Types Writing a Hardware Component Different Update Rates + Asynchronous Execution Handling of errors that happen during read() and write() calls diff --git a/hardware_interface/include/hardware_interface/actuator_interface.hpp b/hardware_interface/include/hardware_interface/actuator_interface.hpp index 8aa214e728..6f4dfa4402 100644 --- a/hardware_interface/include/hardware_interface/actuator_interface.hpp +++ b/hardware_interface/include/hardware_interface/actuator_interface.hpp @@ -27,6 +27,7 @@ #include "hardware_interface/hardware_info.hpp" #include "hardware_interface/types/hardware_interface_return_values.hpp" #include "hardware_interface/types/lifecycle_state_names.hpp" +#include "hardware_interface/types/trigger_type.hpp" #include "lifecycle_msgs/msg/state.hpp" #include "rclcpp/duration.hpp" #include "rclcpp/logger.hpp" @@ -34,6 +35,7 @@ #include "rclcpp/time.hpp" #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" #include "rclcpp_lifecycle/state.hpp" +#include "realtime_tools/async_function_handler.hpp" namespace hardware_interface { @@ -111,6 +113,30 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod clock_interface_ = clock_interface; actuator_logger_ = logger.get_child("hardware_component.actuator." + hardware_info.name); info_ = hardware_info; + if (info_.is_async) + { + RCLCPP_INFO_STREAM( + get_logger(), "Starting async handler with scheduler priority: " << info_.thread_priority); + async_handler_ = std::make_unique>(); + async_handler_->init( + [this](const rclcpp::Time & time, const rclcpp::Duration & period) + { + if (next_trigger_ == TriggerType::READ) + { + const auto ret = read(time, period); + next_trigger_ = TriggerType::WRITE; + return ret; + } + else + { + const auto ret = write(time, period); + next_trigger_ = TriggerType::READ; + return ret; + } + }, + info_.thread_priority); + async_handler_->start_thread(); + } return on_init(hardware_info); }; @@ -321,6 +347,50 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod return return_type::OK; } + /// Triggers the read method synchronously or asynchronously depending on the HardwareInfo + /** + * The data readings from the physical hardware has to be updated + * and reflected accordingly in the exported state interfaces. + * That is, the data pointed by the interfaces shall be updated. + * The method is called in the resource_manager's read loop + * + * \param[in] time The time at the start of this control loop iteration + * \param[in] period The measured time taken by the last control loop iteration + * \return return_type::OK if the read was successful, return_type::ERROR otherwise. + */ + return_type trigger_read(const rclcpp::Time & time, const rclcpp::Duration & period) + { + return_type result = return_type::ERROR; + if (info_.is_async) + { + bool trigger_status = true; + if (next_trigger_ == TriggerType::WRITE) + { + RCLCPP_WARN( + get_logger(), + "Trigger read called while write async handler call is still pending for hardware " + "interface : '%s'. Skipping read cycle and will wait for a write cycle!", + info_.name.c_str()); + return return_type::OK; + } + std::tie(trigger_status, result) = async_handler_->trigger_async_callback(time, period); + if (!trigger_status) + { + RCLCPP_WARN( + get_logger(), + "Trigger read called while write async trigger is still in progress for hardware " + "interface : '%s'. Failed to trigger read cycle!", + info_.name.c_str()); + return return_type::OK; + } + } + else + { + result = read(time, period); + } + return result; + } + /// Read the current state values from the actuator. /** * The data readings from the physical hardware has to be updated @@ -333,6 +403,49 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod */ virtual return_type read(const rclcpp::Time & time, const rclcpp::Duration & period) = 0; + /// Triggers the write method synchronously or asynchronously depending on the HardwareInfo + /** + * The physical hardware shall be updated with the latest value from + * the exported command interfaces. + * The method is called in the resource_manager's write loop + * + * \param[in] time The time at the start of this control loop iteration + * \param[in] period The measured time taken by the last control loop iteration + * \return return_type::OK if the read was successful, return_type::ERROR otherwise. + */ + return_type trigger_write(const rclcpp::Time & time, const rclcpp::Duration & period) + { + return_type result = return_type::ERROR; + if (info_.is_async) + { + bool trigger_status = true; + if (next_trigger_ == TriggerType::READ) + { + RCLCPP_WARN( + get_logger(), + "Trigger write called while read async handler call is still pending for hardware " + "interface : '%s'. Skipping write cycle and will wait for a read cycle!", + info_.name.c_str()); + return return_type::OK; + } + std::tie(trigger_status, result) = async_handler_->trigger_async_callback(time, period); + if (!trigger_status) + { + RCLCPP_WARN( + get_logger(), + "Trigger write called while read async trigger is still in progress for hardware " + "interface : '%s'. Failed to trigger write cycle!", + info_.name.c_str()); + return return_type::OK; + } + } + else + { + result = write(time, period); + } + return result; + } + /// Write the current command values to the actuator. /** * The physical hardware shall be updated with the latest value from @@ -426,6 +539,7 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod std::vector unlisted_commands_; rclcpp_lifecycle::State lifecycle_state_; + std::unique_ptr> async_handler_; private: rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface_; @@ -433,6 +547,7 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod // interface names to Handle accessed through getters/setters std::unordered_map actuator_states_; std::unordered_map actuator_commands_; + std::atomic next_trigger_ = TriggerType::READ; }; } // namespace hardware_interface diff --git a/hardware_interface/include/hardware_interface/controller_info.hpp b/hardware_interface/include/hardware_interface/controller_info.hpp index 3ad89551d5..d814ca7ae2 100644 --- a/hardware_interface/include/hardware_interface/controller_info.hpp +++ b/hardware_interface/include/hardware_interface/controller_info.hpp @@ -41,6 +41,9 @@ struct ControllerInfo /// List of fallback controller names to be activated if this controller fails. std::vector fallback_controllers_names; + + /// Controller node options arguments + std::vector node_options_args; }; } // namespace hardware_interface diff --git a/hardware_interface/include/hardware_interface/hardware_info.hpp b/hardware_interface/include/hardware_interface/hardware_info.hpp index f62329ee62..9d96190954 100644 --- a/hardware_interface/include/hardware_interface/hardware_info.hpp +++ b/hardware_interface/include/hardware_interface/hardware_info.hpp @@ -178,6 +178,8 @@ struct HardwareInfo unsigned int rw_rate; /// Component is async bool is_async; + /// Async thread priority + int thread_priority; /// Name of the pluginlib plugin of the hardware that will be loaded. std::string hardware_plugin_name; /// (Optional) Key-value pairs for hardware parameters. diff --git a/hardware_interface/include/hardware_interface/resource_manager.hpp b/hardware_interface/include/hardware_interface/resource_manager.hpp index a5592fc492..cdde7550d3 100644 --- a/hardware_interface/include/hardware_interface/resource_manager.hpp +++ b/hardware_interface/include/hardware_interface/resource_manager.hpp @@ -426,12 +426,6 @@ class HARDWARE_INTERFACE_PUBLIC ResourceManager return_type set_component_state( const std::string & component_name, rclcpp_lifecycle::State & target_state); - /// Deletes all async components from the resource storage - /** - * Needed to join the threads immediately after the control loop is ended. - */ - void shutdown_async_components(); - /// Reads all loaded hardware components. /** * Reads from all active hardware components. diff --git a/hardware_interface/include/hardware_interface/sensor_interface.hpp b/hardware_interface/include/hardware_interface/sensor_interface.hpp index 455f6216a2..93de2a6494 100644 --- a/hardware_interface/include/hardware_interface/sensor_interface.hpp +++ b/hardware_interface/include/hardware_interface/sensor_interface.hpp @@ -34,6 +34,7 @@ #include "rclcpp/time.hpp" #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" #include "rclcpp_lifecycle/state.hpp" +#include "realtime_tools/async_function_handler.hpp" namespace hardware_interface { @@ -111,6 +112,16 @@ class SensorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI clock_interface_ = clock_interface; sensor_logger_ = logger.get_child("hardware_component.sensor." + hardware_info.name); info_ = hardware_info; + if (info_.is_async) + { + RCLCPP_INFO_STREAM( + get_logger(), "Starting async handler with scheduler priority: " << info_.thread_priority); + read_async_handler_ = std::make_unique>(); + read_async_handler_->init( + std::bind(&SensorInterface::read, this, std::placeholders::_1, std::placeholders::_2), + info_.thread_priority); + read_async_handler_->start_thread(); + } return on_init(hardware_info); }; @@ -214,6 +225,40 @@ class SensorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI return state_interfaces; } + /// Triggers the read method synchronously or asynchronously depending on the HardwareInfo + /** + * The data readings from the physical hardware has to be updated + * and reflected accordingly in the exported state interfaces. + * That is, the data pointed by the interfaces shall be updated. + * + * \param[in] time The time at the start of this control loop iteration + * \param[in] period The measured time taken by the last control loop iteration + * \return return_type::OK if the read was successful, return_type::ERROR otherwise. + */ + return_type trigger_read(const rclcpp::Time & time, const rclcpp::Duration & period) + { + return_type result = return_type::ERROR; + if (info_.is_async) + { + bool trigger_status = true; + std::tie(trigger_status, result) = read_async_handler_->trigger_async_callback(time, period); + if (!trigger_status) + { + RCLCPP_WARN( + get_logger(), + "Trigger read called while read async handler is still in progress for hardware " + "interface : '%s'. Failed to trigger read cycle!", + info_.name.c_str()); + return return_type::OK; + } + } + else + { + result = read(time, period); + } + return result; + } + /// Read the current state values from the actuator. /** * The data readings from the physical hardware has to be updated @@ -294,6 +339,7 @@ class SensorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI std::vector unlisted_states_; rclcpp_lifecycle::State lifecycle_state_; + std::unique_ptr> read_async_handler_; private: rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface_; diff --git a/hardware_interface/include/hardware_interface/system_interface.hpp b/hardware_interface/include/hardware_interface/system_interface.hpp index 32d0b8a48a..e145deae5b 100644 --- a/hardware_interface/include/hardware_interface/system_interface.hpp +++ b/hardware_interface/include/hardware_interface/system_interface.hpp @@ -28,6 +28,7 @@ #include "hardware_interface/types/hardware_interface_return_values.hpp" #include "hardware_interface/types/hardware_interface_type_values.hpp" #include "hardware_interface/types/lifecycle_state_names.hpp" +#include "hardware_interface/types/trigger_type.hpp" #include "lifecycle_msgs/msg/state.hpp" #include "rclcpp/duration.hpp" #include "rclcpp/logger.hpp" @@ -36,6 +37,7 @@ #include "rclcpp/time.hpp" #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" #include "rclcpp_lifecycle/state.hpp" +#include "realtime_tools/async_function_handler.hpp" namespace hardware_interface { @@ -114,6 +116,30 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI clock_interface_ = clock_interface; system_logger_ = logger.get_child("hardware_component.system." + hardware_info.name); info_ = hardware_info; + if (info_.is_async) + { + RCLCPP_INFO_STREAM( + get_logger(), "Starting async handler with scheduler priority: " << info_.thread_priority); + async_handler_ = std::make_unique>(); + async_handler_->init( + [this](const rclcpp::Time & time, const rclcpp::Duration & period) + { + if (next_trigger_ == TriggerType::READ) + { + const auto ret = read(time, period); + next_trigger_ = TriggerType::WRITE; + return ret; + } + else + { + const auto ret = write(time, period); + next_trigger_ = TriggerType::READ; + return ret; + } + }, + info_.thread_priority); + async_handler_->start_thread(); + } return on_init(hardware_info); }; @@ -350,6 +376,50 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI return return_type::OK; } + /// Triggers the read method synchronously or asynchronously depending on the HardwareInfo + /** + * The data readings from the physical hardware has to be updated + * and reflected accordingly in the exported state interfaces. + * That is, the data pointed by the interfaces shall be updated. + * The method is called in the resource_manager's read loop + * + * \param[in] time The time at the start of this control loop iteration + * \param[in] period The measured time taken by the last control loop iteration + * \return return_type::OK if the read was successful, return_type::ERROR otherwise. + */ + return_type trigger_read(const rclcpp::Time & time, const rclcpp::Duration & period) + { + return_type result = return_type::ERROR; + if (info_.is_async) + { + bool trigger_status = true; + if (next_trigger_ == TriggerType::WRITE) + { + RCLCPP_WARN( + get_logger(), + "Trigger read called while write async handler call is still pending for hardware " + "interface : '%s'. Skipping read cycle and will wait for a write cycle!", + info_.name.c_str()); + return return_type::OK; + } + std::tie(trigger_status, result) = async_handler_->trigger_async_callback(time, period); + if (!trigger_status) + { + RCLCPP_WARN( + get_logger(), + "Trigger read called while write async trigger is still in progress for hardware " + "interface : '%s'. Failed to trigger read cycle!", + info_.name.c_str()); + return return_type::OK; + } + } + else + { + result = read(time, period); + } + return result; + } + /// Read the current state values from the actuator. /** * The data readings from the physical hardware has to be updated @@ -362,6 +432,49 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI */ virtual return_type read(const rclcpp::Time & time, const rclcpp::Duration & period) = 0; + /// Triggers the write method synchronously or asynchronously depending on the HardwareInfo + /** + * The physical hardware shall be updated with the latest value from + * the exported command interfaces. + * The method is called in the resource_manager's write loop + * + * \param[in] time The time at the start of this control loop iteration + * \param[in] period The measured time taken by the last control loop iteration + * \return return_type::OK if the read was successful, return_type::ERROR otherwise. + */ + return_type trigger_write(const rclcpp::Time & time, const rclcpp::Duration & period) + { + return_type result = return_type::ERROR; + if (info_.is_async) + { + bool trigger_status = true; + if (next_trigger_ == TriggerType::READ) + { + RCLCPP_WARN( + get_logger(), + "Trigger write called while read async handler call is still pending for hardware " + "interface : '%s'. Skipping write cycle and will wait for a read cycle!", + info_.name.c_str()); + return return_type::OK; + } + std::tie(trigger_status, result) = async_handler_->trigger_async_callback(time, period); + if (!trigger_status) + { + RCLCPP_WARN( + get_logger(), + "Trigger write called while read async trigger is still in progress for hardware " + "interface : '%s'. Failed to trigger write cycle!", + info_.name.c_str()); + return return_type::OK; + } + } + else + { + result = write(time, period); + } + return result; + } + /// Write the current command values to the actuator. /** * The physical hardware shall be updated with the latest value from @@ -453,6 +566,7 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI std::unordered_map unlisted_command_interfaces_; rclcpp_lifecycle::State lifecycle_state_; + std::unique_ptr> async_handler_; // Exported Command- and StateInterfaces in order they are listed in the hardware description. std::vector joint_states_; @@ -472,6 +586,7 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI // interface names to Handle accessed through getters/setters std::unordered_map system_states_; std::unordered_map system_commands_; + std::atomic next_trigger_ = TriggerType::READ; }; } // namespace hardware_interface diff --git a/hardware_interface/include/hardware_interface/types/trigger_type.hpp b/hardware_interface/include/hardware_interface/types/trigger_type.hpp new file mode 100644 index 0000000000..63970bc0f0 --- /dev/null +++ b/hardware_interface/include/hardware_interface/types/trigger_type.hpp @@ -0,0 +1,28 @@ +// Copyright 2024 ros2_control Development Team +// +// 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 HARDWARE_INTERFACE__TYPES__TRIGGER_TYPE_HPP_ +#define HARDWARE_INTERFACE__TYPES__TRIGGER_TYPE_HPP_ + +namespace hardware_interface +{ +enum class TriggerType +{ + READ, + WRITE +}; + +} // namespace hardware_interface + +#endif // HARDWARE_INTERFACE__TYPES__TRIGGER_TYPE_HPP_ diff --git a/hardware_interface/package.xml b/hardware_interface/package.xml index 23b882ab02..5f748e5680 100644 --- a/hardware_interface/package.xml +++ b/hardware_interface/package.xml @@ -1,7 +1,7 @@ hardware_interface - 4.21.0 + 4.22.0 ros2_control hardware interface Bence Magyar Denis Štogl @@ -16,6 +16,7 @@ pluginlib rclcpp_lifecycle rcpputils + realtime_tools tinyxml2_vendor joint_limits urdf diff --git a/hardware_interface/src/actuator.cpp b/hardware_interface/src/actuator.cpp index 0e4ae95ca9..81bebf1182 100644 --- a/hardware_interface/src/actuator.cpp +++ b/hardware_interface/src/actuator.cpp @@ -302,7 +302,7 @@ return_type Actuator::read(const rclcpp::Time & time, const rclcpp::Duration & p impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE || impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) { - result = impl_->read(time, period); + result = impl_->trigger_read(time, period); if (result == return_type::ERROR) { error(); @@ -324,7 +324,7 @@ return_type Actuator::write(const rclcpp::Time & time, const rclcpp::Duration & impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE || impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) { - result = impl_->write(time, period); + result = impl_->trigger_write(time, period); if (result == return_type::ERROR) { error(); diff --git a/hardware_interface/src/component_parser.cpp b/hardware_interface/src/component_parser.cpp index 146a5aef05..a3e9efaa3a 100644 --- a/hardware_interface/src/component_parser.cpp +++ b/hardware_interface/src/component_parser.cpp @@ -60,6 +60,7 @@ constexpr const auto kReductionAttribute = "mechanical_reduction"; constexpr const auto kOffsetAttribute = "offset"; constexpr const auto kReadWriteRateAttribute = "rw_rate"; constexpr const auto kIsAsyncAttribute = "is_async"; +constexpr const auto kThreadPriorityAttribute = "thread_priority"; } // namespace @@ -273,6 +274,35 @@ bool parse_is_async_attribute(const tinyxml2::XMLElement * elem) return attr ? parse_bool(attr->Value()) : false; } +/// Parse thread_priority attribute +/** + * Parses an XMLElement and returns the value of the thread_priority attribute. + * Defaults to 50 if not specified. + * + * \param[in] elem XMLElement that has the thread_priority attribute. + * \return positive integer specifying the thread priority. + */ +int parse_thread_priority_attribute(const tinyxml2::XMLElement * elem) +{ + const tinyxml2::XMLAttribute * attr = elem->FindAttribute(kThreadPriorityAttribute); + if (!attr) + { + return 50; + } + std::string s = attr->Value(); + std::regex int_re("[1-9][0-9]*"); + if (std::regex_match(s, int_re)) + { + return std::stoi(s); + } + else + { + throw std::runtime_error( + "Could not parse thread_priority tag in \"" + std::string(elem->Name()) + "\"." + "Got \"" + + s + "\", but expected a non-zero positive integer."); + } +} + /// Search XML snippet from URDF for parameters. /** * \param[in] params_it pointer to the iterator where parameters info should be found @@ -614,6 +644,8 @@ HardwareInfo parse_resource_from_xml( hardware.type = get_attribute_value(ros2_control_it, kTypeAttribute, kROS2ControlTag); hardware.rw_rate = parse_rw_rate_attribute(ros2_control_it); hardware.is_async = parse_is_async_attribute(ros2_control_it); + hardware.thread_priority = hardware.is_async ? parse_thread_priority_attribute(ros2_control_it) + : std::numeric_limits::max(); // Parse everything under ros2_control tag hardware.hardware_plugin_name = ""; diff --git a/hardware_interface/src/resource_manager.cpp b/hardware_interface/src/resource_manager.cpp index e77917af9d..7a616d890d 100644 --- a/hardware_interface/src/resource_manager.cpp +++ b/hardware_interface/src/resource_manager.cpp @@ -26,7 +26,6 @@ #include "hardware_interface/actuator.hpp" #include "hardware_interface/actuator_interface.hpp" -#include "hardware_interface/async_components.hpp" #include "hardware_interface/component_parser.hpp" #include "hardware_interface/hardware_component_info.hpp" #include "hardware_interface/sensor.hpp" @@ -142,10 +141,7 @@ class ResourceStorage component_info.name = hardware_info.name; component_info.type = hardware_info.type; component_info.group = hardware_info.group; - component_info.rw_rate = - (hardware_info.rw_rate == 0 || hardware_info.rw_rate > cm_update_rate_) - ? cm_update_rate_ - : hardware_info.rw_rate; + component_info.rw_rate = hardware_info.rw_rate; component_info.plugin_name = hardware_info.hardware_plugin_name; component_info.is_async = hardware_info.is_async; @@ -302,15 +298,6 @@ class ResourceStorage hardware.get_name().c_str(), interface.c_str()); } } - - if (hardware_info_map_[hardware.get_name()].is_async) - { - async_component_threads_.emplace( - std::piecewise_construct, std::forward_as_tuple(hardware.get_name()), - std::forward_as_tuple(cm_update_rate_, clock_interface_)); - - async_component_threads_.at(hardware.get_name()).register_component(&hardware); - } } if (!hardware.get_group_name().empty()) { @@ -431,7 +418,6 @@ class ResourceStorage if (result) { remove_all_hardware_interfaces_from_available_list(hardware.get_name()); - async_component_threads_.erase(hardware.get_name()); // TODO(destogl): change this - deimport all things if there is there are interfaces there // deimport_non_movement_command_interfaces(hardware); // deimport_state_interfaces(hardware); @@ -466,16 +452,6 @@ class ResourceStorage get_logger(), "Unknown exception occurred while activating hardware '%s'", hardware.get_name().c_str()); } - - if (result) - { - if (async_component_threads_.find(hardware.get_name()) != async_component_threads_.end()) - { - async_component_threads_.at(hardware.get_name()).activate(); - } - // TODO(destogl): make all command interfaces available (currently are all available) - } - return result; } @@ -827,15 +803,7 @@ class ResourceStorage } return true; }; - - if (hardware_info.is_async) - { - return load_and_init_actuators(async_actuators_); - } - else - { - return load_and_init_actuators(actuators_); - } + return load_and_init_actuators(actuators_); } bool load_and_initialize_sensor(const HardwareInfo & hardware_info) @@ -860,14 +828,7 @@ class ResourceStorage return true; }; - if (hardware_info.is_async) - { - return load_and_init_sensors(async_sensors_); - } - else - { - return load_and_init_sensors(sensors_); - } + return load_and_init_sensors(sensors_); } bool load_and_initialize_system(const HardwareInfo & hardware_info) @@ -892,15 +853,7 @@ class ResourceStorage } return true; }; - - if (hardware_info.is_async) - { - return load_and_init_systems(async_systems_); - } - else - { - return load_and_init_systems(systems_); - } + return load_and_init_systems(systems_); } void initialize_actuator( @@ -922,14 +875,7 @@ class ResourceStorage } }; - if (hardware_info.is_async) - { - init_actuators(async_actuators_); - } - else - { - init_actuators(actuators_); - } + init_actuators(actuators_); } void initialize_sensor( @@ -950,14 +896,7 @@ class ResourceStorage } }; - if (hardware_info.is_async) - { - init_sensors(async_sensors_); - } - else - { - init_sensors(sensors_); - } + init_sensors(sensors_); } void initialize_system( @@ -979,14 +918,7 @@ class ResourceStorage } }; - if (hardware_info.is_async) - { - init_systems(async_systems_); - } - else - { - init_systems(systems_); - } + init_systems(systems_); } void clear() @@ -995,10 +927,6 @@ class ResourceStorage sensors_.clear(); systems_.clear(); - async_actuators_.clear(); - async_sensors_.clear(); - async_systems_.clear(); - hardware_info_map_.clear(); state_interface_map_.clear(); command_interface_map_.clear(); @@ -1056,10 +984,6 @@ class ResourceStorage std::vector sensors_; std::vector systems_; - std::vector async_actuators_; - std::vector async_sensors_; - std::vector async_systems_; - std::unordered_map hardware_info_map_; std::unordered_map hw_group_state_; @@ -1083,9 +1007,6 @@ class ResourceStorage /// List of all claimed command interfaces std::unordered_map claimed_command_interface_map_; - /// List of async components by type - std::unordered_map async_component_threads_; - // Update rate of the controller manager, and the clock interface of its node // Used by async components. unsigned int cm_update_rate_ = 100; @@ -1127,7 +1048,12 @@ bool ResourceManager::load_and_initialize_components( resource_storage_->cm_update_rate_ = update_rate; - const auto hardware_info = hardware_interface::parse_control_resources_from_urdf(urdf); + auto hardware_info = hardware_interface::parse_control_resources_from_urdf(urdf); + // Set the update rate for all hardware components + for (auto & hw : hardware_info) + { + hw.rw_rate = (hw.rw_rate == 0 || hw.rw_rate > update_rate) ? update_rate : hw.rw_rate; + } const std::string system_type = "system"; const std::string sensor_type = "sensor"; @@ -1527,11 +1453,8 @@ std::unordered_map ResourceManager::get_comp }; loop_and_get_state(resource_storage_->actuators_); - loop_and_get_state(resource_storage_->async_actuators_); loop_and_get_state(resource_storage_->sensors_); - loop_and_get_state(resource_storage_->async_sensors_); loop_and_get_state(resource_storage_->systems_); - loop_and_get_state(resource_storage_->async_systems_); return resource_storage_->hardware_info_map_; } @@ -1799,35 +1722,10 @@ return_type ResourceManager::set_component_state( std::bind(&ResourceStorage::set_component_state, resource_storage_.get(), _1, _2), resource_storage_->systems_); } - if (!found) - { - found = find_set_component_state( - std::bind(&ResourceStorage::set_component_state, resource_storage_.get(), _1, _2), - resource_storage_->async_actuators_); - } - if (!found) - { - found = find_set_component_state( - std::bind(&ResourceStorage::set_component_state, resource_storage_.get(), _1, _2), - resource_storage_->async_systems_); - } - if (!found) - { - found = find_set_component_state( - std::bind(&ResourceStorage::set_component_state, resource_storage_.get(), _1, _2), - resource_storage_->async_sensors_); - } return result; } -void ResourceManager::shutdown_async_components() -{ - resource_storage_->async_component_threads_.erase( - resource_storage_->async_component_threads_.begin(), - resource_storage_->async_component_threads_.end()); -} - // CM API: Called in "update"-thread HardwareReadWriteStatus ResourceManager::read( const rclcpp::Time & time, const rclcpp::Duration & period) diff --git a/hardware_interface/src/sensor.cpp b/hardware_interface/src/sensor.cpp index ff193ff8e1..ae4d930bbe 100644 --- a/hardware_interface/src/sensor.cpp +++ b/hardware_interface/src/sensor.cpp @@ -256,7 +256,7 @@ return_type Sensor::read(const rclcpp::Time & time, const rclcpp::Duration & per impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE || impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) { - result = impl_->read(time, period); + result = impl_->trigger_read(time, period); if (result == return_type::ERROR) { error(); diff --git a/hardware_interface/src/system.cpp b/hardware_interface/src/system.cpp index 9a27aa4f68..89d6afd42e 100644 --- a/hardware_interface/src/system.cpp +++ b/hardware_interface/src/system.cpp @@ -300,7 +300,7 @@ return_type System::read(const rclcpp::Time & time, const rclcpp::Duration & per impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE || impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) { - result = impl_->read(time, period); + result = impl_->trigger_read(time, period); if (result == return_type::ERROR) { error(); @@ -322,7 +322,7 @@ return_type System::write(const rclcpp::Time & time, const rclcpp::Duration & pe impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE || impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) { - result = impl_->write(time, period); + result = impl_->trigger_write(time, period); if (result == return_type::ERROR) { error(); diff --git a/hardware_interface/test/mock_components/test_generic_system.cpp b/hardware_interface/test/mock_components/test_generic_system.cpp index 854b35c8d3..446634d12f 100644 --- a/hardware_interface/test/mock_components/test_generic_system.cpp +++ b/hardware_interface/test/mock_components/test_generic_system.cpp @@ -699,9 +699,11 @@ class TestableResourceManager : public hardware_interface::ResourceManager } explicit TestableResourceManager( - rclcpp::Node & node, const std::string & urdf, bool activate_all = false) + rclcpp::Node & node, const std::string & urdf, bool activate_all = false, + unsigned int cm_update_rate = 100) : hardware_interface::ResourceManager( - urdf, node.get_node_clock_interface(), node.get_node_logging_interface(), activate_all, 100) + urdf, node.get_node_clock_interface(), node.get_node_logging_interface(), activate_all, + cm_update_rate) { } }; @@ -842,14 +844,17 @@ void generic_system_functional_test( EXPECT_EQ( status_map[component_name].state.label(), hardware_interface::lifecycle_state_names::UNCONFIGURED); + EXPECT_EQ(status_map[component_name].rw_rate, 100u); configure_components(rm, {component_name}); status_map = rm.get_components_status(); EXPECT_EQ( status_map[component_name].state.label(), hardware_interface::lifecycle_state_names::INACTIVE); + EXPECT_EQ(status_map[component_name].rw_rate, 100u); activate_components(rm, {component_name}); status_map = rm.get_components_status(); EXPECT_EQ( status_map[component_name].state.label(), hardware_interface::lifecycle_state_names::ACTIVE); + EXPECT_EQ(status_map[component_name].rw_rate, 100u); // Check initial values hardware_interface::LoanedStateInterface j1p_s = rm.claim_state_interface("joint1/position"); @@ -935,7 +940,7 @@ void generic_system_error_group_test( const std::string & urdf, const std::string component_prefix, bool validate_same_group) { rclcpp::Node node("test_generic_system"); - TestableResourceManager rm(node, urdf); + TestableResourceManager rm(node, urdf, false, 200u); const std::string component1 = component_prefix + "1"; const std::string component2 = component_prefix + "2"; // check is hardware is configured @@ -944,14 +949,17 @@ void generic_system_error_group_test( { EXPECT_EQ( status_map[component].state.label(), hardware_interface::lifecycle_state_names::UNCONFIGURED); + EXPECT_EQ(status_map[component].rw_rate, 200u); configure_components(rm, {component}); status_map = rm.get_components_status(); EXPECT_EQ( status_map[component].state.label(), hardware_interface::lifecycle_state_names::INACTIVE); + EXPECT_EQ(status_map[component].rw_rate, 200u); activate_components(rm, {component}); status_map = rm.get_components_status(); EXPECT_EQ( status_map[component].state.label(), hardware_interface::lifecycle_state_names::ACTIVE); + EXPECT_EQ(status_map[component].rw_rate, 200u); } // Check initial values diff --git a/hardware_interface/test/test_component_parser.cpp b/hardware_interface/test/test_component_parser.cpp index 8c535f04a9..e1a657e110 100644 --- a/hardware_interface/test/test_component_parser.cpp +++ b/hardware_interface/test/test_component_parser.cpp @@ -879,6 +879,8 @@ TEST_F(TestComponentParser, successfully_parse_valid_urdf_system_robot_with_gpio EXPECT_EQ( hardware_info.hardware_plugin_name, "ros2_control_demo_hardware/RRBotSystemWithGPIOHardware"); + ASSERT_FALSE(hardware_info.is_async); + ASSERT_EQ(hardware_info.thread_priority, std::numeric_limits::max()); ASSERT_THAT(hardware_info.joints, SizeIs(2)); EXPECT_EQ(hardware_info.joints[0].name, "joint1"); @@ -949,6 +951,8 @@ TEST_F(TestComponentParser, successfully_parse_valid_urdf_system_with_size_and_d ASSERT_THAT(hardware_info.joints, SizeIs(1)); + ASSERT_FALSE(hardware_info.is_async); + ASSERT_EQ(hardware_info.thread_priority, std::numeric_limits::max()); EXPECT_EQ(hardware_info.joints[0].name, "joint1"); EXPECT_EQ(hardware_info.joints[0].type, "joint"); EXPECT_THAT(hardware_info.joints[0].command_interfaces, SizeIs(1)); @@ -1337,6 +1341,59 @@ TEST_F(TestComponentParser, successfully_parse_urdf_system_continuous_with_limit EXPECT_FALSE(hardware_info.limits.at("joint2").has_jerk_limits); } +TEST_F(TestComponentParser, successfully_parse_valid_urdf_async_components) +{ + std::string urdf_to_test = ros2_control_test_assets::minimal_async_robot_urdf; + const auto control_hardware = parse_control_resources_from_urdf(urdf_to_test); + ASSERT_THAT(control_hardware, SizeIs(3)); + auto hardware_info = control_hardware[0]; + + // Actuator + EXPECT_EQ(hardware_info.name, "TestActuatorHardware"); + EXPECT_EQ(hardware_info.type, "actuator"); + ASSERT_THAT(hardware_info.group, IsEmpty()); + ASSERT_THAT(hardware_info.joints, SizeIs(1)); + ASSERT_TRUE(hardware_info.is_async); + ASSERT_EQ(hardware_info.thread_priority, 30); + + EXPECT_EQ(hardware_info.joints[0].name, "joint1"); + EXPECT_EQ(hardware_info.joints[0].type, "joint"); + + // Sensor + hardware_info = control_hardware[1]; + EXPECT_EQ(hardware_info.name, "TestSensorHardware"); + EXPECT_EQ(hardware_info.type, "sensor"); + ASSERT_THAT(hardware_info.group, IsEmpty()); + ASSERT_THAT(hardware_info.joints, IsEmpty()); + ASSERT_THAT(hardware_info.sensors, SizeIs(1)); + ASSERT_TRUE(hardware_info.is_async); + ASSERT_EQ(hardware_info.thread_priority, 50); + + EXPECT_EQ(hardware_info.sensors[0].name, "sensor1"); + EXPECT_EQ(hardware_info.sensors[0].type, "sensor"); + EXPECT_THAT(hardware_info.sensors[0].state_interfaces, SizeIs(1)); + EXPECT_THAT(hardware_info.sensors[0].command_interfaces, IsEmpty()); + EXPECT_THAT(hardware_info.sensors[0].state_interfaces[0].name, "velocity"); + + // System + hardware_info = control_hardware[2]; + EXPECT_EQ(hardware_info.name, "TestSystemHardware"); + EXPECT_EQ(hardware_info.type, "system"); + ASSERT_THAT(hardware_info.group, IsEmpty()); + ASSERT_THAT(hardware_info.joints, SizeIs(2)); + ASSERT_THAT(hardware_info.gpios, SizeIs(1)); + + EXPECT_EQ(hardware_info.joints[0].name, "joint2"); + EXPECT_EQ(hardware_info.joints[0].type, "joint"); + + EXPECT_EQ(hardware_info.joints[1].name, "joint3"); + EXPECT_EQ(hardware_info.joints[1].type, "joint"); + EXPECT_EQ(hardware_info.gpios[0].name, "configuration"); + EXPECT_EQ(hardware_info.gpios[0].type, "gpio"); + ASSERT_TRUE(hardware_info.is_async); + ASSERT_EQ(hardware_info.thread_priority, 70); +} + TEST_F(TestComponentParser, successfully_parse_parameter_empty) { const std::string urdf_to_test = @@ -1361,6 +1418,8 @@ TEST_F(TestComponentParser, successfully_parse_parameter_empty) EXPECT_EQ(hardware_info.hardware_parameters.at("example_param_write_for_sec"), ""); EXPECT_EQ(hardware_info.hardware_parameters.at("example_param_read_for_sec"), "2"); + // when not set, rw_rate should be 0 + EXPECT_EQ(hardware_info.rw_rate, 0u); // Verify limits parsed from the URDF ASSERT_THAT(hardware_info.limits, SizeIs(1)); @@ -1377,6 +1436,16 @@ TEST_F(TestComponentParser, successfully_parse_parameter_empty) } } +TEST_F(TestComponentParser, successfully_parse_valid_urdf_async_invalid_thread_priority) +{ + std::string urdf_to_test = std::string(ros2_control_test_assets::urdf_head) + + "" + + std::string(ros2_control_test_assets::urdf_tail); + ; + ASSERT_THROW(parse_control_resources_from_urdf(urdf_to_test), std::runtime_error); +} + TEST_F(TestComponentParser, negative_size_throws_error) { std::string urdf_to_test = std::string(ros2_control_test_assets::urdf_head) + @@ -1425,6 +1494,8 @@ TEST_F(TestComponentParser, gripper_mimic_true_valid_config) EXPECT_DOUBLE_EQ(hw_info[0].mimic_joints[0].offset, 1.0); EXPECT_EQ(hw_info[0].mimic_joints[0].mimicked_joint_index, 0); EXPECT_EQ(hw_info[0].mimic_joints[0].joint_index, 1); + // when not set, rw_rate should be 0 + EXPECT_EQ(hw_info[0].rw_rate, 0u); } TEST_F(TestComponentParser, gripper_no_mimic_valid_config) @@ -1441,6 +1512,8 @@ TEST_F(TestComponentParser, gripper_no_mimic_valid_config) EXPECT_DOUBLE_EQ(hw_info[0].mimic_joints[0].offset, 1.0); EXPECT_EQ(hw_info[0].mimic_joints[0].mimicked_joint_index, 0); EXPECT_EQ(hw_info[0].mimic_joints[0].joint_index, 1); + // when not set, rw_rate should be 0 + EXPECT_EQ(hw_info[0].rw_rate, 0u); } TEST_F(TestComponentParser, negative_rw_rates_throws_error) @@ -1473,6 +1546,38 @@ TEST_F(TestComponentParser, invalid_rw_rates_out_of_range) ASSERT_THROW(parse_control_resources_from_urdf(urdf_to_test), std::runtime_error); } +TEST_F(TestComponentParser, valid_rw_rate) +{ + std::vector hw_info; + ASSERT_NO_THROW( + hw_info = parse_control_resources_from_urdf( + ros2_control_test_assets::minimal_robot_urdf_with_different_hw_rw_rate)); + ASSERT_THAT(hw_info, SizeIs(3)); + EXPECT_EQ(hw_info[0].name, "TestActuatorHardware"); + EXPECT_EQ(hw_info[0].type, "actuator"); + EXPECT_EQ(hw_info[0].hardware_plugin_name, "test_actuator"); + ASSERT_THAT(hw_info[0].joints, SizeIs(1)); + EXPECT_EQ(hw_info[0].joints[0].name, "joint1"); + EXPECT_EQ(hw_info[0].rw_rate, 50u); + + EXPECT_EQ(hw_info[1].name, "TestSensorHardware"); + EXPECT_EQ(hw_info[1].type, "sensor"); + EXPECT_EQ(hw_info[1].hardware_plugin_name, "test_sensor"); + ASSERT_THAT(hw_info[1].sensors, SizeIs(1)); + EXPECT_EQ(hw_info[1].sensors[0].name, "sensor1"); + EXPECT_EQ(hw_info[1].rw_rate, 20u); + + EXPECT_EQ(hw_info[2].name, "TestSystemHardware"); + EXPECT_EQ(hw_info[2].type, "system"); + EXPECT_EQ(hw_info[2].hardware_plugin_name, "test_system"); + ASSERT_THAT(hw_info[2].joints, SizeIs(2)); + EXPECT_EQ(hw_info[2].joints[0].name, "joint2"); + EXPECT_EQ(hw_info[2].joints[1].name, "joint3"); + ASSERT_THAT(hw_info[2].gpios, SizeIs(1)); + EXPECT_EQ(hw_info[2].gpios[0].name, "configuration"); + EXPECT_EQ(hw_info[2].rw_rate, 25u); +} + TEST_F(TestComponentParser, gripper_mimic_with_unknown_joint_throws_error) { const auto urdf_to_test = diff --git a/hardware_interface_testing/CHANGELOG.rst b/hardware_interface_testing/CHANGELOG.rst index fca3037f04..2bdadd883a 100644 --- a/hardware_interface_testing/CHANGELOG.rst +++ b/hardware_interface_testing/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package hardware_interface_testing ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +4.22.0 (2024-12-20) +------------------- +* Propagate read/write rate to the HardwareInfo properly (`#1928 `_) +* Async Hardware Components (`#1567 `_) +* Contributors: Sai Kishor Kothakota + 4.21.0 (2024-12-06) ------------------- * [Feature] Choose different read and write rate for the hardware components (`#1570 `_) diff --git a/hardware_interface_testing/package.xml b/hardware_interface_testing/package.xml index dfccd23a19..8c03aafda8 100644 --- a/hardware_interface_testing/package.xml +++ b/hardware_interface_testing/package.xml @@ -1,7 +1,7 @@ hardware_interface_testing - 4.21.0 + 4.22.0 ros2_control hardware interface testing Bence Magyar Denis Štogl diff --git a/hardware_interface_testing/test/test_components/test_actuator.cpp b/hardware_interface_testing/test/test_components/test_actuator.cpp index 86debc1f4d..630cb9f27c 100644 --- a/hardware_interface_testing/test/test_components/test_actuator.cpp +++ b/hardware_interface_testing/test/test_components/test_actuator.cpp @@ -15,6 +15,7 @@ #include #include "hardware_interface/actuator_interface.hpp" +#include "rclcpp/logging.hpp" #include "ros2_control_test_assets/test_hardware_interface_constants.hpp" using hardware_interface::ActuatorInterface; @@ -35,6 +36,14 @@ class TestActuator : public ActuatorInterface { return CallbackReturn::ERROR; } + if (get_hardware_info().rw_rate == 0u) + { + RCLCPP_WARN( + get_logger(), + "Actuator hardware component '%s' from plugin '%s' failed to initialize as rw_rate is 0.", + get_hardware_info().name.c_str(), get_hardware_info().hardware_plugin_name.c_str()); + return CallbackReturn::ERROR; + } /* * a hardware can optional prove for incorrect info here. diff --git a/hardware_interface_testing/test/test_components/test_sensor.cpp b/hardware_interface_testing/test/test_components/test_sensor.cpp index 16692f55d9..40f27530c0 100644 --- a/hardware_interface_testing/test/test_components/test_sensor.cpp +++ b/hardware_interface_testing/test/test_components/test_sensor.cpp @@ -15,6 +15,7 @@ #include #include "hardware_interface/sensor_interface.hpp" +#include "rclcpp/logging.hpp" using hardware_interface::return_type; using hardware_interface::SensorInterface; @@ -33,6 +34,14 @@ class TestSensor : public SensorInterface { return CallbackReturn::ERROR; } + if (get_hardware_info().rw_rate == 0u) + { + RCLCPP_WARN( + get_logger(), + "Sensor hardware component '%s' from plugin '%s' failed to initialize as rw_rate is 0.", + get_hardware_info().name.c_str(), get_hardware_info().hardware_plugin_name.c_str()); + return CallbackReturn::ERROR; + } return CallbackReturn::SUCCESS; } diff --git a/hardware_interface_testing/test/test_components/test_system.cpp b/hardware_interface_testing/test/test_components/test_system.cpp index 795787eb9e..395935e314 100644 --- a/hardware_interface_testing/test/test_components/test_system.cpp +++ b/hardware_interface_testing/test/test_components/test_system.cpp @@ -17,6 +17,7 @@ #include "hardware_interface/system_interface.hpp" #include "hardware_interface/types/hardware_interface_type_values.hpp" +#include "rclcpp/logging.hpp" #include "ros2_control_test_assets/test_hardware_interface_constants.hpp" using hardware_interface::CommandInterface; @@ -39,6 +40,14 @@ class TestSystem : public SystemInterface return CallbackReturn::ERROR; } + if (get_hardware_info().rw_rate == 0u) + { + RCLCPP_WARN( + get_logger(), + "System hardware component '%s' from plugin '%s' failed to initialize as rw_rate is 0.", + get_hardware_info().name.c_str(), get_hardware_info().hardware_plugin_name.c_str()); + return CallbackReturn::ERROR; + } return CallbackReturn::SUCCESS; } diff --git a/hardware_interface_testing/test/test_resource_manager.cpp b/hardware_interface_testing/test/test_resource_manager.cpp index 5f7640546a..be9e672b3b 100644 --- a/hardware_interface_testing/test/test_resource_manager.cpp +++ b/hardware_interface_testing/test/test_resource_manager.cpp @@ -1958,6 +1958,206 @@ TEST_F( check_read_and_write_cycles(false); } +class ResourceManagerTestAsyncReadWrite : public ResourceManagerTest +{ +public: + void setup_resource_manager_and_do_initial_checks() + { + const auto minimal_robot_urdf_async = + std::string(ros2_control_test_assets::urdf_head) + + std::string(ros2_control_test_assets::async_hardware_resources) + + std::string(ros2_control_test_assets::urdf_tail); + rm = std::make_shared(node_, minimal_robot_urdf_async, false); + activate_components(*rm); + + auto status_map = rm->get_components_status(); + EXPECT_EQ( + status_map[TEST_ACTUATOR_HARDWARE_NAME].state.id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); + EXPECT_EQ( + status_map[TEST_SYSTEM_HARDWARE_NAME].state.id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); + EXPECT_EQ( + status_map[TEST_SENSOR_HARDWARE_NAME].state.id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); + + // Check that all the components are async + ASSERT_TRUE(status_map[TEST_ACTUATOR_HARDWARE_NAME].is_async); + ASSERT_TRUE(status_map[TEST_SYSTEM_HARDWARE_NAME].is_async); + ASSERT_TRUE(status_map[TEST_SENSOR_HARDWARE_NAME].is_async); + + claimed_itfs.push_back( + rm->claim_command_interface(TEST_ACTUATOR_HARDWARE_COMMAND_INTERFACES[0])); + claimed_itfs.push_back(rm->claim_command_interface(TEST_SYSTEM_HARDWARE_COMMAND_INTERFACES[0])); + + state_itfs.push_back(rm->claim_state_interface(TEST_ACTUATOR_HARDWARE_STATE_INTERFACES[1])); + state_itfs.push_back(rm->claim_state_interface(TEST_SYSTEM_HARDWARE_STATE_INTERFACES[1])); + + check_if_interface_available(true, true); + // with default values read and write should run without any problems + { + auto [ok, failed_hardware_names] = rm->read(time, duration); + EXPECT_TRUE(ok); + EXPECT_TRUE(failed_hardware_names.empty()); + } + { + // claimed_itfs[0].set_value(10.0); + // claimed_itfs[1].set_value(20.0); + auto [ok, failed_hardware_names] = rm->write(time, duration); + EXPECT_TRUE(ok); + EXPECT_TRUE(failed_hardware_names.empty()); + } + time = time + duration; + check_if_interface_available(true, true); + } + + // check if all interfaces are available + void check_if_interface_available(const bool actuator_interfaces, const bool system_interfaces) + { + for (const auto & interface : TEST_ACTUATOR_HARDWARE_COMMAND_INTERFACES) + { + EXPECT_EQ(rm->command_interface_is_available(interface), actuator_interfaces); + } + for (const auto & interface : TEST_ACTUATOR_HARDWARE_STATE_INTERFACES) + { + EXPECT_EQ(rm->state_interface_is_available(interface), actuator_interfaces); + } + for (const auto & interface : TEST_SYSTEM_HARDWARE_COMMAND_INTERFACES) + { + EXPECT_EQ(rm->command_interface_is_available(interface), system_interfaces); + } + for (const auto & interface : TEST_SYSTEM_HARDWARE_STATE_INTERFACES) + { + EXPECT_EQ(rm->state_interface_is_available(interface), system_interfaces); + } + }; + + void check_read_and_write_cycles(bool check_for_updated_values) + { + double prev_act_state_value = state_itfs[0].get_value(); + double prev_system_state_value = state_itfs[1].get_value(); + const double actuator_increment = 10.0; + const double system_increment = 20.0; + for (size_t i = 1; i < 100; i++) + { + auto [ok, failed_hardware_names] = rm->read(time, duration); + EXPECT_TRUE(ok); + EXPECT_TRUE(failed_hardware_names.empty()); + std::this_thread::sleep_for(std::chrono::milliseconds(1)); + // The values are computations exactly within the test_components + if (check_for_updated_values) + { + prev_system_state_value = claimed_itfs[1].get_value() / 2.0; + prev_act_state_value = claimed_itfs[0].get_value() / 2.0; + } + claimed_itfs[0].set_value(claimed_itfs[0].get_value() + actuator_increment); + claimed_itfs[1].set_value(claimed_itfs[1].get_value() + system_increment); + // This is needed to account for any missing hits to the read and write cycles as the tests + // are going to be run on a non-RT operating system + ASSERT_NEAR(state_itfs[0].get_value(), prev_act_state_value, actuator_increment / 2.0); + ASSERT_NEAR(state_itfs[1].get_value(), prev_system_state_value, system_increment / 2.0); + auto [ok_write, failed_hardware_names_write] = rm->write(time, duration); + std::this_thread::sleep_for(std::chrono::milliseconds(1)); + EXPECT_TRUE(ok_write); + EXPECT_TRUE(failed_hardware_names_write.empty()); + time = time + duration; + } + } + +public: + std::shared_ptr rm; + unsigned int actuator_rw_rate_, system_rw_rate_, cm_update_rate_; + std::vector claimed_itfs; + std::vector state_itfs; + + rclcpp::Time time = rclcpp::Time(1657232, 0); + const rclcpp::Duration duration = rclcpp::Duration::from_seconds(0.01); +}; + +TEST_F(ResourceManagerTestAsyncReadWrite, test_components_with_async_components_on_activate) +{ + setup_resource_manager_and_do_initial_checks(); + check_read_and_write_cycles(true); +} + +TEST_F(ResourceManagerTestAsyncReadWrite, test_components_with_async_components_on_deactivate) +{ + setup_resource_manager_and_do_initial_checks(); + + // Now deactivate all the components and test the same as above + rclcpp_lifecycle::State state_inactive( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + hardware_interface::lifecycle_state_names::INACTIVE); + rm->set_component_state(TEST_SYSTEM_HARDWARE_NAME, state_inactive); + rm->set_component_state(TEST_ACTUATOR_HARDWARE_NAME, state_inactive); + rm->set_component_state(TEST_SENSOR_HARDWARE_NAME, state_inactive); + + auto status_map = rm->get_components_status(); + EXPECT_EQ( + status_map[TEST_ACTUATOR_HARDWARE_NAME].state.id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); + EXPECT_EQ( + status_map[TEST_SYSTEM_HARDWARE_NAME].state.id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); + EXPECT_EQ( + status_map[TEST_SENSOR_HARDWARE_NAME].state.id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); + + check_read_and_write_cycles(true); +} + +TEST_F(ResourceManagerTestAsyncReadWrite, test_components_with_async_components_on_unconfigured) +{ + setup_resource_manager_and_do_initial_checks(); + + // Now deactivate all the components and test the same as above + rclcpp_lifecycle::State state_unconfigured( + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + hardware_interface::lifecycle_state_names::UNCONFIGURED); + rm->set_component_state(TEST_SYSTEM_HARDWARE_NAME, state_unconfigured); + rm->set_component_state(TEST_ACTUATOR_HARDWARE_NAME, state_unconfigured); + rm->set_component_state(TEST_SENSOR_HARDWARE_NAME, state_unconfigured); + + auto status_map = rm->get_components_status(); + EXPECT_EQ( + status_map[TEST_ACTUATOR_HARDWARE_NAME].state.id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); + EXPECT_EQ( + status_map[TEST_SYSTEM_HARDWARE_NAME].state.id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); + EXPECT_EQ( + status_map[TEST_SENSOR_HARDWARE_NAME].state.id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); + + check_read_and_write_cycles(false); +} + +TEST_F(ResourceManagerTestAsyncReadWrite, test_components_with_async_components_on_finalized) +{ + setup_resource_manager_and_do_initial_checks(); + + // Now deactivate all the components and test the same as above + rclcpp_lifecycle::State state_finalized( + lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, + hardware_interface::lifecycle_state_names::FINALIZED); + rm->set_component_state(TEST_SYSTEM_HARDWARE_NAME, state_finalized); + rm->set_component_state(TEST_ACTUATOR_HARDWARE_NAME, state_finalized); + rm->set_component_state(TEST_SENSOR_HARDWARE_NAME, state_finalized); + + auto status_map = rm->get_components_status(); + EXPECT_EQ( + status_map[TEST_ACTUATOR_HARDWARE_NAME].state.id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED); + EXPECT_EQ( + status_map[TEST_SYSTEM_HARDWARE_NAME].state.id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED); + EXPECT_EQ( + status_map[TEST_SENSOR_HARDWARE_NAME].state.id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED); + + check_read_and_write_cycles(false); +} + int main(int argc, char ** argv) { rclcpp::init(argc, argv); diff --git a/joint_limits/CHANGELOG.rst b/joint_limits/CHANGELOG.rst index 5fbf3a44fa..1fa0b3a392 100644 --- a/joint_limits/CHANGELOG.rst +++ b/joint_limits/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package joint_limits ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +4.22.0 (2024-12-20) +------------------- + 4.21.0 (2024-12-06) ------------------- * Use the .hpp headers from realtime_tools package (`#1916 `_) diff --git a/joint_limits/package.xml b/joint_limits/package.xml index 129551a8b8..44ce8f5c50 100644 --- a/joint_limits/package.xml +++ b/joint_limits/package.xml @@ -1,6 +1,6 @@ joint_limits - 4.21.0 + 4.22.0 Package with interfaces for handling of joint limits in controllers or in hardware. The package also implements Saturation Joint Limiter for position-velocity-acceleration set and other individual interfaces. Bence Magyar diff --git a/ros2_control/CHANGELOG.rst b/ros2_control/CHANGELOG.rst index b448c0996f..231aa395c6 100644 --- a/ros2_control/CHANGELOG.rst +++ b/ros2_control/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ros2_control ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +4.22.0 (2024-12-20) +------------------- + 4.21.0 (2024-12-06) ------------------- diff --git a/ros2_control/package.xml b/ros2_control/package.xml index c883f00b14..7cc5192743 100644 --- a/ros2_control/package.xml +++ b/ros2_control/package.xml @@ -1,7 +1,7 @@ ros2_control - 4.21.0 + 4.22.0 Metapackage for ROS2 control related packages Bence Magyar Denis Štogl diff --git a/ros2_control_test_assets/CHANGELOG.rst b/ros2_control_test_assets/CHANGELOG.rst index 5f9b6bd6b9..bfa3c5aae0 100644 --- a/ros2_control_test_assets/CHANGELOG.rst +++ b/ros2_control_test_assets/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package ros2_control_test_assets ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +4.22.0 (2024-12-20) +------------------- +* Async Hardware Components (`#1567 `_) +* Let sensors also export state interfaces of joints (`#1885 `_) +* Contributors: Christoph Fröhlich, Sai Kishor Kothakota + 4.21.0 (2024-12-06) ------------------- * [Feature] Choose different read and write rate for the hardware components (`#1570 `_) diff --git a/ros2_control_test_assets/include/ros2_control_test_assets/descriptions.hpp b/ros2_control_test_assets/include/ros2_control_test_assets/descriptions.hpp index 5f4512a9d1..1744a806bc 100644 --- a/ros2_control_test_assets/include/ros2_control_test_assets/descriptions.hpp +++ b/ros2_control_test_assets/include/ros2_control_test_assets/descriptions.hpp @@ -662,7 +662,7 @@ const auto hardware_resources = const auto async_hardware_resources = R"( - + test_actuator @@ -683,7 +683,7 @@ const auto async_hardware_resources = - + test_system 2 diff --git a/ros2_control_test_assets/package.xml b/ros2_control_test_assets/package.xml index 5632af7ef3..27ecd415c7 100644 --- a/ros2_control_test_assets/package.xml +++ b/ros2_control_test_assets/package.xml @@ -2,7 +2,7 @@ ros2_control_test_assets - 4.21.0 + 4.22.0 The package provides shared test resources for ros2_control stack Bence Magyar diff --git a/ros2controlcli/CHANGELOG.rst b/ros2controlcli/CHANGELOG.rst index b8545c80da..cb70f08797 100644 --- a/ros2controlcli/CHANGELOG.rst +++ b/ros2controlcli/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ros2controlcli ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +4.22.0 (2024-12-20) +------------------- + 4.21.0 (2024-12-06) ------------------- * [Spawner] Accept parsing multiple `--param-file` arguments to spawner (`#1805 `_) diff --git a/ros2controlcli/package.xml b/ros2controlcli/package.xml index 7e12a15665..4c2a9cb99f 100644 --- a/ros2controlcli/package.xml +++ b/ros2controlcli/package.xml @@ -2,7 +2,7 @@ ros2controlcli - 4.21.0 + 4.22.0 The ROS 2 command line tools for ROS2 Control. diff --git a/ros2controlcli/setup.py b/ros2controlcli/setup.py index 57783e838f..66c614b3ab 100644 --- a/ros2controlcli/setup.py +++ b/ros2controlcli/setup.py @@ -19,7 +19,7 @@ setup( name=package_name, - version="4.21.0", + version="4.22.0", packages=find_packages(exclude=["test"]), data_files=[ ("share/" + package_name, ["package.xml"]), diff --git a/rqt_controller_manager/CHANGELOG.rst b/rqt_controller_manager/CHANGELOG.rst index d345ad040f..3ae6a8a682 100644 --- a/rqt_controller_manager/CHANGELOG.rst +++ b/rqt_controller_manager/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package rqt_controller_manager ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +4.22.0 (2024-12-20) +------------------- + 4.21.0 (2024-12-06) ------------------- diff --git a/rqt_controller_manager/package.xml b/rqt_controller_manager/package.xml index d504a8a0fd..cd944cf445 100644 --- a/rqt_controller_manager/package.xml +++ b/rqt_controller_manager/package.xml @@ -2,7 +2,7 @@ rqt_controller_manager - 4.21.0 + 4.22.0 Graphical frontend for interacting with the controller manager. Bence Magyar Denis Štogl diff --git a/rqt_controller_manager/setup.py b/rqt_controller_manager/setup.py index 0c6898c126..1d65fcef86 100644 --- a/rqt_controller_manager/setup.py +++ b/rqt_controller_manager/setup.py @@ -20,7 +20,7 @@ setup( name=package_name, - version="4.21.0", + version="4.22.0", packages=[package_name], data_files=[ ("share/ament_index/resource_index/packages", ["resource/" + package_name]), diff --git a/transmission_interface/CHANGELOG.rst b/transmission_interface/CHANGELOG.rst index 41e6d0f10c..6e94d7d826 100644 --- a/transmission_interface/CHANGELOG.rst +++ b/transmission_interface/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package transmission_interface ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +4.22.0 (2024-12-20) +------------------- + 4.21.0 (2024-12-06) ------------------- diff --git a/transmission_interface/package.xml b/transmission_interface/package.xml index a5fad4f86c..035e6f0370 100644 --- a/transmission_interface/package.xml +++ b/transmission_interface/package.xml @@ -2,7 +2,7 @@ transmission_interface - 4.21.0 + 4.22.0 transmission_interface contains data structures for representing mechanical transmissions, methods for propagating values between actuator and joint spaces and tooling to support this. Bence Magyar Denis Štogl