From bcfe8b5952901147f8c398f536de6a269f3694e5 Mon Sep 17 00:00:00 2001 From: Maciej Bednarczyk <83034299+mcbed@users.noreply.github.com> Date: Tue, 28 Jun 2022 00:16:47 +0200 Subject: [PATCH 01/35] fixed period always 0 (#738) --- controller_manager/src/ros2_control_node.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/controller_manager/src/ros2_control_node.cpp b/controller_manager/src/ros2_control_node.cpp index a66f930251f..813e8232c79 100644 --- a/controller_manager/src/ros2_control_node.cpp +++ b/controller_manager/src/ros2_control_node.cpp @@ -57,11 +57,11 @@ int main(int argc, char ** argv) std::chrono::nanoseconds((end_period - cm->now()).nanoseconds())); // execute update loop + current_time = cm->now(); auto period = current_time - previous_time; + previous_time = current_time; cm->read(current_time, period); - current_time = cm->now(); cm->update(current_time, period); - previous_time = current_time; cm->write(current_time, period); } }); From 7f7d858af70b5e8c2f958cb3eb83a8e13b9d7c23 Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Tue, 28 Jun 2022 12:09:18 +0100 Subject: [PATCH 02/35] Remove who aren't in the reviewer group --- .github/reviewer-lottery.yml | 1 - 1 file changed, 1 deletion(-) diff --git a/.github/reviewer-lottery.yml b/.github/reviewer-lottery.yml index 8377e131ee6..a5a67c6b564 100644 --- a/.github/reviewer-lottery.yml +++ b/.github/reviewer-lottery.yml @@ -39,5 +39,4 @@ groups: - bijoua29 - kasiceo - lm2292 - - 2b-t - mcbed From 2796b3f8df7c29221b133c13ea96ccfc78d34387 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Wed, 29 Jun 2022 17:20:24 +0200 Subject: [PATCH 03/35] Small but useful output update on controller manager. (#741) --- controller_manager/src/controller_manager.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 7b4fe6bab69..46e4a1260dd 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -244,7 +244,9 @@ controller_interface::ControllerInterfaceBaseSharedPtr ControllerManager::load_c !loader_->isClassAvailable(controller_type) && !chainable_loader_->isClassAvailable(controller_type)) { - RCLCPP_ERROR(get_logger(), "Loader for controller '%s' not found.", controller_name.c_str()); + RCLCPP_ERROR( + get_logger(), "Loader for controller '%s' (type '%s') not found.", controller_name.c_str(), + controller_type.c_str()); RCLCPP_INFO(get_logger(), "Available classes:"); for (const auto & available_class : loader_->getDeclaredClasses()) { From 189c4710695c083d584963d398afcc96cf0575fa Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Thu, 30 Jun 2022 09:25:30 +0200 Subject: [PATCH 04/35] Add missing field to initializer lists in tests (#746) --- .../test/test_hardware_management_srvs.cpp | 32 +++++++++---------- 1 file changed, 16 insertions(+), 16 deletions(-) diff --git a/controller_manager/test/test_hardware_management_srvs.cpp b/controller_manager/test/test_hardware_management_srvs.cpp index f4277ae5436..00a504d69a8 100644 --- a/controller_manager/test/test_hardware_management_srvs.cpp +++ b/controller_manager/test/test_hardware_management_srvs.cpp @@ -243,8 +243,8 @@ TEST_F(TestControllerManagerHWManagementSrvs, list_hardware_components) }), std::vector>>({ // is claimed - {{false, false}, {false, false}}, // actuator - {{}, {false}}, // sensor + {{false, false}, {false, false, false}}, // actuator + {{}, {false}}, // sensor {{false, false, false, false}, {false, false, false, false, false, false, false}}, // system })); } @@ -269,8 +269,8 @@ TEST_F(TestControllerManagerHWManagementSrvs, selective_activate_deactivate_comp }), std::vector>>({ // is claimed - {{false, false}, {false, false}}, // actuator - {{}, {false}}, // sensor + {{false, false}, {false, false, false}}, // actuator + {{}, {false}}, // sensor {{false, false, false, false}, {false, false, false, false, false, false, false}}, // system })); @@ -290,8 +290,8 @@ TEST_F(TestControllerManagerHWManagementSrvs, selective_activate_deactivate_comp }), std::vector>>({ // is claimed - {{false, false}, {false, false}}, // actuator - {{}, {false}}, // sensor + {{false, false}, {false, false, false}}, // actuator + {{}, {false}}, // sensor {{false, false, false, false}, {false, false, false, false, false, false, false}}, // system })); @@ -311,8 +311,8 @@ TEST_F(TestControllerManagerHWManagementSrvs, selective_activate_deactivate_comp }), std::vector>>({ // is claimed - {{false, false}, {false, false}}, // actuator - {{}, {false}}, // sensor + {{false, false}, {false, false, false}}, // actuator + {{}, {false}}, // sensor {{false, false, false, false}, {false, false, false, false, false, false, false}}, // system })); @@ -332,8 +332,8 @@ TEST_F(TestControllerManagerHWManagementSrvs, selective_activate_deactivate_comp }), std::vector>>({ // is claimed - {{false, false}, {false, false}}, // actuator - {{}, {false}}, // sensor + {{false, false}, {false, false, false}}, // actuator + {{}, {false}}, // sensor {{false, false, false, false}, {false, false, false, false, false, false, false}}, // system })); @@ -353,8 +353,8 @@ TEST_F(TestControllerManagerHWManagementSrvs, selective_activate_deactivate_comp }), std::vector>>({ // is claimed - {{false, false}, {false, false}}, // actuator - {{}, {false}}, // sensor + {{false, false}, {false, false, false}}, // actuator + {{}, {false}}, // sensor {{false, false, false, false}, {false, false, false, false, false, false, false}}, // system })); @@ -375,8 +375,8 @@ TEST_F(TestControllerManagerHWManagementSrvs, selective_activate_deactivate_comp }), std::vector>>({ // is claimed - {{false, false}, {false, false}}, // actuator - {{}, {false}}, // sensor + {{false, false}, {false, false, false}}, // actuator + {{}, {false}}, // sensor {{false, false, false, false}, {false, false, false, false, false, false, false}}, // system })); } @@ -399,8 +399,8 @@ TEST_F(TestControllerManagerHWManagementSrvsWithoutParams, test_default_activati }), std::vector>>({ // is claimed - {{false, false}, {false, false}}, // actuator - {{}, {false}}, // sensor + {{false, false}, {false, false, false}}, // actuator + {{}, {false}}, // sensor {{false, false, false, false}, {false, false, false, false, false, false, false}}, // system })); } From 1ef3678302b6ac7a19aa30e40d51cc471c74b517 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Thu, 30 Jun 2022 09:26:24 +0200 Subject: [PATCH 05/35] Fixup ament cpplint on 22.04 (#747) --- .../transmission_interface/simple_transmission_loader.hpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/transmission_interface/include/transmission_interface/simple_transmission_loader.hpp b/transmission_interface/include/transmission_interface/simple_transmission_loader.hpp index 941e07e5d7f..faef32006d6 100644 --- a/transmission_interface/include/transmission_interface/simple_transmission_loader.hpp +++ b/transmission_interface/include/transmission_interface/simple_transmission_loader.hpp @@ -15,11 +15,11 @@ #ifndef TRANSMISSION_INTERFACE__SIMPLE_TRANSMISSION_LOADER_HPP_ #define TRANSMISSION_INTERFACE__SIMPLE_TRANSMISSION_LOADER_HPP_ +#include + #include #include -#include - namespace transmission_interface { /** From 141071621b7693fc110640c27084ce0982a5e9a2 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Thu, 30 Jun 2022 11:09:04 +0200 Subject: [PATCH 06/35] Fixup spanwer and unspawner tests. It changes spawner a bit to handle interupts internally. (#745) --- controller_manager/controller_manager/spawner.py | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/controller_manager/controller_manager/spawner.py b/controller_manager/controller_manager/spawner.py index a5122635ca3..073f44638a8 100644 --- a/controller_manager/controller_manager/spawner.py +++ b/controller_manager/controller_manager/spawner.py @@ -27,6 +27,7 @@ import rclpy from rclpy.duration import Duration from rclpy.node import Node +from rclpy.signals import SignalHandlerOptions # from https://stackoverflow.com/a/287944 @@ -117,7 +118,7 @@ def make_absolute(name): def main(args=None): - rclpy.init(args=args) + rclpy.init(args=args, signal_handler_options=SignalHandlerOptions.NO) parser = argparse.ArgumentParser() parser.add_argument( 'controller_name', help='Name of the controller') From 70514dd0a942051fb77e265e829aebd2ef9f5744 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Thu, 30 Jun 2022 16:50:25 +0200 Subject: [PATCH 07/35] Full functionality of chainable controllers in controller manager (#667) * auto-switching of chained mode in controllers * interface-matching approach for managing chaining controllers --- .../controller_interface.hpp | 2 +- .../include/controller_interface/helpers.hpp | 6 +- controller_manager/CMakeLists.txt | 18 + .../controller_manager/controller_manager.hpp | 90 ++- controller_manager/src/controller_manager.cpp | 614 +++++++++++++++-- .../test_chainable_controller.cpp | 192 ++++++ .../test_chainable_controller.hpp | 93 +++ .../test_chainable_controller.xml | 11 + .../test/test_controller/test_controller.cpp | 10 + .../test/test_controller/test_controller.hpp | 3 + .../test/test_controller_manager_srvs.cpp | 3 + ...llers_chaining_with_controller_manager.cpp | 621 ++++++++++++++++++ .../test/test_hardware_management_srvs.cpp | 2 - .../hardware_interface/resource_manager.hpp | 2 +- .../test/test_components/test_actuator.cpp | 5 + 15 files changed, 1618 insertions(+), 54 deletions(-) create mode 100644 controller_manager/test/test_chainable_controller/test_chainable_controller.cpp create mode 100644 controller_manager/test/test_chainable_controller/test_chainable_controller.hpp create mode 100644 controller_manager/test/test_chainable_controller/test_chainable_controller.xml create mode 100644 controller_manager/test/test_controllers_chaining_with_controller_manager.cpp diff --git a/controller_interface/include/controller_interface/controller_interface.hpp b/controller_interface/include/controller_interface/controller_interface.hpp index efdab02d62e..a3d006755f5 100644 --- a/controller_interface/include/controller_interface/controller_interface.hpp +++ b/controller_interface/include/controller_interface/controller_interface.hpp @@ -53,7 +53,7 @@ class ControllerInterface : public controller_interface::ControllerInterfaceBase /** * Controller is not chainable, therefore no chained mode can be set. * - * \returns false; + * \returns false. */ CONTROLLER_INTERFACE_PUBLIC bool set_chained_mode(bool chained_mode) final; diff --git a/controller_interface/include/controller_interface/helpers.hpp b/controller_interface/include/controller_interface/helpers.hpp index af63f4e9a57..eaaeb9dab83 100644 --- a/controller_interface/include/controller_interface/helpers.hpp +++ b/controller_interface/include/controller_interface/helpers.hpp @@ -48,7 +48,11 @@ bool get_ordered_interfaces( { if (!interface_type.empty()) { - if ((name == interface.get_name()) && (interface_type == interface.get_interface_name())) + // check case where: + // ( == AND == ) OR / == 'full name' + if ( + ((name == interface.get_name()) && (interface_type == interface.get_interface_name())) || + ((name + "/" + interface_type) == interface.get_full_name())) { ordered_interfaces.push_back(std::ref(interface)); } diff --git a/controller_manager/CMakeLists.txt b/controller_manager/CMakeLists.txt index b103e4c0945..be6bd89e4b5 100644 --- a/controller_manager/CMakeLists.txt +++ b/controller_manager/CMakeLists.txt @@ -89,6 +89,16 @@ if(BUILD_TESTING) DESTINATION lib ) + add_library(test_chainable_controller SHARED test/test_chainable_controller/test_chainable_controller.cpp) + target_include_directories(test_chainable_controller PRIVATE include) + target_link_libraries(test_chainable_controller controller_manager) + target_compile_definitions(test_chainable_controller PRIVATE "CONTROLLER_MANAGER_BUILDING_DLL") + pluginlib_export_plugin_description_file( + controller_interface test/test_chainable_controller/test_chainable_controller.xml) + install(TARGETS test_chainable_controller + DESTINATION lib + ) + ament_add_gmock( test_controller_manager test/test_controller_manager.cpp @@ -114,6 +124,14 @@ if(BUILD_TESTING) target_link_libraries(test_load_controller ${PROJECT_NAME} test_controller test_controller_failed_init) ament_target_dependencies(test_load_controller ros2_control_test_assets) + ament_add_gmock( + test_controllers_chaining_with_controller_manager + test/test_controllers_chaining_with_controller_manager.cpp + ) + target_include_directories(test_controllers_chaining_with_controller_manager PRIVATE include) + target_link_libraries(test_controllers_chaining_with_controller_manager controller_manager test_chainable_controller test_controller) + ament_target_dependencies(test_controllers_chaining_with_controller_manager ros2_control_test_assets) + ament_add_gmock( test_controller_manager_srvs test/test_controller_manager_srvs.cpp diff --git a/controller_manager/include/controller_manager/controller_manager.hpp b/controller_manager/include/controller_manager/controller_manager.hpp index e3f253c5e71..b178e21665a 100644 --- a/controller_manager/include/controller_manager/controller_manager.hpp +++ b/controller_manager/include/controller_manager/controller_manager.hpp @@ -15,9 +15,11 @@ #ifndef CONTROLLER_MANAGER__CONTROLLER_MANAGER_HPP_ #define CONTROLLER_MANAGER__CONTROLLER_MANAGER_HPP_ +#include #include #include #include +#include #include #include "controller_interface/chainable_controller_interface.hpp" @@ -40,15 +42,21 @@ #include "controller_manager_msgs/srv/switch_controller.hpp" #include "controller_manager_msgs/srv/unload_controller.hpp" +#include "hardware_interface/handle.hpp" #include "hardware_interface/resource_manager.hpp" #include "pluginlib/class_loader.hpp" #include "rclcpp/executor.hpp" #include "rclcpp/node.hpp" +#include "rclcpp/node_interfaces/node_logging_interface.hpp" +#include "rclcpp/node_interfaces/node_parameters_interface.hpp" +#include "rclcpp/parameter.hpp" namespace controller_manager { +using ControllersListIterator = std::vector::const_iterator; + class ControllerManager : public rclcpp::Node { public: @@ -169,6 +177,18 @@ class ControllerManager : public rclcpp::Node CONTROLLER_MANAGER_PUBLIC void stop_controllers(); + /** + * Switch chained mode for all the controllers with respect to the following cases: + * - a preceding controller is getting activated --> switch controller to chained mode; + * - all preceding controllers are deactivated --> switch controller from chained mode. + * + * \param[in] chained_mode_switch_list list of controller to switch chained mode. + * \param[in] to_chained_mode flag if controller should be switched *to* or *from* chained mode. + */ + CONTROLLER_MANAGER_PUBLIC + void switch_chained_mode( + const std::vector & chained_mode_switch_list, bool to_chained_mode); + CONTROLLER_MANAGER_PUBLIC void start_controllers(); @@ -243,11 +263,78 @@ class ControllerManager : public rclcpp::Node // Per controller update rate support unsigned int update_loop_counter_ = 0; unsigned int update_rate_ = 100; + std::vector> chained_controllers_configuration_; + + std::unique_ptr resource_manager_; private: std::vector get_controller_names(); - std::unique_ptr resource_manager_; + /** + * Clear request lists used when switching controllers. The lists are shared between "callback" and + * "control loop" threads. + */ + void clear_requests(); + + /** + * If a controller is deactivated all following controllers (if any exist) should be switched + * 'from' the chained mode. + * + * \param[in] controllers list with controllers. + */ + void propagate_deactivation_of_chained_mode(const std::vector & controllers); + + /// Check if all the following controllers will be in active state and in the chained mode + /// after controllers' switch. + /** + * Check recursively that all following controllers of the @controller_it + * - are already active, + * - will not be deactivated, + * - or will be activated. + * The following controllers are added to the request to switch in the chained mode or removed + * from the request to switch from the chained mode. + * + * For each controller the whole chain of following controllers is checked. + * + * NOTE: The automatically adding of following controller into starting list is not implemented + * yet. + * + * \param[in] controllers list with controllers. + * \param[in] strictness if value is equal "MANIPULATE_CONTROLLERS_CHAIN" then all following + * controllers will be automatically added to the activate request list if they are not in the + * deactivate request. + * \param[in] controller_it iterator to the controller for which the following controllers are + * checked. + * + * \returns return_type::OK if all following controllers pass the checks, otherwise + * return_type::ERROR. + */ + controller_interface::return_type check_following_controllers_for_activate( + const std::vector & controllers, int strictness, + const ControllersListIterator controller_it); + + /// Check if all the preceding controllers will be in inactive state after controllers' switch. + /** + * Check that all preceding controllers of the @controller_it + * - are inactive, + * - will be deactivated, + * - and will not be activated. + * + * NOTE: The automatically adding of preceding controllers into stopping list is not implemented + * yet. + * + * \param[in] controllers list with controllers. + * \param[in] strictness if value is equal "MANIPULATE_CONTROLLERS_CHAIN" then all preceding + * controllers will be automatically added to the deactivate request list. + * \param[in] controller_it iterator to the controller for which the preceding controllers are + * checked. + * + * \returns return_type::OK if all preceding controllers pass the checks, otherwise + * return_type::ERROR. + */ + controller_interface::return_type check_preceeding_controllers_for_deactivate( + const std::vector & controllers, int strictness, + const ControllersListIterator controller_it); std::shared_ptr executor_; @@ -366,6 +453,7 @@ class ControllerManager : public rclcpp::Node set_hardware_component_state_service_; std::vector start_request_, stop_request_; + std::vector to_chained_mode_request_, from_chained_mode_request_; std::vector start_command_interface_request_, stop_command_interface_request_; struct SwitchParams diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 46e4a1260dd..81aa87f42ad 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -27,9 +27,8 @@ #include "rclcpp/rclcpp.hpp" #include "rclcpp_lifecycle/state.hpp" -namespace -{ // utility - +namespace // utility +{ static constexpr const char * kControllerInterfaceNamespace = "controller_interface"; static constexpr const char * kControllerInterfaceClassName = "controller_interface::ControllerInterface"; @@ -75,6 +74,56 @@ bool controller_name_compare(const controller_manager::ControllerSpec & a, const return a.info.name == name; } +/// Checks if a command interface belongs to a controller based on its prefix. +/** + * A command interface can be provided by a controller in which case is called "reference" + * interface. + * This means that the @interface_name starts with the name of a controller. + * + * \param[in] interface_name to be found in the map. + * \param[in] controllers list of controllers to compare their names to interface's prefix. + * \param[out] following_controller_it iterator to the following controller that reference interface + * @interface_name belongs to. + * \return true if interface has a controller name as prefix, false otherwise. + */ +bool command_interface_is_reference_interface_of_controller( + const std::string interface_name, + const std::vector & controllers, + controller_manager::ControllersListIterator & following_controller_it) +{ + auto split_pos = interface_name.find_first_of('/'); + if (split_pos == std::string::npos) // '/' exist in the string (should be always false) + { + RCLCPP_FATAL( + rclcpp::get_logger("ControllerManager::utils"), + "Character '/', was not find in the interface name '%s'. This should never happen. " + "Stop the controller manager immediately and restart it.", + interface_name.c_str()); + throw std::runtime_error("Mismatched interface name. See the FATAL message above."); + } + + auto interface_prefix = interface_name.substr(0, split_pos); + following_controller_it = std::find_if( + controllers.begin(), controllers.end(), + std::bind(controller_name_compare, std::placeholders::_1, interface_prefix)); + + RCLCPP_DEBUG( + rclcpp::get_logger("ControllerManager::utils"), + "Deduced interface prefix '%s' - searching for the controller with the same name.", + interface_prefix.c_str()); + + if (following_controller_it == controllers.end()) + { + RCLCPP_DEBUG( + rclcpp::get_logger("ControllerManager::utils"), + "Required command interface '%s' with prefix '%s' is not reference interface.", + interface_name.c_str(), interface_prefix.c_str()); + + return false; + } + return true; +} + } // namespace namespace controller_manager @@ -258,6 +307,7 @@ controller_interface::ControllerInterfaceBaseSharedPtr ControllerManager::load_c } return nullptr; } + RCLCPP_DEBUG(get_logger(), "Loader for controller '%s' found.", controller_name.c_str()); controller_interface::ControllerInterfaceBaseSharedPtr controller; @@ -339,6 +389,8 @@ controller_interface::return_type ControllerManager::unload_controller( } RCLCPP_DEBUG(get_logger(), "Cleanup controller"); + // TODO(destogl): remove reference interface if chainable; i.e., add a separate method for + // cleaning-up controllers? controller.c->get_node()->cleanup(); executor_->remove_node(controller.c->get_node()->get_node_base_interface()); to.erase(found_it); @@ -398,6 +450,8 @@ controller_interface::return_type ControllerManager::configure_controller( { RCLCPP_DEBUG( get_logger(), "Controller '%s' is cleaned-up before configuring", controller_name.c_str()); + // TODO(destogl): remove reference interface if chainable; i.e., add a separate method for + // cleaning-up controllers? new_state = controller->get_node()->cleanup(); if (new_state.id() != lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED) { @@ -417,9 +471,40 @@ controller_interface::return_type ControllerManager::configure_controller( return controller_interface::return_type::ERROR; } + // CHAINABLE CONTROLLERS: get reference interfaces from chainable controllers + if (controller->is_chainable()) + { + RCLCPP_DEBUG( + get_logger(), + "Controller '%s' is chainable. Interfaces are being exported to resource manager.", + controller_name.c_str()); + auto interfaces = controller->export_reference_interfaces(); + if (interfaces.empty()) + { + // TODO(destogl): Add test for this! + RCLCPP_ERROR( + get_logger(), "Controller '%s' is chainable, but does not export any reference interfaces.", + controller_name.c_str()); + return controller_interface::return_type::ERROR; + } + resource_manager_->import_controller_reference_interfaces(controller_name, interfaces); + + // TODO(destogl): check and resort controllers in the vector + } + return controller_interface::return_type::OK; } +void ControllerManager::clear_requests() +{ + stop_request_.clear(); + start_request_.clear(); + to_chained_mode_request_.clear(); + from_chained_mode_request_.clear(); + start_command_interface_request_.clear(); + stop_command_interface_request_.clear(); +} + controller_interface::return_type ControllerManager::switch_controller( const std::vector & start_controllers, const std::vector & stop_controllers, int strictness, bool start_asap, @@ -432,14 +517,24 @@ controller_interface::return_type ControllerManager::switch_controller( RCLCPP_FATAL( get_logger(), "The internal stop and start request lists are not empty at the beginning of the " - "switchController() call. This should not happen."); + "switch_controller() call. This should never happen."); + throw std::runtime_error("CM's internal state is not correct. See the FATAL message above."); } if (!stop_command_interface_request_.empty() || !start_command_interface_request_.empty()) { RCLCPP_FATAL( get_logger(), "The internal stop and start requests command interface lists are not empty at the " - "switch_controller() call. This should not happen."); + "switch_controller() call. This should never happen."); + throw std::runtime_error("CM's internal state is not correct. See the FATAL message above."); + } + if (!from_chained_mode_request_.empty() || !to_chained_mode_request_.empty()) + { + RCLCPP_FATAL( + get_logger(), + "The internal 'from' and 'to' chained mode requests are not empty at the " + "switch_controller() call. This should never happen."); + throw std::runtime_error("CM's internal state is not correct. See the FATAL message above."); } if (strictness == 0) { @@ -482,19 +577,15 @@ controller_interface::return_type ControllerManager::switch_controller( if (found_it == updated_controllers.end()) { - if (strictness == controller_manager_msgs::srv::SwitchController::Request::STRICT) - { - RCLCPP_ERROR( - get_logger(), - R"(Could not '%s' controller with name '%s' because - no controller with this name exists)", - action.c_str(), controller.c_str()); - return controller_interface::return_type::ERROR; - } RCLCPP_WARN( get_logger(), "Could not '%s' controller with name '%s' because no controller with this name exists", action.c_str(), controller.c_str()); + if (strictness == controller_manager_msgs::srv::SwitchController::Request::STRICT) + { + RCLCPP_ERROR(get_logger(), "Aborting, no controller is switched! ('STRICT' switch)"); + return controller_interface::return_type::ERROR; + } } else { @@ -510,7 +601,7 @@ controller_interface::return_type ControllerManager::switch_controller( return controller_interface::return_type::OK; }; - // list all controllers to stop + // list all controllers to stop (check if all controllers exist) auto ret = list_controllers(stop_controllers, stop_request_, "stop"); if (ret != controller_interface::return_type::OK) { @@ -518,7 +609,7 @@ controller_interface::return_type ControllerManager::switch_controller( return ret; } - // list all controllers to start + // list all controllers to start (check if all controllers exist) ret = list_controllers(start_controllers, start_request_, "start"); if (ret != controller_interface::return_type::OK) { @@ -527,42 +618,146 @@ controller_interface::return_type ControllerManager::switch_controller( return ret; } - const auto list_interfaces = - [this](const ControllerSpec controller, std::vector & request_interface_list) + // lock controllers + std::lock_guard guard(rt_controllers_wrapper_.controllers_lock_); + + const std::vector & controllers = rt_controllers_wrapper_.get_updated_list(guard); + + // if a preceding controller is deactivated, all first-level controllers should be switched 'from' + // chained mode + propagate_deactivation_of_chained_mode(controllers); + + // check if controllers should be switched 'to' chained mode when controllers are activated + for (auto ctrl_it = start_request_.begin(); ctrl_it != start_request_.end(); ++ctrl_it) { - auto command_interface_config = controller.c->command_interface_configuration(); - std::vector command_interface_names = {}; - if (command_interface_config.type == controller_interface::interface_configuration_type::ALL) + auto controller_it = std::find_if( + controllers.begin(), controllers.end(), + std::bind(controller_name_compare, std::placeholders::_1, *ctrl_it)); + controller_interface::return_type ret = controller_interface::return_type::OK; + + // if controller is not inactive then do not do any following-controllers checks + if (!is_controller_inactive(controller_it->c)) { - command_interface_names = resource_manager_->available_command_interfaces(); + RCLCPP_WARN( + get_logger(), + "Controller with name '%s' is not inactive so its following" + "controllers do not have to be checked, because it cannot be activated.", + controller_it->info.name.c_str()); + ret = controller_interface::return_type::ERROR; } - if ( - command_interface_config.type == - controller_interface::interface_configuration_type::INDIVIDUAL) + else { - command_interface_names = command_interface_config.names; + ret = check_following_controllers_for_activate(controllers, strictness, controller_it); } - request_interface_list.insert( - request_interface_list.end(), command_interface_names.begin(), command_interface_names.end()); - }; - // lock controllers - std::lock_guard guard(rt_controllers_wrapper_.controllers_lock_); + if (ret != controller_interface::return_type::OK) + { + RCLCPP_WARN( + get_logger(), + "Could not activate controller with name '%s'. (Check above warnings for more details.) " + "Check the state of the controllers and their required interfaces using " + "`ros2 control list_controllers -v` to get more information.", + (*ctrl_it).c_str()); + if (strictness == controller_manager_msgs::srv::SwitchController::Request::BEST_EFFORT) + { + // TODO(destogl): automatic manipulation of the chain: + // || strictness == + // controller_manager_msgs::srv::SwitchController::Request::MANIPULATE_CONTROLLERS_CHAIN); + // remove controller that can not be activated from the activation request and step-back + // iterator to correctly step to the next element in the list in the loop + start_request_.erase(ctrl_it); + --ctrl_it; + } + if (strictness == controller_manager_msgs::srv::SwitchController::Request::STRICT) + { + RCLCPP_ERROR(get_logger(), "Aborting, no controller is switched! (::STRICT switch)"); + // reset all lists + clear_requests(); + return controller_interface::return_type::ERROR; + } + } + } - const std::vector & controllers = rt_controllers_wrapper_.get_updated_list(guard); + // check if controllers should be deactivated if used in chained mode + for (auto ctrl_it = stop_request_.begin(); ctrl_it != stop_request_.end(); ++ctrl_it) + { + auto controller_it = std::find_if( + controllers.begin(), controllers.end(), + std::bind(controller_name_compare, std::placeholders::_1, *ctrl_it)); + controller_interface::return_type ret = controller_interface::return_type::OK; + + // if controller is not active then skip preceding-controllers checks + if (!is_controller_active(controller_it->c)) + { + RCLCPP_WARN( + get_logger(), "Controller with name '%s' can not be deactivated since is not active.", + controller_it->info.name.c_str()); + ret = controller_interface::return_type::ERROR; + } + else + { + ret = check_preceeding_controllers_for_deactivate(controllers, strictness, controller_it); + } + + if (ret != controller_interface::return_type::OK) + { + RCLCPP_WARN( + get_logger(), + "Could not deactivate controller with name '%s'. (Check above warnings for more details.)" + "Check the state of the controllers and their required interfaces using " + "`ros2 control list_controllers -v` to get more information.", + (*ctrl_it).c_str()); + if (strictness == controller_manager_msgs::srv::SwitchController::Request::BEST_EFFORT) + { + // remove controller that can not be activated from the activation request and step-back + // iterator to correctly step to the next element in the list in the loop + stop_request_.erase(ctrl_it); + --ctrl_it; + } + if (strictness == controller_manager_msgs::srv::SwitchController::Request::STRICT) + { + RCLCPP_ERROR(get_logger(), "Aborting, no controller is switched! (::STRICT switch)"); + // reset all lists + clear_requests(); + return controller_interface::return_type::ERROR; + } + } + } for (const auto & controller : controllers) { + auto to_chained_mode_list_it = std::find( + to_chained_mode_request_.begin(), to_chained_mode_request_.end(), controller.info.name); + bool in_to_chained_mode_list = to_chained_mode_list_it != to_chained_mode_request_.end(); + + auto from_chained_mode_list_it = std::find( + from_chained_mode_request_.begin(), from_chained_mode_request_.end(), controller.info.name); + bool in_from_chained_mode_list = from_chained_mode_list_it != from_chained_mode_request_.end(); + auto stop_list_it = std::find(stop_request_.begin(), stop_request_.end(), controller.info.name); bool in_stop_list = stop_list_it != stop_request_.end(); + const bool is_active = is_controller_active(*controller.c); + const bool is_inactive = is_controller_inactive(*controller.c); + + // restart controllers that need to switch their 'chained mode' - add to stop/start lists + if (in_to_chained_mode_list || in_from_chained_mode_list) + { + if (is_active && !in_stop_list) + { + stop_request_.push_back(controller.info.name); + start_request_.push_back(controller.info.name); + } + } + + // get pointers to places in stop and start lists (start/stop lists have changed) + stop_list_it = std::find(stop_request_.begin(), stop_request_.end(), controller.info.name); + in_stop_list = stop_list_it != stop_request_.end(); + auto start_list_it = std::find(start_request_.begin(), start_request_.end(), controller.info.name); bool in_start_list = start_list_it != start_request_.end(); - const bool is_active = is_controller_active(*controller.c); - const bool is_inactive = is_controller_inactive(*controller.c); - auto handle_conflict = [&](const std::string & msg) { if (strictness == controller_manager_msgs::srv::SwitchController::Request::STRICT) @@ -572,6 +767,8 @@ controller_interface::return_type ControllerManager::switch_controller( stop_command_interface_request_.clear(); start_request_.clear(); start_command_interface_request_.clear(); + to_chained_mode_request_.clear(); + from_chained_mode_request_.clear(); return controller_interface::return_type::ERROR; } RCLCPP_WARN(get_logger(), "%s", msg.c_str()); @@ -618,21 +815,40 @@ controller_interface::return_type ControllerManager::switch_controller( start_request_.erase(start_list_it); } + const auto extract_interfaces_for_controller = + [this](const ControllerSpec controller, std::vector & request_interface_list) + { + auto command_interface_config = controller.c->command_interface_configuration(); + std::vector command_interface_names = {}; + if (command_interface_config.type == controller_interface::interface_configuration_type::ALL) + { + command_interface_names = resource_manager_->available_command_interfaces(); + } + if ( + command_interface_config.type == + controller_interface::interface_configuration_type::INDIVIDUAL) + { + command_interface_names = command_interface_config.names; + } + request_interface_list.insert( + request_interface_list.end(), command_interface_names.begin(), + command_interface_names.end()); + }; + if (in_start_list) { - list_interfaces(controller, start_command_interface_request_); + extract_interfaces_for_controller(controller, start_command_interface_request_); } if (in_stop_list) { - list_interfaces(controller, stop_command_interface_request_); + extract_interfaces_for_controller(controller, stop_command_interface_request_); } } if (start_request_.empty() && stop_request_.empty()) { RCLCPP_INFO(get_logger(), "Empty start and stop list, not requesting switch"); - start_command_interface_request_.clear(); - stop_command_interface_request_.clear(); + clear_requests(); return controller_interface::return_type::OK; } @@ -644,10 +860,7 @@ controller_interface::return_type ControllerManager::switch_controller( RCLCPP_ERROR( get_logger(), "Could not switch controllers since prepare command mode switch was rejected."); - start_request_.clear(); - stop_request_.clear(); - start_command_interface_request_.clear(); - stop_command_interface_request_.clear(); + clear_requests(); return controller_interface::return_type::ERROR; } } @@ -659,7 +872,7 @@ controller_interface::return_type ControllerManager::switch_controller( switch_params_.do_switch = true; // wait until switch is finished - RCLCPP_DEBUG(get_logger(), "Request atomic controller switch from realtime loop"); + RCLCPP_DEBUG(get_logger(), "Requested atomic controller switch from realtime loop"); while (rclcpp::ok() && switch_params_.do_switch) { if (!rclcpp::ok()) @@ -700,11 +913,8 @@ controller_interface::return_type ControllerManager::switch_controller( // clear unused list rt_controllers_wrapper_.get_unused_list(guard).clear(); - start_request_.clear(); - stop_request_.clear(); + clear_requests(); - start_command_interface_request_.clear(); - stop_command_interface_request_.clear(); RCLCPP_DEBUG(get_logger(), "Successfully switched controllers"); return controller_interface::return_type::OK; } @@ -780,6 +990,9 @@ void ControllerManager::manage_switch() stop_controllers(); + switch_chained_mode(to_chained_mode_request_, true); + switch_chained_mode(from_chained_mode_request_, false); + // start controllers once the switch is fully complete if (!switch_params_.start_asap) { @@ -790,6 +1003,8 @@ void ControllerManager::manage_switch() // start controllers as soon as their required joints are done switching start_controllers_asap(); } + + // TODO(destogl): move here "do_switch = false" } void ControllerManager::stop_controllers() @@ -825,6 +1040,63 @@ void ControllerManager::stop_controllers() } } +void ControllerManager::switch_chained_mode( + const std::vector & chained_mode_switch_list, bool to_chained_mode) +{ + std::vector & rt_controller_list = + rt_controllers_wrapper_.update_and_get_used_by_rt_list(); + + for (const auto & request : chained_mode_switch_list) + { + auto found_it = std::find_if( + rt_controller_list.begin(), rt_controller_list.end(), + std::bind(controller_name_compare, std::placeholders::_1, request)); + if (found_it == rt_controller_list.end()) + { + RCLCPP_FATAL( + get_logger(), + "Got request to turn %s chained mode for controller '%s', but controller is not in the " + "realtime controller list. (This should never happen!)", + (to_chained_mode ? "ON" : "OFF"), request.c_str()); + continue; + } + auto controller = found_it->c; + if (!is_controller_active(*controller)) + { + if (controller->set_chained_mode(to_chained_mode)) + { + if (to_chained_mode) + { + resource_manager_->make_controller_reference_interfaces_available(request); + } + else + { + resource_manager_->make_controller_reference_interfaces_unavailable(request); + } + } + else + { + RCLCPP_ERROR( + get_logger(), + "Got request to turn %s chained mode for controller '%s', but controller refused to do " + "it! The control will probably not work as expected. Try to restart all controllers. " + "If " + "the error persist check controllers' individual configuration.", + (to_chained_mode ? "ON" : "OFF"), request.c_str()); + } + } + else + { + RCLCPP_FATAL( + get_logger(), + "Got request to turn %s chained mode for controller '%s', but this can not happen if " + "controller is in '%s' state. (This should never happen!)", + (to_chained_mode ? "ON" : "OFF"), request.c_str(), + hardware_interface::lifecycle_state_names::ACTIVE); + } + } +} + void ControllerManager::start_controllers() { std::vector & rt_controller_list = @@ -1504,4 +1776,250 @@ void ControllerManager::RTControllerListWrapper::wait_until_rt_not_using( unsigned int ControllerManager::get_update_rate() const { return update_rate_; } +void ControllerManager::propagate_deactivation_of_chained_mode( + const std::vector & controllers) +{ + for (const auto & controller : controllers) + { + // get pointers to places in stop and start lists (start/stop lists have changed) + auto stop_list_it = std::find(stop_request_.begin(), stop_request_.end(), controller.info.name); + + if (stop_list_it != stop_request_.end()) + { + // if controller is not active then skip adding following-controllers to "from" chained mode + // request + if (!is_controller_active(controller.c)) + { + RCLCPP_DEBUG( + get_logger(), + "Controller with name '%s' can not be deactivated since is not active. " + "The controller will be removed from the list later." + "Skipping adding following controllers to 'from' chained mode request.", + controller.info.name.c_str()); + break; + } + + for (const auto & cmd_itf_name : controller.c->command_interface_configuration().names) + { + // controller that 'cmd_tf_name' belongs to + ControllersListIterator following_ctrl_it; + if (command_interface_is_reference_interface_of_controller( + cmd_itf_name, controllers, following_ctrl_it)) + { + // currently iterated "controller" is preceding controller --> add following controller + // with matching interface name to "from" chained mode list (if not already in it) + if ( + std::find( + from_chained_mode_request_.begin(), from_chained_mode_request_.end(), + following_ctrl_it->info.name) == from_chained_mode_request_.end()) + { + from_chained_mode_request_.push_back(following_ctrl_it->info.name); + RCLCPP_DEBUG( + get_logger(), "Adding controller '%s' in 'from chained mode' request.", + following_ctrl_it->info.name.c_str()); + } + } + } + } + } +} + +controller_interface::return_type ControllerManager::check_following_controllers_for_activate( + const std::vector & controllers, int strictness, + const ControllersListIterator controller_it) +{ + // we assume that the controller exists is checked in advance + RCLCPP_DEBUG( + get_logger(), "Checking following controllers of preceding controller with name '%s'.", + controller_it->info.name.c_str()); + + for (const auto & cmd_itf_name : controller_it->c->command_interface_configuration().names) + { + ControllersListIterator following_ctrl_it; + // Check if interface if reference interface and following controller exist. + if (!command_interface_is_reference_interface_of_controller( + cmd_itf_name, controllers, following_ctrl_it)) + { + continue; + } + // TODO(destogl): performance of this code could be optimized by adding additional lists with + // controllers that cache if the check has failed and has succeeded. Then the following would be + // done only once per controller, otherwise in complex scenarios the same controller is checked + // multiple times + + // check that all following controllers exits, are either: activated, will be activated, or + // will not be deactivated + RCLCPP_DEBUG( + get_logger(), "Checking following controller with name '%s'.", + following_ctrl_it->info.name.c_str()); + + // check if following controller is chainable + if (!following_ctrl_it->c->is_chainable()) + { + RCLCPP_WARN( + get_logger(), + "No reference interface '%s' exist, since the following controller with name '%s' " + "is not chainable.", + cmd_itf_name.c_str(), following_ctrl_it->info.name.c_str()); + return controller_interface::return_type::ERROR; + } + + if (is_controller_active(following_ctrl_it->c)) + { + // will following controller be deactivated? + if ( + std::find(stop_request_.begin(), stop_request_.end(), following_ctrl_it->info.name) != + stop_request_.end()) + { + RCLCPP_WARN( + get_logger(), "The following controller with name '%s' will be deactivated.", + following_ctrl_it->info.name.c_str()); + return controller_interface::return_type::ERROR; + } + } + // check if following controller will not be activated + else if ( + std::find(start_request_.begin(), start_request_.end(), following_ctrl_it->info.name) == + start_request_.end()) + { + RCLCPP_WARN( + get_logger(), + "The following controller with name '%s' is not active and will not be activated.", + following_ctrl_it->info.name.c_str()); + return controller_interface::return_type::ERROR; + } + + // Trigger recursion to check all the following controllers only if they are OK, add this + // controller update chained mode requests + if ( + check_following_controllers_for_activate(controllers, strictness, following_ctrl_it) == + controller_interface::return_type::ERROR) + { + return controller_interface::return_type::ERROR; + } + + // TODO(destogl): this should be discussed how to it the best - just a placeholder for now + // else if (strictness == + // controller_manager_msgs::srv::SwitchController::Request::MANIPULATE_CONTROLLERS_CHAIN) + // { + // // insert to the begin of start request list to be started before preceding controller + // start_request_.insert(start_request_.begin(), following_ctrl_name); + // } + if (!following_ctrl_it->c->is_in_chained_mode()) + { + auto found_it = std::find( + to_chained_mode_request_.begin(), to_chained_mode_request_.end(), + following_ctrl_it->info.name); + if (found_it == to_chained_mode_request_.end()) + { + to_chained_mode_request_.push_back(following_ctrl_it->info.name); + RCLCPP_DEBUG( + get_logger(), "Adding controller '%s' in 'to chained mode' request.", + following_ctrl_it->info.name.c_str()); + } + } + else + { + // Check if following controller is in 'from' chained mode list and remove it, if so + auto found_it = std::find( + from_chained_mode_request_.begin(), from_chained_mode_request_.end(), + following_ctrl_it->info.name); + if (found_it != from_chained_mode_request_.end()) + { + from_chained_mode_request_.erase(found_it); + RCLCPP_DEBUG( + get_logger(), + "Removing controller '%s' in 'from chained mode' request because it " + "should stay in chained mode.", + following_ctrl_it->info.name.c_str()); + } + } + } + return controller_interface::return_type::OK; +}; + +controller_interface::return_type ControllerManager::check_preceeding_controllers_for_deactivate( + const std::vector & controllers, int /*strictness*/, + const ControllersListIterator controller_it) +{ + // if not chainable no need for any checks + if (!controller_it->c->is_chainable()) + { + return controller_interface::return_type::OK; + } + + if (!controller_it->c->is_in_chained_mode()) + { + RCLCPP_DEBUG( + get_logger(), + "Controller with name '%s' is chainable but not in chained mode. " + "No need to do any checks of preceding controllers when stopping it.", + controller_it->info.name.c_str()); + return controller_interface::return_type::OK; + } + + RCLCPP_DEBUG( + get_logger(), "Checking preceding controller of following controller with name '%s'.", + controller_it->info.name.c_str()); + + for (const auto & ref_itf_name : + resource_manager_->get_controller_reference_interface_names(controller_it->info.name)) + { + std::vector preceding_controllers_using_ref_itf; + + // TODO(destogl): This data could be cached after configuring controller into a map for faster + // access here + for (auto preceding_ctrl_it = controllers.begin(); preceding_ctrl_it != controllers.end(); + ++preceding_ctrl_it) + { + const auto preceding_ctrl_cmd_itfs = + preceding_ctrl_it->c->command_interface_configuration().names; + + // if controller is not preceding go the next one + if ( + std::find(preceding_ctrl_cmd_itfs.begin(), preceding_ctrl_cmd_itfs.end(), ref_itf_name) == + preceding_ctrl_cmd_itfs.end()) + { + continue; + } + + // check if preceding controller will be activated + if ( + is_controller_inactive(preceding_ctrl_it->c) && + std::find(start_request_.begin(), start_request_.end(), preceding_ctrl_it->info.name) != + start_request_.end()) + { + RCLCPP_WARN( + get_logger(), + "Could not deactivate controller with name '%s' because " + "preceding controller with name '%s' will be activated. ", + controller_it->info.name.c_str(), preceding_ctrl_it->info.name.c_str()); + return controller_interface::return_type::ERROR; + } + // check if preceding controller will not be deactivated + else if ( + is_controller_active(preceding_ctrl_it->c) && + std::find(stop_request_.begin(), stop_request_.end(), preceding_ctrl_it->info.name) == + stop_request_.end()) + { + RCLCPP_WARN( + get_logger(), + "Could not deactivate controller with name '%s' because " + "preceding controller with name '%s' is active and will not be deactivated.", + controller_it->info.name.c_str(), preceding_ctrl_it->info.name.c_str()); + return controller_interface::return_type::ERROR; + } + // TODO(destogl): this should be discussed how to it the best - just a placeholder for now + // else if ( + // strictness == + // controller_manager_msgs::srv::SwitchController::Request::MANIPULATE_CONTROLLERS_CHAIN) + // { + // // insert to the begin of start request list to be started before preceding controller + // start_request_.insert(start_request_.begin(), preceding_ctrl_name); + // } + } + } + return controller_interface::return_type::OK; +}; + } // namespace controller_manager diff --git a/controller_manager/test/test_chainable_controller/test_chainable_controller.cpp b/controller_manager/test/test_chainable_controller/test_chainable_controller.cpp new file mode 100644 index 00000000000..ba1132e68b9 --- /dev/null +++ b/controller_manager/test/test_chainable_controller/test_chainable_controller.cpp @@ -0,0 +1,192 @@ +// Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "test_chainable_controller.hpp" + +#include +#include +#include + +#include "lifecycle_msgs/msg/state.hpp" + +namespace test_chainable_controller +{ +TestChainableController::TestChainableController() +: controller_interface::ChainableControllerInterface(), + cmd_iface_cfg_{controller_interface::interface_configuration_type::NONE}, + state_iface_cfg_{controller_interface::interface_configuration_type::NONE} +{ +} + +controller_interface::InterfaceConfiguration +TestChainableController::command_interface_configuration() const +{ + if ( + get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE || + get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) + { + return cmd_iface_cfg_; + } + else + { + throw std::runtime_error( + "Can not get command interface configuration until the controller is configured."); + } +} + +controller_interface::InterfaceConfiguration +TestChainableController::state_interface_configuration() const +{ + if ( + get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE || + get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) + { + return state_iface_cfg_; + } + else + { + throw std::runtime_error( + "Can not get state interface configuration until the controller is configured."); + } +} + +controller_interface::return_type TestChainableController::update_reference_from_subscribers() +{ + for (size_t i = 0; i < reference_interfaces_.size(); ++i) + { + RCLCPP_INFO( + get_node()->get_logger(), + "Value of reference interface '%s' before checking external input is %f", + (std::string(get_node()->get_name()) + "/" + reference_interface_names_[i]).c_str(), + reference_interfaces_[i]); + } + + auto joint_commands = rt_command_ptr_.readFromRT(); + reference_interfaces_ = (*joint_commands)->data; + for (size_t i = 0; i < reference_interfaces_.size(); ++i) + { + RCLCPP_INFO( + get_node()->get_logger(), + "Updated value of reference interface '%s' after applying external input is %f", + (std::string(get_node()->get_name()) + "/" + reference_interface_names_[i]).c_str(), + reference_interfaces_[i]); + } + + return controller_interface::return_type::OK; +} + +controller_interface::return_type TestChainableController::update_and_write_commands( + const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) +{ + ++internal_counter; + + for (size_t i = 0; i < command_interfaces_.size(); ++i) + { + command_interfaces_[i].set_value(reference_interfaces_[i] - state_interfaces_[i].get_value()); + } + + return controller_interface::return_type::OK; +} + +CallbackReturn TestChainableController::on_init() { return CallbackReturn::SUCCESS; } + +CallbackReturn TestChainableController::on_configure( + const rclcpp_lifecycle::State & /*previous_state*/) +{ + joints_command_subscriber_ = get_node()->create_subscription( + "~/commands", rclcpp::SystemDefaultsQoS(), + [this](const CmdType::SharedPtr msg) + { + auto joint_commands = rt_command_ptr_.readFromNonRT(); + + if (msg->data.size() != (*joint_commands)->data.size()) + { + rt_command_ptr_.writeFromNonRT(msg); + } + else + { + RCLCPP_ERROR_THROTTLE( + get_node()->get_logger(), *get_node()->get_clock(), 1000, + "command size (%zu) does not match number of reference interfaces (%zu)", + (*joint_commands)->data.size(), reference_interfaces_.size()); + } + }); + + auto msg = std::make_shared(); + msg->data.resize(reference_interfaces_.size()); + rt_command_ptr_.writeFromNonRT(msg); + + return CallbackReturn::SUCCESS; +} + +CallbackReturn TestChainableController::on_activate( + const rclcpp_lifecycle::State & /*previous_state*/) +{ + if (!is_in_chained_mode()) + { + auto msg = rt_command_ptr_.readFromRT(); + (*msg)->data = reference_interfaces_; + } + + return CallbackReturn::SUCCESS; +} + +CallbackReturn TestChainableController::on_cleanup( + const rclcpp_lifecycle::State & /*previous_state*/) +{ + joints_command_subscriber_.reset(); + return CallbackReturn::SUCCESS; +} + +std::vector +TestChainableController::on_export_reference_interfaces() +{ + std::vector reference_interfaces; + + for (size_t i = 0; i < reference_interface_names_.size(); ++i) + { + reference_interfaces.push_back(hardware_interface::CommandInterface( + get_node()->get_name(), reference_interface_names_[i], &reference_interfaces_[i])); + } + + return reference_interfaces; +} + +void TestChainableController::set_command_interface_configuration( + const controller_interface::InterfaceConfiguration & cfg) +{ + cmd_iface_cfg_ = cfg; +} + +void TestChainableController::set_state_interface_configuration( + const controller_interface::InterfaceConfiguration & cfg) +{ + state_iface_cfg_ = cfg; +} + +void TestChainableController::set_reference_interface_names( + const std::vector & reference_interface_names) +{ + reference_interface_names_ = reference_interface_names; + + reference_interfaces_.resize(reference_interface_names.size(), 0.0); +} + +} // namespace test_chainable_controller + +#include "pluginlib/class_list_macros.hpp" + +PLUGINLIB_EXPORT_CLASS( + test_chainable_controller::TestChainableController, + controller_interface::ChainableControllerInterface) diff --git a/controller_manager/test/test_chainable_controller/test_chainable_controller.hpp b/controller_manager/test/test_chainable_controller/test_chainable_controller.hpp new file mode 100644 index 00000000000..a7205d30249 --- /dev/null +++ b/controller_manager/test/test_chainable_controller/test_chainable_controller.hpp @@ -0,0 +1,93 @@ +// Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef TEST_CHAINABLE_CONTROLLER__TEST_CHAINABLE_CONTROLLER_HPP_ +#define TEST_CHAINABLE_CONTROLLER__TEST_CHAINABLE_CONTROLLER_HPP_ + +#include +#include +#include + +#include "controller_interface/chainable_controller_interface.hpp" +#include "controller_manager/visibility_control.h" +#include "rclcpp/subscription.hpp" +#include "realtime_tools/realtime_buffer.h" +#include "std_msgs/msg/float64_multi_array.hpp" + +namespace test_chainable_controller +{ +using CmdType = std_msgs::msg::Float64MultiArray; +using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; + +// indicating the node name under which the controller node +// is being loaded. +constexpr char TEST_CONTROLLER_NAME[] = "test_chainable_controller_name"; +// corresponds to the name listed within the pluginlib xml +constexpr char TEST_CONTROLLER_CLASS_NAME[] = "controller_manager/test_chainable_controller"; +class TestChainableController : public controller_interface::ChainableControllerInterface +{ +public: + CONTROLLER_MANAGER_PUBLIC + TestChainableController(); + + CONTROLLER_MANAGER_PUBLIC + virtual ~TestChainableController() = default; + + controller_interface::InterfaceConfiguration command_interface_configuration() const override; + + controller_interface::InterfaceConfiguration state_interface_configuration() const override; + + CONTROLLER_MANAGER_PUBLIC + CallbackReturn on_init() override; + + CONTROLLER_MANAGER_PUBLIC + CallbackReturn on_configure(const rclcpp_lifecycle::State & previous_state) override; + + CONTROLLER_MANAGER_PUBLIC + CallbackReturn on_activate(const rclcpp_lifecycle::State & previous_state) override; + + CONTROLLER_MANAGER_PUBLIC + CallbackReturn on_cleanup(const rclcpp_lifecycle::State & previous_state) override; + + CONTROLLER_MANAGER_PUBLIC + std::vector on_export_reference_interfaces() override; + + controller_interface::return_type update_reference_from_subscribers() override; + + controller_interface::return_type update_and_write_commands( + const rclcpp::Time & time, const rclcpp::Duration & period) override; + + // Testing-relevant methods + CONTROLLER_MANAGER_PUBLIC + void set_command_interface_configuration( + const controller_interface::InterfaceConfiguration & cfg); + + CONTROLLER_MANAGER_PUBLIC + void set_state_interface_configuration(const controller_interface::InterfaceConfiguration & cfg); + + CONTROLLER_MANAGER_PUBLIC + void set_reference_interface_names(const std::vector & reference_interface_names); + + size_t internal_counter; + controller_interface::InterfaceConfiguration cmd_iface_cfg_; + controller_interface::InterfaceConfiguration state_iface_cfg_; + std::vector reference_interface_names_; + + realtime_tools::RealtimeBuffer> rt_command_ptr_; + rclcpp::Subscription::SharedPtr joints_command_subscriber_; +}; + +} // namespace test_chainable_controller + +#endif // TEST_CHAINABLE_CONTROLLER__TEST_CHAINABLE_CONTROLLER_HPP_ diff --git a/controller_manager/test/test_chainable_controller/test_chainable_controller.xml b/controller_manager/test/test_chainable_controller/test_chainable_controller.xml new file mode 100644 index 00000000000..30ae99a946b --- /dev/null +++ b/controller_manager/test/test_chainable_controller/test_chainable_controller.xml @@ -0,0 +1,11 @@ + + + + + Controller used for testing + + + + diff --git a/controller_manager/test/test_controller/test_controller.cpp b/controller_manager/test/test_controller/test_controller.cpp index 2b8b8bc262e..51e83ac452d 100644 --- a/controller_manager/test/test_controller/test_controller.cpp +++ b/controller_manager/test/test_controller/test_controller.cpp @@ -61,6 +61,15 @@ controller_interface::return_type TestController::update( const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) { ++internal_counter; + + for (size_t i = 0; i < command_interfaces_.size(); ++i) + { + RCLCPP_INFO( + get_node()->get_logger(), "Setting value of command interface '%s' to %f", + command_interfaces_[i].get_full_name().c_str(), external_commands_for_testing_[i]); + command_interfaces_[i].set_value(external_commands_for_testing_[i]); + } + return controller_interface::return_type::OK; } @@ -89,6 +98,7 @@ void TestController::set_command_interface_configuration( const controller_interface::InterfaceConfiguration & cfg) { cmd_iface_cfg_ = cfg; + external_commands_for_testing_.resize(cmd_iface_cfg_.names.size(), 0.0); } void TestController::set_state_interface_configuration( diff --git a/controller_manager/test/test_controller/test_controller.hpp b/controller_manager/test/test_controller/test_controller.hpp index 4e4ec44d17b..403d7d12112 100644 --- a/controller_manager/test/test_controller/test_controller.hpp +++ b/controller_manager/test/test_controller/test_controller.hpp @@ -17,6 +17,7 @@ #include #include +#include #include "controller_interface/controller_interface.hpp" #include "controller_manager/visibility_control.h" @@ -71,6 +72,8 @@ class TestController : public controller_interface::ControllerInterface size_t * cleanup_calls = nullptr; controller_interface::InterfaceConfiguration cmd_iface_cfg_; controller_interface::InterfaceConfiguration state_iface_cfg_; + + std::vector external_commands_for_testing_; }; } // namespace test_controller diff --git a/controller_manager/test/test_controller_manager_srvs.cpp b/controller_manager/test/test_controller_manager_srvs.cpp index baf1f3ba0fd..b9277346401 100644 --- a/controller_manager/test/test_controller_manager_srvs.cpp +++ b/controller_manager/test/test_controller_manager_srvs.cpp @@ -50,6 +50,9 @@ TEST_F(TestControllerManagerSrvs, list_controller_types) ASSERT_THAT(result->types, ::testing::Contains(test_controller::TEST_CONTROLLER_CLASS_NAME)); ASSERT_THAT( result->base_classes, ::testing::Contains("controller_interface::ControllerInterface")); + ASSERT_THAT( + result->base_classes, + ::testing::Contains("controller_interface::ChainableControllerInterface")); } TEST_F(TestControllerManagerSrvs, list_controllers_srv) diff --git a/controller_manager/test/test_controllers_chaining_with_controller_manager.cpp b/controller_manager/test/test_controllers_chaining_with_controller_manager.cpp new file mode 100644 index 00000000000..0d798a1b799 --- /dev/null +++ b/controller_manager/test/test_controllers_chaining_with_controller_manager.cpp @@ -0,0 +1,621 @@ +// Copyright 2022 Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include +#include +#include + +#include "controller_interface/controller_interface.hpp" +#include "controller_manager/controller_manager.hpp" +#include "controller_manager_msgs/srv/list_controllers.hpp" +#include "controller_manager_test_common.hpp" +#include "lifecycle_msgs/msg/state.hpp" +#include "rclcpp/parameter.hpp" +#include "test_chainable_controller/test_chainable_controller.hpp" +#include "test_controller/test_controller.hpp" + +// The tests in this file are implementing example of chained-control for DiffDrive robot example: +// https://github.com/ros-controls/roadmap/blob/9f32e215a84347aee0b519cb24d081f23bbbb224/design_drafts/cascade_control.md#motivation-purpose-and-use +// The controller have the names as stated in figure, but they are simply forwarding values without +// functionality that their name would suggest + +class TestControllerChainingWithControllerManager; + +class TestableTestChainableController : public test_chainable_controller::TestChainableController +{ + friend TestControllerChainingWithControllerManager; + + FRIEND_TEST(TestControllerChainingWithControllerManager, test_chained_controllers); + FRIEND_TEST( + TestControllerChainingWithControllerManager, + test_chained_controllers_auto_switch_to_chained_mode); +}; + +class TestableControllerManager : public controller_manager::ControllerManager +{ + friend TestControllerChainingWithControllerManager; + + FRIEND_TEST( + TestControllerChainingWithControllerManagerAndChainedControllersParameter, + test_cm_reading_chained_controllers_parameter); + FRIEND_TEST( + TestControllerChainingWithControllerManagerAndChainedControllersParameter, + test_cm_reading_chained_controllers_parameter_failure_group0); + FRIEND_TEST( + TestControllerChainingWithControllerManagerAndChainedControllersParameter, + test_cm_reading_chained_controllers_parameter_failure_wrong_type); + FRIEND_TEST( + TestControllerChainingWithControllerManagerAndChainedControllersParameter, + test_cm_reading_chained_controllers_parameter_failure_duplicated_controller); + + FRIEND_TEST(TestControllerChainingWithControllerManager, test_chained_controllers); + FRIEND_TEST( + TestControllerChainingWithControllerManager, + test_chained_controllers_auto_switch_to_chained_mode); + +public: + TestableControllerManager( + std::unique_ptr resource_manager, + std::shared_ptr executor, + const std::string & manager_node_name = "controller_manager", + const std::string & namespace_ = "") + : controller_manager::ControllerManager( + std::move(resource_manager), executor, manager_node_name, namespace_) + { + } +}; + +class TestControllerChainingWithControllerManager +: public ControllerManagerFixture, + public testing::WithParamInterface +{ +public: + void SetUp() + { + executor_ = std::make_shared(); + cm_ = std::make_shared( + std::make_unique( + ros2_control_test_assets::diffbot_urdf, true, true), + executor_, TEST_CM_NAME); + run_updater_ = false; + } + + void SetupControllers() + { + test_param = GetParam(); + + pid_left_wheel_controller = std::make_shared(); + pid_right_wheel_controller = std::make_shared(); + diff_drive_controller = std::make_shared(); + position_tracking_controller = std::make_shared(); + + // configure Left Wheel controller + controller_interface::InterfaceConfiguration pid_left_cmd_ifs_cfg = { + controller_interface::interface_configuration_type::INDIVIDUAL, {"wheel_left/velocity"}}; + controller_interface::InterfaceConfiguration pid_left_state_ifs_cfg = { + controller_interface::interface_configuration_type::INDIVIDUAL, {"wheel_left/velocity"}}; + pid_left_wheel_controller->set_command_interface_configuration(pid_left_cmd_ifs_cfg); + pid_left_wheel_controller->set_state_interface_configuration(pid_left_state_ifs_cfg); + pid_left_wheel_controller->set_reference_interface_names({"velocity"}); + + // configure Left Wheel controller + controller_interface::InterfaceConfiguration pid_right_cmd_ifs_cfg = { + controller_interface::interface_configuration_type::INDIVIDUAL, {"wheel_right/velocity"}}; + controller_interface::InterfaceConfiguration pid_right_state_ifs_cfg = { + controller_interface::interface_configuration_type::INDIVIDUAL, {"wheel_right/velocity"}}; + pid_right_wheel_controller->set_command_interface_configuration(pid_right_cmd_ifs_cfg); + pid_right_wheel_controller->set_state_interface_configuration(pid_right_state_ifs_cfg); + pid_right_wheel_controller->set_reference_interface_names({"velocity"}); + + // configure Diff Drive controller + controller_interface::InterfaceConfiguration diff_drive_cmd_ifs_cfg = { + controller_interface::interface_configuration_type::INDIVIDUAL, + {std::string(PID_LEFT_WHEEL) + "/velocity", std::string(PID_RIGHT_WHEEL) + "/velocity"}}; + controller_interface::InterfaceConfiguration diff_drive_state_ifs_cfg = { + controller_interface::interface_configuration_type::INDIVIDUAL, + {"wheel_left/velocity", "wheel_right/velocity"}}; + diff_drive_controller->set_command_interface_configuration(diff_drive_cmd_ifs_cfg); + diff_drive_controller->set_state_interface_configuration(diff_drive_state_ifs_cfg); + diff_drive_controller->set_reference_interface_names({"vel_x", "vel_y", "rot_z"}); + + // configure Position Tracking controller + controller_interface::InterfaceConfiguration position_tracking_cmd_ifs_cfg = { + controller_interface::interface_configuration_type::INDIVIDUAL, + {std::string(DIFF_DRIVE_CONTROLLER) + "/vel_x", + std::string(DIFF_DRIVE_CONTROLLER) + "/vel_y"}}; + // in this simple example "vel_x" == "velocity left wheel" and "vel_y" == "velocity right wheel" + position_tracking_controller->set_command_interface_configuration( + position_tracking_cmd_ifs_cfg); + } + + void CheckIfControllersAreAddedCorrectly() + { + EXPECT_EQ(4u, cm_->get_loaded_controllers().size()); + EXPECT_EQ(2, pid_left_wheel_controller.use_count()); + EXPECT_EQ(2, pid_right_wheel_controller.use_count()); + EXPECT_EQ(2, diff_drive_controller.use_count()); + EXPECT_EQ(2, position_tracking_controller.use_count()); + + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + pid_left_wheel_controller->get_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + pid_right_wheel_controller->get_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + diff_drive_controller->get_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + position_tracking_controller->get_state().id()); + } + + // order or controller configuration is not important therefore we can reuse the same method + void ConfigureAndCheckControllers() + { + // Store initial values of command interfaces + size_t number_of_cmd_itfs = cm_->resource_manager_->command_interface_keys().size(); + + // configure chainable controller and check exported interfaces + cm_->configure_controller(PID_LEFT_WHEEL); + EXPECT_EQ( + pid_left_wheel_controller->get_state().id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); + EXPECT_EQ(cm_->resource_manager_->command_interface_keys().size(), number_of_cmd_itfs + 1); + for (const auto & interface : {"pid_left_wheel_controller/velocity"}) + { + EXPECT_TRUE(cm_->resource_manager_->command_interface_exists(interface)); + EXPECT_FALSE(cm_->resource_manager_->command_interface_is_available(interface)); + EXPECT_FALSE(cm_->resource_manager_->command_interface_is_claimed(interface)); + } + + cm_->configure_controller(PID_RIGHT_WHEEL); + EXPECT_EQ( + pid_right_wheel_controller->get_state().id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); + EXPECT_EQ(cm_->resource_manager_->command_interface_keys().size(), number_of_cmd_itfs + 2); + for (const auto & interface : {"pid_right_wheel_controller/velocity"}) + { + EXPECT_TRUE(cm_->resource_manager_->command_interface_exists(interface)); + EXPECT_FALSE(cm_->resource_manager_->command_interface_is_available(interface)); + EXPECT_FALSE(cm_->resource_manager_->command_interface_is_claimed(interface)); + } + + cm_->configure_controller(DIFF_DRIVE_CONTROLLER); + EXPECT_EQ( + diff_drive_controller->get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); + EXPECT_EQ(cm_->resource_manager_->command_interface_keys().size(), number_of_cmd_itfs + 5); + for (const auto & interface : + {"diff_drive_controller/vel_x", "diff_drive_controller/vel_y", + "diff_drive_controller/rot_z"}) + { + EXPECT_TRUE(cm_->resource_manager_->command_interface_exists(interface)); + EXPECT_FALSE(cm_->resource_manager_->command_interface_is_available(interface)); + EXPECT_FALSE(cm_->resource_manager_->command_interface_is_claimed(interface)); + } + + cm_->configure_controller(POSITION_TRACKING_CONTROLLER); + EXPECT_EQ( + position_tracking_controller->get_state().id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); + EXPECT_EQ(cm_->resource_manager_->command_interface_keys().size(), number_of_cmd_itfs + 5); + } + + template < + typename T, typename std::enable_if< + std::is_convertible::value, + T>::type * = nullptr> + void SetToChainedModeAndMakeReferenceInterfacesAvailable( + std::shared_ptr & controller, const std::string & controller_name, + const std::vector & reference_interfaces) + { + controller->set_chained_mode(true); + EXPECT_TRUE(controller->is_in_chained_mode()); + // make reference interface command_interfaces available + cm_->resource_manager_->make_controller_reference_interfaces_available(controller_name); + for (const auto & interface : reference_interfaces) + { + EXPECT_TRUE(cm_->resource_manager_->command_interface_exists(interface)); + EXPECT_TRUE(cm_->resource_manager_->command_interface_is_available(interface)); + EXPECT_FALSE(cm_->resource_manager_->command_interface_is_claimed(interface)); + } + } + + template < + typename T, typename std::enable_if< + std::is_convertible::value, + T>::type * = nullptr> + void check_after_de_activate( + std::shared_ptr & controller, const std::vector & claimed_command_itfs, + size_t expected_internal_counter, const controller_interface::return_type expected_return, + bool deactivated, bool claimed_interfaces_from_hw = false) + { + for (const auto & interface : claimed_command_itfs) + { + EXPECT_TRUE(cm_->resource_manager_->command_interface_exists(interface)); + // successful xor deactivated + if ((expected_return == controller_interface::return_type::OK) != deactivated) + { + EXPECT_TRUE(cm_->resource_manager_->command_interface_exists(interface)); + EXPECT_TRUE(cm_->resource_manager_->command_interface_is_claimed(interface)); + } + else + { + if (claimed_interfaces_from_hw) + { + EXPECT_TRUE(cm_->resource_manager_->command_interface_is_available(interface)); + } + else + { + EXPECT_FALSE(cm_->resource_manager_->command_interface_is_available(interface)); + } + EXPECT_FALSE(cm_->resource_manager_->command_interface_is_claimed(interface)); + } + } + if (expected_internal_counter != 0) + { + ASSERT_EQ(controller->internal_counter, expected_internal_counter); + } + } + + template < + typename T, typename std::enable_if< + std::is_convertible::value, + T>::type * = nullptr> + void ActivateAndCheckController( + std::shared_ptr & controller, const std::string & controller_name, + const std::vector & claimed_command_itfs, size_t expected_internal_counter = 0u, + const controller_interface::return_type expected_return = controller_interface::return_type::OK) + { + switch_test_controllers( + {controller_name}, {}, test_param.strictness, std::future_status::timeout, expected_return); + check_after_de_activate( + controller, claimed_command_itfs, expected_internal_counter, expected_return, false); + } + + template < + typename T, typename std::enable_if< + std::is_convertible::value, + T>::type * = nullptr> + void DeactivateAndCheckController( + std::shared_ptr & controller, const std::string & controller_name, + const std::vector & claimed_command_itfs, size_t expected_internal_counter = 0u, + const bool claimed_interfaces_from_hw = false, + const controller_interface::return_type expected_return = controller_interface::return_type::OK) + { + switch_test_controllers( + {}, {controller_name}, test_param.strictness, std::future_status::timeout, expected_return); + check_after_de_activate( + controller, claimed_command_itfs, expected_internal_counter, expected_return, true, + claimed_interfaces_from_hw); + } + + void UpdateAllControllerAndCheck( + const std::vector & reference, size_t exp_internal_counter_pos_ctrl) + { + // test value that could cause bad-memory access --> cleaner error during writing tests + ASSERT_EQ(reference.size(), 2u); + + position_tracking_controller->external_commands_for_testing_[0] = reference[0]; + position_tracking_controller->external_commands_for_testing_[1] = reference[1]; + + cm_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); + cm_->resource_manager_->read(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); + + // check if all controllers are updated + ASSERT_EQ(position_tracking_controller->internal_counter, exp_internal_counter_pos_ctrl); + ASSERT_EQ(diff_drive_controller->internal_counter, exp_internal_counter_pos_ctrl + 2); + ASSERT_EQ(pid_left_wheel_controller->internal_counter, exp_internal_counter_pos_ctrl + 6); + ASSERT_EQ(pid_right_wheel_controller->internal_counter, exp_internal_counter_pos_ctrl + 4); + + // check if values are set properly in controllers + ASSERT_EQ( + diff_drive_controller->reference_interfaces_[0], reference[0]); // command from Position to + ASSERT_EQ( + diff_drive_controller->reference_interfaces_[1], reference[1]); // DiffDrive is forwarded + + // Command of DiffDrive controller are references of PID controllers + EXP_LEFT_WHEEL_REF = chained_ctrl_calculation(reference[0], EXP_LEFT_WHEEL_HW_STATE); + EXP_RIGHT_WHEEL_REF = chained_ctrl_calculation(reference[1], EXP_RIGHT_WHEEL_HW_STATE); + ASSERT_EQ(diff_drive_controller->command_interfaces_[0].get_value(), EXP_LEFT_WHEEL_REF); + ASSERT_EQ(diff_drive_controller->command_interfaces_[1].get_value(), EXP_RIGHT_WHEEL_REF); + ASSERT_EQ(pid_left_wheel_controller->reference_interfaces_[0], EXP_LEFT_WHEEL_REF); + ASSERT_EQ(pid_right_wheel_controller->reference_interfaces_[0], EXP_RIGHT_WHEEL_REF); + + EXP_LEFT_WHEEL_CMD = chained_ctrl_calculation(EXP_LEFT_WHEEL_REF, EXP_LEFT_WHEEL_HW_STATE); + EXP_LEFT_WHEEL_HW_STATE = hardware_calculation(EXP_LEFT_WHEEL_CMD); + ASSERT_EQ(pid_left_wheel_controller->command_interfaces_[0].get_value(), EXP_LEFT_WHEEL_CMD); + ASSERT_EQ(pid_left_wheel_controller->state_interfaces_[0].get_value(), EXP_LEFT_WHEEL_HW_STATE); + // DiffDrive uses the same state + ASSERT_EQ(diff_drive_controller->state_interfaces_[0].get_value(), EXP_LEFT_WHEEL_HW_STATE); + + EXP_RIGHT_WHEEL_CMD = chained_ctrl_calculation(EXP_RIGHT_WHEEL_REF, EXP_RIGHT_WHEEL_HW_STATE); + EXP_RIGHT_WHEEL_HW_STATE = hardware_calculation(EXP_RIGHT_WHEEL_CMD); + ASSERT_EQ(pid_right_wheel_controller->command_interfaces_[0].get_value(), EXP_RIGHT_WHEEL_CMD); + ASSERT_EQ( + pid_right_wheel_controller->state_interfaces_[0].get_value(), EXP_RIGHT_WHEEL_HW_STATE); + // DiffDrive uses the same state + ASSERT_EQ(diff_drive_controller->state_interfaces_[1].get_value(), EXP_RIGHT_WHEEL_HW_STATE); + } + + // check data propagation through controllers and if individual controllers are working + double chained_ctrl_calculation(double reference, double state) { return reference - state; } + double hardware_calculation(double command) { return command / 2.0; } + + // set controllers' names + static constexpr char PID_LEFT_WHEEL[] = "pid_left_wheel_controller"; + static constexpr char PID_RIGHT_WHEEL[] = "pid_right_wheel_controller"; + static constexpr char DIFF_DRIVE_CONTROLLER[] = "diff_drive_controller"; + static constexpr char POSITION_TRACKING_CONTROLLER[] = "position_tracking_controller"; + + const std::vector PID_LEFT_WHEEL_REFERENCE_INTERFACES = { + "pid_left_wheel_controller/velocity"}; + const std::vector PID_RIGHT_WHEEL_REFERENCE_INTERFACES = { + "pid_right_wheel_controller/velocity"}; + const std::vector DIFF_DRIVE_REFERENCE_INTERFACES = { + "diff_drive_controller/vel_x", "diff_drive_controller/vel_y", "diff_drive_controller/rot_z"}; + + const std::vector PID_LEFT_WHEEL_CLAIMED_INTERFACES = {"wheel_left/velocity"}; + const std::vector PID_RIGHT_WHEEL_CLAIMED_INTERFACES = {"wheel_right/velocity"}; + const std::vector DIFF_DRIVE_CLAIMED_INTERFACES = { + "pid_left_wheel_controller/velocity", "pid_right_wheel_controller/velocity"}; + const std::vector POSITION_CONTROLLER_CLAIMED_INTERFACES = { + "diff_drive_controller/vel_x", "diff_drive_controller/vel_y"}; + + // controllers objects + std::shared_ptr pid_left_wheel_controller; + std::shared_ptr pid_right_wheel_controller; + std::shared_ptr diff_drive_controller; + std::shared_ptr position_tracking_controller; + + testing::WithParamInterface::ParamType test_param; + + // expected values for tests - shared between multiple test runs + double EXP_LEFT_WHEEL_CMD = 0.0; + double EXP_LEFT_WHEEL_HW_STATE = 0.0; + double EXP_RIGHT_WHEEL_CMD = 0.0; + double EXP_RIGHT_WHEEL_HW_STATE = 0.0; + double EXP_LEFT_WHEEL_REF = 0.0; + double EXP_RIGHT_WHEEL_REF = 0.0; +}; + +// The tests are implementing example of chained-control for DiffDrive robot shown here: +// https://github.com/ros-controls/roadmap/blob/9f32e215a84347aee0b519cb24d081f23bbbb224/design_drafts/cascade_control.md#motivation-purpose-and-use +// The controller have the names as stated in figure, but they are simply forwarding values without +// functionality that their name would suggest +TEST_P(TestControllerChainingWithControllerManager, test_chained_controllers) +{ + SetupControllers(); + + // add all controllers - CONTROLLERS HAVE TO ADDED IN EXECUTION ORDER + cm_->add_controller( + position_tracking_controller, POSITION_TRACKING_CONTROLLER, + test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); + cm_->add_controller( + diff_drive_controller, DIFF_DRIVE_CONTROLLER, + test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); + cm_->add_controller( + pid_left_wheel_controller, PID_LEFT_WHEEL, + test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); + cm_->add_controller( + pid_right_wheel_controller, PID_RIGHT_WHEEL, + test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); + + CheckIfControllersAreAddedCorrectly(); + + ConfigureAndCheckControllers(); + + SetToChainedModeAndMakeReferenceInterfacesAvailable( + pid_left_wheel_controller, PID_LEFT_WHEEL, PID_LEFT_WHEEL_REFERENCE_INTERFACES); + SetToChainedModeAndMakeReferenceInterfacesAvailable( + pid_right_wheel_controller, PID_RIGHT_WHEEL, PID_RIGHT_WHEEL_REFERENCE_INTERFACES); + SetToChainedModeAndMakeReferenceInterfacesAvailable( + diff_drive_controller, DIFF_DRIVE_CONTROLLER, DIFF_DRIVE_REFERENCE_INTERFACES); + + EXPECT_THROW( + cm_->resource_manager_->make_controller_reference_interfaces_available( + POSITION_TRACKING_CONTROLLER), + std::out_of_range); + + // Set ControllerManager into Debug-Mode output to have detailed output on updating controllers + cm_->get_logger().set_level(rclcpp::Logger::Level::Debug); + + // activate controllers - CONTROLLERS HAVE TO ADDED REVERSE EXECUTION ORDER + // (otherwise, interface will be missing) + ActivateAndCheckController( + pid_left_wheel_controller, PID_LEFT_WHEEL, PID_LEFT_WHEEL_CLAIMED_INTERFACES, 1u); + ActivateAndCheckController( + pid_right_wheel_controller, PID_RIGHT_WHEEL, PID_RIGHT_WHEEL_CLAIMED_INTERFACES, 1u); + ASSERT_EQ(pid_left_wheel_controller->internal_counter, 3u); + + // Diff-Drive Controller claims the reference interfaces of PID controllers + ActivateAndCheckController( + diff_drive_controller, DIFF_DRIVE_CONTROLLER, DIFF_DRIVE_CLAIMED_INTERFACES, 1u); + ASSERT_EQ(pid_right_wheel_controller->internal_counter, 3u); + ASSERT_EQ(pid_left_wheel_controller->internal_counter, 5u); + + // Position-Tracking Controller uses reference interfaces of Diff-Drive Controller + ActivateAndCheckController( + position_tracking_controller, POSITION_TRACKING_CONTROLLER, + POSITION_CONTROLLER_CLAIMED_INTERFACES, 1u); + // 'rot_z' reference interface is not claimed + for (const auto & interface : {"diff_drive_controller/rot_z"}) + { + EXPECT_TRUE(cm_->resource_manager_->command_interface_exists(interface)); + EXPECT_TRUE(cm_->resource_manager_->command_interface_is_available(interface)); + EXPECT_FALSE(cm_->resource_manager_->command_interface_is_claimed(interface)); + } + ASSERT_EQ(diff_drive_controller->internal_counter, 3u); + ASSERT_EQ(pid_right_wheel_controller->internal_counter, 5u); + ASSERT_EQ(pid_left_wheel_controller->internal_counter, 7u); + + // update controllers + std::vector reference = {32.0, 128.0}; + + // update 'Position Tracking' controller + for (auto & value : diff_drive_controller->reference_interfaces_) + { + ASSERT_EQ(value, 0.0); // default reference values are 0.0 + } + position_tracking_controller->external_commands_for_testing_[0] = reference[0]; + position_tracking_controller->external_commands_for_testing_[1] = reference[1]; + position_tracking_controller->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); + ASSERT_EQ(position_tracking_controller->internal_counter, 2u); + + ASSERT_EQ(diff_drive_controller->reference_interfaces_[0], reference[0]); // position_controller + ASSERT_EQ(diff_drive_controller->reference_interfaces_[1], reference[1]); // is pass-through + + // update 'Diff Drive' Controller + diff_drive_controller->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); + ASSERT_EQ(diff_drive_controller->internal_counter, 4u); + // default reference values are 0.0 - they should be changed now + EXP_LEFT_WHEEL_REF = chained_ctrl_calculation(reference[0], EXP_LEFT_WHEEL_HW_STATE); // 32-0 + EXP_RIGHT_WHEEL_REF = chained_ctrl_calculation(reference[1], EXP_RIGHT_WHEEL_HW_STATE); // 128-0 + ASSERT_EQ(pid_left_wheel_controller->reference_interfaces_[0], EXP_LEFT_WHEEL_REF); + ASSERT_EQ(pid_right_wheel_controller->reference_interfaces_[0], EXP_RIGHT_WHEEL_REF); + + // update PID controllers that are writing to hardware + pid_left_wheel_controller->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); + ASSERT_EQ(pid_left_wheel_controller->internal_counter, 8u); + pid_right_wheel_controller->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); + ASSERT_EQ(pid_right_wheel_controller->internal_counter, 6u); + + // update hardware ('read' is sufficient for test hardware) + cm_->resource_manager_->read(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); + // 32 - 0 + EXP_LEFT_WHEEL_CMD = chained_ctrl_calculation(EXP_LEFT_WHEEL_REF, EXP_LEFT_WHEEL_HW_STATE); + // 32 / 2 + EXP_LEFT_WHEEL_HW_STATE = hardware_calculation(EXP_LEFT_WHEEL_CMD); + ASSERT_EQ(pid_left_wheel_controller->command_interfaces_[0].get_value(), EXP_LEFT_WHEEL_CMD); + ASSERT_EQ(pid_left_wheel_controller->state_interfaces_[0].get_value(), EXP_LEFT_WHEEL_HW_STATE); + // DiffDrive uses the same state + ASSERT_EQ(diff_drive_controller->state_interfaces_[0].get_value(), EXP_LEFT_WHEEL_HW_STATE); + + // 128 - 0 + EXP_RIGHT_WHEEL_CMD = chained_ctrl_calculation(EXP_RIGHT_WHEEL_REF, EXP_RIGHT_WHEEL_HW_STATE); + // 128 / 2 + EXP_RIGHT_WHEEL_HW_STATE = hardware_calculation(EXP_RIGHT_WHEEL_CMD); + ASSERT_EQ(pid_right_wheel_controller->command_interfaces_[0].get_value(), EXP_RIGHT_WHEEL_CMD); + ASSERT_EQ(pid_right_wheel_controller->state_interfaces_[0].get_value(), EXP_RIGHT_WHEEL_HW_STATE); + // DiffDrive uses the same state + ASSERT_EQ(diff_drive_controller->state_interfaces_[1].get_value(), EXP_RIGHT_WHEEL_HW_STATE); + + // update all controllers at once and see that all have expected values --> also checks the order + // of controller execution + + reference = {1024.0, 4096.0}; + UpdateAllControllerAndCheck(reference, 3u); + + // TODO(destogl): Add here also slow disabling of controllers + + // TODO(destogl): Activate test parameter use +} + +TEST_P( + TestControllerChainingWithControllerManager, test_chained_controllers_auto_switch_to_chained_mode) +{ + SetupControllers(); + + // add all controllers - CONTROLLERS HAVE TO ADDED IN EXECUTION ORDER + cm_->add_controller( + position_tracking_controller, POSITION_TRACKING_CONTROLLER, + test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); + cm_->add_controller( + diff_drive_controller, DIFF_DRIVE_CONTROLLER, + test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); + cm_->add_controller( + pid_left_wheel_controller, PID_LEFT_WHEEL, + test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); + cm_->add_controller( + pid_right_wheel_controller, PID_RIGHT_WHEEL, + test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); + + CheckIfControllersAreAddedCorrectly(); + + ConfigureAndCheckControllers(); + + // Set ControllerManager into Debug-Mode output to have detailed output on updating controllers + cm_->get_logger().set_level(rclcpp::Logger::Level::Debug); + rclcpp::get_logger("ControllerManager::utils").set_level(rclcpp::Logger::Level::Debug); + + // at beginning controllers are not in chained mode + EXPECT_FALSE(pid_left_wheel_controller->is_in_chained_mode()); + EXPECT_FALSE(pid_right_wheel_controller->is_in_chained_mode()); + ASSERT_FALSE(diff_drive_controller->is_in_chained_mode()); + + // still not in chained mode because no preceding controller is activated + ActivateAndCheckController( + pid_left_wheel_controller, PID_LEFT_WHEEL, PID_LEFT_WHEEL_CLAIMED_INTERFACES, 1u); + ActivateAndCheckController( + pid_right_wheel_controller, PID_RIGHT_WHEEL, PID_RIGHT_WHEEL_CLAIMED_INTERFACES, 1u); + EXPECT_FALSE(pid_left_wheel_controller->is_in_chained_mode()); + EXPECT_FALSE(pid_right_wheel_controller->is_in_chained_mode()); + ASSERT_FALSE(diff_drive_controller->is_in_chained_mode()); + + // DiffDrive (preceding) controller is activated --> PID controller in chained mode + ActivateAndCheckController( + diff_drive_controller, DIFF_DRIVE_CONTROLLER, DIFF_DRIVE_CLAIMED_INTERFACES, 1u); + EXPECT_TRUE(pid_left_wheel_controller->is_in_chained_mode()); + EXPECT_TRUE(pid_right_wheel_controller->is_in_chained_mode()); + ASSERT_FALSE(diff_drive_controller->is_in_chained_mode()); + + // PositionController is activated --> all following controller in chained mode + ActivateAndCheckController( + position_tracking_controller, POSITION_TRACKING_CONTROLLER, + POSITION_CONTROLLER_CLAIMED_INTERFACES, 1u); + EXPECT_TRUE(pid_left_wheel_controller->is_in_chained_mode()); + EXPECT_TRUE(pid_right_wheel_controller->is_in_chained_mode()); + ASSERT_TRUE(diff_drive_controller->is_in_chained_mode()); + + UpdateAllControllerAndCheck({32.0, 128.0}, 2u); + UpdateAllControllerAndCheck({1024.0, 4096.0}, 3u); + + // Test switch 'from chained mode' when controllers are deactivated + + // PositionController is deactivated --> DiffDrive controller is not in chained mode anymore + DeactivateAndCheckController( + position_tracking_controller, POSITION_TRACKING_CONTROLLER, + POSITION_CONTROLLER_CLAIMED_INTERFACES, 4u); + EXPECT_TRUE(pid_left_wheel_controller->is_in_chained_mode()); + EXPECT_TRUE(pid_right_wheel_controller->is_in_chained_mode()); + ASSERT_FALSE(diff_drive_controller->is_in_chained_mode()); + + // DiffDrive (preceding) controller is activated --> PID controller in chained mode + DeactivateAndCheckController( + diff_drive_controller, DIFF_DRIVE_CONTROLLER, DIFF_DRIVE_CLAIMED_INTERFACES, 8u); + EXPECT_FALSE(pid_left_wheel_controller->is_in_chained_mode()); + EXPECT_FALSE(pid_right_wheel_controller->is_in_chained_mode()); + ASSERT_FALSE(diff_drive_controller->is_in_chained_mode()); + + // all controllers are deactivated --> chained mode is not changed + DeactivateAndCheckController( + pid_left_wheel_controller, PID_LEFT_WHEEL, PID_LEFT_WHEEL_CLAIMED_INTERFACES, 14u, true); + DeactivateAndCheckController( + pid_right_wheel_controller, PID_RIGHT_WHEEL, PID_RIGHT_WHEEL_CLAIMED_INTERFACES, 14u, true); + EXPECT_FALSE(pid_left_wheel_controller->is_in_chained_mode()); + EXPECT_FALSE(pid_right_wheel_controller->is_in_chained_mode()); + ASSERT_FALSE(diff_drive_controller->is_in_chained_mode()); +} + +// TODO(destogl): Add test case with controllers added in "random" order + +// TODO(destogl): Think about strictness and chaining controllers +// new value: "START_DOWNSTREAM_CTRLS" --> start "downstream" controllers in a controllers chain +// + +INSTANTIATE_TEST_SUITE_P( + test_strict_best_effort, TestControllerChainingWithControllerManager, + testing::Values(strict, best_effort)); diff --git a/controller_manager/test/test_hardware_management_srvs.cpp b/controller_manager/test/test_hardware_management_srvs.cpp index 00a504d69a8..d3a92bfce49 100644 --- a/controller_manager/test/test_hardware_management_srvs.cpp +++ b/controller_manager/test/test_hardware_management_srvs.cpp @@ -66,8 +66,6 @@ class TestControllerManagerHWManagementSrvs : public TestControllerManagerSrvs std::make_unique(), executor_, TEST_CM_NAME); run_updater_ = false; - // TODO(destogl): separate this to init_tests method where parameter can be set for each test - // separately cm_->set_parameter( rclcpp::Parameter("robot_description", ros2_control_test_assets::minimal_robot_urdf)); cm_->set_parameter(rclcpp::Parameter( diff --git a/hardware_interface/include/hardware_interface/resource_manager.hpp b/hardware_interface/include/hardware_interface/resource_manager.hpp index d77950103ca..5a919289b87 100644 --- a/hardware_interface/include/hardware_interface/resource_manager.hpp +++ b/hardware_interface/include/hardware_interface/resource_manager.hpp @@ -245,7 +245,7 @@ class HARDWARE_INTERFACE_PUBLIC ResourceManager * \note given that no hardware_info is available, the component has to be configured * externally and prior to the call to import. * \param[in] actuator pointer to the actuator interface. - * \param[in]hardware_info hardware info + * \param[in] hardware_info hardware info */ void import_component( std::unique_ptr actuator, const HardwareInfo & hardware_info); diff --git a/hardware_interface/test/test_components/test_actuator.cpp b/hardware_interface/test/test_components/test_actuator.cpp index 23a7d811dfa..355583a61d1 100644 --- a/hardware_interface/test/test_components/test_actuator.cpp +++ b/hardware_interface/test/test_components/test_actuator.cpp @@ -75,6 +75,11 @@ class TestActuator : public ActuatorInterface return_type read(const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) override { + // The next line is for the testing purposes. We need value to be changed to be sure that + // the feedback from hardware to controllers in the chain is working as it should. + // This makes value checks clearer and confirms there is no "state = command" line or some + // other mixture of interfaces somewhere in the test stack. + velocity_state_ = velocity_command_ / 2; return return_type::OK; } From 76b7370cd1a24e7e8970b1fb7a0b2366dfbbaacf Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Thu, 30 Jun 2022 15:51:29 +0100 Subject: [PATCH 08/35] Remove ament autolint (#749) --- controller_manager/CMakeLists.txt | 8 -------- controller_manager/package.xml | 3 --- controller_manager_msgs/CMakeLists.txt | 5 ----- controller_manager_msgs/package.xml | 3 --- hardware_interface/CMakeLists.txt | 6 ------ hardware_interface/package.xml | 2 -- joint_limits_interface/CMakeLists.txt | 7 ------- joint_limits_interface/package.xml | 2 -- ros2_control_test_assets/CMakeLists.txt | 9 --------- ros2_control_test_assets/package.xml | 1 - transmission_interface/CMakeLists.txt | 6 ------ transmission_interface/package.xml | 2 -- 12 files changed, 54 deletions(-) diff --git a/controller_manager/CMakeLists.txt b/controller_manager/CMakeLists.txt index be6bd89e4b5..33a0c8d131e 100644 --- a/controller_manager/CMakeLists.txt +++ b/controller_manager/CMakeLists.txt @@ -51,16 +51,8 @@ install(DIRECTORY include/ if(BUILD_TESTING) find_package(ament_cmake_gmock REQUIRED) - find_package(ament_cmake_gtest REQUIRED) - find_package(ament_lint_auto REQUIRED) find_package(ros2_control_test_assets REQUIRED) - list(APPEND AMENT_LINT_AUTO_EXCLUDE - ament_cmake_uncrustify - ament_cmake_cpplint - ) - ament_lint_auto_find_test_dependencies() - ament_index_get_prefix_path(ament_index_build_path SKIP_AMENT_PREFIX_PATH) # Get the first item (it will be the build space version of the build path). list(GET ament_index_build_path 0 ament_index_build_path) diff --git a/controller_manager/package.xml b/controller_manager/package.xml index cf1ae4084d8..e63bc63d0b9 100644 --- a/controller_manager/package.xml +++ b/controller_manager/package.xml @@ -24,9 +24,6 @@ ros2run ament_cmake_gmock - ament_cmake_gtest - ament_lint_auto - ament_lint_common ament_cmake diff --git a/controller_manager_msgs/CMakeLists.txt b/controller_manager_msgs/CMakeLists.txt index 3f0ce8a0b2a..16617c3613a 100644 --- a/controller_manager_msgs/CMakeLists.txt +++ b/controller_manager_msgs/CMakeLists.txt @@ -31,11 +31,6 @@ set(srv_files srv/UnloadController.srv ) -if(BUILD_TESTING) - find_package(ament_lint_auto REQUIRED) - ament_lint_auto_find_test_dependencies() -endif() - rosidl_generate_interfaces(${PROJECT_NAME} ${msg_files} ${srv_files} diff --git a/controller_manager_msgs/package.xml b/controller_manager_msgs/package.xml index 9a6d0d865fb..76b59a4fe2a 100644 --- a/controller_manager_msgs/package.xml +++ b/controller_manager_msgs/package.xml @@ -19,9 +19,6 @@ lifecycle_msgs rosidl_default_runtime - ament_lint_auto - ament_lint_common - rosidl_interface_packages ament_cmake diff --git a/hardware_interface/CMakeLists.txt b/hardware_interface/CMakeLists.txt index 69f1a4d5dae..27b7c6fa0d9 100644 --- a/hardware_interface/CMakeLists.txt +++ b/hardware_interface/CMakeLists.txt @@ -82,12 +82,6 @@ install( ) if(BUILD_TESTING) - find_package(ament_lint_auto REQUIRED) - list(APPEND AMENT_LINT_AUTO_EXCLUDE - ament_cmake_uncrustify - ament_cmake_cpplint - ) - ament_lint_auto_find_test_dependencies() find_package(ament_cmake_gmock REQUIRED) find_package(ros2_control_test_assets REQUIRED) diff --git a/hardware_interface/package.xml b/hardware_interface/package.xml index a0226c7c537..a5a0b1e0018 100644 --- a/hardware_interface/package.xml +++ b/hardware_interface/package.xml @@ -20,8 +20,6 @@ rcutils ament_cmake_gmock - ament_lint_auto - ament_lint_common ros2_control_test_assets diff --git a/joint_limits_interface/CMakeLists.txt b/joint_limits_interface/CMakeLists.txt index 1e0362e4ecb..a5d45773432 100644 --- a/joint_limits_interface/CMakeLists.txt +++ b/joint_limits_interface/CMakeLists.txt @@ -8,15 +8,8 @@ find_package(urdf REQUIRED) if(BUILD_TESTING) find_package(ament_cmake_gtest REQUIRED) - find_package(ament_lint_auto REQUIRED) find_package(launch_testing_ament_cmake REQUIRED) - list(APPEND AMENT_LINT_AUTO_EXCLUDE - ament_cmake_uncrustify - ament_cmake_cpplint - ) - ament_lint_auto_find_test_dependencies() - ament_add_gtest(joint_limits_interface_test test/joint_limits_interface_test.cpp) target_include_directories(joint_limits_interface_test PUBLIC include) ament_target_dependencies(joint_limits_interface_test hardware_interface rclcpp) diff --git a/joint_limits_interface/package.xml b/joint_limits_interface/package.xml index 2170268c125..c1fd16fa49b 100644 --- a/joint_limits_interface/package.xml +++ b/joint_limits_interface/package.xml @@ -24,8 +24,6 @@ hardware_interface ament_cmake_gtest - ament_lint_auto - ament_lint_common hardware_interface launch launch_ros diff --git a/ros2_control_test_assets/CMakeLists.txt b/ros2_control_test_assets/CMakeLists.txt index 970b3abd015..47fe15a7f75 100644 --- a/ros2_control_test_assets/CMakeLists.txt +++ b/ros2_control_test_assets/CMakeLists.txt @@ -8,15 +8,6 @@ install( DESTINATION include ) -if(BUILD_TESTING) - find_package(ament_lint_auto REQUIRED) - list(APPEND AMENT_LINT_AUTO_EXCLUDE - ament_cmake_uncrustify - ament_cmake_cpplint - ) - ament_lint_auto_find_test_dependencies() -endif() - ament_export_include_directories( include ) diff --git a/ros2_control_test_assets/package.xml b/ros2_control_test_assets/package.xml index 7bdfc8c0fb5..8d3ef783b16 100644 --- a/ros2_control_test_assets/package.xml +++ b/ros2_control_test_assets/package.xml @@ -10,7 +10,6 @@ Apache License 2.0 ament_cmake - ament_lint_auto ament_cmake diff --git a/transmission_interface/CMakeLists.txt b/transmission_interface/CMakeLists.txt index 72130c28c94..bc5b8253dec 100644 --- a/transmission_interface/CMakeLists.txt +++ b/transmission_interface/CMakeLists.txt @@ -40,12 +40,6 @@ install(TARGETS ${PROJECT_NAME} if(BUILD_TESTING) find_package(ament_cmake_gmock REQUIRED) - find_package(ament_lint_auto REQUIRED) - list(APPEND AMENT_LINT_AUTO_EXCLUDE - ament_cmake_uncrustify - ament_cmake_cpplint - ) - ament_lint_auto_find_test_dependencies() ament_add_gmock( test_simple_transmission diff --git a/transmission_interface/package.xml b/transmission_interface/package.xml index 4fcdb5f3571..64d3fb19dc4 100644 --- a/transmission_interface/package.xml +++ b/transmission_interface/package.xml @@ -14,8 +14,6 @@ pluginlib ament_cmake_gmock - ament_lint_auto - ament_lint_common ament_cmake From 1822e517c4f34b617d5f5b15a10fe8a79e9159c8 Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Thu, 30 Jun 2022 17:10:55 +0100 Subject: [PATCH 09/35] Fix test dependency for chainable test (#751) --- controller_manager/CMakeLists.txt | 10 ++-------- controller_manager/package.xml | 1 + 2 files changed, 3 insertions(+), 8 deletions(-) diff --git a/controller_manager/CMakeLists.txt b/controller_manager/CMakeLists.txt index 33a0c8d131e..4165c28e091 100644 --- a/controller_manager/CMakeLists.txt +++ b/controller_manager/CMakeLists.txt @@ -52,14 +52,7 @@ install(DIRECTORY include/ if(BUILD_TESTING) find_package(ament_cmake_gmock REQUIRED) find_package(ros2_control_test_assets REQUIRED) - - ament_index_get_prefix_path(ament_index_build_path SKIP_AMENT_PREFIX_PATH) - # Get the first item (it will be the build space version of the build path). - list(GET ament_index_build_path 0 ament_index_build_path) - if(WIN32) - # On Windows prevent CMake errors and prevent it being evaluated as a list. - string(REPLACE "\\" "/" ament_index_build_path "${ament_index_build_path}") - endif() + find_package(realtime_tools REQUIRED) add_library(test_controller SHARED test/test_controller/test_controller.cpp) target_include_directories(test_controller PRIVATE include) @@ -82,6 +75,7 @@ if(BUILD_TESTING) ) add_library(test_chainable_controller SHARED test/test_chainable_controller/test_chainable_controller.cpp) + ament_target_dependencies(test_chainable_controller realtime_tools) target_include_directories(test_chainable_controller PRIVATE include) target_link_libraries(test_chainable_controller controller_manager) target_compile_definitions(test_chainable_controller PRIVATE "CONTROLLER_MANAGER_BUILDING_DLL") diff --git a/controller_manager/package.xml b/controller_manager/package.xml index e63bc63d0b9..456f1c2e246 100644 --- a/controller_manager/package.xml +++ b/controller_manager/package.xml @@ -24,6 +24,7 @@ ros2run ament_cmake_gmock + realtime_tools ament_cmake From e2697ea9551d7f314400861058cf4d7ca80aa9e9 Mon Sep 17 00:00:00 2001 From: Leander Stephen D'Souza Date: Thu, 30 Jun 2022 21:43:32 +0530 Subject: [PATCH 10/35] Add available status and moved to fstrings when listing hardware interfaces (#739) --- .../ros2controlcli/verb/list_hardware_interfaces.py | 10 ++++------ 1 file changed, 4 insertions(+), 6 deletions(-) diff --git a/ros2controlcli/ros2controlcli/verb/list_hardware_interfaces.py b/ros2controlcli/ros2controlcli/verb/list_hardware_interfaces.py index 5fd7c66e0c0..0bde1876cee 100644 --- a/ros2controlcli/ros2controlcli/verb/list_hardware_interfaces.py +++ b/ros2controlcli/ros2controlcli/verb/list_hardware_interfaces.py @@ -39,14 +39,12 @@ def main(self, *, args): print('command interfaces') for command_interface in command_interfaces: print( - '\t%s [%s]' - % ( - command_interface.name, - 'claimed' if command_interface.is_claimed else 'unclaimed', - ) + f'\t{command_interface.name} ' + f'{"[available]" if command_interface.is_available else "[unavailable]"} ' + f'{"[claimed]" if command_interface.is_claimed else "[unclaimed]"}' ) print('state interfaces') for state_interface in state_interfaces: - print('\t', state_interface.name) + print(f'\t{state_interface.name}') return 0 From 5e876d2d01b5bf679df79949f77f0e9d70424475 Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Thu, 30 Jun 2022 18:45:34 +0100 Subject: [PATCH 11/35] Update maintainers of packages (#753) --- controller_interface/package.xml | 2 +- controller_manager/package.xml | 3 ++- controller_manager_msgs/package.xml | 1 + hardware_interface/package.xml | 4 ++-- joint_limits_interface/package.xml | 2 +- ros2_control/package.xml | 3 +-- ros2_control_test_assets/package.xml | 2 +- ros2controlcli/package.xml | 4 +++- transmission_interface/package.xml | 2 +- 9 files changed, 13 insertions(+), 10 deletions(-) diff --git a/controller_interface/package.xml b/controller_interface/package.xml index 644b06dd635..917845964e2 100644 --- a/controller_interface/package.xml +++ b/controller_interface/package.xml @@ -5,7 +5,7 @@ 2.10.0 Description of controller_interface Bence Magyar - Karsten Knese + Denis Štogl Apache License 2.0 ament_cmake diff --git a/controller_manager/package.xml b/controller_manager/package.xml index 456f1c2e246..5ce8af43826 100644 --- a/controller_manager/package.xml +++ b/controller_manager/package.xml @@ -4,7 +4,8 @@ controller_manager 2.10.0 Description of controller_manager - karsten + Bence Magyar + Denis Štogl Apache License 2.0 ament_cmake diff --git a/controller_manager_msgs/package.xml b/controller_manager_msgs/package.xml index 76b59a4fe2a..4ff02ebbfbf 100644 --- a/controller_manager_msgs/package.xml +++ b/controller_manager_msgs/package.xml @@ -5,6 +5,7 @@ 2.10.0 Messages and services for the controller manager. Bence Magyar + Denis Štogl BSD http://ros.org/wiki/controller_manager_msgs Stuart Glaser diff --git a/hardware_interface/package.xml b/hardware_interface/package.xml index a5a0b1e0018..6271023d2ac 100644 --- a/hardware_interface/package.xml +++ b/hardware_interface/package.xml @@ -2,9 +2,9 @@ hardware_interface 2.10.0 - ROS2 ros_control hardware interface - Karsten Knese + ros2_control hardware interface Bence Magyar + Denis Štogl Apache License 2.0 ament_cmake diff --git a/joint_limits_interface/package.xml b/joint_limits_interface/package.xml index c1fd16fa49b..badbb517732 100644 --- a/joint_limits_interface/package.xml +++ b/joint_limits_interface/package.xml @@ -4,7 +4,7 @@ Interface for enforcing joint limits. Bence Magyar - Jordan Palacios + Denis Štogl Apache License 2.0 diff --git a/ros2_control/package.xml b/ros2_control/package.xml index 128a53eb1bc..7b734e40018 100644 --- a/ros2_control/package.xml +++ b/ros2_control/package.xml @@ -4,8 +4,7 @@ 2.10.0 Metapackage for ROS2 control related packages Bence Magyar - Jordan Palacios - Denis Štogl + Denis Štogl Apache License 2.0 diff --git a/ros2_control_test_assets/package.xml b/ros2_control_test_assets/package.xml index 8d3ef783b16..e52e50a79e9 100644 --- a/ros2_control_test_assets/package.xml +++ b/ros2_control_test_assets/package.xml @@ -6,7 +6,7 @@ The package provides shared test resources for ros2_control stack Bence Magyar - Denis Štogl + Denis Štogl Apache License 2.0 ament_cmake diff --git a/ros2controlcli/package.xml b/ros2controlcli/package.xml index 855812eab23..afc8dab5280 100644 --- a/ros2controlcli/package.xml +++ b/ros2controlcli/package.xml @@ -6,8 +6,10 @@ The ROS 2 command line tools for ROS2 Control. - Victor Lopez + Bence Magyar + Denis Štogl Apache License 2.0 + Victor Lopez rclpy ros2cli diff --git a/transmission_interface/package.xml b/transmission_interface/package.xml index 64d3fb19dc4..58c94bd1525 100644 --- a/transmission_interface/package.xml +++ b/transmission_interface/package.xml @@ -5,7 +5,7 @@ 2.10.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 - Karsten Knese + Denis Štogl Apache License 2.0 ament_cmake From dabe19edf952c87f7ffa97ce49ed4bcd1e277cec Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Thu, 30 Jun 2022 19:50:08 +0200 Subject: [PATCH 12/35] Update and fix CI setup (#752) * Add names to ABI compatibility workflows. * Don't use source build on the old distros. * Disable rhel build on PRs since it will failanyway. * We need common linters in packages that generate messages since those auto automatically used. * remove disabling of tests. --- .github/workflows/README.md | 7 ++++ .github/workflows/ci-coverage-build.yml | 17 ++++++---- .github/workflows/ci-ros-lint.yml | 14 ++++---- .github/workflows/foxy-abi-compatibility.yml | 5 ++- ...y-build.yml => foxy-binary-build-main.yml} | 6 +++- ...ld.yml => foxy-semi-binary-build-main.yml} | 6 +++- .github/workflows/foxy-source-build.yml | 15 --------- .../workflows/galactic-abi-compatibility.yml | 5 ++- ...ild.yml => galactic-binary-build-main.yml} | 6 +++- .../workflows/galactic-rhel-binary-build.yml | 33 +++++++++++++++++++ ...ml => galactic-semi-binary-build-main.yml} | 5 +-- ...focal.yml => humble-abi-compatibility.yml} | 11 +++---- .../workflows/humble-binary-build-main.yml | 26 +++++++++++++++ .../workflows/humble-binary-build-testing.yml | 26 +++++++++++++++ .../workflows/humble-rhel-binary-build.yml | 33 +++++++++++++++++++ .../humble-semi-binary-build-main.yml | 25 ++++++++++++++ .../humble-semi-binary-build-testing.yml | 25 ++++++++++++++ ...urce-build.yml => humble-source-build.yml} | 12 ++++--- .../reusable-industrial-ci-with-cache.yml | 30 ++++++----------- .../reusable-ros-tooling-source-build.yml | 17 ++++++---- .../workflows/rolling-abi-compatibility.yml | 4 ++- ...uild.yml => rolling-binary-build-main.yml} | 6 +++- ...l.yml => rolling-binary-build-testing.yml} | 11 +++---- .../workflows/rolling-rhel-binary-build.yml | 3 ++ ...yml => rolling-semi-binary-build-main.yml} | 6 +++- ... => rolling-semi-binary-build-testing.yml} | 11 +++---- .github/workflows/rolling-source-build.yml | 4 +++ README.md | 10 +++--- controller_manager_msgs/package.xml | 2 ++ ros2_control-not-released.humble.repos | 1 + ros2_control.humble.repos | 9 +++++ 31 files changed, 301 insertions(+), 90 deletions(-) create mode 100644 .github/workflows/README.md rename .github/workflows/{foxy-binary-build.yml => foxy-binary-build-main.yml} (84%) rename .github/workflows/{foxy-semi-binary-build.yml => foxy-semi-binary-build-main.yml} (81%) delete mode 100644 .github/workflows/foxy-source-build.yml rename .github/workflows/{galactic-binary-build.yml => galactic-binary-build-main.yml} (83%) create mode 100644 .github/workflows/galactic-rhel-binary-build.yml rename .github/workflows/{galactic-semi-binary-build.yml => galactic-semi-binary-build-main.yml} (85%) rename .github/workflows/{rolling-abi-compatibility-last-focal.yml => humble-abi-compatibility.yml} (53%) create mode 100644 .github/workflows/humble-binary-build-main.yml create mode 100644 .github/workflows/humble-binary-build-testing.yml create mode 100644 .github/workflows/humble-rhel-binary-build.yml create mode 100644 .github/workflows/humble-semi-binary-build-main.yml create mode 100644 .github/workflows/humble-semi-binary-build-testing.yml rename .github/workflows/{galactic-source-build.yml => humble-source-build.yml} (57%) rename .github/workflows/{rolling-binary-build.yml => rolling-binary-build-main.yml} (83%) rename .github/workflows/{rolling-binary-build-last-focal.yml => rolling-binary-build-testing.yml} (66%) rename .github/workflows/{rolling-semi-binary-build.yml => rolling-semi-binary-build-main.yml} (81%) rename .github/workflows/{rolling-semi-binary-build-last-focal.yml => rolling-semi-binary-build-testing.yml} (63%) create mode 100644 ros2_control-not-released.humble.repos create mode 100644 ros2_control.humble.repos diff --git a/.github/workflows/README.md b/.github/workflows/README.md new file mode 100644 index 00000000000..fd751faeef0 --- /dev/null +++ b/.github/workflows/README.md @@ -0,0 +1,7 @@ + +ROS2 Distro | Branch | Build status | Documentation | Released packages +:---------: | :----: | :----------: | :-----------: | :---------------: +**Rolling** | [`rolling`](https://github.com/ros-controls/ros2_control/tree/rolling) | [![Rolling Binary Build](https://github.com/ros-controls/ros2_control/actions/workflows/rolling-binary-build-main.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_control/actions/workflows/rolling-binary-build-main.yml?branch=master)
[![Rolling Binary Build](https://github.com/ros-controls/ros2_control/actions/workflows/rolling-binary-build-testing.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_control/actions/workflows/rolling-binary-build-testing.yml?branch=master)
[![Rolling Semi-Binary Build](https://github.com/ros-controls/ros2_control/actions/workflows/rolling-semi-binary-build-main.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_control/actions/workflows/rolling-semi-binary-build-main.yml?branch=master)
[![Rolling Semi-Binary Build](https://github.com/ros-controls/ros2_control/actions/workflows/rolling-semi-binary-build-testing.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_control/actions/workflows/rolling-semi-binary-build-testing.yml?branch=master)
[![Rolling Source Build](https://github.com/ros-controls/ros2_control/actions/workflows/rolling-source-build.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_control/actions/workflows/rolling-source-build.yml?branch=master) | [Documentation](https://control.ros.org/master/index.html)
[API Reference](https://control.ros.org/master/doc/api/index.html) | [ros2_control](https://index.ros.org/p/ros2_control/#rolling) +**Humble** | [`humble`](https://github.com/ros-controls/ros2_control/tree/humble) | [![Humble Binary Build](https://github.com/ros-controls/ros2_control/actions/workflows/humble-binary-build-main.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_control/actions/workflows/humble-binary-build-main.yml?branch=master)
[![Humble Binary Build](https://github.com/ros-controls/ros2_control/actions/workflows/humble-binary-build-testing.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_control/actions/workflows/humble-binary-build-testing.yml?branch=master)
[![Humble Semi-Binary Build](https://github.com/ros-controls/ros2_control/actions/workflows/humble-semi-binary-build-main.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_control/actions/workflows/humble-semi-binary-build-main.yml?branch=master)
[![Humble Semi-Binary Build](https://github.com/ros-controls/ros2_control/actions/workflows/humble-semi-binary-build-testing.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_control/actions/workflows/humble-semi-binary-build-testing.yml?branch=master)
[![Humble Source Build](https://github.com/ros-controls/ros2_control/actions/workflows/humble-source-build.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_control/actions/workflows/humble-source-build.yml?branch=master) | [Documentation](https://control.ros.org/master/index.html)
[API Reference](https://control.ros.org/master/doc/api/index.html) | [ros2_control](https://index.ros.org/p/ros2_control/#humble) +**Galactic** | [`galactic`](https://github.com/ros-controls/ros2_control/tree/galactic) | [![Galactic Binary Build](https://github.com/ros-controls/ros2_control/actions/workflows/galactic-binary-build-main.yml/badge.svg?branch=galactic)](https://github.com/ros-controls/ros2_control/actions/workflows/galactic-binary-build-main.yml?branch=galactic)
[![Galactic Binary Build](https://github.com/ros-controls/ros2_control/actions/workflows/galactic-binary-build-testing.yml/badge.svg?branch=galactic)](https://github.com/ros-controls/ros2_control/actions/workflows/galactic-binary-build-testing.yml?branch=galactic)
[![Galactic Semi-Binary Build](https://github.com/ros-controls/ros2_control/actions/workflows/galactic-semi-binary-build-main.yml/badge.svg?branch=galactic)](https://github.com/ros-controls/ros2_control/actions/workflows/galactic-semi-binary-build-main.yml?branch=galactic)
[![Galactic Semi-Binary Build](https://github.com/ros-controls/ros2_control/actions/workflows/galactic-semi-binary-build-testing.yml/badge.svg?branch=galactic)](https://github.com/ros-controls/ros2_control/actions/workflows/galactic-semi-binary-build-testing.yml?branch=galactic) | [Documentation](https://control.ros.org/galactic/index.html)
[API Reference](https://control.ros.org/galactic/doc/api/index.html) | [ros2_control](https://index.ros.org/p/ros2_control/#galactic) +**Foxy** | [`foxy`](https://github.com/ros-controls/ros2_control/tree/foxy) | [![Foxy Binary Build](https://github.com/ros-controls/ros2_control/actions/workflows/foxy-binary-build-main.yml/badge.svg?branch=foxy)](https://github.com/ros-controls/ros2_control/actions/workflows/foxy-binary-build-main.yml?branch=foxy)
[![Foxy Binary Build](https://github.com/ros-controls/ros2_control/actions/workflows/foxy-binary-build-testing.yml/badge.svg?branch=foxy)](https://github.com/ros-controls/ros2_control/actions/workflows/foxy-binary-build-testing.yml?branch=foxy)
[![Foxy Semi-Binary Build](https://github.com/ros-controls/ros2_control/actions/workflows/foxy-semi-binary-build-main.yml/badge.svg?branch=foxy)](https://github.com/ros-controls/ros2_control/actions/workflows/foxy-semi-binary-build-main.yml?branch=foxy)
[![Foxy Semi-Binary Build](https://github.com/ros-controls/ros2_control/actions/workflows/foxy-semi-binary-build-testing.yml/badge.svg?branch=foxy)](https://github.com/ros-controls/ros2_control/actions/workflows/foxy-semi-binary-build-testing.yml?branch=foxy) | [Documentation](https://control.ros.org/foxy/index.html)
[API Reference](https://control.ros.org/foxy/doc/api/index.html) | [ros2_control](https://index.ros.org/p/ros2_control/#foxy) diff --git a/.github/workflows/ci-coverage-build.yml b/.github/workflows/ci-coverage-build.yml index 6fa6724f641..a2d02ed8a04 100644 --- a/.github/workflows/ci-coverage-build.yml +++ b/.github/workflows/ci-coverage-build.yml @@ -1,5 +1,8 @@ name: Coverage Build on: + workflow_dispatch: + branches: + - master pull_request: branches: - master @@ -7,17 +10,17 @@ on: jobs: coverage: name: coverage build - runs-on: ubuntu-20.04 + runs-on: ubuntu-22.04 strategy: fail-fast: false env: ROS_DISTRO: rolling steps: - - uses: ros-tooling/setup-ros@v0.2 + - uses: ros-tooling/setup-ros@0.3.4 with: required-ros-distributions: ${{ env.ROS_DISTRO }} - uses: actions/checkout@v3 - - uses: ros-tooling/action-ros-ci@v0.2 + - uses: ros-tooling/action-ros-ci@0.2.6 with: target-ros2-distro: ${{ env.ROS_DISTRO }} import-token: ${{ secrets.GITHUB_TOKEN }} @@ -31,6 +34,7 @@ jobs: ros2_control ros2_control_test_assets transmission_interface + vcs-repo-file-url: | https://raw.githubusercontent.com/${{ github.repository }}/${{ github.sha }}/ros2_control-not-released.${{ env.ROS_DISTRO }}.repos?token=${{ secrets.GITHUB_TOKEN }} colcon-defaults: | @@ -40,13 +44,12 @@ jobs: } } colcon-mixin-repository: https://raw.githubusercontent.com/colcon/colcon-mixin-repository/master/index.yaml - skip-tests: true - - uses: codecov/codecov-action@v1.0.14 + - uses: codecov/codecov-action@v3.1.0 with: file: ros_ws/lcov/total_coverage.info flags: unittests name: codecov-umbrella - - uses: actions/upload-artifact@v1 + - uses: actions/upload-artifact@v3.1.0 with: - name: colcon-logs-${{ matrix.os }} + name: colcon-logs-ubuntu-22.04-coverage-rolling path: ros_ws/log diff --git a/.github/workflows/ci-ros-lint.yml b/.github/workflows/ci-ros-lint.yml index 55e1dad0c91..009c394bfe0 100644 --- a/.github/workflows/ci-ros-lint.yml +++ b/.github/workflows/ci-ros-lint.yml @@ -9,13 +9,13 @@ jobs: strategy: fail-fast: false matrix: - linter: [copyright, cppcheck, lint_cmake] + linter: [cppcheck, copyright, lint_cmake] steps: - uses: actions/checkout@v3 - uses: ros-tooling/setup-ros@v0.2 - uses: ros-tooling/action-ros-lint@v0.1 with: - distribution: galactic + distribution: rolling linter: ${{ matrix.linter }} package-name: controller_interface @@ -27,19 +27,21 @@ jobs: ros2_control_test_assets transmission_interface - ament_lint_cpplint: - name: ament_lint_cpplint + ament_lint_100: + name: ament_${{ matrix.linter }} runs-on: ubuntu-20.04 strategy: fail-fast: false + matrix: + linter: [cpplint] steps: - uses: actions/checkout@v3 - uses: ros-tooling/setup-ros@v0.2 - uses: ros-tooling/action-ros-lint@v0.1 with: - distribution: galactic + distribution: rolling linter: cpplint - arguments: "--filter=-whitespace/newline" + arguments: "--linelength=100 --filter=-whitespace/newline" package-name: controller_interface controller_manager diff --git a/.github/workflows/foxy-abi-compatibility.yml b/.github/workflows/foxy-abi-compatibility.yml index 40b1e1133f1..7ce17effd03 100644 --- a/.github/workflows/foxy-abi-compatibility.yml +++ b/.github/workflows/foxy-abi-compatibility.yml @@ -1,5 +1,8 @@ -name: ABI Compatibility Check +name: Foxy - ABI Compatibility Check on: + workflow_dispatch: + branches: + - foxy pull_request: branches: - foxy diff --git a/.github/workflows/foxy-binary-build.yml b/.github/workflows/foxy-binary-build-main.yml similarity index 84% rename from .github/workflows/foxy-binary-build.yml rename to .github/workflows/foxy-binary-build-main.yml index da67426ec52..0d74ce64e4d 100644 --- a/.github/workflows/foxy-binary-build.yml +++ b/.github/workflows/foxy-binary-build-main.yml @@ -1,8 +1,11 @@ -name: Foxy Binary Build +name: Foxy Binary Build - main # author: Denis Štogl # description: 'Build & test all dependencies from released (binary) packages.' on: + workflow_dispatch: + branches: + - foxy pull_request: branches: - foxy @@ -18,5 +21,6 @@ jobs: uses: ./.github/workflows/reusable-industrial-ci-with-cache.yml with: ros_distro: foxy + ros_repo: main upstream_workspace: ros2_control-not-released.foxy.repos ref_for_scheduled_build: foxy diff --git a/.github/workflows/foxy-semi-binary-build.yml b/.github/workflows/foxy-semi-binary-build-main.yml similarity index 81% rename from .github/workflows/foxy-semi-binary-build.yml rename to .github/workflows/foxy-semi-binary-build-main.yml index 93f75616d37..68e709cc000 100644 --- a/.github/workflows/foxy-semi-binary-build.yml +++ b/.github/workflows/foxy-semi-binary-build-main.yml @@ -1,7 +1,10 @@ -name: Foxy Semi-Binary Build +name: Foxy Semi-Binary Build - main # description: 'Build & test that compiles the main dependencies from source.' on: + workflow_dispatch: + branches: + - foxy pull_request: branches: - foxy @@ -17,5 +20,6 @@ jobs: uses: ./.github/workflows/reusable-industrial-ci-with-cache.yml with: ros_distro: foxy + ros_repo: main upstream_workspace: ros2_control.foxy.repos ref_for_scheduled_build: foxy diff --git a/.github/workflows/foxy-source-build.yml b/.github/workflows/foxy-source-build.yml deleted file mode 100644 index 94be8d8b57a..00000000000 --- a/.github/workflows/foxy-source-build.yml +++ /dev/null @@ -1,15 +0,0 @@ -name: Foxy Source Build -on: - push: - branches: - - foxy - schedule: - # Run every day to detect flakiness and broken dependencies - - cron: '03 3 * * *' - -jobs: - source: - uses: ./.github/workflows/reusable-ros-tooling-source-build.yml - with: - ros_distro: foxy - ref: foxy diff --git a/.github/workflows/galactic-abi-compatibility.yml b/.github/workflows/galactic-abi-compatibility.yml index 41c663dd9c2..06a48ef9c71 100644 --- a/.github/workflows/galactic-abi-compatibility.yml +++ b/.github/workflows/galactic-abi-compatibility.yml @@ -1,5 +1,8 @@ -name: ABI Compatibility Check +name: Galactic - ABI Compatibility Check on: + workflow_dispatch: + branches: + - galactic pull_request: branches: - galactic diff --git a/.github/workflows/galactic-binary-build.yml b/.github/workflows/galactic-binary-build-main.yml similarity index 83% rename from .github/workflows/galactic-binary-build.yml rename to .github/workflows/galactic-binary-build-main.yml index 97f8a8f231a..f50c7734446 100644 --- a/.github/workflows/galactic-binary-build.yml +++ b/.github/workflows/galactic-binary-build-main.yml @@ -1,8 +1,11 @@ -name: Galactic Binary Build +name: Galactic Binary Build - main # author: Denis Štogl # description: 'Build & test all dependencies from released (binary) packages.' on: + workflow_dispatch: + branches: + - galactic pull_request: branches: - galactic @@ -18,5 +21,6 @@ jobs: uses: ./.github/workflows/reusable-industrial-ci-with-cache.yml with: ros_distro: galactic + ros_repo: main upstream_workspace: ros2_control-not-released.galactic.repos ref_for_scheduled_build: galactic diff --git a/.github/workflows/galactic-rhel-binary-build.yml b/.github/workflows/galactic-rhel-binary-build.yml new file mode 100644 index 00000000000..cd2d5f0862a --- /dev/null +++ b/.github/workflows/galactic-rhel-binary-build.yml @@ -0,0 +1,33 @@ +name: Galactic RHEL Binary Build +on: + workflow_dispatch: + branches: + - galactic + pull_request: + branches: + - galactic + push: + branches: + - galactic + schedule: + # Run every day to detect flakiness and broken dependencies + - cron: '03 1 * * *' + + +jobs: + galactic_rhel_binary: + name: Galactic RHEL binary build + runs-on: ubuntu-latest + env: + ROS_DISTRO: galactic + container: jaronl/ros:galactic-alma + steps: + - uses: actions/checkout@v3 + with: + path: src/ros2_control + - run: | + rosdep update + rosdep install -iy --from-path src/ros2_control + source /opt/ros/${{ env.ROS_DISTRO }}/setup.bash + colcon build + colcon test diff --git a/.github/workflows/galactic-semi-binary-build.yml b/.github/workflows/galactic-semi-binary-build-main.yml similarity index 85% rename from .github/workflows/galactic-semi-binary-build.yml rename to .github/workflows/galactic-semi-binary-build-main.yml index 9e7073e2634..0578366a456 100644 --- a/.github/workflows/galactic-semi-binary-build.yml +++ b/.github/workflows/galactic-semi-binary-build-main.yml @@ -1,8 +1,8 @@ -name: Galactic Semi-Binary Build +name: Galactic Semi-Binary Build - main # description: 'Build & test that compiles the main dependencies from source.' on: - pull_request: + workflow_dispatch: branches: - galactic push: @@ -17,5 +17,6 @@ jobs: uses: ./.github/workflows/reusable-industrial-ci-with-cache.yml with: ros_distro: galactic + ros_repo: main upstream_workspace: ros2_control.galactic.repos ref_for_scheduled_build: galactic diff --git a/.github/workflows/rolling-abi-compatibility-last-focal.yml b/.github/workflows/humble-abi-compatibility.yml similarity index 53% rename from .github/workflows/rolling-abi-compatibility-last-focal.yml rename to .github/workflows/humble-abi-compatibility.yml index 80433e2494e..1be00cfc763 100644 --- a/.github/workflows/rolling-abi-compatibility-last-focal.yml +++ b/.github/workflows/humble-abi-compatibility.yml @@ -1,5 +1,8 @@ -name: ABI Compatibility Check - Last Focal +name: Humble - ABI Compatibility Check on: + workflow_dispatch: + branches: + - master pull_request: branches: - master @@ -11,11 +14,7 @@ jobs: - uses: actions/checkout@v3 - uses: ros-industrial/industrial_ci@master env: - ROS_DISTRO: rolling + ROS_DISTRO: humble ROS_REPO: main ABICHECK_URL: github:${{ github.repository }}#${{ github.base_ref }} NOT_TEST_BUILD: true - OS_CODE_NAME: focal - BEFORE_INSTALL_UPSTREAM_DEPENDENCIES: | - export ROSDISTRO_INDEX_URL=https://raw.githubusercontent.com/ros/rosdistro/rolling/2022-01-28/index-v4.yaml - rosdep update diff --git a/.github/workflows/humble-binary-build-main.yml b/.github/workflows/humble-binary-build-main.yml new file mode 100644 index 00000000000..365744af4b3 --- /dev/null +++ b/.github/workflows/humble-binary-build-main.yml @@ -0,0 +1,26 @@ +name: Humble Binary Build - main +# author: Denis Štogl +# description: 'Build & test all dependencies from released (binary) packages.' + +on: + workflow_dispatch: + branches: + - master + pull_request: + branches: + - master + push: + branches: + - master + schedule: + # Run every morning to detect flakiness and broken dependencies + - cron: '03 1 * * *' + +jobs: + binary: + uses: ./.github/workflows/reusable-industrial-ci-with-cache.yml + with: + ros_distro: humble + ros_repo: main + upstream_workspace: ros2_control-not-released.humble.repos + ref_for_scheduled_build: master diff --git a/.github/workflows/humble-binary-build-testing.yml b/.github/workflows/humble-binary-build-testing.yml new file mode 100644 index 00000000000..edb94f3fe8f --- /dev/null +++ b/.github/workflows/humble-binary-build-testing.yml @@ -0,0 +1,26 @@ +name: Humble Binary Build - testing +# author: Denis Štogl +# description: 'Build & test all dependencies from released (binary) packages.' + +on: + workflow_dispatch: + branches: + - master + pull_request: + branches: + - master + push: + branches: + - master + schedule: + # Run every morning to detect flakiness and broken dependencies + - cron: '03 1 * * *' + +jobs: + binary: + uses: ./.github/workflows/reusable-industrial-ci-with-cache.yml + with: + ros_distro: humble + ros_repo: testing + upstream_workspace: ros2_control-not-released.humble.repos + ref_for_scheduled_build: master diff --git a/.github/workflows/humble-rhel-binary-build.yml b/.github/workflows/humble-rhel-binary-build.yml new file mode 100644 index 00000000000..554e306d653 --- /dev/null +++ b/.github/workflows/humble-rhel-binary-build.yml @@ -0,0 +1,33 @@ +name: Humble RHEL Binary Build +on: + workflow_dispatch: + branches: + - master +# pull_request: +# branches: +# - master +# push: +# branches: +# - master +# schedule: +# # Run every day to detect flakiness and broken dependencies +# - cron: '03 1 * * *' + + +jobs: + humble_rhel_binary: + name: Humble RHEL binary build + runs-on: ubuntu-latest + env: + ROS_DISTRO: humble + container: jaronl/ros:humble-alma + steps: + - uses: actions/checkout@v3 + with: + path: src/ros2_control + - run: | + rosdep update + rosdep install -iy --from-path src/ros2_control + source /opt/ros/${{ env.ROS_DISTRO }}/setup.bash + colcon build + colcon test diff --git a/.github/workflows/humble-semi-binary-build-main.yml b/.github/workflows/humble-semi-binary-build-main.yml new file mode 100644 index 00000000000..7e79db32640 --- /dev/null +++ b/.github/workflows/humble-semi-binary-build-main.yml @@ -0,0 +1,25 @@ +name: Humble Semi-Binary Build - main +# description: 'Build & test that compiles the main dependencies from source.' + +on: + workflow_dispatch: + branches: + - master + pull_request: + branches: + - master + push: + branches: + - master + schedule: + # Run every morning to detect flakiness and broken dependencies + - cron: '33 1 * * *' + +jobs: + semi_binary: + uses: ./.github/workflows/reusable-industrial-ci-with-cache.yml + with: + ros_distro: humble + ros_repo: main + upstream_workspace: ros2_control.humble.repos + ref_for_scheduled_build: master diff --git a/.github/workflows/humble-semi-binary-build-testing.yml b/.github/workflows/humble-semi-binary-build-testing.yml new file mode 100644 index 00000000000..485e49f2cfb --- /dev/null +++ b/.github/workflows/humble-semi-binary-build-testing.yml @@ -0,0 +1,25 @@ +name: Humble Semi-Binary Build - testing +# description: 'Build & test that compiles the main dependencies from source.' + +on: + workflow_dispatch: + branches: + - master + pull_request: + branches: + - master + push: + branches: + - master + schedule: + # Run every morning to detect flakiness and broken dependencies + - cron: '33 1 * * *' + +jobs: + semi_binary: + uses: ./.github/workflows/reusable-industrial-ci-with-cache.yml + with: + ros_distro: humble + ros_repo: testing + upstream_workspace: ros2_control.humble.repos + ref_for_scheduled_build: master diff --git a/.github/workflows/galactic-source-build.yml b/.github/workflows/humble-source-build.yml similarity index 57% rename from .github/workflows/galactic-source-build.yml rename to .github/workflows/humble-source-build.yml index d1c3aa97e8a..18c33b6d524 100644 --- a/.github/workflows/galactic-source-build.yml +++ b/.github/workflows/humble-source-build.yml @@ -1,8 +1,11 @@ -name: Galactic Source Build +name: Humble Source Build on: + workflow_dispatch: + branches: + - master push: branches: - - galactic + - master schedule: # Run every day to detect flakiness and broken dependencies - cron: '03 3 * * *' @@ -11,5 +14,6 @@ jobs: source: uses: ./.github/workflows/reusable-ros-tooling-source-build.yml with: - ros_distro: galactic - ref: galactic + ros_distro: humble + ref: master + ros2_repo_branch: humble diff --git a/.github/workflows/reusable-industrial-ci-with-cache.yml b/.github/workflows/reusable-industrial-ci-with-cache.yml index 9d2722099d1..0ff359d091e 100644 --- a/.github/workflows/reusable-industrial-ci-with-cache.yml +++ b/.github/workflows/reusable-industrial-ci-with-cache.yml @@ -20,8 +20,8 @@ on: required: true type: string ros_repo: - description: 'ROS_REPO to run for industrial_ci. Possible values: "all", "main", "testing"' - default: 'all' + description: 'ROS_REPO to run for industrial_ci. Possible values: "main", "testing"' + default: 'main' required: false type: string os_code_name: @@ -49,27 +49,23 @@ on: jobs: reusable_industrial_ci_with_cache: - name: ${{ matrix.ROS_REPO }} ${{ inputs.ros_distro }} ${{ inputs.os_code_name }} - strategy: - fail-fast: false - matrix: - ROS_REPO: [ main, testing ] + name: ${{ inputs.ros_distro }} ${{ inputs.ros_repo }} ${{ inputs.os_code_name }} runs-on: ubuntu-latest env: CCACHE_DIR: ${{ github.workspace }}/${{ inputs.ccache_dir }} BASEDIR: ${{ github.workspace }}/${{ inputs.basedir }} - CACHE_PREFIX: ${{ inputs.ros_distro }}-${{ inputs.os_code_name }}-${{ matrix.ROS_REPO }}-${{ github.job }} + CACHE_PREFIX: ${{ inputs.ros_distro }}-${{ inputs.os_code_name }}-${{ inputs.ros_repo }}-${{ github.job }} steps: - name: Checkout ${{ inputs.ref }} when build is not scheduled - if: ${{ (inputs.ros_repo == 'all' || inputs.ros_repo == matrix.ROS_REPO) && github.event_name != 'schedule' }} + if: ${{ github.event_name != 'schedule' }} uses: actions/checkout@v3 - name: Checkout ${{ inputs.ref }} on scheduled build - if: ${{ (inputs.ros_repo == 'all' || inputs.ros_repo == matrix.ROS_REPO) && github.event_name == 'schedule' }} + if: ${{ github.event_name == 'schedule' }} uses: actions/checkout@v3 with: ref: ${{ inputs.ref_for_scheduled_build }} - name: cache target_ws - if: ${{ ! matrix.env.CCOV && (inputs.ros_repo == 'all' || inputs.ros_repo == matrix.ROS_REPO) }} + if: ${{ ! matrix.env.CCOV }} uses: pat-s/always-upload-cache@v2.1.5 with: path: ${{ env.BASEDIR }}/target_ws @@ -77,7 +73,6 @@ jobs: restore-keys: | target_ws-${{ env.CACHE_PREFIX }}-${{ hashFiles('**/CMakeLists.txt', '**/package.xml') }} - name: cache ccache - if: ${{ inputs.ros_repo == 'all' || inputs.ros_repo == matrix.ROS_REPO }} uses: pat-s/always-upload-cache@v2.1.5 with: path: ${{ env.CCACHE_DIR }} @@ -85,22 +80,17 @@ jobs: restore-keys: | ccache-${{ env.CACHE_PREFIX }}-${{ github.sha }} ccache-${{ env.CACHE_PREFIX }} - - if: ${{ inputs.ros_repo == 'all' || inputs.ros_repo == matrix.ROS_REPO }} - uses: 'ros-industrial/industrial_ci@master' + - uses: 'ros-industrial/industrial_ci@master' env: UPSTREAM_WORKSPACE: ${{ inputs.upstream_workspace }} ROS_DISTRO: ${{ inputs.ros_distro }} - ROS_REPO: ${{ matrix.ROS_REPO }} + ROS_REPO: ${{ inputs.ros_repo }} OS_CODE_NAME: ${{ inputs.os_code_name }} BEFORE_INSTALL_UPSTREAM_DEPENDENCIES: ${{ inputs.before_install_upstream_dependencies }} - name: prepare target_ws for cache - if: ${{ always() && ! matrix.env.CCOV && (inputs.ros_repo == 'all' || inputs.ros_repo == matrix.ROS_REPO) }} + if: ${{ always() && ! matrix.env.CCOV }} run: | du -sh ${{ env.BASEDIR }}/target_ws sudo find ${{ env.BASEDIR }}/target_ws -wholename '*/test_results/*' -delete sudo rm -rf ${{ env.BASEDIR }}/target_ws/src du -sh ${{ env.BASEDIR }}/target_ws - - name: Is job skipped? - if: ${{ ! (inputs.ros_repo == 'all' || inputs.ros_repo == matrix.ROS_REPO) }} - run: | - echo "This job is skpped!" diff --git a/.github/workflows/reusable-ros-tooling-source-build.yml b/.github/workflows/reusable-ros-tooling-source-build.yml index 71afebac1e2..4e5174405d4 100644 --- a/.github/workflows/reusable-ros-tooling-source-build.yml +++ b/.github/workflows/reusable-ros-tooling-source-build.yml @@ -13,21 +13,26 @@ on: description: 'Reference on which the repo should be checkout. Usually is this name of a branch or a tag.' required: true type: string + ros2_repo_branch: + description: 'Branch in the ros2/ros2 repozitory from which ".repos" should be used. Possible values: master (Rolling), humble, galactic, foxy.' + default: 'master' + required: false + type: string jobs: reusable_ros_tooling_source_build: - name: ${{ inputs.ros_distro }} ubuntu-20.04 - runs-on: ubuntu-20.04 + name: ${{ inputs.ros_distro }} ubuntu-22.04 + runs-on: ubuntu-22.04 strategy: fail-fast: false steps: - - uses: ros-tooling/setup-ros@v0.3 + - uses: ros-tooling/setup-ros@0.3.4 with: required-ros-distributions: ${{ inputs.ros_distro }} - uses: actions/checkout@v3 with: ref: ${{ inputs.ref }} - - uses: ros-tooling/action-ros-ci@v0.2 + - uses: ros-tooling/action-ros-ci@0.2.6 with: target-ros2-distro: ${{ inputs.ros_distro }} # build all packages listed in the meta package @@ -41,10 +46,10 @@ jobs: ros2_control_test_assets transmission_interface vcs-repo-file-url: | - https://raw.githubusercontent.com/ros2/ros2/${{ inputs.ros_distro }}/ros2.repos + https://raw.githubusercontent.com/ros2/ros2/${{ inputs.ros2_repo_branch }}/ros2.repos https://raw.githubusercontent.com/${{ github.repository }}/${{ github.sha }}/ros2_control.${{ inputs.ros_distro }}.repos?token=${{ secrets.GITHUB_TOKEN }} colcon-mixin-repository: https://raw.githubusercontent.com/colcon/colcon-mixin-repository/master/index.yaml - uses: actions/upload-artifact@v1 with: - name: colcon-logs-ubuntu-20.04 + name: colcon-logs-ubuntu-22.04 path: ros_ws/log diff --git a/.github/workflows/rolling-abi-compatibility.yml b/.github/workflows/rolling-abi-compatibility.yml index dc797d77188..4589e92e3b3 100644 --- a/.github/workflows/rolling-abi-compatibility.yml +++ b/.github/workflows/rolling-abi-compatibility.yml @@ -1,6 +1,8 @@ -name: ABI Compatibility Check +name: Rolling - ABI Compatibility Check on: workflow_dispatch: + branches: + - master pull_request: branches: - master diff --git a/.github/workflows/rolling-binary-build.yml b/.github/workflows/rolling-binary-build-main.yml similarity index 83% rename from .github/workflows/rolling-binary-build.yml rename to .github/workflows/rolling-binary-build-main.yml index 419d78e850f..05a9b9c0b22 100644 --- a/.github/workflows/rolling-binary-build.yml +++ b/.github/workflows/rolling-binary-build-main.yml @@ -1,8 +1,11 @@ -name: Rolling Binary Build +name: Rolling Binary Build - main # author: Denis Štogl # description: 'Build & test all dependencies from released (binary) packages.' on: + workflow_dispatch: + branches: + - master pull_request: branches: - master @@ -18,5 +21,6 @@ jobs: uses: ./.github/workflows/reusable-industrial-ci-with-cache.yml with: ros_distro: rolling + ros_repo: main upstream_workspace: ros2_control-not-released.rolling.repos ref_for_scheduled_build: master diff --git a/.github/workflows/rolling-binary-build-last-focal.yml b/.github/workflows/rolling-binary-build-testing.yml similarity index 66% rename from .github/workflows/rolling-binary-build-last-focal.yml rename to .github/workflows/rolling-binary-build-testing.yml index d9bfc0e3341..811c96fce42 100644 --- a/.github/workflows/rolling-binary-build-last-focal.yml +++ b/.github/workflows/rolling-binary-build-testing.yml @@ -1,8 +1,11 @@ -name: Rolling Binary Build - Last Focal +name: Rolling Binary Build - testing # author: Denis Štogl # description: 'Build & test all dependencies from released (binary) packages.' on: + workflow_dispatch: + branches: + - master pull_request: branches: - master @@ -18,10 +21,6 @@ jobs: uses: ./.github/workflows/reusable-industrial-ci-with-cache.yml with: ros_distro: rolling + ros_repo: testing upstream_workspace: ros2_control-not-released.rolling.repos ref_for_scheduled_build: master - ros_repo: main - os_code_name: focal - before_install_upstream_dependencies: | - export ROSDISTRO_INDEX_URL=https://raw.githubusercontent.com/ros/rosdistro/rolling/2022-01-28/index-v4.yaml - rosdep update diff --git a/.github/workflows/rolling-rhel-binary-build.yml b/.github/workflows/rolling-rhel-binary-build.yml index c7d1ffc6754..fe7386b75b2 100644 --- a/.github/workflows/rolling-rhel-binary-build.yml +++ b/.github/workflows/rolling-rhel-binary-build.yml @@ -1,5 +1,8 @@ name: Rolling RHEL Binary Build on: + workflow_dispatch: + branches: + - master pull_request: branches: - master diff --git a/.github/workflows/rolling-semi-binary-build.yml b/.github/workflows/rolling-semi-binary-build-main.yml similarity index 81% rename from .github/workflows/rolling-semi-binary-build.yml rename to .github/workflows/rolling-semi-binary-build-main.yml index c6b0535ef9c..1033dd1e6cd 100644 --- a/.github/workflows/rolling-semi-binary-build.yml +++ b/.github/workflows/rolling-semi-binary-build-main.yml @@ -1,7 +1,10 @@ -name: Rolling Semi-Binary Build +name: Rolling Semi-Binary Build - main # description: 'Build & test that compiles the main dependencies from source.' on: + workflow_dispatch: + branches: + - master pull_request: branches: - master @@ -17,5 +20,6 @@ jobs: uses: ./.github/workflows/reusable-industrial-ci-with-cache.yml with: ros_distro: rolling + ros_repo: main upstream_workspace: ros2_control.rolling.repos ref_for_scheduled_build: master diff --git a/.github/workflows/rolling-semi-binary-build-last-focal.yml b/.github/workflows/rolling-semi-binary-build-testing.yml similarity index 63% rename from .github/workflows/rolling-semi-binary-build-last-focal.yml rename to .github/workflows/rolling-semi-binary-build-testing.yml index 04538e34b8a..4ddfcf50579 100644 --- a/.github/workflows/rolling-semi-binary-build-last-focal.yml +++ b/.github/workflows/rolling-semi-binary-build-testing.yml @@ -1,7 +1,10 @@ -name: Rolling Semi-Binary Build - Last Focal +name: Rolling Semi-Binary Build - testing # description: 'Build & test that compiles the main dependencies from source.' on: + workflow_dispatch: + branches: + - master pull_request: branches: - master @@ -17,10 +20,6 @@ jobs: uses: ./.github/workflows/reusable-industrial-ci-with-cache.yml with: ros_distro: rolling + ros_repo: testing upstream_workspace: ros2_control.rolling.repos ref_for_scheduled_build: master - ros_repo: main - os_code_name: focal - before_install_upstream_dependencies: | - export ROSDISTRO_INDEX_URL=https://raw.githubusercontent.com/ros/rosdistro/rolling/2022-01-28/index-v4.yaml - rosdep update diff --git a/.github/workflows/rolling-source-build.yml b/.github/workflows/rolling-source-build.yml index 0a0ae8de430..b96ad0298e4 100644 --- a/.github/workflows/rolling-source-build.yml +++ b/.github/workflows/rolling-source-build.yml @@ -1,5 +1,8 @@ name: Rolling Source Build on: + workflow_dispatch: + branches: + - master push: branches: - master @@ -13,3 +16,4 @@ jobs: with: ros_distro: rolling ref: master + ros2_repo_branch: master diff --git a/README.md b/README.md index a4a55c7c874..0d060c76f58 100644 --- a/README.md +++ b/README.md @@ -10,10 +10,12 @@ For more, please check the [documentation](https://ros-controls.github.io/contro ROS2 Distro | Branch | Build status | Documentation | Released packages :---------: | :----: | :----------: | :-----------: | :---------------: -**Rolling** | [`master`](https://github.com/ros-controls/ros2_control/tree/rolling) | [![Rolling Binary Build](https://github.com/ros-controls/ros2_control/actions/workflows/rolling-binary-build.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_control/actions/workflows/rolling-binary-build.yml?branch=master)
[![Rolling Semi-Binary Build](https://github.com/ros-controls/ros2_control/actions/workflows/rolling-semi-binary-build.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_control/actions/workflows/rolling-semi-binary-build.yml?branch=master)
[![Rolling Source Build](https://github.com/ros-controls/ros2_control/actions/workflows/rolling-source-build.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_control/actions/workflows/rolling-source-build.yml?branch=master) | [Documentation](https://control.ros.org)
[API Reference](https://control.ros.org/rolling/api/) | [ros2_control](https://index.ros.org/p/ros2_control/#rolling) -**Rolling - last Focal** | [`master`](https://github.com/ros-controls/ros2_control/tree/rolling) | [![Rolling Binary Build](https://github.com/ros-controls/ros2_control/actions/workflows/rolling-binary-build-last-focal.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_control/actions/workflows/rolling-binary-build-last-focal.yml?branch=master)
[![Rolling Semi-Binary Build](https://github.com/ros-controls/ros2_control/actions/workflows/rolling-semi-binary-build-last-focal.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_control/actions/workflows/rolling-semi-binary-build-last-focal.yml?branch=master) | [Documentation](https://control.ros.org)
[API Reference](https://control.ros.org/rolling/api/) | [ros2_control](https://index.ros.org/p/ros2_control/#rolling) -**Galactic** | [`galactic`](https://github.com/ros-controls/ros2_control/tree/galactic) | [![Galactic Binary Build](https://github.com/ros-controls/ros2_control/actions/workflows/galactic-binary-build.yml/badge.svg?branch=galactic)](https://github.com/ros-controls/ros2_control/actions/workflows/galactic-binary-build.yml?branch=galactic)
[![Galactic Semi-Binary Build](https://github.com/ros-controls/ros2_control/actions/workflows/galactic-semi-binary-build.yml/badge.svg?branch=galactic)](https://github.com/ros-controls/ros2_control/actions/workflows/galactic-semi-binary-build.yml?branch=galactic)
[![Galactic Source Build](https://github.com/ros-controls/ros2_control/actions/workflows/galactic-source-build.yml/badge.svg?branch=galactic)](https://github.com/ros-controls/ros2_control/actions/workflows/galactic-source-build.yml?branch=galactic) | [Documentation](https://control.ros.org)
[API Reference](https://control.ros.org/rolling/api/) | [ros2_control](https://index.ros.org/p/ros2_control/#galactic) -**Foxy** | [`foxy`](https://github.com/ros-controls/ros2_control/tree/foxy) | [![Foxy Binary Build](https://github.com/ros-controls/ros2_control/actions/workflows/foxy-binary-build.yml/badge.svg?branch=foxy)](https://github.com/ros-controls/ros2_control/actions/workflows/foxy-binary-build.yml?branch=foxy)
[![Foxy Semi-Binary Build](https://github.com/ros-controls/ros2_control/actions/workflows/foxy-semi-binary-build.yml/badge.svg?branch=foxy)](https://github.com/ros-controls/ros2_control/actions/workflows/foxy-semi-binary-build.yml?branch=foxy)
[![Foxy Source Build](https://github.com/ros-controls/ros2_control/actions/workflows/foxy-source-build.yml/badge.svg?branch=foxy)](https://github.com/ros-controls/ros2_control/actions/workflows/foxy-source-build.yml?branch=foxy) | [Documentation](https://control.ros.org)
[API Reference](https://control.ros.org/rolling/api/) | [ros2_control](https://index.ros.org/p/ros2_control/#foxy) +**Rolling** | [`rolling`](https://github.com/ros-controls/ros2_control/tree/rolling) | [![Rolling Binary Build](https://github.com/ros-controls/ros2_control/actions/workflows/rolling-binary-build-main.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_control/actions/workflows/rolling-binary-build-main.yml?branch=master)
[![Rolling Semi-Binary Build](https://github.com/ros-controls/ros2_control/actions/workflows/rolling-semi-binary-build-main.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_control/actions/workflows/rolling-semi-binary-build-main.yml?branch=master) | [Documentation](https://control.ros.org/master/index.html)
[API Reference](https://control.ros.org/master/doc/api/index.html) | [ros2_control](https://index.ros.org/p/ros2_control/#rolling) +**Humble** | [`humble`](https://github.com/ros-controls/ros2_control/tree/humble) | [![Humble Binary Build](https://github.com/ros-controls/ros2_control/actions/workflows/humble-binary-build-main.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_control/actions/workflows/humble-binary-build-main.yml?branch=master)
[![Humble Semi-Binary Build](https://github.com/ros-controls/ros2_control/actions/workflows/humble-semi-binary-build-main.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_control/actions/workflows/humble-semi-binary-build-main.yml?branch=master) | [Documentation](https://control.ros.org/master/index.html)
[API Reference](https://control.ros.org/master/doc/api/index.html) | [ros2_control](https://index.ros.org/p/ros2_control/#humble) +**Galactic** | [`galactic`](https://github.com/ros-controls/ros2_control/tree/galactic) | [![Galactic Binary Build](https://github.com/ros-controls/ros2_control/actions/workflows/galactic-binary-build-main.yml/badge.svg?branch=galactic)](https://github.com/ros-controls/ros2_control/actions/workflows/galactic-binary-build-main.yml?branch=galactic)
[![Galactic Semi-Binary Build](https://github.com/ros-controls/ros2_control/actions/workflows/galactic-semi-binary-build-main.yml/badge.svg?branch=galactic)](https://github.com/ros-controls/ros2_control/actions/workflows/galactic-semi-binary-build-main.yml?branch=galactic) | [Documentation](https://control.ros.org/galactic/index.html)
[API Reference](https://control.ros.org/galactic/doc/api/index.html) | [ros2_control](https://index.ros.org/p/ros2_control/#galactic) +**Foxy** | [`foxy`](https://github.com/ros-controls/ros2_control/tree/foxy) | [![Foxy Binary Build](https://github.com/ros-controls/ros2_control/actions/workflows/foxy-binary-build-main.yml/badge.svg?branch=foxy)](https://github.com/ros-controls/ros2_control/actions/workflows/foxy-binary-build-main.yml?branch=foxy)
[![Foxy Semi-Binary Build](https://github.com/ros-controls/ros2_control/actions/workflows/foxy-semi-binary-build-main.yml/badge.svg?branch=foxy)](https://github.com/ros-controls/ros2_control/actions/workflows/foxy-semi-binary-build-main.yml?branch=foxy) | [Documentation](https://control.ros.org/foxy/index.html)
[API Reference](https://control.ros.org/foxy/doc/api/index.html) | [ros2_control](https://index.ros.org/p/ros2_control/#foxy) + +[Detailed build status](.github/workflows/README.md) ### Explanation of different build types diff --git a/controller_manager_msgs/package.xml b/controller_manager_msgs/package.xml index 4ff02ebbfbf..de444ff1689 100644 --- a/controller_manager_msgs/package.xml +++ b/controller_manager_msgs/package.xml @@ -20,6 +20,8 @@ lifecycle_msgs rosidl_default_runtime + ament_lint_common + rosidl_interface_packages ament_cmake diff --git a/ros2_control-not-released.humble.repos b/ros2_control-not-released.humble.repos new file mode 100644 index 00000000000..56f46b6f79b --- /dev/null +++ b/ros2_control-not-released.humble.repos @@ -0,0 +1 @@ +repositories: diff --git a/ros2_control.humble.repos b/ros2_control.humble.repos new file mode 100644 index 00000000000..afb1cbf640d --- /dev/null +++ b/ros2_control.humble.repos @@ -0,0 +1,9 @@ +repositories: + ros-controls/realtime_tools: + type: git + url: https://github.com/ros-controls/realtime_tools.git + version: foxy-devel + ros-controls/control_msgs: + type: git + url: https://github.com/ros-controls/control_msgs.git + version: galactic-devel From d46bd5885121ae08e3ad15e8ecd03d8e2094f254 Mon Sep 17 00:00:00 2001 From: Schulze Date: Sat, 2 Jul 2022 18:55:51 -0300 Subject: [PATCH 13/35] [Interfaces] Improved ```get_name()``` method of hardware interfaces #api-breaking (#737) --- .../workflows/humble-rhel-binary-build.yml | 1 - .../include/controller_interface/helpers.hpp | 7 ++-- .../src/chainable_controller_interface.cpp | 4 +- .../test_chainable_controller_interface.cpp | 2 +- .../test/test_controller/test_controller.cpp | 2 +- .../include/hardware_interface/handle.hpp | 23 +++++++---- .../loaned_command_interface.hpp | 11 +++++- .../loaned_state_interface.hpp | 11 +++++- hardware_interface/src/resource_manager.cpp | 4 +- .../test/test_component_interfaces.cpp | 39 ++++++++++++------- hardware_interface/test/test_handle.cpp | 3 +- .../simple_transmission.hpp | 4 +- 12 files changed, 74 insertions(+), 37 deletions(-) diff --git a/.github/workflows/humble-rhel-binary-build.yml b/.github/workflows/humble-rhel-binary-build.yml index 554e306d653..eda3b3d85d3 100644 --- a/.github/workflows/humble-rhel-binary-build.yml +++ b/.github/workflows/humble-rhel-binary-build.yml @@ -13,7 +13,6 @@ on: # # Run every day to detect flakiness and broken dependencies # - cron: '03 1 * * *' - jobs: humble_rhel_binary: name: Humble RHEL binary build diff --git a/controller_interface/include/controller_interface/helpers.hpp b/controller_interface/include/controller_interface/helpers.hpp index eaaeb9dab83..45b048fc728 100644 --- a/controller_interface/include/controller_interface/helpers.hpp +++ b/controller_interface/include/controller_interface/helpers.hpp @@ -51,15 +51,16 @@ bool get_ordered_interfaces( // check case where: // ( == AND == ) OR / == 'full name' if ( - ((name == interface.get_name()) && (interface_type == interface.get_interface_name())) || - ((name + "/" + interface_type) == interface.get_full_name())) + ((name == interface.get_prefix_name()) && + (interface_type == interface.get_interface_name())) || + ((name + "/" + interface_type) == interface.get_name())) { ordered_interfaces.push_back(std::ref(interface)); } } else { - if (name == interface.get_full_name()) + if (name == interface.get_name()) { ordered_interfaces.push_back(std::ref(interface)); } diff --git a/controller_interface/src/chainable_controller_interface.cpp b/controller_interface/src/chainable_controller_interface.cpp index aa6e39782ba..45e2f1a32e5 100644 --- a/controller_interface/src/chainable_controller_interface.cpp +++ b/controller_interface/src/chainable_controller_interface.cpp @@ -67,7 +67,7 @@ ChainableControllerInterface::export_reference_interfaces() // check if the names of the reference interfaces begin with the controller's name for (const auto & interface : reference_interfaces) { - if (interface.get_name() != get_node()->get_name()) + if (interface.get_prefix_name() != get_node()->get_name()) { RCLCPP_FATAL( get_node()->get_logger(), @@ -75,7 +75,7 @@ ChainableControllerInterface::export_reference_interfaces() "mandatory " " for reference interfaces. No reference interface will be exported. Please correct and " "recompile the controller with name '%s' and try again.", - interface.get_full_name().c_str(), get_node()->get_name()); + interface.get_name().c_str(), get_node()->get_name()); reference_interfaces.clear(); break; } diff --git a/controller_interface/test/test_chainable_controller_interface.cpp b/controller_interface/test/test_chainable_controller_interface.cpp index 6839d595ea5..6ae36b39ced 100644 --- a/controller_interface/test/test_chainable_controller_interface.cpp +++ b/controller_interface/test/test_chainable_controller_interface.cpp @@ -39,7 +39,7 @@ TEST_F(ChainableControllerInterfaceTest, export_reference_interfaces) auto reference_interfaces = controller.export_reference_interfaces(); ASSERT_EQ(reference_interfaces.size(), 1u); - EXPECT_EQ(reference_interfaces[0].get_name(), TEST_CONTROLLER_NAME); + EXPECT_EQ(reference_interfaces[0].get_prefix_name(), TEST_CONTROLLER_NAME); EXPECT_EQ(reference_interfaces[0].get_interface_name(), "test_itf"); EXPECT_EQ(reference_interfaces[0].get_value(), INTERFACE_VALUE); diff --git a/controller_manager/test/test_controller/test_controller.cpp b/controller_manager/test/test_controller/test_controller.cpp index 51e83ac452d..b6eac9b689b 100644 --- a/controller_manager/test/test_controller/test_controller.cpp +++ b/controller_manager/test/test_controller/test_controller.cpp @@ -66,7 +66,7 @@ controller_interface::return_type TestController::update( { RCLCPP_INFO( get_node()->get_logger(), "Setting value of command interface '%s' to %f", - command_interfaces_[i].get_full_name().c_str(), external_commands_for_testing_[i]); + command_interfaces_[i].get_name().c_str(), external_commands_for_testing_[i]); command_interfaces_[i].set_value(external_commands_for_testing_[i]); } diff --git a/hardware_interface/include/hardware_interface/handle.hpp b/hardware_interface/include/hardware_interface/handle.hpp index 30908cb40b3..1aea017754f 100644 --- a/hardware_interface/include/hardware_interface/handle.hpp +++ b/hardware_interface/include/hardware_interface/handle.hpp @@ -28,8 +28,9 @@ class ReadOnlyHandle { public: ReadOnlyHandle( - const std::string & name, const std::string & interface_name, double * value_ptr = nullptr) - : name_(name), interface_name_(interface_name), value_ptr_(value_ptr) + const std::string & prefix_name, const std::string & interface_name, + double * value_ptr = nullptr) + : prefix_name_(prefix_name), interface_name_(interface_name), value_ptr_(value_ptr) { } @@ -56,11 +57,18 @@ class ReadOnlyHandle /// Returns true if handle references a value. inline operator bool() const { return value_ptr_ != nullptr; } - const std::string & get_name() const { return name_; } + const std::string get_name() const { return prefix_name_ + "/" + interface_name_; } const std::string & get_interface_name() const { return interface_name_; } - const std::string get_full_name() const { return name_ + "/" + interface_name_; } + [[deprecated( + "Replaced by get_name method, which is semantically more correct")]] const std::string + get_full_name() const + { + return get_name(); + } + + const std::string & get_prefix_name() const { return prefix_name_; } double get_value() const { @@ -69,7 +77,7 @@ class ReadOnlyHandle } protected: - std::string name_; + std::string prefix_name_; std::string interface_name_; double * value_ptr_; }; @@ -78,8 +86,9 @@ class ReadWriteHandle : public ReadOnlyHandle { public: ReadWriteHandle( - const std::string & name, const std::string & interface_name, double * value_ptr = nullptr) - : ReadOnlyHandle(name, interface_name, value_ptr) + const std::string & prefix_name, const std::string & interface_name, + double * value_ptr = nullptr) + : ReadOnlyHandle(prefix_name, interface_name, value_ptr) { } diff --git a/hardware_interface/include/hardware_interface/loaned_command_interface.hpp b/hardware_interface/include/hardware_interface/loaned_command_interface.hpp index 15b5abb513e..bb5807c398f 100644 --- a/hardware_interface/include/hardware_interface/loaned_command_interface.hpp +++ b/hardware_interface/include/hardware_interface/loaned_command_interface.hpp @@ -50,11 +50,18 @@ class LoanedCommandInterface } } - const std::string & get_name() const { return command_interface_.get_name(); } + const std::string get_name() const { return command_interface_.get_name(); } const std::string & get_interface_name() const { return command_interface_.get_interface_name(); } - const std::string get_full_name() const { return command_interface_.get_full_name(); } + [[deprecated( + "Replaced by get_name method, which is semantically more correct")]] const std::string + get_full_name() const + { + return command_interface_.get_name(); + } + + const std::string & get_prefix_name() const { return command_interface_.get_prefix_name(); } void set_value(double val) { command_interface_.set_value(val); } diff --git a/hardware_interface/include/hardware_interface/loaned_state_interface.hpp b/hardware_interface/include/hardware_interface/loaned_state_interface.hpp index 80cf1c0449f..4fe67d1290f 100644 --- a/hardware_interface/include/hardware_interface/loaned_state_interface.hpp +++ b/hardware_interface/include/hardware_interface/loaned_state_interface.hpp @@ -50,11 +50,18 @@ class LoanedStateInterface } } - const std::string & get_name() const { return state_interface_.get_name(); } + const std::string get_name() const { return state_interface_.get_name(); } const std::string & get_interface_name() const { return state_interface_.get_interface_name(); } - const std::string get_full_name() const { return state_interface_.get_full_name(); } + [[deprecated( + "Replaced by get_name method, which is semantically more correct")]] const std::string + get_full_name() const + { + return state_interface_.get_name(); + } + + const std::string & get_prefix_name() const { return state_interface_.get_prefix_name(); } double get_value() const { return state_interface_.get_value(); } diff --git a/hardware_interface/src/resource_manager.cpp b/hardware_interface/src/resource_manager.cpp index e224c1e1485..14129512120 100644 --- a/hardware_interface/src/resource_manager.cpp +++ b/hardware_interface/src/resource_manager.cpp @@ -411,7 +411,7 @@ class ResourceStorage interface_names.reserve(interfaces.size()); for (auto & interface : interfaces) { - auto key = interface.get_full_name(); + auto key = interface.get_name(); state_interface_map_.emplace(std::make_pair(key, std::move(interface))); interface_names.push_back(key); } @@ -444,7 +444,7 @@ class ResourceStorage interface_names.reserve(interfaces.size()); for (auto & interface : interfaces) { - auto key = interface.get_full_name(); + auto key = interface.get_name(); command_interface_map_.emplace(std::make_pair(key, std::move(interface))); claimed_command_interface_map_.emplace(std::make_pair(key, false)); interface_names.push_back(key); diff --git a/hardware_interface/test/test_component_interfaces.cpp b/hardware_interface/test/test_component_interfaces.cpp index e12b43017b3..128771058b9 100644 --- a/hardware_interface/test/test_component_interfaces.cpp +++ b/hardware_interface/test/test_component_interfaces.cpp @@ -431,15 +431,18 @@ TEST(TestComponentInterfaces, dummy_actuator) auto state_interfaces = actuator_hw.export_state_interfaces(); ASSERT_EQ(2u, state_interfaces.size()); - EXPECT_EQ("joint1", state_interfaces[0].get_name()); + EXPECT_EQ("joint1/position", state_interfaces[0].get_name()); EXPECT_EQ(hardware_interface::HW_IF_POSITION, state_interfaces[0].get_interface_name()); - EXPECT_EQ("joint1", state_interfaces[1].get_name()); + EXPECT_EQ("joint1", state_interfaces[0].get_prefix_name()); + EXPECT_EQ("joint1/velocity", state_interfaces[1].get_name()); EXPECT_EQ(hardware_interface::HW_IF_VELOCITY, state_interfaces[1].get_interface_name()); + EXPECT_EQ("joint1", state_interfaces[1].get_prefix_name()); auto command_interfaces = actuator_hw.export_command_interfaces(); ASSERT_EQ(1u, command_interfaces.size()); - EXPECT_EQ("joint1", command_interfaces[0].get_name()); + EXPECT_EQ("joint1/velocity", command_interfaces[0].get_name()); EXPECT_EQ(hardware_interface::HW_IF_VELOCITY, command_interfaces[0].get_interface_name()); + EXPECT_EQ("joint1", command_interfaces[0].get_prefix_name()); double velocity_value = 1.0; command_interfaces[0].set_value(velocity_value); // velocity @@ -518,8 +521,9 @@ TEST(TestComponentInterfaces, dummy_sensor) auto state_interfaces = sensor_hw.export_state_interfaces(); ASSERT_EQ(1u, state_interfaces.size()); - EXPECT_EQ("joint1", state_interfaces[0].get_name()); + EXPECT_EQ("joint1/voltage", state_interfaces[0].get_name()); EXPECT_EQ("voltage", state_interfaces[0].get_interface_name()); + EXPECT_EQ("joint1", state_interfaces[0].get_prefix_name()); EXPECT_TRUE(std::isnan(state_interfaces[0].get_value())); // Not updated because is is UNCONFIGURED @@ -548,27 +552,36 @@ TEST(TestComponentInterfaces, dummy_system) auto state_interfaces = system_hw.export_state_interfaces(); ASSERT_EQ(6u, state_interfaces.size()); - EXPECT_EQ("joint1", state_interfaces[0].get_name()); + EXPECT_EQ("joint1/position", state_interfaces[0].get_name()); EXPECT_EQ(hardware_interface::HW_IF_POSITION, state_interfaces[0].get_interface_name()); - EXPECT_EQ("joint1", state_interfaces[1].get_name()); + EXPECT_EQ("joint1", state_interfaces[0].get_prefix_name()); + EXPECT_EQ("joint1/velocity", state_interfaces[1].get_name()); EXPECT_EQ(hardware_interface::HW_IF_VELOCITY, state_interfaces[1].get_interface_name()); - EXPECT_EQ("joint2", state_interfaces[2].get_name()); + EXPECT_EQ("joint1", state_interfaces[1].get_prefix_name()); + EXPECT_EQ("joint2/position", state_interfaces[2].get_name()); EXPECT_EQ(hardware_interface::HW_IF_POSITION, state_interfaces[2].get_interface_name()); - EXPECT_EQ("joint2", state_interfaces[3].get_name()); + EXPECT_EQ("joint2", state_interfaces[2].get_prefix_name()); + EXPECT_EQ("joint2/velocity", state_interfaces[3].get_name()); EXPECT_EQ(hardware_interface::HW_IF_VELOCITY, state_interfaces[3].get_interface_name()); - EXPECT_EQ("joint3", state_interfaces[4].get_name()); + EXPECT_EQ("joint2", state_interfaces[3].get_prefix_name()); + EXPECT_EQ("joint3/position", state_interfaces[4].get_name()); EXPECT_EQ(hardware_interface::HW_IF_POSITION, state_interfaces[4].get_interface_name()); - EXPECT_EQ("joint3", state_interfaces[5].get_name()); + EXPECT_EQ("joint3", state_interfaces[4].get_prefix_name()); + EXPECT_EQ("joint3/velocity", state_interfaces[5].get_name()); EXPECT_EQ(hardware_interface::HW_IF_VELOCITY, state_interfaces[5].get_interface_name()); + EXPECT_EQ("joint3", state_interfaces[5].get_prefix_name()); auto command_interfaces = system_hw.export_command_interfaces(); ASSERT_EQ(3u, command_interfaces.size()); - EXPECT_EQ("joint1", command_interfaces[0].get_name()); + EXPECT_EQ("joint1/velocity", command_interfaces[0].get_name()); EXPECT_EQ(hardware_interface::HW_IF_VELOCITY, command_interfaces[0].get_interface_name()); - EXPECT_EQ("joint2", command_interfaces[1].get_name()); + EXPECT_EQ("joint1", command_interfaces[0].get_prefix_name()); + EXPECT_EQ("joint2/velocity", command_interfaces[1].get_name()); EXPECT_EQ(hardware_interface::HW_IF_VELOCITY, command_interfaces[1].get_interface_name()); - EXPECT_EQ("joint3", command_interfaces[2].get_name()); + EXPECT_EQ("joint2", command_interfaces[1].get_prefix_name()); + EXPECT_EQ("joint3/velocity", command_interfaces[2].get_name()); EXPECT_EQ(hardware_interface::HW_IF_VELOCITY, command_interfaces[2].get_interface_name()); + EXPECT_EQ("joint3", command_interfaces[2].get_prefix_name()); double velocity_value = 1.0; command_interfaces[0].set_value(velocity_value); // velocity diff --git a/hardware_interface/test/test_handle.cpp b/hardware_interface/test/test_handle.cpp index 647b247e286..da8258c6435 100644 --- a/hardware_interface/test/test_handle.cpp +++ b/hardware_interface/test/test_handle.cpp @@ -44,8 +44,9 @@ TEST(TestHandle, state_interface) TEST(TestHandle, name_getters_work) { StateInterface handle{JOINT_NAME, FOO_INTERFACE}; - EXPECT_EQ(handle.get_name(), JOINT_NAME); + EXPECT_EQ(handle.get_name(), std::string(JOINT_NAME) + "/" + std::string(FOO_INTERFACE)); EXPECT_EQ(handle.get_interface_name(), FOO_INTERFACE); + EXPECT_EQ(handle.get_prefix_name(), JOINT_NAME); } TEST(TestHandle, value_methods_throw_for_nullptr) diff --git a/transmission_interface/include/transmission_interface/simple_transmission.hpp b/transmission_interface/include/transmission_interface/simple_transmission.hpp index 1ba196fb341..a74cc3dddde 100644 --- a/transmission_interface/include/transmission_interface/simple_transmission.hpp +++ b/transmission_interface/include/transmission_interface/simple_transmission.hpp @@ -151,7 +151,7 @@ HandleType get_by_interface( [&interface_name](const auto handle) { return handle.get_interface_name() == interface_name; }); if (result == handles.cend()) { - return HandleType(handles.cbegin()->get_name(), interface_name, nullptr); + return HandleType(handles.cbegin()->get_prefix_name(), interface_name, nullptr); } return *result; } @@ -162,7 +162,7 @@ bool are_names_identical(const std::vector & handles) std::vector names; std::transform( handles.cbegin(), handles.cend(), std::back_inserter(names), - [](const auto & handle) { return handle.get_name(); }); + [](const auto & handle) { return handle.get_prefix_name(); }); return std::equal(names.cbegin() + 1, names.cend(), names.cbegin()); } From e861ad19e6f578f23aec352fccb955b161149175 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Sun, 3 Jul 2022 17:39:14 +0200 Subject: [PATCH 14/35] Remove hybrid services in controller manager. They are just overhead. (#761) --- .../controller_manager/controller_manager.hpp | 24 ----- controller_manager/src/controller_manager.cpp | 99 ------------------- .../test/test_controller_manager_srvs.cpp | 81 --------------- controller_manager_msgs/CMakeLists.txt | 3 - .../srv/ConfigureStartController.srv | 10 -- .../srv/LoadConfigureController.srv | 10 -- .../srv/LoadStartController.srv | 10 -- ros2controlcli/ros2controlcli/api/__init__.py | 34 ------- 8 files changed, 271 deletions(-) delete mode 100644 controller_manager_msgs/srv/ConfigureStartController.srv delete mode 100644 controller_manager_msgs/srv/LoadConfigureController.srv delete mode 100644 controller_manager_msgs/srv/LoadStartController.srv diff --git a/controller_manager/include/controller_manager/controller_manager.hpp b/controller_manager/include/controller_manager/controller_manager.hpp index b178e21665a..b922d1f935b 100644 --- a/controller_manager/include/controller_manager/controller_manager.hpp +++ b/controller_manager/include/controller_manager/controller_manager.hpp @@ -29,14 +29,11 @@ #include "controller_manager/controller_spec.hpp" #include "controller_manager/visibility_control.h" #include "controller_manager_msgs/srv/configure_controller.hpp" -#include "controller_manager_msgs/srv/configure_start_controller.hpp" #include "controller_manager_msgs/srv/list_controller_types.hpp" #include "controller_manager_msgs/srv/list_controllers.hpp" #include "controller_manager_msgs/srv/list_hardware_components.hpp" #include "controller_manager_msgs/srv/list_hardware_interfaces.hpp" -#include "controller_manager_msgs/srv/load_configure_controller.hpp" #include "controller_manager_msgs/srv/load_controller.hpp" -#include "controller_manager_msgs/srv/load_start_controller.hpp" #include "controller_manager_msgs/srv/reload_controller_libraries.hpp" #include "controller_manager_msgs/srv/set_hardware_component_state.hpp" #include "controller_manager_msgs/srv/switch_controller.hpp" @@ -215,21 +212,6 @@ class ControllerManager : public rclcpp::Node const std::shared_ptr request, std::shared_ptr response); - CONTROLLER_MANAGER_PUBLIC - void load_and_configure_controller_service_cb( - const std::shared_ptr request, - std::shared_ptr response); - - CONTROLLER_MANAGER_PUBLIC - void load_and_start_controller_service_cb( - const std::shared_ptr request, - std::shared_ptr response); - - CONTROLLER_MANAGER_PUBLIC - void configure_and_start_controller_service_cb( - const std::shared_ptr request, - std::shared_ptr response); - CONTROLLER_MANAGER_PUBLIC void reload_controller_libraries_service_cb( const std::shared_ptr request, @@ -432,12 +414,6 @@ class ControllerManager : public rclcpp::Node rclcpp::Service::SharedPtr load_controller_service_; rclcpp::Service::SharedPtr configure_controller_service_; - rclcpp::Service::SharedPtr - load_and_configure_controller_service_; - rclcpp::Service::SharedPtr - load_and_start_controller_service_; - rclcpp::Service::SharedPtr - configure_and_start_controller_service_; rclcpp::Service::SharedPtr reload_controller_libraries_service_; rclcpp::Service::SharedPtr diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 81aa87f42ad..72239f18e1f 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -239,21 +239,6 @@ void ControllerManager::init_services() "~/configure_controller", std::bind(&ControllerManager::configure_controller_service_cb, this, _1, _2), rmw_qos_profile_services_hist_keep_all, best_effort_callback_group_); - load_and_configure_controller_service_ = - create_service( - "~/load_and_configure_controller", - std::bind(&ControllerManager::load_and_configure_controller_service_cb, this, _1, _2), - rmw_qos_profile_services_hist_keep_all, best_effort_callback_group_); - load_and_start_controller_service_ = - create_service( - "~/load_and_start_controller", - std::bind(&ControllerManager::load_and_start_controller_service_cb, this, _1, _2), - rmw_qos_profile_services_hist_keep_all, best_effort_callback_group_); - configure_and_start_controller_service_ = - create_service( - "~/configure_and_start_controller", - std::bind(&ControllerManager::configure_and_start_controller_service_cb, this, _1, _2), - rmw_qos_profile_services_hist_keep_all, best_effort_callback_group_); reload_controller_libraries_service_ = create_service( "~/reload_controller_libraries", @@ -1326,90 +1311,6 @@ void ControllerManager::configure_controller_service_cb( get_logger(), "configuring service finished for controller '%s' ", request->name.c_str()); } -void ControllerManager::load_and_configure_controller_service_cb( - const std::shared_ptr request, - std::shared_ptr response) -{ - // lock services - RCLCPP_DEBUG( - get_logger(), "loading and configure service called for controller '%s' ", - request->name.c_str()); - std::lock_guard guard(services_lock_); - RCLCPP_DEBUG(get_logger(), "loading and configure service locked"); - - response->ok = load_controller(request->name).get(); - - if (response->ok) - { - response->ok = configure_controller(request->name) == controller_interface::return_type::OK; - } - - RCLCPP_DEBUG( - get_logger(), "loading and configure service finished for controller '%s' ", - request->name.c_str()); -} - -void ControllerManager::load_and_start_controller_service_cb( - const std::shared_ptr request, - std::shared_ptr response) -{ - // lock services - RCLCPP_DEBUG( - get_logger(), "loading and activating service called for controller '%s' ", - request->name.c_str()); - std::lock_guard guard(services_lock_); - RCLCPP_DEBUG(get_logger(), "loading and activating service locked"); - - response->ok = load_controller(request->name).get(); - - if (response->ok) - { - response->ok = configure_controller(request->name) == controller_interface::return_type::OK; - } - - if (response->ok) - { - std::vector start_controller = {request->name}; - std::vector empty; - response->ok = switch_controller( - start_controller, empty, - controller_manager_msgs::srv::SwitchController::Request::BEST_EFFORT) == - controller_interface::return_type::OK; - } - - RCLCPP_DEBUG( - get_logger(), "loading and activating service finished for controller '%s' ", - request->name.c_str()); -} - -void ControllerManager::configure_and_start_controller_service_cb( - const std::shared_ptr request, - std::shared_ptr response) -{ - // lock services - RCLCPP_DEBUG( - get_logger(), "configuring and activating service called for controller '%s' ", - request->name.c_str()); - std::lock_guard guard(services_lock_); - RCLCPP_DEBUG(get_logger(), "configuring and activating service locked"); - - response->ok = configure_controller(request->name) == controller_interface::return_type::OK; - - if (response->ok) - { - std::vector start_controller = {request->name}; - std::vector empty; - response->ok = switch_controller( - start_controller, empty, - controller_manager_msgs::srv::SwitchController::Request::BEST_EFFORT) == - controller_interface::return_type::OK; - } - - RCLCPP_DEBUG( - get_logger(), "configuring and activating service finished for controller '%s' ", - request->name.c_str()); -} - void ControllerManager::reload_controller_libraries_service_cb( const std::shared_ptr request, std::shared_ptr response) diff --git a/controller_manager/test/test_controller_manager_srvs.cpp b/controller_manager/test/test_controller_manager_srvs.cpp index b9277346401..aebf38e256e 100644 --- a/controller_manager/test/test_controller_manager_srvs.cpp +++ b/controller_manager/test/test_controller_manager_srvs.cpp @@ -365,84 +365,3 @@ TEST_F(TestControllerManagerSrvs, configure_controller_srv) lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, cm_->get_loaded_controllers()[0].c->get_state().id()); } - -TEST_F(TestControllerManagerSrvs, load_configure_controller_srv) -{ - rclcpp::executors::SingleThreadedExecutor srv_executor; - rclcpp::Node::SharedPtr srv_node = std::make_shared("srv_client"); - srv_executor.add_node(srv_node); - rclcpp::Client::SharedPtr client = - srv_node->create_client( - "test_controller_manager/load_and_configure_controller"); - - auto request = std::make_shared(); - request->name = test_controller::TEST_CONTROLLER_NAME; - auto result = call_service_and_wait(*client, request, srv_executor); - ASSERT_FALSE(result->ok) << "There's no param specifying the type for " << request->name; - rclcpp::Parameter controller_type_parameter( - std::string(test_controller::TEST_CONTROLLER_NAME) + ".type", - test_controller::TEST_CONTROLLER_CLASS_NAME); - cm_->set_parameter(controller_type_parameter); - result = call_service_and_wait(*client, request, srv_executor, true); - ASSERT_TRUE(result->ok); - EXPECT_EQ(1u, cm_->get_loaded_controllers().size()); - EXPECT_EQ(test_controller::TEST_CONTROLLER_NAME, cm_->get_loaded_controllers()[0].info.name); - EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, - cm_->get_loaded_controllers()[0].c->get_state().id()); -} - -TEST_F(TestControllerManagerSrvs, load_start_controller_srv) -{ - rclcpp::executors::SingleThreadedExecutor srv_executor; - rclcpp::Node::SharedPtr srv_node = std::make_shared("srv_client"); - srv_executor.add_node(srv_node); - rclcpp::Client::SharedPtr client = - srv_node->create_client( - "test_controller_manager/load_and_start_controller"); - - auto request = std::make_shared(); - request->name = test_controller::TEST_CONTROLLER_NAME; - auto result = call_service_and_wait(*client, request, srv_executor); - ASSERT_FALSE(result->ok) << "There's no param specifying the type for " << request->name; - rclcpp::Parameter controller_type_parameter( - std::string(test_controller::TEST_CONTROLLER_NAME) + ".type", - test_controller::TEST_CONTROLLER_CLASS_NAME); - cm_->set_parameter(controller_type_parameter); - result = call_service_and_wait(*client, request, srv_executor, true); - ASSERT_TRUE(result->ok); - EXPECT_EQ(1u, cm_->get_loaded_controllers().size()); - EXPECT_EQ(test_controller::TEST_CONTROLLER_NAME, cm_->get_loaded_controllers()[0].info.name); - EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, - cm_->get_loaded_controllers()[0].c->get_state().id()); -} - -TEST_F(TestControllerManagerSrvs, configure_start_controller_srv) -{ - rclcpp::executors::SingleThreadedExecutor srv_executor; - rclcpp::Node::SharedPtr srv_node = std::make_shared("srv_client"); - srv_executor.add_node(srv_node); - rclcpp::Client::SharedPtr client = - srv_node->create_client( - "test_controller_manager/configure_and_start_controller"); - - auto request = - std::make_shared(); - request->name = test_controller::TEST_CONTROLLER_NAME; - auto result = call_service_and_wait(*client, request, srv_executor); - ASSERT_FALSE(result->ok) << "Controller not loaded: " << request->name; - - auto test_controller = std::make_shared(); - auto abstract_test_controller = cm_->add_controller( - test_controller, test_controller::TEST_CONTROLLER_NAME, - test_controller::TEST_CONTROLLER_CLASS_NAME); - EXPECT_EQ(1u, cm_->get_loaded_controllers().size()); - - result = call_service_and_wait(*client, request, srv_executor, true); - ASSERT_TRUE(result->ok); - EXPECT_EQ(1u, cm_->get_loaded_controllers().size()); - EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, - cm_->get_loaded_controllers()[0].c->get_state().id()); -} diff --git a/controller_manager_msgs/CMakeLists.txt b/controller_manager_msgs/CMakeLists.txt index 16617c3613a..7b76586a511 100644 --- a/controller_manager_msgs/CMakeLists.txt +++ b/controller_manager_msgs/CMakeLists.txt @@ -17,14 +17,11 @@ set(msg_files ) set(srv_files srv/ConfigureController.srv - srv/ConfigureStartController.srv srv/ListControllers.srv srv/ListControllerTypes.srv srv/ListHardwareComponents.srv srv/ListHardwareInterfaces.srv - srv/LoadConfigureController.srv srv/LoadController.srv - srv/LoadStartController.srv srv/ReloadControllerLibraries.srv srv/SetHardwareComponentState.srv srv/SwitchController.srv diff --git a/controller_manager_msgs/srv/ConfigureStartController.srv b/controller_manager_msgs/srv/ConfigureStartController.srv deleted file mode 100644 index 29e34b47857..00000000000 --- a/controller_manager_msgs/srv/ConfigureStartController.srv +++ /dev/null @@ -1,10 +0,0 @@ -# The ConfigureStartController service allows you to configure and start a single controller -# inside controller_manager - -# To configure and start a controller, specify the "name" of the controller. -# The return value "ok" indicates if the controller was successfully -# configured and started or not. - -string name ---- -bool ok diff --git a/controller_manager_msgs/srv/LoadConfigureController.srv b/controller_manager_msgs/srv/LoadConfigureController.srv deleted file mode 100644 index cf6af41fbe4..00000000000 --- a/controller_manager_msgs/srv/LoadConfigureController.srv +++ /dev/null @@ -1,10 +0,0 @@ -# The LoadConfigureController service allows you to load and configure a single controller -# inside controller_manager - -# To load and configure a controller, specify the "name" of the controller. -# The return value "ok" indicates if the controller was successfully -# constructed and configured or not. - -string name ---- -bool ok diff --git a/controller_manager_msgs/srv/LoadStartController.srv b/controller_manager_msgs/srv/LoadStartController.srv deleted file mode 100644 index 2aa82f387bd..00000000000 --- a/controller_manager_msgs/srv/LoadStartController.srv +++ /dev/null @@ -1,10 +0,0 @@ -# The LoadStartController service allows you to load, configure and start a single controller -# inside controller_manager - -# To load, configure and start a controller, specify the "name" of the controller. -# The return value "ok" indicates if the controller was successfully -# constructed, configured and started or not. - -string name ---- -bool ok diff --git a/ros2controlcli/ros2controlcli/api/__init__.py b/ros2controlcli/ros2controlcli/api/__init__.py index 5c48acf5bb3..9729b90c7b2 100644 --- a/ros2controlcli/ros2controlcli/api/__init__.py +++ b/ros2controlcli/ros2controlcli/api/__init__.py @@ -15,12 +15,6 @@ from controller_manager import list_controllers -from controller_manager_msgs.srv import ( - ConfigureStartController, - LoadConfigureController, - LoadStartController, -) - import rclpy from ros2cli.node.direct import DirectNode @@ -57,34 +51,6 @@ def service_caller(service_name, service_type, request): rclpy.shutdown() -def load_configure_controller(controller_manager_name, controller_name): - request = LoadConfigureController.Request() - request.name = controller_name - return service_caller( - f'{controller_manager_name}/load_and_configure_controller', - LoadConfigureController, - request, - ) - - -def load_start_controller(controller_manager_name, controller_name): - request = LoadStartController.Request() - request.name = controller_name - return service_caller( - f'{controller_manager_name}/load_and_start_controller', LoadStartController, request - ) - - -def configure_start_controller(controller_manager_name, controller_name): - request = ConfigureStartController.Request() - request.name = controller_name - return service_caller( - f'{controller_manager_name}/configure_and_start_controller', - ConfigureStartController, - request, - ) - - class ControllerNameCompleter: """Callable returning a list of controllers parameter names.""" From 8a4e372fb9ec2afa5a898b8506b924623fc1817a Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Sun, 3 Jul 2022 17:05:37 +0100 Subject: [PATCH 15/35] Update changelogs --- controller_interface/CHANGELOG.rst | 9 +++++++++ controller_manager/CHANGELOG.rst | 16 ++++++++++++++++ controller_manager_msgs/CHANGELOG.rst | 8 ++++++++ hardware_interface/CHANGELOG.rst | 10 ++++++++++ ros2_control/CHANGELOG.rst | 5 +++++ ros2_control_test_assets/CHANGELOG.rst | 6 ++++++ ros2controlcli/CHANGELOG.rst | 7 +++++++ transmission_interface/CHANGELOG.rst | 8 ++++++++ 8 files changed, 69 insertions(+) diff --git a/controller_interface/CHANGELOG.rst b/controller_interface/CHANGELOG.rst index 155e8b6e4c0..ae35be27f0e 100644 --- a/controller_interface/CHANGELOG.rst +++ b/controller_interface/CHANGELOG.rst @@ -2,6 +2,15 @@ Changelog for package controller_interface ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* [Interfaces] Improved ```get_name()``` method of hardware interfaces (soft) #api-breaking (`#737 `_) +* Update maintainers of packages (`#753 `_) +* Full functionality of chainable controllers in controller manager (`#667 `_) + * auto-switching of chained mode in controllers + * interface-matching approach for managing chaining controllers +* Contributors: Bence Magyar, Denis Štogl, Lucas Schulze + 2.10.0 (2022-06-18) ------------------- * CMakeLists cleanup (`#733 `_) diff --git a/controller_manager/CHANGELOG.rst b/controller_manager/CHANGELOG.rst index 152c515b682..f5950234660 100644 --- a/controller_manager/CHANGELOG.rst +++ b/controller_manager/CHANGELOG.rst @@ -2,6 +2,22 @@ Changelog for package controller_manager ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Remove hybrid services in controller manager. (`#761 `_) +* [Interfaces] Improved ```get_name()``` method of hardware interfaces #api-breaking (`#737 `_) +* Update maintainers of packages (`#753 `_) +* Fix test dependency for chainable test (`#751 `_) +* Remove ament autolint (`#749 `_) +* Full functionality of chainable controllers in controller manager (`#667 `_) + * auto-switching of chained mode in controllers + * interface-matching approach for managing chaining controllers +* Fixup spanwer and unspawner tests. It changes spawner a bit to handle interupts internally. (`#745 `_) +* Add missing field to initializer lists in tests (`#746 `_) +* Small but useful output update on controller manager. (`#741 `_) +* Fixed period passed to hardware components always 0 (`#738 `_) +* Contributors: Bence Magyar, Denis Štogl, Maciej Bednarczyk, Lucas Schulze + 2.10.0 (2022-06-18) ------------------- * Make RHEL CI happy! (`#730 `_) diff --git a/controller_manager_msgs/CHANGELOG.rst b/controller_manager_msgs/CHANGELOG.rst index af5b58f3299..c51c828ac60 100644 --- a/controller_manager_msgs/CHANGELOG.rst +++ b/controller_manager_msgs/CHANGELOG.rst @@ -2,6 +2,14 @@ Changelog for package controller_manager_msgs ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Remove hybrid services in controller manager. They are just overhead. (`#761 `_) +* Update and fix CI setup (`#752 `_) +* Update maintainers of packages (`#753 `_) +* Remove ament autolint (`#749 `_) +* Contributors: Bence Magyar, Denis Štogl + 2.10.0 (2022-06-18) ------------------- diff --git a/hardware_interface/CHANGELOG.rst b/hardware_interface/CHANGELOG.rst index e005f4f798c..8db5bbe73b8 100644 --- a/hardware_interface/CHANGELOG.rst +++ b/hardware_interface/CHANGELOG.rst @@ -2,6 +2,16 @@ Changelog for package hardware_interface ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* [Interfaces] Improved ```get_name()``` method of hardware interfaces #api-breaking (`#737 `_) +* Update maintainers of packages (`#753 `_) +* Remove ament autolint (`#749 `_) +* Full functionality of chainable controllers in controller manager (`#667 `_) + * auto-switching of chained mode in controllers + * interface-matching approach for managing chaining controllers +* Contributors: Bence Magyar, Denis Štogl, Lucas Schulze + 2.10.0 (2022-06-18) ------------------- * Make RHEL CI happy! (`#730 `_) diff --git a/ros2_control/CHANGELOG.rst b/ros2_control/CHANGELOG.rst index 4a2192edded..302adda8a52 100644 --- a/ros2_control/CHANGELOG.rst +++ b/ros2_control/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package ros2_control ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Update maintainers of packages (`#753 `_) +* Contributors: Bence Magyar + 2.10.0 (2022-06-18) ------------------- diff --git a/ros2_control_test_assets/CHANGELOG.rst b/ros2_control_test_assets/CHANGELOG.rst index 4a8544ea2b2..01935007742 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 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Update maintainers of packages (`#753 `_) +* Remove ament autolint (`#749 `_) +* Contributors: Bence Magyar + 2.10.0 (2022-06-18) ------------------- * Make RHEL CI happy! (`#730 `_) diff --git a/ros2controlcli/CHANGELOG.rst b/ros2controlcli/CHANGELOG.rst index 95bd97768dc..230d4dce0df 100644 --- a/ros2controlcli/CHANGELOG.rst +++ b/ros2controlcli/CHANGELOG.rst @@ -2,6 +2,13 @@ Changelog for package ros2controlcli ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Remove hybrid services in controller manager. They are just overhead. (`#761 `_) +* Update maintainers of packages (`#753 `_) +* Add available status and moved to fstrings when listing hardware interfaces (`#739 `_) +* Contributors: Bence Magyar, Denis Štogl, Leander Stephen D'Souza + 2.10.0 (2022-06-18) ------------------- diff --git a/transmission_interface/CHANGELOG.rst b/transmission_interface/CHANGELOG.rst index 40a7ff55276..f2adc99be08 100644 --- a/transmission_interface/CHANGELOG.rst +++ b/transmission_interface/CHANGELOG.rst @@ -2,6 +2,14 @@ Changelog for package transmission_interface ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* [Interfaces] Improved ```get_name()``` method of hardware interfaces #api-breaking (`#737 `_) +* Update maintainers of packages (`#753 `_) +* Remove ament autolint (`#749 `_) +* Fixup ament cpplint on 22.04 (`#747 `_) +* Contributors: Bence Magyar, Denis Štogl, Lucas Schulze + 2.10.0 (2022-06-18) ------------------- * CMakeLists cleanup (`#733 `_) From 22da62dea9bc0c2c1184babd95a606927fd6de29 Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Sun, 3 Jul 2022 17:06:03 +0100 Subject: [PATCH 16/35] 2.11.0 --- controller_interface/CHANGELOG.rst | 4 ++-- controller_interface/package.xml | 2 +- controller_manager/CHANGELOG.rst | 4 ++-- controller_manager/package.xml | 2 +- controller_manager_msgs/CHANGELOG.rst | 4 ++-- controller_manager_msgs/package.xml | 2 +- hardware_interface/CHANGELOG.rst | 4 ++-- hardware_interface/package.xml | 2 +- ros2_control/CHANGELOG.rst | 4 ++-- ros2_control/package.xml | 2 +- ros2_control_test_assets/CHANGELOG.rst | 4 ++-- ros2_control_test_assets/package.xml | 2 +- ros2controlcli/CHANGELOG.rst | 4 ++-- ros2controlcli/package.xml | 2 +- ros2controlcli/setup.py | 2 +- transmission_interface/CHANGELOG.rst | 4 ++-- transmission_interface/package.xml | 2 +- 17 files changed, 25 insertions(+), 25 deletions(-) diff --git a/controller_interface/CHANGELOG.rst b/controller_interface/CHANGELOG.rst index ae35be27f0e..d52816691f3 100644 --- a/controller_interface/CHANGELOG.rst +++ b/controller_interface/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package controller_interface ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.11.0 (2022-07-03) +------------------- * [Interfaces] Improved ```get_name()``` method of hardware interfaces (soft) #api-breaking (`#737 `_) * Update maintainers of packages (`#753 `_) * Full functionality of chainable controllers in controller manager (`#667 `_) diff --git a/controller_interface/package.xml b/controller_interface/package.xml index 917845964e2..639c4c14d59 100644 --- a/controller_interface/package.xml +++ b/controller_interface/package.xml @@ -2,7 +2,7 @@ controller_interface - 2.10.0 + 2.11.0 Description of controller_interface Bence Magyar Denis Štogl diff --git a/controller_manager/CHANGELOG.rst b/controller_manager/CHANGELOG.rst index f5950234660..096b991ebf2 100644 --- a/controller_manager/CHANGELOG.rst +++ b/controller_manager/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package controller_manager ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.11.0 (2022-07-03) +------------------- * Remove hybrid services in controller manager. (`#761 `_) * [Interfaces] Improved ```get_name()``` method of hardware interfaces #api-breaking (`#737 `_) * Update maintainers of packages (`#753 `_) diff --git a/controller_manager/package.xml b/controller_manager/package.xml index 5ce8af43826..c9ca543e63f 100644 --- a/controller_manager/package.xml +++ b/controller_manager/package.xml @@ -2,7 +2,7 @@ controller_manager - 2.10.0 + 2.11.0 Description of controller_manager Bence Magyar Denis Štogl diff --git a/controller_manager_msgs/CHANGELOG.rst b/controller_manager_msgs/CHANGELOG.rst index c51c828ac60..a11f4a42ed4 100644 --- a/controller_manager_msgs/CHANGELOG.rst +++ b/controller_manager_msgs/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package controller_manager_msgs ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.11.0 (2022-07-03) +------------------- * Remove hybrid services in controller manager. They are just overhead. (`#761 `_) * Update and fix CI setup (`#752 `_) * Update maintainers of packages (`#753 `_) diff --git a/controller_manager_msgs/package.xml b/controller_manager_msgs/package.xml index de444ff1689..304c033a15e 100644 --- a/controller_manager_msgs/package.xml +++ b/controller_manager_msgs/package.xml @@ -2,7 +2,7 @@ controller_manager_msgs - 2.10.0 + 2.11.0 Messages and services for the controller manager. Bence Magyar Denis Štogl diff --git a/hardware_interface/CHANGELOG.rst b/hardware_interface/CHANGELOG.rst index 8db5bbe73b8..2a18135f5d0 100644 --- a/hardware_interface/CHANGELOG.rst +++ b/hardware_interface/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package hardware_interface ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.11.0 (2022-07-03) +------------------- * [Interfaces] Improved ```get_name()``` method of hardware interfaces #api-breaking (`#737 `_) * Update maintainers of packages (`#753 `_) * Remove ament autolint (`#749 `_) diff --git a/hardware_interface/package.xml b/hardware_interface/package.xml index 6271023d2ac..c1ceef11e6f 100644 --- a/hardware_interface/package.xml +++ b/hardware_interface/package.xml @@ -1,7 +1,7 @@ hardware_interface - 2.10.0 + 2.11.0 ros2_control hardware interface Bence Magyar Denis Štogl diff --git a/ros2_control/CHANGELOG.rst b/ros2_control/CHANGELOG.rst index 302adda8a52..cc64f91efe3 100644 --- a/ros2_control/CHANGELOG.rst +++ b/ros2_control/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ros2_control ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.11.0 (2022-07-03) +------------------- * Update maintainers of packages (`#753 `_) * Contributors: Bence Magyar diff --git a/ros2_control/package.xml b/ros2_control/package.xml index 7b734e40018..bb3078e65aa 100644 --- a/ros2_control/package.xml +++ b/ros2_control/package.xml @@ -1,7 +1,7 @@ ros2_control - 2.10.0 + 2.11.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 01935007742..5e96c4769f6 100644 --- a/ros2_control_test_assets/CHANGELOG.rst +++ b/ros2_control_test_assets/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ros2_control_test_assets ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.11.0 (2022-07-03) +------------------- * Update maintainers of packages (`#753 `_) * Remove ament autolint (`#749 `_) * Contributors: Bence Magyar diff --git a/ros2_control_test_assets/package.xml b/ros2_control_test_assets/package.xml index e52e50a79e9..bc45ba9e13e 100644 --- a/ros2_control_test_assets/package.xml +++ b/ros2_control_test_assets/package.xml @@ -2,7 +2,7 @@ ros2_control_test_assets - 2.10.0 + 2.11.0 The package provides shared test resources for ros2_control stack Bence Magyar diff --git a/ros2controlcli/CHANGELOG.rst b/ros2controlcli/CHANGELOG.rst index 230d4dce0df..1ee32d8a005 100644 --- a/ros2controlcli/CHANGELOG.rst +++ b/ros2controlcli/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ros2controlcli ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.11.0 (2022-07-03) +------------------- * Remove hybrid services in controller manager. They are just overhead. (`#761 `_) * Update maintainers of packages (`#753 `_) * Add available status and moved to fstrings when listing hardware interfaces (`#739 `_) diff --git a/ros2controlcli/package.xml b/ros2controlcli/package.xml index afc8dab5280..193f42e5814 100644 --- a/ros2controlcli/package.xml +++ b/ros2controlcli/package.xml @@ -2,7 +2,7 @@ ros2controlcli - 2.10.0 + 2.11.0 The ROS 2 command line tools for ROS2 Control. diff --git a/ros2controlcli/setup.py b/ros2controlcli/setup.py index c76fdd7ab99..3c3f27c8603 100644 --- a/ros2controlcli/setup.py +++ b/ros2controlcli/setup.py @@ -5,7 +5,7 @@ setup( name=package_name, - version='2.10.0', + version='2.11.0', packages=find_packages(exclude=['test']), data_files=[ ('share/' + package_name, ['package.xml']), diff --git a/transmission_interface/CHANGELOG.rst b/transmission_interface/CHANGELOG.rst index f2adc99be08..92394ad2270 100644 --- a/transmission_interface/CHANGELOG.rst +++ b/transmission_interface/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package transmission_interface ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.11.0 (2022-07-03) +------------------- * [Interfaces] Improved ```get_name()``` method of hardware interfaces #api-breaking (`#737 `_) * Update maintainers of packages (`#753 `_) * Remove ament autolint (`#749 `_) diff --git a/transmission_interface/package.xml b/transmission_interface/package.xml index 58c94bd1525..c5c39fc934e 100644 --- a/transmission_interface/package.xml +++ b/transmission_interface/package.xml @@ -2,7 +2,7 @@ transmission_interface - 2.10.0 + 2.11.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 From 1967cfa179248ce2e72f4f974f20d6e19fe56da4 Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Tue, 5 Jul 2022 17:39:51 +0100 Subject: [PATCH 17/35] Deprecate fake components, long live mock components (#762) --- doc/index.rst | 2 +- hardware_interface/CMakeLists.txt | 36 +++++- ...serdoc.rst => mock_components_userdoc.rst} | 6 +- .../doc/writing_new_hardware_interface.rst | 6 +- .../fake_components/generic_system.hpp | 98 +------------- .../mock_components/generic_system.hpp | 122 ++++++++++++++++++ .../mock_components/visibility_control.h | 49 +++++++ .../mock_components_plugin_description.xml | 9 ++ .../generic_system.cpp | 10 +- .../test_generic_system.cpp | 26 ++-- 10 files changed, 242 insertions(+), 122 deletions(-) rename hardware_interface/doc/{fake_components_userdoc.rst => mock_components_userdoc.rst} (95%) create mode 100644 hardware_interface/include/mock_components/generic_system.hpp create mode 100644 hardware_interface/include/mock_components/visibility_control.h create mode 100644 hardware_interface/mock_components_plugin_description.xml rename hardware_interface/src/{fake_components => mock_components}/generic_system.cpp (98%) rename hardware_interface/test/{fake_components => mock_components}/test_generic_system.cpp (98%) diff --git a/doc/index.rst b/doc/index.rst index 4dd668f4d04..9d614e19109 100644 --- a/doc/index.rst +++ b/doc/index.rst @@ -26,4 +26,4 @@ Concepts Controller Manager <../controller_manager/doc/userdoc.rst> Hardware Components <../hardware_interface/doc/hardware_components_userdoc.rst> - Fake Components <../hardware_interface/doc/fake_components_userdoc.rst> + Mock Components <../hardware_interface/doc/mock_components_userdoc.rst> diff --git a/hardware_interface/CMakeLists.txt b/hardware_interface/CMakeLists.txt index 27b7c6fa0d9..7fec4293dd8 100644 --- a/hardware_interface/CMakeLists.txt +++ b/hardware_interface/CMakeLists.txt @@ -40,11 +40,39 @@ ament_target_dependencies(${PROJECT_NAME} ${THIS_PACKAGE_INCLUDE_DEPENDS}) # which is appropriate when building the dll but not consuming it. target_compile_definitions(${PROJECT_NAME} PRIVATE "HARDWARE_INTERFACE_BUILDING_DLL") +# Mock components +add_library( + mock_components + SHARED + src/mock_components/generic_system.cpp +) +target_include_directories( + mock_components + PUBLIC + include +) +target_link_libraries( + mock_components + ${PROJECT_NAME} +) +ament_target_dependencies( + mock_components + pluginlib + rcpputils +) + +# Causes the visibility macros to use dllexport rather than dllimport, +# which is appropriate when building the dll but not consuming it. +target_compile_definitions(mock_components PRIVATE "HARDWARE_INTERFACE_BUILDING_DLL") + +pluginlib_export_plugin_description_file( + ${PROJECT_NAME} mock_components_plugin_description.xml) + # Fake components add_library( fake_components SHARED - src/fake_components/generic_system.cpp + src/mock_components/generic_system.cpp ) target_include_directories( fake_components @@ -60,6 +88,7 @@ ament_target_dependencies( pluginlib rcpputils ) + # Causes the visibility macros to use dllexport rather than dllimport, # which is appropriate when building the dll but not consuming it. target_compile_definitions(fake_components PRIVATE "HARDWARE_INTERFACE_BUILDING_DLL") @@ -67,6 +96,7 @@ target_compile_definitions(fake_components PRIVATE "HARDWARE_INTERFACE_BUILDING_ pluginlib_export_plugin_description_file( ${PROJECT_NAME} fake_components_plugin_description.xml) + install( DIRECTORY include/ DESTINATION include @@ -75,6 +105,7 @@ install( install( TARGETS fake_components + mock_components ${PROJECT_NAME} RUNTIME DESTINATION bin ARCHIVE DESTINATION lib @@ -134,7 +165,7 @@ if(BUILD_TESTING) target_link_libraries(test_resource_manager ${PROJECT_NAME}) ament_target_dependencies(test_resource_manager ros2_control_test_assets) - ament_add_gmock(test_generic_system test/fake_components/test_generic_system.cpp) + ament_add_gmock(test_generic_system test/mock_components/test_generic_system.cpp) target_include_directories(test_generic_system PRIVATE include) target_link_libraries(test_generic_system ${PROJECT_NAME}) ament_target_dependencies(test_generic_system @@ -148,6 +179,7 @@ ament_export_include_directories( ) ament_export_libraries( fake_components + mock_components ${PROJECT_NAME} ) ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS}) diff --git a/hardware_interface/doc/fake_components_userdoc.rst b/hardware_interface/doc/mock_components_userdoc.rst similarity index 95% rename from hardware_interface/doc/fake_components_userdoc.rst rename to hardware_interface/doc/mock_components_userdoc.rst index dbb3c0c7ada..a285408b5f7 100644 --- a/hardware_interface/doc/fake_components_userdoc.rst +++ b/hardware_interface/doc/mock_components_userdoc.rst @@ -1,8 +1,8 @@ -.. _fake_components_userdoc: +.. _mock_components_userdoc: -Fake Components +Mock Components ---------------- -Fake components are trivial "simulations" of the hardware components, i.e., System, Sensor, and Actuator. +Mock components are trivial "simulations" of the hardware components, i.e., System, Sensor, and Actuator. They provide ideal behavior by mirroring commands to their states. The corresponding hardware interface can be added instead of real hardware for offline testing of ros2_control framework. The main advantage is that you can test all the "piping" inside the framework without having access to the hardware. diff --git a/hardware_interface/doc/writing_new_hardware_interface.rst b/hardware_interface/doc/writing_new_hardware_interface.rst index 900fba3aaf8..eb4c5282160 100644 --- a/hardware_interface/doc/writing_new_hardware_interface.rst +++ b/hardware_interface/doc/writing_new_hardware_interface.rst @@ -77,7 +77,7 @@ The following is a step-by-step guide to create source files, basic tests, and c 5. **Writing export definition for pluginlib** 1. Create the ``.xml`` file in the package and add a definition of the library and hardware interface's class which has to be visible for the pluginlib. - The easiest way to do that is to check definition for fake components in the `hardware_interface fake_components `_ section. + The easiest way to do that is to check definition for mock components in the `hardware_interface mock_components `_ section. 2. Usually, the plugin name is defined by the package (namespace) and the class name, e.g., ``/``. @@ -88,7 +88,7 @@ The following is a step-by-step guide to create source files, basic tests, and c 1. Create the folder ``test`` in your package, if it does not exist already, and add a file named ``test_load_.cpp``. - 2. You can copy the ``load_generic_system_2dof`` content defined in the `test_generic_system.cpp `_ package. + 2. You can copy the ``load_generic_system_2dof`` content defined in the `test_generic_system.cpp `_ package. 3. Change the name of the copied test and in the last line, where hardware interface type is specified put the name defined in ``.xml`` file, e.g., ``/``. @@ -113,7 +113,7 @@ The following is a step-by-step guide to create source files, basic tests, and c 7. In the test section add the following dependencies: ``ament_cmake_gmock``, ``hardware_interface``. 8. Add compile definitions for the tests using the ``ament_add_gmock`` directive. - For details, see how it is done for fake hardware in the `ros2_control `_ package. + For details, see how it is done for mock hardware in the `ros2_control `_ package. 9. (optional) Add your hardware interface`s library into ``ament_export_libraries`` before ``ament_package()``. diff --git a/hardware_interface/include/fake_components/generic_system.hpp b/hardware_interface/include/fake_components/generic_system.hpp index e4a9815ae1d..c671f165be2 100644 --- a/hardware_interface/include/fake_components/generic_system.hpp +++ b/hardware_interface/include/fake_components/generic_system.hpp @@ -17,105 +17,13 @@ #ifndef FAKE_COMPONENTS__GENERIC_SYSTEM_HPP_ #define FAKE_COMPONENTS__GENERIC_SYSTEM_HPP_ -#include -#include - -#include "hardware_interface/handle.hpp" -#include "hardware_interface/hardware_info.hpp" -#include "hardware_interface/system_interface.hpp" -#include "hardware_interface/types/hardware_interface_return_values.hpp" -#include "hardware_interface/types/hardware_interface_type_values.hpp" - -using hardware_interface::return_type; +#include "hardware_interface/mock_components/generic_system.hpp" namespace fake_components { -using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; - -class HARDWARE_INTERFACE_PUBLIC GenericSystem : public hardware_interface::SystemInterface -{ -public: - CallbackReturn on_init(const hardware_interface::HardwareInfo & info) override; - - std::vector export_state_interfaces() override; - - std::vector export_command_interfaces() override; - - return_type read(const rclcpp::Time & time, const rclcpp::Duration & period) override; - - return_type write(const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) override - { - return return_type::OK; - } - -protected: - /// Use standard interfaces for joints because they are relevant for dynamic behavior - /** - * By splitting the standard interfaces from other type, the users are able to inherit this - * class and simply create small "simulation" with desired dynamic behavior. - * The advantage over using Gazebo is that enables "quick & dirty" tests of robot's URDF and - * controllers. - */ - const std::vector standard_interfaces_ = { - hardware_interface::HW_IF_POSITION, hardware_interface::HW_IF_VELOCITY, - hardware_interface::HW_IF_ACCELERATION, hardware_interface::HW_IF_EFFORT}; - - const size_t POSITION_INTERFACE_INDEX = 0; - - struct MimicJoint - { - std::size_t joint_index; - std::size_t mimicked_joint_index; - double multiplier = 1.0; - }; - std::vector mimic_joints_; - - /// The size of this vector is (standard_interfaces_.size() x nr_joints) - std::vector> joint_commands_; - std::vector> joint_states_; - - std::vector other_interfaces_; - /// The size of this vector is (other_interfaces_.size() x nr_joints) - std::vector> other_commands_; - std::vector> other_states_; - - std::vector sensor_interfaces_; - /// The size of this vector is (sensor_interfaces_.size() x nr_joints) - std::vector> sensor_fake_commands_; - std::vector> sensor_states_; - - std::vector gpio_interfaces_; - /// The size of this vector is (gpio_interfaces_.size() x nr_joints) - std::vector> gpio_fake_commands_; - std::vector> gpio_commands_; - std::vector> gpio_states_; - -private: - template - bool get_interface( - const std::string & name, const std::vector & interface_list, - const std::string & interface_name, const size_t vector_index, - std::vector> & values, std::vector & interfaces); - - void initialize_storage_vectors( - std::vector> & commands, std::vector> & states, - const std::vector & interfaces); - - template - bool populate_interfaces( - const std::vector & components, - std::vector & interfaces, std::vector> & storage, - std::vector & target_interfaces, bool using_state_interfaces); - - bool use_fake_gpio_command_interfaces_; - bool use_fake_sensor_command_interfaces_; - - double position_state_following_offset_; - std::string custom_interface_with_following_offset_; - size_t index_custom_interface_with_following_offset_; -}; +[[deprecated]] using GenericSystem = mock_components::GenericSystem; -typedef GenericSystem GenericRobot; +[[deprecated]] using GenericSystem = GenericRobot; } // namespace fake_components diff --git a/hardware_interface/include/mock_components/generic_system.hpp b/hardware_interface/include/mock_components/generic_system.hpp new file mode 100644 index 00000000000..968c781e443 --- /dev/null +++ b/hardware_interface/include/mock_components/generic_system.hpp @@ -0,0 +1,122 @@ +// Copyright (c) 2021 PickNik, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// +// Author: Jafar Abdi, Denis Stogl + +#ifndef MOCK_COMPONENTS__GENERIC_SYSTEM_HPP_ +#define MOCK_COMPONENTS__GENERIC_SYSTEM_HPP_ + +#include +#include + +#include "hardware_interface/handle.hpp" +#include "hardware_interface/hardware_info.hpp" +#include "hardware_interface/system_interface.hpp" +#include "hardware_interface/types/hardware_interface_return_values.hpp" +#include "hardware_interface/types/hardware_interface_type_values.hpp" + +using hardware_interface::return_type; + +namespace mock_components +{ +using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; + +class HARDWARE_INTERFACE_PUBLIC GenericSystem : public hardware_interface::SystemInterface +{ +public: + CallbackReturn on_init(const hardware_interface::HardwareInfo & info) override; + + std::vector export_state_interfaces() override; + + std::vector export_command_interfaces() override; + + return_type read(const rclcpp::Time & time, const rclcpp::Duration & period) override; + + return_type write(const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) override + { + return return_type::OK; + } + +protected: + /// Use standard interfaces for joints because they are relevant for dynamic behavior + /** + * By splitting the standard interfaces from other type, the users are able to inherit this + * class and simply create small "simulation" with desired dynamic behavior. + * The advantage over using Gazebo is that enables "quick & dirty" tests of robot's URDF and + * controllers. + */ + const std::vector standard_interfaces_ = { + hardware_interface::HW_IF_POSITION, hardware_interface::HW_IF_VELOCITY, + hardware_interface::HW_IF_ACCELERATION, hardware_interface::HW_IF_EFFORT}; + + const size_t POSITION_INTERFACE_INDEX = 0; + + struct MimicJoint + { + std::size_t joint_index; + std::size_t mimicked_joint_index; + double multiplier = 1.0; + }; + std::vector mimic_joints_; + + /// The size of this vector is (standard_interfaces_.size() x nr_joints) + std::vector> joint_commands_; + std::vector> joint_states_; + + std::vector other_interfaces_; + /// The size of this vector is (other_interfaces_.size() x nr_joints) + std::vector> other_commands_; + std::vector> other_states_; + + std::vector sensor_interfaces_; + /// The size of this vector is (sensor_interfaces_.size() x nr_joints) + std::vector> sensor_fake_commands_; + std::vector> sensor_states_; + + std::vector gpio_interfaces_; + /// The size of this vector is (gpio_interfaces_.size() x nr_joints) + std::vector> gpio_fake_commands_; + std::vector> gpio_commands_; + std::vector> gpio_states_; + +private: + template + bool get_interface( + const std::string & name, const std::vector & interface_list, + const std::string & interface_name, const size_t vector_index, + std::vector> & values, std::vector & interfaces); + + void initialize_storage_vectors( + std::vector> & commands, std::vector> & states, + const std::vector & interfaces); + + template + bool populate_interfaces( + const std::vector & components, + std::vector & interfaces, std::vector> & storage, + std::vector & target_interfaces, bool using_state_interfaces); + + bool use_fake_gpio_command_interfaces_; + bool use_fake_sensor_command_interfaces_; + + double position_state_following_offset_; + std::string custom_interface_with_following_offset_; + size_t index_custom_interface_with_following_offset_; +}; + +typedef GenericSystem GenericRobot; + +} // namespace mock_components + +#endif // MOCK_COMPONENTS__GENERIC_SYSTEM_HPP_ diff --git a/hardware_interface/include/mock_components/visibility_control.h b/hardware_interface/include/mock_components/visibility_control.h new file mode 100644 index 00000000000..ccc695456e3 --- /dev/null +++ b/hardware_interface/include/mock_components/visibility_control.h @@ -0,0 +1,49 @@ +// Copyright (c) 2021 PickNik, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef MOCK_COMPONENTS__VISIBILITY_CONTROL_H_ +#define MOCK_COMPONENTS__VISIBILITY_CONTROL_H_ + +// This logic was borrowed (then namespaced) from the examples on the gcc wiki: +// https://gcc.gnu.org/wiki/Visibility + +#if defined _WIN32 || defined __CYGWIN__ +#ifdef __GNUC__ +#define MOCK_COMPONENTS_EXPORT __attribute__((dllexport)) +#define MOCK_COMPONENTS_IMPORT __attribute__((dllimport)) +#else +#define MOCK_COMPONENTS_EXPORT __declspec(dllexport) +#define MOCK_COMPONENTS_IMPORT __declspec(dllimport) +#endif +#ifdef MOCK_COMPONENTS_BUILDING_DLL +#define MOCK_COMPONENTS_PUBLIC MOCK_COMPONENTS_EXPORT +#else +#define MOCK_COMPONENTS_PUBLIC MOCK_COMPONENTS_IMPORT +#endif +#define MOCK_COMPONENTS_PUBLIC_TYPE MOCK_COMPONENTS_PUBLIC +#define MOCK_COMPONENTS_LOCAL +#else +#define MOCK_COMPONENTS_EXPORT __attribute__((visibility("default"))) +#define MOCK_COMPONENTS_IMPORT +#if __GNUC__ >= 4 +#define MOCK_COMPONENTS_PUBLIC __attribute__((visibility("default"))) +#define MOCK_COMPONENTS_LOCAL __attribute__((visibility("hidden"))) +#else +#define MOCK_COMPONENTS_PUBLIC +#define MOCK_COMPONENTS_LOCAL +#endif +#define MOCK_COMPONENTS_PUBLIC_TYPE +#endif + +#endif // MOCK_COMPONENTS__VISIBILITY_CONTROL_H_ diff --git a/hardware_interface/mock_components_plugin_description.xml b/hardware_interface/mock_components_plugin_description.xml new file mode 100644 index 00000000000..edecaa56b2f --- /dev/null +++ b/hardware_interface/mock_components_plugin_description.xml @@ -0,0 +1,9 @@ + + + + + Generic components for simple mocking of system hardware. + + + + diff --git a/hardware_interface/src/fake_components/generic_system.cpp b/hardware_interface/src/mock_components/generic_system.cpp similarity index 98% rename from hardware_interface/src/fake_components/generic_system.cpp rename to hardware_interface/src/mock_components/generic_system.cpp index ec246819b08..3b9746ad470 100644 --- a/hardware_interface/src/fake_components/generic_system.cpp +++ b/hardware_interface/src/mock_components/generic_system.cpp @@ -14,7 +14,7 @@ // // Author: Jafar Abdi, Denis Stogl -#include "fake_components/generic_system.hpp" +#include "mock_components/generic_system.hpp" #include #include @@ -27,7 +27,7 @@ #include "hardware_interface/types/hardware_interface_type_values.hpp" #include "rcutils/logging_macros.h" -namespace fake_components +namespace mock_components { CallbackReturn GenericSystem::on_init(const hardware_interface::HardwareInfo & info) { @@ -462,7 +462,7 @@ void GenericSystem::initialize_storage_vectors( if (print_hint) { RCUTILS_LOG_WARN_ONCE_NAMED( - "fake_generic_system", + "mock_generic_system", "Parsing of optional initial interface values failed or uses a deprecated format. Add " "initial values for every state interface in the ros2_control.xacro. For example: \n" " \n" @@ -494,7 +494,7 @@ bool GenericSystem::populate_interfaces( return true; } -} // namespace fake_components +} // namespace mock_components #include "pluginlib/class_list_macros.hpp" -PLUGINLIB_EXPORT_CLASS(fake_components::GenericSystem, hardware_interface::SystemInterface) +PLUGINLIB_EXPORT_CLASS(mock_components::GenericSystem, hardware_interface::SystemInterface) diff --git a/hardware_interface/test/fake_components/test_generic_system.cpp b/hardware_interface/test/mock_components/test_generic_system.cpp similarity index 98% rename from hardware_interface/test/fake_components/test_generic_system.cpp rename to hardware_interface/test/mock_components/test_generic_system.cpp index 4b9eb204c28..fae841fdc25 100644 --- a/hardware_interface/test/fake_components/test_generic_system.cpp +++ b/hardware_interface/test/mock_components/test_generic_system.cpp @@ -45,7 +45,7 @@ class TestGenericSystem : public ::testing::Test R"( - fake_components/GenericSystem + mock_components/GenericSystem @@ -64,7 +64,7 @@ class TestGenericSystem : public ::testing::Test R"( - fake_components/GenericSystem + mock_components/GenericSystem @@ -84,7 +84,7 @@ class TestGenericSystem : public ::testing::Test R"( - fake_components/GenericSystem + mock_components/GenericSystem @@ -107,7 +107,7 @@ class TestGenericSystem : public ::testing::Test R"( - fake_components/GenericSystem + mock_components/GenericSystem @@ -137,7 +137,7 @@ class TestGenericSystem : public ::testing::Test R"( - fake_components/GenericSystem + mock_components/GenericSystem @@ -165,7 +165,7 @@ class TestGenericSystem : public ::testing::Test R"( - fake_components/GenericSystem + mock_components/GenericSystem true @@ -194,7 +194,7 @@ class TestGenericSystem : public ::testing::Test R"( - fake_components/GenericSystem + mock_components/GenericSystem True @@ -223,7 +223,7 @@ class TestGenericSystem : public ::testing::Test R"( - fake_components/GenericSystem + mock_components/GenericSystem @@ -247,7 +247,7 @@ class TestGenericSystem : public ::testing::Test R"( - fake_components/GenericSystem + mock_components/GenericSystem -3 @@ -273,7 +273,7 @@ class TestGenericSystem : public ::testing::Test R"( - fake_components/GenericSystem + mock_components/GenericSystem -3 actual_position @@ -298,7 +298,7 @@ class TestGenericSystem : public ::testing::Test R"( - fake_components/GenericSystem + mock_components/GenericSystem -3 actual_position @@ -325,7 +325,7 @@ class TestGenericSystem : public ::testing::Test R"( - fake_components/GenericSystem + mock_components/GenericSystem 2 2 @@ -361,7 +361,7 @@ class TestGenericSystem : public ::testing::Test R"( - fake_components/GenericSystem + mock_components/GenericSystem True From 49ed4f3dd9022fd3690fa2da58de7ad677fa6849 Mon Sep 17 00:00:00 2001 From: livanov93 Date: Tue, 5 Jul 2022 23:06:49 +0200 Subject: [PATCH 18/35] Hardware interface specific update rate and best practices about it (#716) --- hardware_interface/README.md | 129 +++++++++++++++++++++++++++++++++++ 1 file changed, 129 insertions(+) diff --git a/hardware_interface/README.md b/hardware_interface/README.md index 8efb0ffea86..343158b6988 100644 --- a/hardware_interface/README.md +++ b/hardware_interface/README.md @@ -1,2 +1,131 @@ robot agnostic hardware_interface package. This package will eventually be moved into its own repo. + +## Best practice for `hardware_interface` implementation +In the following section you can find some advices which will help you implement interface +for your specific hardware. + +### Best practice for having different update rate for each `hardware_interface` by counting loops +Current implementation of [ros2_control main node](https://github.com/ros-controls/ros2_control/blob/master/controller_manager/src/ros2_control_node.cpp) +has one update rate that controls the rate of the [`read()`](https://github.com/ros-controls/ros2_control/blob/fe462926416d527d1da163bc3eabd02ee1de9be9/hardware_interface/include/hardware_interface/system_interface.hpp#L169) and [`write()`](https://github.com/ros-controls/ros2_control/blob/fe462926416d527d1da163bc3eabd02ee1de9be9/hardware_interface/include/hardware_interface/system_interface.hpp#L178) +calls in [`hardware_interface(s)`](https://github.com/ros-controls/ros2_control/blob/master/hardware_interface/include/hardware_interface/system_interface.hpp). +In this section suggestion on how to run each interface implementation on its own update rate is introduced. + +1. Add parameters of main control loop update rate and desired update rate for your hardware interface. +``` + + + + + + + + my_system_interface/MySystemHardware + ${main_loop_update_rate} + ${desired_hw_update_rate} + + ... + + + + + +``` + +2. In you [`on_init()`](https://github.com/ros-controls/ros2_control/blob/fe462926416d527d1da163bc3eabd02ee1de9be9/hardware_interface/include/hardware_interface/system_interface.hpp#L94) specific implementation fetch desired parameters: +``` +namespace my_system_interface +{ +hardware_interface::CallbackReturn MySystemHardware::on_init( + const hardware_interface::HardwareInfo & info) +{ + if ( + hardware_interface::SystemInterface::on_init(info) != + hardware_interface::CallbackReturn::SUCCESS) + { + return hardware_interface::CallbackReturn::ERROR; + } + + // declaration in *.hpp file --> unsigned int main_loop_update_rate_, desired_hw_update_rate_ = 100 ; + main_loop_update_rate_ = stoi(info_.hardware_parameters["main_loop_update_rate"]); + desired_hw_update_rate_ = stoi(info_.hardware_parameters["desired_hw_update_rate"]); + + ... +} +... +} // my_system_interface +``` + +3. In your `on_activate()` specific implementation reset internal loop counter +``` +hardware_interface::CallbackReturn MySystemHardware::on_activate( + const rclcpp_lifecycle::State & /*previous_state*/) +{ + // declaration in *.hpp file --> unsigned int update_loop_counter_ ; + update_loop_counter_ = 0; + ... +} +``` + +4. In your `read(const rclcpp::Time & time, const rclcpp::Duration & period)` and/or + `write(const rclcpp::Time & time, const rclcpp::Duration & period)` + specific implementations decide if you should interfere with your hardware +``` +hardware_interface::return_type MySystemHardware::read(const rclcpp::Time & time, const rclcpp::Duration & period) +{ + + bool hardware_go = main_loop_update_rate_ == 0 || + desired_hw_update_rate_ == 0 || + ((update_loop_counter_ % desired_hw_update_rate_) == 0); + + if (hardware_go){ + // hardware comms and operations + + } + ... + + // update counter + ++update_loop_counter_; + update_loop_counter_ %= main_loop_update_rate_; +} +``` + +### Best practice for having different update rate for each `hardware_interface` by measuring elapsed time +Another way to decide if hardware communication should be executed in the`read(const rclcpp::Time & time, const rclcpp::Duration & period)` and/or +`write(const rclcpp::Time & time, const rclcpp::Duration & period)` implementations is to measure elapsed time since last pass: + +``` +hardware_interface::CallbackReturn MySystemHardware::on_activate( + const rclcpp_lifecycle::State & /*previous_state*/) +{ + // declaration in *.hpp file --> bool first_read_pass_, first_write_pass_ = true ; + first_read_pass_ = first_write_pass_ = true; + ... +} + +hardware_interface::return_type MySystemHardware::read(const rclcpp::Time & time, const rclcpp::Duration & period) +{ + if (first_read_pass_ || (time - last_read_time_ ) > period) + { + first_read_pass_ = false; + // declaration in *.hpp file --> rclcpp::Time last_read_time_ ; + last_read_time_ = time; + // hardware comms and operations + ... + } + ... +} + +hardware_interface::return_type MySystemHardware::write(const rclcpp::Time & time, const rclcpp::Duration & period) +{ + if (first_write_pass_ || (time - last_write_time_ ) > period) + { + first_write_pass_ = false; + // declaration in *.hpp file --> rclcpp::Time last_write_time_ ; + last_write_time_ = time; + // hardware comms and operations + ... + } + ... +} +``` From 8283124e1de7363f0950c8f3cac1ef5d8e2a039b Mon Sep 17 00:00:00 2001 From: Tyler Weaver Date: Thu, 7 Jul 2022 14:48:18 -0600 Subject: [PATCH 19/35] [ros2_control_node] Automatically detect if RT kernel is used and opportunistically enable SCHED_FIFO (#748) --- controller_manager/CMakeLists.txt | 1 + controller_manager/doc/userdoc.rst | 8 +++ .../include/controller_manager/realtime.hpp | 35 +++++++++++ controller_manager/src/realtime.cpp | 47 +++++++++++++++ controller_manager/src/ros2_control_node.cpp | 60 ++++++++++++------- 5 files changed, 131 insertions(+), 20 deletions(-) create mode 100644 controller_manager/include/controller_manager/realtime.hpp create mode 100644 controller_manager/src/realtime.cpp diff --git a/controller_manager/CMakeLists.txt b/controller_manager/CMakeLists.txt index 4165c28e091..1ec77135f2d 100644 --- a/controller_manager/CMakeLists.txt +++ b/controller_manager/CMakeLists.txt @@ -23,6 +23,7 @@ endforeach() add_library(${PROJECT_NAME} SHARED src/controller_manager.cpp + src/realtime.cpp ) target_include_directories(${PROJECT_NAME} PRIVATE include) ament_target_dependencies(${PROJECT_NAME} ${THIS_PACKAGE_INCLUDE_DEPENDS}) diff --git a/controller_manager/doc/userdoc.rst b/controller_manager/doc/userdoc.rst index 70847d05a74..b086c90a8a5 100644 --- a/controller_manager/doc/userdoc.rst +++ b/controller_manager/doc/userdoc.rst @@ -5,6 +5,14 @@ Controller Manager Controller Manager is the main component in the ros2_control framework. It manages lifecycle of controllers, access to the hardware interfaces and offers services to the ROS-world. +Determinism +----------- + +For best performance when controlling hardware you want the controller manager to have as little jitter as possible in the main control loop. +The normal linux kernel is optimized for computational throughput and therefore is not well suited for hardware control. +The main thread of Controller Manager attempts to configure ``SCHED_FIFO`` with a priority of ``50``. +To enable this functionality install a RT kernel and run the Controller Manager with permissions to make syscalls to set its thread priorities. +The two easiest options for this are using the [Real-time Ubuntu 22.04 LTS Beta](https://ubuntu.com/blog/real-time-ubuntu-released) or [linux-image-rt-amd64](https://packages.debian.org/bullseye/linux-image-rt-amd64) on Debian Bullseye. Parameters ----------- diff --git a/controller_manager/include/controller_manager/realtime.hpp b/controller_manager/include/controller_manager/realtime.hpp new file mode 100644 index 00000000000..2bc92bbcc31 --- /dev/null +++ b/controller_manager/include/controller_manager/realtime.hpp @@ -0,0 +1,35 @@ +// Copyright 2022 PickNik Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef CONTROLLER_MANAGER__REALTIME_HPP_ +#define CONTROLLER_MANAGER__REALTIME_HPP_ + +namespace controller_manager +{ +/** + * Detect if realtime kernel is present. + * \returns true if realtime kernel is detected + */ +bool has_realtime_kernel(); + +/** + * Configure SCHED_FIFO thread priority for the thread that calls this function + * \param[in] priority the priority of this thread from 0-99 + * \returns true if configuring scheduler succeeded + */ +bool configure_sched_fifo(int priority); + +} // namespace controller_manager + +#endif // CONTROLLER_MANAGER__REALTIME_HPP_ diff --git a/controller_manager/src/realtime.cpp b/controller_manager/src/realtime.cpp new file mode 100644 index 00000000000..54725f42087 --- /dev/null +++ b/controller_manager/src/realtime.cpp @@ -0,0 +1,47 @@ +// Copyright 2022 PickNik Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "controller_manager/realtime.hpp" + +#include + +#include +#include + +namespace controller_manager +{ +bool has_realtime_kernel() +{ + std::ifstream realtime_file("/sys/kernel/realtime", std::ios::in); + bool has_realtime = false; + if (realtime_file.is_open()) + { + realtime_file >> has_realtime; + } + return has_realtime; +} + +bool configure_sched_fifo(int priority) +{ + struct sched_param schedp; + memset(&schedp, 0, sizeof(schedp)); + schedp.sched_priority = priority; + if (sched_setscheduler(0, SCHED_FIFO, &schedp)) + { + return false; + } + return true; +} + +} // namespace controller_manager diff --git a/controller_manager/src/ros2_control_node.cpp b/controller_manager/src/ros2_control_node.cpp index 813e8232c79..0c16e8d7a16 100644 --- a/controller_manager/src/ros2_control_node.cpp +++ b/controller_manager/src/ros2_control_node.cpp @@ -19,10 +19,20 @@ #include #include "controller_manager/controller_manager.hpp" +#include "controller_manager/realtime.hpp" #include "rclcpp/rclcpp.hpp" using namespace std::chrono_literals; +namespace +{ +// Reference: https://man7.org/linux/man-pages/man2/sched_setparam.2.html +// This value is used when configuring the main loop to use SCHED_FIFO scheduling +// We use a midpoint RT priority to allow maximum flexibility to users +int const kSchedPriority = 50; + +} // namespace + int main(int argc, char ** argv) { rclcpp::init(argc, argv); @@ -33,36 +43,46 @@ int main(int argc, char ** argv) auto cm = std::make_shared(executor, manager_node_name); - // TODO(anyone): Due to issues with the MutliThreadedExecutor, this control loop does not rely on - // the executor (see issue #260). - // When the MutliThreadedExecutor issues are fixed (ros2/rclcpp#1168), this loop should be - // converted back to a timer. + RCLCPP_INFO(cm->get_logger(), "update rate is %d Hz", cm->get_update_rate()); + std::thread cm_thread( [cm]() { - RCLCPP_INFO(cm->get_logger(), "update rate is %d Hz", cm->get_update_rate()); + if (controller_manager::has_realtime_kernel()) + { + if (!controller_manager::configure_sched_fifo(kSchedPriority)) + { + RCLCPP_WARN(cm->get_logger(), "Could not enable FIFO RT scheduling policy"); + } + } + else + { + RCLCPP_INFO(cm->get_logger(), "RT kernel is recommended for better performance"); + } - rclcpp::Time current_time = cm->now(); - rclcpp::Time previous_time = current_time; - rclcpp::Time end_period = current_time; + // for calcuating sleep time + auto const period = std::chrono::nanoseconds(1'000'000'000 / cm->get_update_rate()); + std::chrono::system_clock::time_point next_iteration_time = + std::chrono::system_clock::time_point(std::chrono::nanoseconds(cm->now().nanoseconds())); - // Use nanoseconds to avoid chrono's rounding - rclcpp::Duration period(std::chrono::nanoseconds(1000000000 / cm->get_update_rate())); + // for calclating the measured period of the loop + rclcpp::Time previous_time = cm->now(); while (rclcpp::ok()) { - // wait until we hit the end of the period - end_period += period; - std::this_thread::sleep_for( - std::chrono::nanoseconds((end_period - cm->now()).nanoseconds())); + // calculate measured period + auto const current_time = cm->now(); + auto const measured_period = current_time - previous_time; + previous_time = current_time; // execute update loop - current_time = cm->now(); - auto period = current_time - previous_time; - previous_time = current_time; - cm->read(current_time, period); - cm->update(current_time, period); - cm->write(current_time, period); + cm->read(cm->now(), measured_period); + cm->update(cm->now(), measured_period); + cm->write(cm->now(), measured_period); + + // wait until we hit the end of the period + next_iteration_time += period; + std::this_thread::sleep_until(next_iteration_time); } }); From e68cbb13cd91841b5c43a7371bb5ac4887f47909 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Fri, 8 Jul 2022 08:47:39 +0200 Subject: [PATCH 20/35] Move Joint Limits structures for use in controllers (#462) Co-authored-by: AndyZe Co-authored-by: Bence Magyar --- joint_limits/CMakeLists.txt | 53 ++ .../include/joint_limits/joint_limits.hpp | 140 ++++ .../joint_limits/joint_limits_rosparam.hpp | 523 +++++++++++++++ joint_limits/package.xml | 26 + .../test/joint_limits_rosparam.launch.py | 31 +- .../test/joint_limits_rosparam.yaml | 14 +- .../test/joint_limits_rosparam_test.cpp | 420 ++++++++++++ .../joint_limits_interface/joint_limits.hpp | 67 -- .../joint_limits_rosparam.hpp | 270 -------- .../test/joint_limits_interface_test.cpp | 627 ------------------ .../test/joint_limits_rosparam_test.cpp | 200 ------ ros2_control/package.xml | 1 + 12 files changed, 1191 insertions(+), 1181 deletions(-) create mode 100644 joint_limits/CMakeLists.txt create mode 100644 joint_limits/include/joint_limits/joint_limits.hpp create mode 100644 joint_limits/include/joint_limits/joint_limits_rosparam.hpp create mode 100644 joint_limits/package.xml rename {joint_limits_interface => joint_limits}/test/joint_limits_rosparam.launch.py (72%) rename {joint_limits_interface => joint_limits}/test/joint_limits_rosparam.yaml (70%) create mode 100644 joint_limits/test/joint_limits_rosparam_test.cpp delete mode 100644 joint_limits_interface/include/joint_limits_interface/joint_limits.hpp delete mode 100644 joint_limits_interface/include/joint_limits_interface/joint_limits_rosparam.hpp delete mode 100644 joint_limits_interface/test/joint_limits_interface_test.cpp delete mode 100644 joint_limits_interface/test/joint_limits_rosparam_test.cpp diff --git a/joint_limits/CMakeLists.txt b/joint_limits/CMakeLists.txt new file mode 100644 index 00000000000..314ec67272d --- /dev/null +++ b/joint_limits/CMakeLists.txt @@ -0,0 +1,53 @@ +cmake_minimum_required(VERSION 3.5) +project(joint_limits) + +# Default to C++14 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) +endif() + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra) +endif() + +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rclcpp_lifecycle REQUIRED) + +install(DIRECTORY include/ + DESTINATION include +) + +if(BUILD_TESTING) + find_package(ament_cmake_gtest REQUIRED) + find_package(launch_testing_ament_cmake) + find_package(rclcpp) + find_package(rclcpp_lifecycle) + + ament_add_gtest_executable(joint_limits_rosparam_test test/joint_limits_rosparam_test.cpp) + target_include_directories(joint_limits_rosparam_test PRIVATE include) + ament_target_dependencies(joint_limits_rosparam_test rclcpp rclcpp_lifecycle) + + add_launch_test(test/joint_limits_rosparam.launch.py) + install( + TARGETS + joint_limits_rosparam_test + DESTINATION lib/${PROJECT_NAME} + ) + install( + FILES + test/joint_limits_rosparam.yaml + DESTINATION share/${PROJECT_NAME}/test + ) + +endif() + +ament_export_dependencies( + rclcpp +) + +ament_export_include_directories( + include +) + +ament_package() diff --git a/joint_limits/include/joint_limits/joint_limits.hpp b/joint_limits/include/joint_limits/joint_limits.hpp new file mode 100644 index 00000000000..ae1168d27d3 --- /dev/null +++ b/joint_limits/include/joint_limits/joint_limits.hpp @@ -0,0 +1,140 @@ +// Copyright 2020 PAL Robotics S.L. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/// \author Adolfo Rodriguez Tsouroukdissian + +#ifndef JOINT_LIMITS__JOINT_LIMITS_HPP_ +#define JOINT_LIMITS__JOINT_LIMITS_HPP_ + +#include +#include +#include + +namespace joint_limits +{ +/** + * JointLimits structure stores values from from yaml definition or `` tag in URDF. + * The mapping from URDF attributes to members is the following: + * lower --> min_position + * upper --> max_position + * velocity --> max_velocity + * effort --> max_effort + */ +struct JointLimits +{ + JointLimits() + : min_position(std::numeric_limits::quiet_NaN()), + max_position(std::numeric_limits::quiet_NaN()), + max_velocity(std::numeric_limits::quiet_NaN()), + max_acceleration(std::numeric_limits::quiet_NaN()), + max_jerk(std::numeric_limits::quiet_NaN()), + max_effort(std::numeric_limits::quiet_NaN()), + has_position_limits(false), + has_velocity_limits(false), + has_acceleration_limits(false), + has_jerk_limits(false), + has_effort_limits(false), + angle_wraparound(false) + { + } + + double min_position; + double max_position; + double max_velocity; + double max_acceleration; + double max_jerk; + double max_effort; + + bool has_position_limits; + bool has_velocity_limits; + bool has_acceleration_limits; + bool has_jerk_limits; + bool has_effort_limits; + bool angle_wraparound; + + std::string to_string() + { + std::stringstream ss_output; + + ss_output << " has position limits: " << (has_position_limits ? "true" : "false") << "[" + << min_position << ", " << max_position << "]\n"; + ss_output << " has velocity limits: " << (has_velocity_limits ? "true" : "false") << "[" + << max_velocity << "]\n"; + ss_output << " has acceleration limits: " << (has_acceleration_limits ? "true" : "false") + << " [" << max_acceleration << "]\n"; + ss_output << " has jerk limits: " << (has_jerk_limits ? "true" : "false") << "[" << max_jerk + << "]\n"; + ss_output << " has effort limits: " << (has_effort_limits ? "true" : "false") << "[" + << max_effort << "]\n"; + ss_output << " angle wraparound: " << (angle_wraparound ? "true" : "false"); + + return ss_output.str(); + } +}; + +/** + * SoftJointLimits stores values from the `` tag of URDF. + * The meaning of the fields are: + * + * An element can contain the following attributes: + * + * **soft_lower_limit** (optional, defaults to 0) - An attribute specifying the lower joint boundary + * where the safety controller starts limiting the position of the joint. This limit needs to be + * larger than the lower joint limit (see above). See See safety limits for more details. + * + * **soft_upper_limit** (optional, defaults to 0) - An attribute specifying the upper joint boundary + * where the safety controller starts limiting the position of the joint. This limit needs to be + * smaller than the upper joint limit (see above). See See safety limits for more details. + * + * **k_position** (optional, defaults to 0) - An attribute specifying the relation between position + * and velocity limits. See See safety limits for more details. + * + * k_velocity (required) - An attribute specifying the relation between effort and velocity limits. + * See See safety limits for more details. + */ +struct SoftJointLimits +{ + SoftJointLimits() + : min_position(std::numeric_limits::quiet_NaN()), + max_position(std::numeric_limits::quiet_NaN()), + k_position(std::numeric_limits::quiet_NaN()), + k_velocity(std::numeric_limits::quiet_NaN()) + { + } + + double min_position; + double max_position; + double k_position; + double k_velocity; + + std::string to_string() + { + std::stringstream ss_output; + + ss_output << " soft position limits: " + << "[" << min_position << ", " << max_position << "]\n"; + + ss_output << " k-position: " + << "[" << k_position << "]\n"; + + ss_output << " k-velocity: " + << "[" << k_velocity << "]\n"; + + return ss_output.str(); + } +}; + +} // namespace joint_limits + +#endif // JOINT_LIMITS__JOINT_LIMITS_HPP_ diff --git a/joint_limits/include/joint_limits/joint_limits_rosparam.hpp b/joint_limits/include/joint_limits/joint_limits_rosparam.hpp new file mode 100644 index 00000000000..6e0b66641e9 --- /dev/null +++ b/joint_limits/include/joint_limits/joint_limits_rosparam.hpp @@ -0,0 +1,523 @@ +// Copyright 2020 PAL Robotics S.L. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/// \author Adolfo Rodriguez Tsouroukdissian + +#ifndef JOINT_LIMITS__JOINT_LIMITS_ROSPARAM_HPP_ +#define JOINT_LIMITS__JOINT_LIMITS_ROSPARAM_HPP_ + +#include +#include + +#include "joint_limits/joint_limits.hpp" +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_lifecycle/lifecycle_node.hpp" + +namespace // utilities +{ +/// Declare and initialize a parameter with a type. +/** + * + * Wrapper function for templated node's declare_parameter() which checks if + * parameter is already declared. + * For use in all components that inherit from ControllerInterface + */ +template +auto auto_declare( + const rclcpp::node_interfaces::NodeParametersInterface::SharedPtr & param_itf, + const std::string & name, const ParameterT & default_value) +{ + if (!param_itf->has_parameter(name)) + { + auto param_default_value = rclcpp::ParameterValue(default_value); + param_itf->declare_parameter(name, param_default_value); + } + return param_itf->get_parameter(name).get_value(); +} +} // namespace + +namespace joint_limits +{ +/** + * Declare JointLimits and SoftJointLimits parameters for joint with \p joint_name using node + * parameters interface \p param_itf. + * + * The following parameter structure is declared with base name `joint_limits.joint_name`: + * \code + * has_position_limits: bool + * min_position: double + * max_position: double + * has_velocity_limits: bool + * max_velocity: double + * has_acceleration_limits: bool + * max_acceleration: double + * has_jerk_limits: bool + * max_jerk: double + * has_effort_limits: bool + * max_effort: double + * angle_wraparound: bool + * has_soft_limits: bool + * k_position: double + * k_velocity: double + * soft_lower_limit: double + * soft_upper_limit: double + * \endcode + * + * \param[in] joint_name name of the joint for which parameters will be declared. + * \param[in] param_itf node parameters interface object to access parameters. + * \param[in] logging_itf node logging interface to log if error happens. + * + * \returns True if parameters are successfully declared, false otherwise. + */ +inline bool declare_parameters( + const std::string & joint_name, + const rclcpp::node_interfaces::NodeParametersInterface::SharedPtr & param_itf, + const rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr & logging_itf) +{ + const std::string param_base_name = "joint_limits." + joint_name; + try + { + auto_declare(param_itf, param_base_name + ".has_position_limits", false); + auto_declare( + param_itf, param_base_name + ".min_position", std::numeric_limits::quiet_NaN()); + auto_declare( + param_itf, param_base_name + ".max_position", std::numeric_limits::quiet_NaN()); + auto_declare(param_itf, param_base_name + ".has_velocity_limits", false); + auto_declare( + param_itf, param_base_name + ".min_velocity", std::numeric_limits::quiet_NaN()); + auto_declare( + param_itf, param_base_name + ".max_velocity", std::numeric_limits::quiet_NaN()); + auto_declare(param_itf, param_base_name + ".has_acceleration_limits", false); + auto_declare( + param_itf, param_base_name + ".max_acceleration", std::numeric_limits::quiet_NaN()); + auto_declare(param_itf, param_base_name + ".has_jerk_limits", false); + auto_declare( + param_itf, param_base_name + ".max_jerk", std::numeric_limits::quiet_NaN()); + auto_declare(param_itf, param_base_name + ".has_effort_limits", false); + auto_declare( + param_itf, param_base_name + ".max_effort", std::numeric_limits::quiet_NaN()); + auto_declare(param_itf, param_base_name + ".angle_wraparound", false); + auto_declare(param_itf, param_base_name + ".has_soft_limits", false); + auto_declare( + param_itf, param_base_name + ".k_position", std::numeric_limits::quiet_NaN()); + auto_declare( + param_itf, param_base_name + ".k_velocity", std::numeric_limits::quiet_NaN()); + auto_declare( + param_itf, param_base_name + ".soft_lower_limit", std::numeric_limits::quiet_NaN()); + auto_declare( + param_itf, param_base_name + ".soft_upper_limit", std::numeric_limits::quiet_NaN()); + } + catch (const std::exception & ex) + { + RCLCPP_ERROR(logging_itf->get_logger(), "%s", ex.what()); + return false; + } + return true; +} + +/** + * Declare JointLimits and SoftJointLimits parameters for joint with \p joint_name for the \p node + * object. + * This is a convenience function. + * For parameters structure see the underlying `declare_parameters` function. + * + * \param[in] joint_name name of the joint for which parameters will be declared. + * \param[in] node node for parameters should be declared. + * + * \returns True if parameters are successfully declared, false otherwise. + */ +inline bool declare_parameters(const std::string & joint_name, const rclcpp::Node::SharedPtr & node) +{ + return declare_parameters( + joint_name, node->get_node_parameters_interface(), node->get_node_logging_interface()); +} + +/** + * Declare JointLimits and SoftJointLimits parameters for joint with joint_name for the lifecycle_node + * object. + * This is a convenience function. + * For parameters structure see the underlying `declare_parameters` function. + * + * \param[in] joint_name name of the joint for which parameters will be declared. + * \param[in] lifecycle_node lifecycle node for parameters should be declared. + * + * \returns True if parameters are successfully declared, false otherwise. + */ +inline bool declare_parameters( + const std::string & joint_name, const rclcpp_lifecycle::LifecycleNode::SharedPtr & lifecycle_node) +{ + return declare_parameters( + joint_name, lifecycle_node->get_node_parameters_interface(), + lifecycle_node->get_node_logging_interface()); +} + +/// Populate a JointLimits instance from the node parameters. +/** + * It is assumed that parameter structure is the following: + * \code + * has_position_limits: bool + * min_position: double + * max_position: double + * has_velocity_limits: bool + * max_velocity: double + * has_acceleration_limits: bool + * max_acceleration: double + * has_jerk_limits: bool + * max_jerk: double + * has_effort_limits: bool + * max_effort: double + * angle_wraparound: bool # will be ignored if there are position limits + * \endcode + * + * Unspecified parameters are not added to the joint limits specification. + * A specification in a yaml would look like this: + * \code + * + * ros__parameters: + * joint_limits: + * foo_joint: + * has_position_limits: true + * min_position: 0.0 + * max_position: 1.0 + * has_velocity_limits: true + * max_velocity: 2.0 + * has_acceleration_limits: true + * max_acceleration: 5.0 + * has_jerk_limits: true + * max_jerk: 100.0 + * has_effort_limits: true + * max_effort: 20.0 + * bar_joint: + * has_position_limits: false # Continuous joint + * angle_wraparound: true # available only for continuous joints + * has_velocity_limits: true + * max_velocity: 4.0 + * \endcode + * + * \param[in] joint_name Name of joint whose limits are to be fetched, e.g., "foo_joint". + * \param[in] param_itf node parameters interface of the node where parameters are specified. + * \param[in] logging_itf node logging interface to provide log errors. + * \param[out] limits Where joint limit data gets written into. Limits specified in the parameter server will overwrite + * existing values. Values in \p limits not specified in the parameter server remain unchanged. + * + * \returns True if a limits specification is found (i.e., the \p joint_limits/joint_name parameter exists in \p node), false otherwise. + */ +inline bool get_joint_limits( + const std::string & joint_name, + const rclcpp::node_interfaces::NodeParametersInterface::SharedPtr & param_itf, + const rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr & logging_itf, + JointLimits & limits) +{ + const std::string param_base_name = "joint_limits." + joint_name; + try + { + if ( + !param_itf->has_parameter(param_base_name + ".has_position_limits") && + !param_itf->has_parameter(param_base_name + ".min_position") && + !param_itf->has_parameter(param_base_name + ".max_position") && + !param_itf->has_parameter(param_base_name + ".has_velocity_limits") && + !param_itf->has_parameter(param_base_name + ".min_velocity") && + !param_itf->has_parameter(param_base_name + ".max_velocity") && + !param_itf->has_parameter(param_base_name + ".has_acceleration_limits") && + !param_itf->has_parameter(param_base_name + ".max_acceleration") && + !param_itf->has_parameter(param_base_name + ".has_jerk_limits") && + !param_itf->has_parameter(param_base_name + ".max_jerk") && + !param_itf->has_parameter(param_base_name + ".has_effort_limits") && + !param_itf->has_parameter(param_base_name + ".max_effort") && + !param_itf->has_parameter(param_base_name + ".angle_wraparound") && + !param_itf->has_parameter(param_base_name + ".has_soft_limits") && + !param_itf->has_parameter(param_base_name + ".k_position") && + !param_itf->has_parameter(param_base_name + ".k_velocity") && + !param_itf->has_parameter(param_base_name + ".soft_lower_limit") && + !param_itf->has_parameter(param_base_name + ".soft_upper_limit")) + { + RCLCPP_ERROR( + logging_itf->get_logger(), + "No joint limits specification found for joint '%s' in the parameter server " + "(param name: %s).", + joint_name.c_str(), param_base_name.c_str()); + return false; + } + } + catch (const std::exception & ex) + { + RCLCPP_ERROR(logging_itf->get_logger(), "%s", ex.what()); + return false; + } + + // Position limits + if (param_itf->has_parameter(param_base_name + ".has_position_limits")) + { + limits.has_position_limits = + param_itf->get_parameter(param_base_name + ".has_position_limits").as_bool(); + if ( + limits.has_position_limits && param_itf->has_parameter(param_base_name + ".min_position") && + param_itf->has_parameter(param_base_name + ".max_position")) + { + limits.min_position = param_itf->get_parameter(param_base_name + ".min_position").as_double(); + limits.max_position = param_itf->get_parameter(param_base_name + ".max_position").as_double(); + } + else + { + limits.has_position_limits = false; + } + + if ( + !limits.has_position_limits && + param_itf->has_parameter(param_base_name + ".angle_wraparound")) + { + limits.angle_wraparound = + param_itf->get_parameter(param_base_name + ".angle_wraparound").as_bool(); + } + } + + // Velocity limits + if (param_itf->has_parameter(param_base_name + ".has_velocity_limits")) + { + limits.has_velocity_limits = + param_itf->get_parameter(param_base_name + ".has_velocity_limits").as_bool(); + if (limits.has_velocity_limits && param_itf->has_parameter(param_base_name + ".max_velocity")) + { + limits.max_velocity = param_itf->get_parameter(param_base_name + ".max_velocity").as_double(); + } + else + { + limits.has_velocity_limits = false; + } + } + + // Acceleration limits + if (param_itf->has_parameter(param_base_name + ".has_acceleration_limits")) + { + limits.has_acceleration_limits = + param_itf->get_parameter(param_base_name + ".has_acceleration_limits").as_bool(); + if ( + limits.has_acceleration_limits && + param_itf->has_parameter(param_base_name + ".max_acceleration")) + { + limits.max_acceleration = + param_itf->get_parameter(param_base_name + ".max_acceleration").as_double(); + } + else + { + limits.has_acceleration_limits = false; + } + } + + // Jerk limits + if (param_itf->has_parameter(param_base_name + ".has_jerk_limits")) + { + limits.has_jerk_limits = + param_itf->get_parameter(param_base_name + ".has_jerk_limits").as_bool(); + if (limits.has_jerk_limits && param_itf->has_parameter(param_base_name + ".max_jerk")) + { + limits.max_jerk = param_itf->get_parameter(param_base_name + ".max_jerk").as_double(); + } + else + { + limits.has_jerk_limits = false; + } + } + + // Effort limits + if (param_itf->has_parameter(param_base_name + ".has_effort_limits")) + { + limits.has_effort_limits = + param_itf->get_parameter(param_base_name + ".has_effort_limits").as_bool(); + if (limits.has_effort_limits && param_itf->has_parameter(param_base_name + ".max_effort")) + { + limits.has_effort_limits = true; + limits.max_effort = param_itf->get_parameter(param_base_name + ".max_effort").as_double(); + } + else + { + limits.has_effort_limits = false; + } + } + + return true; +} + +/** + * Populate a JointLimits instance from the node parameters. + * This is a convenience function. + * For parameters structure see the underlying `get_joint_limits` function. + * + * \param[in] joint_name Name of joint whose limits are to be fetched. + * \param[in] node Node object for which parameters should be fetched. + * \param[out] limits Where joint limit data gets written into. Limits specified in the parameter server will overwrite + * existing values. Values in \p limits not specified in the parameter server remain unchanged. + * + * \returns True if a limits specification is found, false otherwise. + */ +inline bool get_joint_limits( + const std::string & joint_name, const rclcpp::Node::SharedPtr & node, JointLimits & limits) +{ + return get_joint_limits( + joint_name, node->get_node_parameters_interface(), node->get_node_logging_interface(), limits); +} + +/** + * Populate a JointLimits instance from the node parameters. + * This is a convenience function. + * For parameters structure see the underlying `get_joint_limits` function. + * + * \param[in] joint_name Name of joint whose limits are to be fetched. + * \param[in] lifecycle_node Lifecycle node object for which parameters should be fetched. + * \param[out] limits Where joint limit data gets written into. Limits specified in the parameter server will overwrite + * existing values. Values in \p limits not specified in the parameter server remain unchanged. + * + * \returns True if a limits specification is found, false otherwise. + */ +inline bool get_joint_limits( + const std::string & joint_name, const rclcpp_lifecycle::LifecycleNode::SharedPtr & lifecycle_node, + JointLimits & limits) +{ + return get_joint_limits( + joint_name, lifecycle_node->get_node_parameters_interface(), + lifecycle_node->get_node_logging_interface(), limits); +} + +/// Populate a SoftJointLimits instance from the ROS parameter server. +/** + * It is assumed that the parameter structure is the following: + * \code + * has_soft_limits: bool + * k_position: double + * k_velocity: double + * soft_lower_limit: double + * soft_upper_limit: double + * \endcode + * + * Only completely specified soft joint limits specifications will be considered valid. + * For example a valid yaml configuration would look like: + * \code + * + * ros__parameters: + * joint_limits: + * foo_joint: + * soft_lower_limit: 0.0 + * soft_upper_limit: 1.0 + * k_position: 10.0 + * k_velocity: 10.0 + * \endcode + * + * \param[in] joint_name Name of joint whose limits are to be fetched, e.g., "foo_joint". + * \param[in] param_itf node parameters interface of the node where parameters are specified. + * \param[in] logging_itf node logging interface to provide log errors. + * \param[out] soft_limits Where soft joint limit data gets written into. Limits specified in the parameter server will overwrite + * existing values. + * \return True if a complete soft limits specification is found (ie. if all \p k_position, \p k_velocity, \p soft_lower_limit and + * \p soft_upper_limit exist in \p joint_limits/joint_name namespace), false otherwise. + */ +inline bool get_joint_limits( + const std::string & joint_name, + const rclcpp::node_interfaces::NodeParametersInterface::SharedPtr & param_itf, + const rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr & logging_itf, + SoftJointLimits & soft_limits) +{ + const std::string param_base_name = "joint_limits." + joint_name; + try + { + if ( + !param_itf->has_parameter(param_base_name + ".has_soft_limits") && + !param_itf->has_parameter(param_base_name + ".k_velocity") && + !param_itf->has_parameter(param_base_name + ".k_position") && + !param_itf->has_parameter(param_base_name + ".soft_lower_limit") && + !param_itf->has_parameter(param_base_name + ".soft_upper_limit")) + { + RCLCPP_DEBUG( + logging_itf->get_logger(), + "No soft joint limits specification found for joint '%s' in the parameter server " + "(param name: %s).", + joint_name.c_str(), param_base_name.c_str()); + return false; + } + } + catch (const std::exception & ex) + { + RCLCPP_ERROR(logging_itf->get_logger(), "%s", ex.what()); + return false; + } + + // Override soft limits if complete specification is found + if (param_itf->has_parameter(param_base_name + ".has_soft_limits")) + { + if ( + param_itf->get_parameter(param_base_name + ".has_soft_limits").as_bool() && + param_itf->has_parameter(param_base_name + ".k_position") && + param_itf->has_parameter(param_base_name + ".k_velocity") && + param_itf->has_parameter(param_base_name + ".soft_lower_limit") && + param_itf->has_parameter(param_base_name + ".soft_upper_limit")) + { + soft_limits.k_position = + param_itf->get_parameter(param_base_name + ".k_position").as_double(); + soft_limits.k_velocity = + param_itf->get_parameter(param_base_name + ".k_velocity").as_double(); + soft_limits.min_position = + param_itf->get_parameter(param_base_name + ".soft_lower_limit").as_double(); + soft_limits.max_position = + param_itf->get_parameter(param_base_name + ".soft_upper_limit").as_double(); + return true; + } + } + + return false; +} + +/** + * Populate a JointLimits instance from the node parameters. + * This is a convenience function. + * For parameters structure see the underlying `get_joint_limits` function. + * + * \param[in] joint_name Name of joint whose limits are to be fetched. + * \param[in] node Node object for which parameters should be fetched. + * \param[out] soft_limits Where soft joint limit data gets written into. Limits specified in the parameter server will overwrite + * existing values. + * + * \returns True if a soft limits specification is found, false otherwise. + */ +inline bool get_joint_limits( + const std::string & joint_name, const rclcpp::Node::SharedPtr & node, + SoftJointLimits & soft_limits) +{ + return get_joint_limits( + joint_name, node->get_node_parameters_interface(), node->get_node_logging_interface(), + soft_limits); +} + +/** + * Populate a JointLimits instance from the node parameters. + * This is a convenience function. + * For parameters structure see the underlying `get_joint_limits` function. + * + * \param[in] joint_name Name of joint whose limits are to be fetched. + * \param[in] lifecycle_node Lifecycle node object for which parameters should be fetched. + * \param[out] soft_limits Where soft joint limit data gets written into. Limits specified in the parameter server will overwrite + * existing values. + * + * \returns True if a soft limits specification is found, false otherwise. + */ +inline bool get_joint_limits( + const std::string & joint_name, const rclcpp_lifecycle::LifecycleNode::SharedPtr & lifecycle_node, + SoftJointLimits & soft_limits) +{ + return get_joint_limits( + joint_name, lifecycle_node->get_node_parameters_interface(), + lifecycle_node->get_node_logging_interface(), soft_limits); +} + +} // namespace joint_limits + +#endif // JOINT_LIMITS__JOINT_LIMITS_ROSPARAM_HPP_ diff --git a/joint_limits/package.xml b/joint_limits/package.xml new file mode 100644 index 00000000000..12e11638d2f --- /dev/null +++ b/joint_limits/package.xml @@ -0,0 +1,26 @@ + + joint_limits + 0.0.0 + Interfaces for handling of joint limits for controllers or hardware. + + Bence Magyar + Denis Štogl + + Apache License 2.0 + + https://github.com/ros-controls/ros2_control/wiki + https://github.com/ros-controls/ros2_control/issues + https://github.com/ros-controls/ros2_control + + ament_cmake + + rclcpp + rclcpp_lifecycle + + launch_testing_ament_cmake + ament_cmake_gtest + + + ament_cmake + + diff --git a/joint_limits_interface/test/joint_limits_rosparam.launch.py b/joint_limits/test/joint_limits_rosparam.launch.py similarity index 72% rename from joint_limits_interface/test/joint_limits_rosparam.launch.py rename to joint_limits/test/joint_limits_rosparam.launch.py index 9d0f31e0a3d..c808954207b 100644 --- a/joint_limits_interface/test/joint_limits_rosparam.launch.py +++ b/joint_limits/test/joint_limits_rosparam.launch.py @@ -27,32 +27,35 @@ def generate_test_description(): - joint_limits_interface_path = get_package_share_directory('joint_limits_interface') + joint_limits_path = get_package_share_directory("joint_limits") node_under_test = Node( - package='joint_limits_interface', - executable='joint_limits_rosparam_test', - output='screen', - parameters=[os.path.join(joint_limits_interface_path, - 'test', - 'joint_limits_rosparam.yaml')], + package="joint_limits", + executable="joint_limits_rosparam_test", + output="screen", + parameters=[ + os.path.join(joint_limits_path, "test", "joint_limits_rosparam.yaml") + ], + ) + return ( + LaunchDescription( + [ + node_under_test, + launch_testing.util.KeepAliveProc(), + launch_testing.actions.ReadyToTest(), + ] + ), + locals(), ) - return LaunchDescription([ - node_under_test, - launch_testing.util.KeepAliveProc(), - launch_testing.actions.ReadyToTest(), - ]), locals() class TestJointLimitInterface(unittest.TestCase): - def test_termination(self, node_under_test, proc_info): proc_info.assertWaitForShutdown(process=node_under_test, timeout=(10)) @launch_testing.post_shutdown_test() class TestJointLimitInterfaceTestAfterShutdown(unittest.TestCase): - def test_exit_code(self, proc_info): # Check that all processes in the launch (in this case, there's just one) exit # with code 0 diff --git a/joint_limits_interface/test/joint_limits_rosparam.yaml b/joint_limits/test/joint_limits_rosparam.yaml similarity index 70% rename from joint_limits_interface/test/joint_limits_rosparam.yaml rename to joint_limits/test/joint_limits_rosparam.yaml index 0ebb43af055..521fbf93f85 100644 --- a/joint_limits_interface/test/joint_limits_rosparam.yaml +++ b/joint_limits/test/joint_limits_rosparam.yaml @@ -1,6 +1,7 @@ JointLimitsRosparamTestNode: ros__parameters: joint_limits: + # Get full specification from parameter server foo_joint: has_position_limits: true min_position: 0.0 @@ -13,13 +14,14 @@ JointLimitsRosparamTestNode: max_jerk: 100.0 has_effort_limits: true max_effort: 20.0 - angle_wraparound: true # should be ignored, has position limits + angle_wraparound: true # should be ignored, has position limits has_soft_limits: true k_position: 10.0 k_velocity: 20.0 soft_lower_limit: 0.1 soft_upper_limit: 0.9 + # Specifying flags but not values should set nothing yinfoo_joint: has_position_limits: true has_velocity_limits: true @@ -27,6 +29,7 @@ JointLimitsRosparamTestNode: has_jerk_limits: true has_effort_limits: true + # Specifying values but not flags should set nothing yangfoo_joint: min_position: 0.0 max_position: 1.0 @@ -35,23 +38,27 @@ JointLimitsRosparamTestNode: max_jerk: 100.0 max_effort: 20.0 + # Disable already set values antifoo_joint: has_position_limits: false has_velocity_limits: false has_acceleration_limits: false has_jerk_limits: false has_effort_limits: false - angle_wraparound: true # should be accepted, has no position limits + angle_wraparound: true # should be accepted, has no position limits + # Override only one field, leave all others unchanged bar_joint: has_velocity_limits: true max_velocity: 2.0 + # Incomplete position limits specification does not get loaded baz_joint: has_position_limits: true # Missing min_position max_position: 1.0 + # Skip parsing soft limits if has_soft_limits is false foobar_joint: has_soft_limits: false k_velocity: 20.0 @@ -59,8 +66,9 @@ JointLimitsRosparamTestNode: soft_lower_limit: 0.1 soft_upper_limit: 0.9 + # Incomplete soft limits specification does not get loaded barbaz_joint: - has_soft_limits: false + has_soft_limits: true k_position: 10.0 soft_lower_limit: 0.1 soft_upper_limit: 0.9 diff --git a/joint_limits/test/joint_limits_rosparam_test.cpp b/joint_limits/test/joint_limits_rosparam_test.cpp new file mode 100644 index 00000000000..c5ddb8f5859 --- /dev/null +++ b/joint_limits/test/joint_limits_rosparam_test.cpp @@ -0,0 +1,420 @@ +// Copyright 2020 PAL Robotics S.L. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/// \author Adolfo Rodriguez Tsouroukdissian + +#include + +#include "gtest/gtest.h" + +#include "joint_limits/joint_limits_rosparam.hpp" +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_lifecycle/lifecycle_node.hpp" + +class JointLimitsRosParamTest : public ::testing::Test +{ +public: + void SetUp() + { + rclcpp::NodeOptions node_options; + node_options.allow_undeclared_parameters(true).automatically_declare_parameters_from_overrides( + true); + + node_ = rclcpp::Node::make_shared("JointLimitsRosparamTestNode", node_options); + } + + void TearDown() { node_.reset(); } + +protected: + rclcpp::Node::SharedPtr node_; +}; + +TEST_F(JointLimitsRosParamTest, parse_joint_limits) +{ + // Invalid specification + { + joint_limits::JointLimits limits; + + // test default values + EXPECT_FALSE(limits.has_position_limits); + EXPECT_TRUE(std::isnan(limits.min_position)); + EXPECT_TRUE(std::isnan(limits.max_position)); + EXPECT_FALSE(limits.has_velocity_limits); + EXPECT_TRUE(std::isnan(limits.max_velocity)); + EXPECT_FALSE(limits.has_acceleration_limits); + EXPECT_TRUE(std::isnan(limits.max_acceleration)); + EXPECT_FALSE(limits.has_jerk_limits); + EXPECT_TRUE(std::isnan(limits.max_jerk)); + EXPECT_FALSE(limits.has_effort_limits); + EXPECT_TRUE(std::isnan(limits.max_effort)); + EXPECT_FALSE(limits.angle_wraparound); + + // try to read limits for not-existing joints + EXPECT_FALSE(get_joint_limits( + "bad_joint", node_->get_node_parameters_interface(), node_->get_node_logging_interface(), + limits)); + EXPECT_FALSE(get_joint_limits( + "unknown_joint", node_->get_node_parameters_interface(), node_->get_node_logging_interface(), + limits)); + + // default values should not change + EXPECT_FALSE(limits.has_position_limits); + EXPECT_TRUE(std::isnan(limits.min_position)); + EXPECT_TRUE(std::isnan(limits.max_position)); + EXPECT_FALSE(limits.has_velocity_limits); + EXPECT_TRUE(std::isnan(limits.max_velocity)); + EXPECT_FALSE(limits.has_acceleration_limits); + EXPECT_TRUE(std::isnan(limits.max_acceleration)); + EXPECT_FALSE(limits.has_jerk_limits); + EXPECT_TRUE(std::isnan(limits.max_jerk)); + EXPECT_FALSE(limits.has_effort_limits); + EXPECT_TRUE(std::isnan(limits.max_effort)); + EXPECT_FALSE(limits.angle_wraparound); + } + + // Get full specification from parameter server + { + joint_limits::JointLimits limits; + EXPECT_TRUE(get_joint_limits( + "foo_joint", node_->get_node_parameters_interface(), node_->get_node_logging_interface(), + limits)); + + EXPECT_TRUE(limits.has_position_limits); + EXPECT_EQ(0.0, limits.min_position); + EXPECT_EQ(1.0, limits.max_position); + + EXPECT_TRUE(limits.has_velocity_limits); + EXPECT_EQ(2.0, limits.max_velocity); + + EXPECT_TRUE(limits.has_acceleration_limits); + EXPECT_EQ(5.0, limits.max_acceleration); + + EXPECT_TRUE(limits.has_jerk_limits); + EXPECT_EQ(100.0, limits.max_jerk); + + EXPECT_TRUE(limits.has_effort_limits); + EXPECT_EQ(20.0, limits.max_effort); + + // parameters is 'true', but because there are position limits it is ignored + EXPECT_FALSE(limits.angle_wraparound); + } + + // Specifying flags but not values should set nothing + { + joint_limits::JointLimits limits; + EXPECT_TRUE(get_joint_limits( + "yinfoo_joint", node_->get_node_parameters_interface(), node_->get_node_logging_interface(), + limits)); + + EXPECT_FALSE(limits.has_position_limits); + EXPECT_FALSE(limits.has_velocity_limits); + EXPECT_FALSE(limits.has_acceleration_limits); + EXPECT_FALSE(limits.has_jerk_limits); + EXPECT_FALSE(limits.has_effort_limits); + } + + // Specifying values but not flags should set nothing + { + joint_limits::JointLimits limits; + EXPECT_TRUE(get_joint_limits( + "yangfoo_joint", node_->get_node_parameters_interface(), node_->get_node_logging_interface(), + limits)); + + EXPECT_FALSE(limits.has_position_limits); + EXPECT_FALSE(limits.has_velocity_limits); + EXPECT_FALSE(limits.has_acceleration_limits); + EXPECT_FALSE(limits.has_jerk_limits); + EXPECT_FALSE(limits.has_effort_limits); + } + + // Disable already set values + { + joint_limits::JointLimits limits; + EXPECT_TRUE(get_joint_limits( + "foo_joint", node_->get_node_parameters_interface(), node_->get_node_logging_interface(), + limits)); + EXPECT_TRUE(limits.has_position_limits); + EXPECT_TRUE(limits.has_velocity_limits); + EXPECT_TRUE(limits.has_acceleration_limits); + EXPECT_TRUE(limits.has_jerk_limits); + EXPECT_TRUE(limits.has_effort_limits); + + EXPECT_TRUE(get_joint_limits( + "antifoo_joint", node_->get_node_parameters_interface(), node_->get_node_logging_interface(), + limits)); + EXPECT_FALSE(limits.has_position_limits); + EXPECT_FALSE(limits.has_velocity_limits); + EXPECT_FALSE(limits.has_acceleration_limits); + EXPECT_FALSE(limits.has_jerk_limits); + EXPECT_FALSE(limits.has_effort_limits); + EXPECT_TRUE(limits.angle_wraparound); + } + + // Incomplete position limits specification does not get loaded + { + joint_limits::JointLimits limits; + EXPECT_TRUE(get_joint_limits( + "baz_joint", node_->get_node_parameters_interface(), node_->get_node_logging_interface(), + limits)); + + EXPECT_FALSE(limits.has_position_limits); + EXPECT_TRUE(std::isnan(limits.min_position)); + EXPECT_TRUE(std::isnan(limits.max_position)); + } + + // Override only one field, leave all others unchanged + { + joint_limits::JointLimits limits; + EXPECT_TRUE(get_joint_limits( + "bar_joint", node_->get_node_parameters_interface(), node_->get_node_logging_interface(), + limits)); + + EXPECT_FALSE(limits.has_position_limits); + EXPECT_TRUE(std::isnan(limits.min_position)); + EXPECT_TRUE(std::isnan(limits.max_position)); + + // Max velocity is overridden + EXPECT_TRUE(limits.has_velocity_limits); + EXPECT_EQ(2.0, limits.max_velocity); + + EXPECT_FALSE(limits.has_acceleration_limits); + EXPECT_TRUE(std::isnan(limits.max_acceleration)); + + EXPECT_FALSE(limits.has_jerk_limits); + EXPECT_TRUE(std::isnan(limits.max_jerk)); + + EXPECT_FALSE(limits.has_effort_limits); + EXPECT_TRUE(std::isnan(limits.max_effort)); + } +} + +TEST_F(JointLimitsRosParamTest, parse_soft_joint_limits) +{ + // Invalid specification + { + joint_limits::SoftJointLimits soft_limits; + + // test default values + EXPECT_TRUE(std::isnan(soft_limits.min_position)); + EXPECT_TRUE(std::isnan(soft_limits.max_position)); + EXPECT_TRUE(std::isnan(soft_limits.k_position)); + EXPECT_TRUE(std::isnan(soft_limits.k_velocity)); + + // try to read limits for not-existing joints + EXPECT_FALSE(get_joint_limits( + "bad_joint", node_->get_node_parameters_interface(), node_->get_node_logging_interface(), + soft_limits)); + EXPECT_FALSE(get_joint_limits( + "unknown_joint", node_->get_node_parameters_interface(), node_->get_node_logging_interface(), + soft_limits)); + + // default values should not change + EXPECT_TRUE(std::isnan(soft_limits.min_position)); + EXPECT_TRUE(std::isnan(soft_limits.max_position)); + EXPECT_TRUE(std::isnan(soft_limits.k_position)); + EXPECT_TRUE(std::isnan(soft_limits.k_velocity)); + } + + // Get full specification from parameter server + { + joint_limits::SoftJointLimits soft_limits; + EXPECT_TRUE(get_joint_limits( + "foo_joint", node_->get_node_parameters_interface(), node_->get_node_logging_interface(), + soft_limits)); + + EXPECT_EQ(10.0, soft_limits.k_position); + EXPECT_EQ(20.0, soft_limits.k_velocity); + EXPECT_EQ(0.1, soft_limits.min_position); + EXPECT_EQ(0.9, soft_limits.max_position); + } + + // Skip parsing soft limits if has_soft_limits is false + { + joint_limits::SoftJointLimits soft_limits; + EXPECT_FALSE(get_joint_limits( + "foobar_joint", node_->get_node_parameters_interface(), node_->get_node_logging_interface(), + soft_limits)); + EXPECT_TRUE(std::isnan(soft_limits.min_position)); + EXPECT_TRUE(std::isnan(soft_limits.max_position)); + EXPECT_TRUE(std::isnan(soft_limits.k_position)); + EXPECT_TRUE(std::isnan(soft_limits.k_velocity)); + } + + // Incomplete soft limits specification does not get loaded + { + joint_limits::SoftJointLimits soft_limits; + EXPECT_FALSE(get_joint_limits( + "barbaz_joint", node_->get_node_parameters_interface(), node_->get_node_logging_interface(), + soft_limits)); + EXPECT_TRUE(std::isnan(soft_limits.min_position)); + EXPECT_TRUE(std::isnan(soft_limits.max_position)); + EXPECT_TRUE(std::isnan(soft_limits.k_position)); + EXPECT_TRUE(std::isnan(soft_limits.k_velocity)); + } +} + +class JointLimitsUndeclaredRosParamTest : public ::testing::Test +{ +public: + void SetUp() { node_ = rclcpp::Node::make_shared("JointLimitsRosparamTestNode"); } + + void TearDown() { node_.reset(); } + +protected: + rclcpp::Node::SharedPtr node_; +}; + +class JointLimitsLifecycleNodeUndeclaredRosParamTest : public ::testing::Test +{ +public: + void SetUp() + { + lifecycle_node_ = rclcpp_lifecycle::LifecycleNode::make_shared("JointLimitsRosparamTestNode"); + } + + void TearDown() { lifecycle_node_.reset(); } + +protected: + rclcpp_lifecycle::LifecycleNode::SharedPtr lifecycle_node_; +}; + +TEST_F(JointLimitsUndeclaredRosParamTest, parse_declared_joint_limits_node) +{ + // Get full specification from parameter server - no need to test logic + { + joint_limits::JointLimits limits; + // try to read limits for not-existing joints + EXPECT_FALSE(get_joint_limits("bad_joint", node_, limits)); + EXPECT_FALSE(get_joint_limits("unknown_joint", node_, limits)); + + // try to read existing but undeclared joint + EXPECT_FALSE(get_joint_limits("foo_joint", node_, limits)); + + // declare parameters + EXPECT_TRUE(joint_limits::declare_parameters("foo_joint", node_)); + + // now should be successful + EXPECT_TRUE(get_joint_limits("foo_joint", node_, limits)); + + EXPECT_TRUE(limits.has_position_limits); + EXPECT_EQ(0.0, limits.min_position); + EXPECT_EQ(1.0, limits.max_position); + + EXPECT_TRUE(limits.has_velocity_limits); + EXPECT_EQ(2.0, limits.max_velocity); + + EXPECT_TRUE(limits.has_acceleration_limits); + EXPECT_EQ(5.0, limits.max_acceleration); + + EXPECT_TRUE(limits.has_jerk_limits); + EXPECT_EQ(100.0, limits.max_jerk); + + EXPECT_TRUE(limits.has_effort_limits); + EXPECT_EQ(20.0, limits.max_effort); + + // parameters is 'true', but because there are position limits it is ignored + EXPECT_FALSE(limits.angle_wraparound); + } +} + +TEST_F(JointLimitsLifecycleNodeUndeclaredRosParamTest, parse_declared_joint_limits_lifecycle_node) +{ + // Get full specification from parameter server - no need to test logic + { + joint_limits::JointLimits limits; + // try to read limits for not-existing joints + EXPECT_FALSE(get_joint_limits("bad_joint", lifecycle_node_, limits)); + EXPECT_FALSE(get_joint_limits("unknown_joint", lifecycle_node_, limits)); + + // try to read existing but undeclared joint + EXPECT_FALSE(get_joint_limits("foo_joint", lifecycle_node_, limits)); + + // declare parameters + EXPECT_TRUE(joint_limits::declare_parameters("foo_joint", lifecycle_node_)); + + // now should be successful + EXPECT_TRUE(get_joint_limits("foo_joint", lifecycle_node_, limits)); + + EXPECT_TRUE(limits.has_position_limits); + EXPECT_EQ(0.0, limits.min_position); + EXPECT_EQ(1.0, limits.max_position); + + EXPECT_TRUE(limits.has_velocity_limits); + EXPECT_EQ(2.0, limits.max_velocity); + + EXPECT_TRUE(limits.has_acceleration_limits); + EXPECT_EQ(5.0, limits.max_acceleration); + + EXPECT_TRUE(limits.has_jerk_limits); + EXPECT_EQ(100.0, limits.max_jerk); + + EXPECT_TRUE(limits.has_effort_limits); + EXPECT_EQ(20.0, limits.max_effort); + + // parameters is 'true', but because there are position limits it is ignored + EXPECT_FALSE(limits.angle_wraparound); + } +} + +TEST_F(JointLimitsUndeclaredRosParamTest, parse_declared_soft_joint_limits_node) +{ + // Get full specification from parameter server - no need to test logic + { + joint_limits::SoftJointLimits soft_limits; + // try to read existing but undeclared joint + EXPECT_FALSE(get_joint_limits("foo_joint", node_, soft_limits)); + + // declare parameters + EXPECT_TRUE(joint_limits::declare_parameters("foo_joint", node_)); + + // now should be successful + EXPECT_TRUE(get_joint_limits("foo_joint", node_, soft_limits)); + + EXPECT_EQ(10.0, soft_limits.k_position); + EXPECT_EQ(20.0, soft_limits.k_velocity); + EXPECT_EQ(0.1, soft_limits.min_position); + EXPECT_EQ(0.9, soft_limits.max_position); + } +} + +TEST_F( + JointLimitsLifecycleNodeUndeclaredRosParamTest, parse_declared_soft_joint_limits_lifecycle_node) +{ + // Get full specification from parameter server - no need to test logic + { + joint_limits::SoftJointLimits soft_limits; + // try to read existing but undeclared joint + EXPECT_FALSE(get_joint_limits("foo_joint", lifecycle_node_, soft_limits)); + + // declare parameters + EXPECT_TRUE(joint_limits::declare_parameters("foo_joint", lifecycle_node_)); + + // now should be successful + EXPECT_TRUE(get_joint_limits("foo_joint", lifecycle_node_, soft_limits)); + + EXPECT_EQ(10.0, soft_limits.k_position); + EXPECT_EQ(20.0, soft_limits.k_velocity); + EXPECT_EQ(0.1, soft_limits.min_position); + EXPECT_EQ(0.9, soft_limits.max_position); + } +} + +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + testing::InitGoogleTest(&argc, argv); + int ret = RUN_ALL_TESTS(); + rclcpp::shutdown(); + return ret; +} diff --git a/joint_limits_interface/include/joint_limits_interface/joint_limits.hpp b/joint_limits_interface/include/joint_limits_interface/joint_limits.hpp deleted file mode 100644 index ab69f751229..00000000000 --- a/joint_limits_interface/include/joint_limits_interface/joint_limits.hpp +++ /dev/null @@ -1,67 +0,0 @@ -// Copyright 2020 PAL Robotics S.L. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -/// \author Adolfo Rodriguez Tsouroukdissian - -#ifndef JOINT_LIMITS_INTERFACE__JOINT_LIMITS_HPP_ -#define JOINT_LIMITS_INTERFACE__JOINT_LIMITS_HPP_ - -namespace joint_limits_interface -{ -struct JointLimits -{ - JointLimits() - : min_position(0.0), - max_position(0.0), - max_velocity(0.0), - max_acceleration(0.0), - max_jerk(0.0), - max_effort(0.0), - has_position_limits(false), - has_velocity_limits(false), - has_acceleration_limits(false), - has_jerk_limits(false), - has_effort_limits(false), - angle_wraparound(false) - { - } - - double min_position; - double max_position; - double max_velocity; - double max_acceleration; - double max_jerk; - double max_effort; - - bool has_position_limits; - bool has_velocity_limits; - bool has_acceleration_limits; - bool has_jerk_limits; - bool has_effort_limits; - bool angle_wraparound; -}; - -struct SoftJointLimits -{ - SoftJointLimits() : min_position(0.0), max_position(0.0), k_position(0.0), k_velocity(0.0) {} - - double min_position; - double max_position; - double k_position; - double k_velocity; -}; - -} // namespace joint_limits_interface - -#endif // JOINT_LIMITS_INTERFACE__JOINT_LIMITS_HPP_ diff --git a/joint_limits_interface/include/joint_limits_interface/joint_limits_rosparam.hpp b/joint_limits_interface/include/joint_limits_interface/joint_limits_rosparam.hpp deleted file mode 100644 index 26298475ea3..00000000000 --- a/joint_limits_interface/include/joint_limits_interface/joint_limits_rosparam.hpp +++ /dev/null @@ -1,270 +0,0 @@ -// Copyright 2020 PAL Robotics S.L. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -/// \author Adolfo Rodriguez Tsouroukdissian - -#ifndef JOINT_LIMITS_INTERFACE__JOINT_LIMITS_ROSPARAM_HPP_ -#define JOINT_LIMITS_INTERFACE__JOINT_LIMITS_ROSPARAM_HPP_ - -#include - -#include - -#include - -namespace joint_limits_interface -{ -/// Populate a JointLimits instance from the ROS parameter server. -/** - * It is assumed that the following parameter structure is followed on the provided NodeHandle. Unspecified parameters - * are simply not added to the joint limits specification. - * \code - * joint_limits: - * foo_joint: - * has_position_limits: true - * min_position: 0.0 - * max_position: 1.0 - * has_velocity_limits: true - * max_velocity: 2.0 - * has_acceleration_limits: true - * max_acceleration: 5.0 - * has_jerk_limits: true - * max_jerk: 100.0 - * has_effort_limits: true - * max_effort: 20.0 - * bar_joint: - * has_position_limits: false # Continuous joint - * has_velocity_limits: true - * max_velocity: 4.0 - * \endcode - * - * This specification is similar to the one used by MoveIt!, - * but additionally supports jerk and effort limits. - * - * \param[in] joint_name Name of joint whose limits are to be fetched. - * \param[in] node NodeHandle where the joint limits are specified. - * \param[out] limits Where joint limit data gets written into. Limits specified in the parameter server will overwrite - * existing values. Values in \p limits not specified in the parameter server remain unchanged. - * \return True if a limits specification is found (ie. the \p joint_limits/joint_name parameter exists in \p node), false otherwise. - */ -inline bool getJointLimits( - const std::string & joint_name, const rclcpp::Node::SharedPtr & node, JointLimits & limits) -{ - const std::string param_base_name = "joint_limits." + joint_name; - try - { - if ( - !node->has_parameter(param_base_name + ".has_position_limits") && - !node->has_parameter(param_base_name + ".min_position") && - !node->has_parameter(param_base_name + ".max_position") && - !node->has_parameter(param_base_name + ".has_velocity_limits") && - !node->has_parameter(param_base_name + ".min_velocity") && - !node->has_parameter(param_base_name + ".max_velocity") && - !node->has_parameter(param_base_name + ".has_acceleration_limits") && - !node->has_parameter(param_base_name + ".max_acceleration") && - !node->has_parameter(param_base_name + ".has_jerk_limits") && - !node->has_parameter(param_base_name + ".max_jerk") && - !node->has_parameter(param_base_name + ".has_effort_limits") && - !node->has_parameter(param_base_name + ".max_effort") && - !node->has_parameter(param_base_name + ".angle_wraparound") && - !node->has_parameter(param_base_name + ".has_soft_limits") && - !node->has_parameter(param_base_name + ".k_position") && - !node->has_parameter(param_base_name + ".k_velocity") && - !node->has_parameter(param_base_name + ".soft_lower_limit") && - !node->has_parameter(param_base_name + ".soft_upper_limit")) - { - RCLCPP_ERROR( - node->get_logger(), - "No joint limits specification found for joint '%s' in the parameter server " - "(node: %s param name: %s).", - joint_name.c_str(), node->get_name(), param_base_name.c_str()); - return false; - } - } - catch (const std::exception & ex) - { - RCLCPP_ERROR(node->get_logger(), "%s", ex.what()); - return false; - } - - // Position limits - bool has_position_limits = false; - if (node->get_parameter(param_base_name + ".has_position_limits", has_position_limits)) - { - if (!has_position_limits) - { - limits.has_position_limits = false; - } - double min_pos, max_pos; - if ( - has_position_limits && node->get_parameter(param_base_name + ".min_position", min_pos) && - node->get_parameter(param_base_name + ".max_position", max_pos)) - { - limits.has_position_limits = true; - limits.min_position = min_pos; - limits.max_position = max_pos; - } - - bool angle_wraparound; - if ( - !has_position_limits && - node->get_parameter(param_base_name + ".angle_wraparound", angle_wraparound)) - { - limits.angle_wraparound = angle_wraparound; - } - } - - // Velocity limits - bool has_velocity_limits = false; - if (node->get_parameter(param_base_name + ".has_velocity_limits", has_velocity_limits)) - { - if (!has_velocity_limits) - { - limits.has_velocity_limits = false; - } - double max_vel; - if (has_velocity_limits && node->get_parameter(param_base_name + ".max_velocity", max_vel)) - { - limits.has_velocity_limits = true; - limits.max_velocity = max_vel; - } - } - - // Acceleration limits - bool has_acceleration_limits = false; - if (node->get_parameter(param_base_name + ".has_acceleration_limits", has_acceleration_limits)) - { - if (!has_acceleration_limits) - { - limits.has_acceleration_limits = false; - } - double max_acc; - if ( - has_acceleration_limits && - node->get_parameter(param_base_name + ".max_acceleration", max_acc)) - { - limits.has_acceleration_limits = true; - limits.max_acceleration = max_acc; - } - } - - // Jerk limits - bool has_jerk_limits = false; - if (node->get_parameter(param_base_name + ".has_jerk_limits", has_jerk_limits)) - { - if (!has_jerk_limits) - { - limits.has_jerk_limits = false; - } - double max_jerk; - if (has_jerk_limits && node->get_parameter(param_base_name + ".max_jerk", max_jerk)) - { - limits.has_jerk_limits = true; - limits.max_jerk = max_jerk; - } - } - - // Effort limits - bool has_effort_limits = false; - if (node->get_parameter(param_base_name + ".has_effort_limits", has_effort_limits)) - { - if (!has_effort_limits) - { - limits.has_effort_limits = false; - } - double max_effort; - if (has_effort_limits && node->get_parameter(param_base_name + ".max_effort", max_effort)) - { - limits.has_effort_limits = true; - limits.max_effort = max_effort; - } - } - - return true; -} - -/// Populate a SoftJointLimits instance from the ROS parameter server. -/** - * It is assumed that the following parameter structure is followed on the provided NodeHandle. Only completely specified soft - * joint limits specifications will be considered valid. - * \code - * joint_limits: - * foo_joint: - * soft_lower_limit: 0.0 - * soft_upper_limit: 1.0 - * k_position: 10.0 - * k_velocity: 10.0 - * \endcode - * - * This specification is similar to the specification of the safety_controller tag in the URDF, adapted to the parameter server. - * - * \param[in] joint_name Name of joint whose limits are to be fetched. - * \param[in] node NodeHandle where the joint limits are specified. - * \param[out] soft_limits Where soft joint limit data gets written into. Limits specified in the parameter server will overwrite - * existing values. - * \return True if a complete soft limits specification is found (ie. if all \p k_position, \p k_velocity, \p soft_lower_limit and - * \p soft_upper_limit exist in \p joint_limits/joint_name namespace), false otherwise. - */ -inline bool getSoftJointLimits( - const std::string & joint_name, const rclcpp::Node::SharedPtr & node, - SoftJointLimits & soft_limits) -{ - const std::string param_base_name = "joint_limits." + joint_name; - try - { - if ( - !node->has_parameter(param_base_name + ".has_soft_limits") && - !node->has_parameter(param_base_name + ".k_velocity") && - !node->has_parameter(param_base_name + ".k_position") && - !node->has_parameter(param_base_name + ".soft_lower_limit") && - !node->has_parameter(param_base_name + ".soft_upper_limit")) - { - RCLCPP_DEBUG( - node->get_logger(), - "No soft joint limits specification found for joint '%s' in the parameter server " - "(node: %s param name: %s).", - joint_name.c_str(), node->get_name(), param_base_name.c_str()); - return false; - } - } - catch (const std::exception & ex) - { - RCLCPP_ERROR(node->get_logger(), "%s", ex.what()); - return false; - } - - // Override soft limits if complete specification is found - bool has_soft_limits; - if (node->get_parameter(param_base_name + ".has_soft_limits", has_soft_limits)) - { - if ( - has_soft_limits && node->has_parameter(param_base_name + ".k_position") && - node->has_parameter(param_base_name + ".k_velocity") && - node->has_parameter(param_base_name + ".soft_lower_limit") && - node->has_parameter(param_base_name + ".soft_upper_limit")) - { - node->get_parameter(param_base_name + ".k_position", soft_limits.k_position); - node->get_parameter(param_base_name + ".k_velocity", soft_limits.k_velocity); - node->get_parameter(param_base_name + ".soft_lower_limit", soft_limits.min_position); - node->get_parameter(param_base_name + ".soft_upper_limit", soft_limits.max_position); - return true; - } - } - - return false; -} - -} // namespace joint_limits_interface - -#endif // JOINT_LIMITS_INTERFACE__JOINT_LIMITS_ROSPARAM_HPP_ diff --git a/joint_limits_interface/test/joint_limits_interface_test.cpp b/joint_limits_interface/test/joint_limits_interface_test.cpp deleted file mode 100644 index aaa3b63f701..00000000000 --- a/joint_limits_interface/test/joint_limits_interface_test.cpp +++ /dev/null @@ -1,627 +0,0 @@ -// Copyright 2020 PAL Robotics S.L. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -/// \author Adolfo Rodriguez Tsouroukdissian - -#include - -#include -#include - -#include -#include - -#include - -#include -#include - -// Floating-point value comparison threshold -const double EPS = 1e-12; - -TEST(SaturateTest, Saturate) -{ - const double min = -1.0; - const double max = 2.0; - double val; - - val = -0.5; - EXPECT_NEAR(val, rcppmath::clamp(val, min, max), EPS); - - val = 0.5; - EXPECT_NEAR(val, rcppmath::clamp(val, min, max), EPS); - - val = -1.0; - EXPECT_NEAR(val, rcppmath::clamp(min, min, max), EPS); - - val = -2.0; - EXPECT_NEAR(min, rcppmath::clamp(val, min, max), EPS); - - val = 2.0; - EXPECT_NEAR(val, rcppmath::clamp(val, min, max), EPS); - - val = 3.0; - EXPECT_NEAR(max, rcppmath::clamp(val, min, max), EPS); -} - -class JointLimitsTest -{ -public: - JointLimitsTest() - : pos(0.0), - vel(0.0), - eff(0.0), - cmd(0.0), - name("joint_name"), - period(0, 100000000), - cmd_handle(hardware_interface::JointHandle(name, "position_command", &cmd)), - pos_handle(hardware_interface::JointHandle(name, hardware_interface::HW_IF_POSITION, &pos)), - vel_handle(hardware_interface::JointHandle(name, hardware_interface::HW_IF_VELOCITY, &vel)), - eff_handle(hardware_interface::JointHandle(name, hardware_interface::HW_IF_EFFORT, &eff)) - { - limits.has_position_limits = true; - limits.min_position = -1.0; - limits.max_position = 1.0; - - limits.has_velocity_limits = true; - limits.max_velocity = 2.0; - - limits.has_effort_limits = true; - limits.max_effort = 8.0; - - soft_limits.min_position = -0.8; - soft_limits.max_position = 0.8; - soft_limits.k_position = 20.0; - // TODO(anyone): Tune value - soft_limits.k_velocity = 40.0; - } - -protected: - double pos, vel, eff, cmd; - std::string name; - rclcpp::Duration period; - hardware_interface::JointHandle cmd_handle; - hardware_interface::JointHandle pos_handle, vel_handle, eff_handle; - joint_limits_interface::JointLimits limits; - joint_limits_interface::SoftJointLimits soft_limits; -}; - -class JointLimitsHandleTest : public JointLimitsTest, public ::testing::Test -{ -}; - -TEST_F(JointLimitsHandleTest, HandleConstruction) -{ - { - joint_limits_interface::JointLimits limits_bad; - EXPECT_THROW( - joint_limits_interface::PositionJointSoftLimitsHandle( - pos_handle, cmd_handle, limits_bad, soft_limits), - joint_limits_interface::JointLimitsInterfaceException); - - // Print error messages. Requires manual output inspection, but exception message should be - // descriptive - try - { - joint_limits_interface::PositionJointSoftLimitsHandle( - pos_handle, cmd_handle, limits_bad, soft_limits); - } - catch (const joint_limits_interface::JointLimitsInterfaceException & e) - { - RCLCPP_ERROR(rclcpp::get_logger("joint_limits_interface_test"), "%s", e.what()); - } - } - - { - joint_limits_interface::JointLimits limits_bad; - limits_bad.has_effort_limits = true; - EXPECT_THROW( - joint_limits_interface::EffortJointSoftLimitsHandle( - pos_handle, cmd_handle, limits_bad, soft_limits), - joint_limits_interface::JointLimitsInterfaceException); - - // Print error messages. Requires manual output inspection, - // but exception message should be descriptive - try - { - joint_limits_interface::EffortJointSoftLimitsHandle( - pos_handle, cmd_handle, limits_bad, soft_limits); - } - catch (const joint_limits_interface::JointLimitsInterfaceException & e) - { - RCLCPP_ERROR(rclcpp::get_logger("joint_limits_interface_test"), "%s", e.what()); - } - } - - { - joint_limits_interface::JointLimits limits_bad; - limits_bad.has_velocity_limits = true; - EXPECT_THROW( - joint_limits_interface::EffortJointSoftLimitsHandle( - pos_handle, cmd_handle, limits_bad, soft_limits), - joint_limits_interface::JointLimitsInterfaceException); - - // Print error messages. Requires manual output inspection, but exception message should - // be descriptive - try - { - joint_limits_interface::EffortJointSoftLimitsHandle( - pos_handle, cmd_handle, limits_bad, soft_limits); - } - catch (const joint_limits_interface::JointLimitsInterfaceException & e) - { - RCLCPP_ERROR(rclcpp::get_logger("joint_limits_interface_test"), "%s", e.what()); - } - } - - { - joint_limits_interface::JointLimits limits_bad; - EXPECT_THROW( - joint_limits_interface::VelocityJointSaturationHandle(pos_handle, cmd_handle, limits_bad), - joint_limits_interface::JointLimitsInterfaceException); - - // Print error messages. Requires manual output inspection, but exception message should - // be descriptive - try - { - joint_limits_interface::VelocityJointSaturationHandle(pos_handle, cmd_handle, limits_bad); - } - catch (const joint_limits_interface::JointLimitsInterfaceException & e) - { - RCLCPP_ERROR(rclcpp::get_logger("joint_limits_interface_test"), "%s", e.what()); - } - } - - EXPECT_NO_THROW(joint_limits_interface::PositionJointSoftLimitsHandle( - pos_handle, cmd_handle, limits, soft_limits)); - EXPECT_NO_THROW(joint_limits_interface::EffortJointSoftLimitsHandle( - pos_handle, cmd_handle, limits, soft_limits)); - EXPECT_NO_THROW( - joint_limits_interface::VelocityJointSaturationHandle(pos_handle, cmd_handle, limits)); -} - -class PositionJointSoftLimitsHandleTest : public JointLimitsTest, public ::testing::Test -{ -}; - -TEST_F(PositionJointSoftLimitsHandleTest, EnforceVelocityBounds) -{ - // Test setup - const double max_increment = period.seconds() * limits.max_velocity; - pos = 0.0; - - double cmd; - - // Move slower than maximum velocity - { - joint_limits_interface::PositionJointSoftLimitsHandle limits_handle( - pos_handle, cmd_handle, limits, soft_limits); - cmd = max_increment / 2.0; - cmd_handle.set_value(cmd); - limits_handle.enforce_limits(period); - EXPECT_NEAR(cmd, cmd_handle.get_value(), EPS); - } - { - joint_limits_interface::PositionJointSoftLimitsHandle limits_handle( - pos_handle, cmd_handle, limits, soft_limits); - cmd = -max_increment / 2.0; - cmd_handle.set_value(cmd); - limits_handle.enforce_limits(period); - EXPECT_NEAR(cmd, cmd_handle.get_value(), EPS); - } - - // Move at maximum velocity - { - joint_limits_interface::PositionJointSoftLimitsHandle limits_handle( - pos_handle, cmd_handle, limits, soft_limits); - cmd = max_increment; - cmd_handle.set_value(cmd); - limits_handle.enforce_limits(period); - EXPECT_NEAR(cmd, cmd_handle.get_value(), EPS); - } - { - joint_limits_interface::PositionJointSoftLimitsHandle limits_handle( - pos_handle, cmd_handle, limits, soft_limits); - cmd = -max_increment; - cmd_handle.set_value(cmd); - limits_handle.enforce_limits(period); - EXPECT_NEAR(cmd, cmd_handle.get_value(), EPS); - } - - // Try to move faster than the maximum velocity, enforce velocity limits - { - joint_limits_interface::PositionJointSoftLimitsHandle limits_handle( - pos_handle, cmd_handle, limits, soft_limits); - cmd = 2.0 * max_increment; - cmd_handle.set_value(cmd); - limits_handle.enforce_limits(period); - EXPECT_NEAR(max_increment, cmd_handle.get_value(), EPS); - } - { - joint_limits_interface::PositionJointSoftLimitsHandle limits_handle( - pos_handle, cmd_handle, limits, soft_limits); - cmd = -2.0 * max_increment; - cmd_handle.set_value(cmd); - limits_handle.enforce_limits(period); - EXPECT_NEAR(-max_increment, cmd_handle.get_value(), EPS); - } -} - -// This is a black box test and does not verify against random precomputed values, but rather that -// the expected qualitative behavior is honored -TEST_F(PositionJointSoftLimitsHandleTest, EnforcePositionBounds) -{ - // Test setup - const double workspace_center = (limits.min_position + limits.max_position) / 2.0; - - // Current position == upper soft limit - { - joint_limits_interface::PositionJointSoftLimitsHandle limits_handle( - pos_handle, cmd_handle, limits, soft_limits); - - // Can't get any closer to hard limit (zero max velocity) - pos = soft_limits.max_position; - // Try to get closer to the hard limit - cmd_handle.set_value(limits.max_position); - limits_handle.enforce_limits(period); - EXPECT_NEAR(pos_handle.get_value(), cmd_handle.get_value(), EPS); - - // OK to move away from hard limit - // Try to go to workspace center - cmd_handle.set_value(workspace_center); - limits_handle.enforce_limits(period); - EXPECT_GT(pos_handle.get_value(), cmd_handle.get_value()); - } - - // Current position == lower soft limit - { - joint_limits_interface::PositionJointSoftLimitsHandle limits_handle( - pos_handle, cmd_handle, limits, soft_limits); - - // Can't get any closer to hard limit (zero min velocity) - pos = soft_limits.min_position; - // Try to get closer to the hard limit - cmd_handle.set_value(limits.min_position); - limits_handle.enforce_limits(period); - EXPECT_NEAR(pos_handle.get_value(), cmd_handle.get_value(), EPS); - - // OK to move away from hard limit - // Try to go to workspace center - cmd_handle.set_value(workspace_center); - limits_handle.enforce_limits(period); - EXPECT_LT(pos_handle.get_value(), cmd_handle.get_value()); - } - - // Current position > upper soft limit - { - joint_limits_interface::PositionJointSoftLimitsHandle limits_handle( - pos_handle, cmd_handle, limits, soft_limits); - - // Can't get any closer to hard limit (negative max velocity) - // Halfway between soft and hard limit - pos = (soft_limits.max_position + limits.max_position) / 2.0; - // Try to get closer to the hard limit - cmd_handle.set_value(limits.max_position); - limits_handle.enforce_limits(period); - EXPECT_GT(pos_handle.get_value(), cmd_handle.get_value()); - - // OK to move away from hard limit - // Try to go to workspace center - cmd_handle.set_value(workspace_center); - limits_handle.enforce_limits(period); - EXPECT_GT(pos_handle.get_value(), cmd_handle.get_value()); - } - - // Current position < lower soft limit - { - joint_limits_interface::PositionJointSoftLimitsHandle limits_handle( - pos_handle, cmd_handle, limits, soft_limits); - - // Can't get any closer to hard limit (positive min velocity) - // Halfway between soft and hard limit - pos = (soft_limits.min_position + limits.min_position) / 2.0; - // Try to get closer to the hard limit - cmd_handle.set_value(limits.min_position); - limits_handle.enforce_limits(period); - EXPECT_LT(pos_handle.get_value(), cmd_handle.get_value()); - - // OK to move away from hard limit - // Try to go to workspace center - cmd_handle.set_value(workspace_center); - limits_handle.enforce_limits(period); - EXPECT_LT(pos_handle.get_value(), cmd_handle.get_value()); - } -} - -TEST_F(PositionJointSoftLimitsHandleTest, PathologicalSoftBounds) -{ - // Safety limits are past the hard limits - soft_limits.min_position = - limits.min_position * (1.0 - 0.5 * limits.min_position / std::abs(limits.min_position)); - soft_limits.max_position = - limits.max_position * (1.0 + 0.5 * limits.max_position / std::abs(limits.max_position)); - - // Current position == higher hard limit - { - joint_limits_interface::PositionJointSoftLimitsHandle limits_handle( - pos_handle, cmd_handle, limits, soft_limits); - - // Hit hard limit - // On hard limit - pos = limits.max_position; - // Way beyond hard limit - cmd_handle.set_value(2.0 * limits.max_position); - limits_handle.enforce_limits(period); - EXPECT_NEAR(limits.max_position, cmd_handle.get_value(), EPS); - } - - // Current position == lower hard limit - { - joint_limits_interface::PositionJointSoftLimitsHandle limits_handle( - pos_handle, cmd_handle, limits, soft_limits); - - // Hit hard limit - // On hard limit - pos = limits.min_position; - // Way beyond hard limit - cmd_handle.set_value(2.0 * limits.min_position); - limits_handle.enforce_limits(period); - EXPECT_NEAR(limits.min_position, cmd_handle.get_value(), EPS); - } -} - -class VelocityJointSaturationHandleTest : public JointLimitsTest, public ::testing::Test -{ -}; - -TEST_F(VelocityJointSaturationHandleTest, EnforceVelocityBounds) -{ - // Test setup - joint_limits_interface::VelocityJointSaturationHandle limits_handle( - pos_handle, cmd_handle, limits); - - pos = 0.0; - double cmd; - - // Velocity within bounds - cmd = limits.max_velocity / 2.0; - cmd_handle.set_value(cmd); - limits_handle.enforce_limits(period); - EXPECT_NEAR(cmd, cmd_handle.get_value(), EPS); - - cmd = -limits.max_velocity / 2.0; - cmd_handle.set_value(cmd); - limits_handle.enforce_limits(period); - EXPECT_NEAR(cmd, cmd_handle.get_value(), EPS); - - // Velocity at bounds - cmd = limits.max_velocity; - cmd_handle.set_value(cmd); - limits_handle.enforce_limits(period); - EXPECT_NEAR(cmd, cmd_handle.get_value(), EPS); - - cmd = -limits.max_velocity; - cmd_handle.set_value(cmd); - limits_handle.enforce_limits(period); - EXPECT_NEAR(cmd, cmd_handle.get_value(), EPS); - - // Velocity beyond bounds - cmd = 2.0 * limits.max_velocity; - cmd_handle.set_value(cmd); - limits_handle.enforce_limits(period); - EXPECT_NEAR(limits.max_velocity, cmd_handle.get_value(), EPS); - - cmd = -2.0 * limits.max_velocity; - cmd_handle.set_value(cmd); - limits_handle.enforce_limits(period); - EXPECT_NEAR(-limits.max_velocity, cmd_handle.get_value(), EPS); -} - -TEST_F(VelocityJointSaturationHandleTest, EnforceAccelerationBounds) -{ - // Test setup - limits.has_acceleration_limits = true; - limits.max_acceleration = limits.max_velocity / period.seconds(); - joint_limits_interface::VelocityJointSaturationHandle limits_handle( - pos_handle, cmd_handle, limits); - - pos = 0.0; - double cmd; - // An arbitrarily long time, sufficient to suppress acceleration limits - const rclcpp::Duration long_enough(1000, 0); - - // Positive velocity - // register last command - cmd_handle.set_value(limits.max_velocity / 2.0); - // make sure the prev_cmd is registered - // without triggering the acceleration limits - limits_handle.enforce_limits(long_enough); - - // Try to go beyond +max velocity - cmd = limits.max_velocity * 2.0; - cmd_handle.set_value(cmd); - limits_handle.enforce_limits(period); - // Max velocity bounded by velocity limit - EXPECT_NEAR(limits.max_velocity, cmd_handle.get_value(), EPS); - - // register last command - cmd_handle.set_value(limits.max_velocity / 2.0); - // make sure the prev_cmd is registered - // without triggering the acceleration limits - limits_handle.enforce_limits(long_enough); - - // Try to go beyond -max velocity - cmd = -limits.max_velocity * 2.0; - cmd_handle.set_value(cmd); - limits_handle.enforce_limits(period); - // Max velocity bounded by acceleration limit - EXPECT_NEAR(-limits.max_velocity / 2.0, cmd_handle.get_value(), EPS); - - // Negative velocity - // register last command - cmd_handle.set_value(-limits.max_velocity / 2.0); - // make sure the prev_cmd is registered - // without triggering the acceleration limits - limits_handle.enforce_limits(long_enough); - - // Try to go beyond +max velocity - cmd = limits.max_velocity * 2.0; - cmd_handle.set_value(cmd); - limits_handle.enforce_limits(period); - // Max velocity bounded by acceleration limit - EXPECT_NEAR(limits.max_velocity / 2.0, cmd_handle.get_value(), EPS); - - // register last command - cmd_handle.set_value(-limits.max_velocity / 2.0); - // make sure the prev_cmd is registered - // without triggering the acceleration limits - limits_handle.enforce_limits(long_enough); - - // Try to go beyond -max velocity - cmd = -limits.max_velocity * 2.0; - cmd_handle.set_value(cmd); - limits_handle.enforce_limits(period); - // Max velocity bounded by velocity limit - EXPECT_NEAR(-limits.max_velocity, cmd_handle.get_value(), EPS); -} - -class JointLimitsInterfaceTest : public JointLimitsTest, public ::testing::Test -{ -public: - JointLimitsInterfaceTest() - : JointLimitsTest(), - pos2(0.0), - vel2(0.0), - eff2(0.0), - cmd2(0.0), - name2("joint2_name"), - cmd2_handle( - std::make_shared(name2, "position_command", &cmd2)), - pos2_handle(std::make_shared( - name2, hardware_interface::HW_IF_POSITION, &pos2)), - vel2_handle(std::make_shared( - name2, hardware_interface::HW_IF_VELOCITY, &vel2)), - eff2_handle(std::make_shared( - name2, hardware_interface::HW_IF_EFFORT, &eff2)) - { - } - -protected: - double pos2, vel2, eff2, cmd2; - std::string name2; - std::shared_ptr cmd2_handle; - std::shared_ptr pos2_handle, vel2_handle, eff2_handle; -}; - -// TEST_F(JointLimitsInterfaceTest, InterfaceRegistration) -// { -// // Populate interface -// joint_limits_interface::PositionJointSoftLimitsHandle limits_handle1( -// pos_handle, cmd_handle, limits, soft_limits); -// joint_limits_interface::PositionJointSoftLimitsHandle limits_handle2( -// pos_handle2, cmd_handle2, limits, soft_limits); -// -// joint_limits_interface::PositionJointSoftLimitsInterface iface; -// iface.registerHandle(limits_handle1); -// iface.registerHandle(limits_handle2); -// -// // Get handles -// EXPECT_NO_THROW(iface.getHandle(name)); -// EXPECT_NO_THROW(iface.getHandle(name2)); -// -// PositionJointSoftLimitsHandle h1_tmp = iface.getHandle(name); -// EXPECT_EQ(name, h1_tmp.getName()); -// -// PositionJointSoftLimitsHandle h2_tmp = iface.getHandle(name2); -// EXPECT_EQ(name2, h2_tmp.getName()); -// -// // Print error message -// // Requires manual output inspection, but exception message should contain the interface name -// // (not its base class) -// try { -// iface.getHandle("unknown_name"); -// } catch (const JointLimitsInterfaceException & e) { -// ROS_ERROR("%s", e.what()); -// } -// -// // Enforce limits of all managed joints -// // Halfway between soft and hard limit -// pos = pos2 = (soft_limits.max_position + limits.max_position) / 2.0; -// // Try to get closer to the hard limit -// cmd_handle.set_value(limits.max_position); -// cmd_handle2.set_cmd(limits.max_position); -// iface.enforce_limits(period); -// EXPECT_GT(pos_handle.get_value(), cmd_handle.get_value()); -// EXPECT_GT(cmd_handle2.getPosition(), cmd_handle2.get_cmd()); -// } -// -#if 0 // todo: implement the interfaces -TEST_F(JointLimitsHandleTest, ResetSaturationInterface) -{ - // Populate interface - joint_limits_interface::PositionJointSaturationHandle limits_handle1 - (pos_handle, cmd_handle, limits); - - PositionJointSaturationInterface iface; - iface.registerHandle(limits_handle1); - - iface.enforce_limits(period); // initialize limit handles - - const double max_increment = period.seconds() * limits.max_velocity; - - cmd_handle.set_value(limits.max_position); - iface.enforce_limits(period); - - EXPECT_NEAR(cmd_handle.get_value(), max_increment, EPS); - - iface.reset(); - pos = limits.max_position; - cmd_handle.set_value(limits.max_position); - iface.enforce_limits(period); - - EXPECT_NEAR(cmd_handle.get_value(), limits.max_position, EPS); -} -#endif - -// TEST_F(JointLimitsHandleTest, ResetSoftLimitsInterface) -// { -// // Populate interface -// PositionJointSoftLimitsHandle limits_handle1(cmd_handle, limits, soft_limits); -// -// PositionJointSoftLimitsInterface iface; -// iface.registerHandle(limits_handle1); -// -// iface.enforce_limits(period); // initialize limit handles -// -// const double max_increment = period.seconds() * limits.max_velocity; -// -// cmd_handle.set_value(limits.max_position); -// iface.enforce_limits(period); -// -// EXPECT_NEAR(cmd_handle.get_value(), max_increment, EPS); -// -// iface.reset(); -// pos = limits.max_position; -// cmd_handle.set_value(soft_limits.max_position); -// iface.enforce_limits(period); -// -// EXPECT_NEAR(cmd_handle.get_value(), soft_limits.max_position, EPS); -// -// } - -int main(int argc, char ** argv) -{ - testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); -} diff --git a/joint_limits_interface/test/joint_limits_rosparam_test.cpp b/joint_limits_interface/test/joint_limits_rosparam_test.cpp deleted file mode 100644 index 066f98ccf8a..00000000000 --- a/joint_limits_interface/test/joint_limits_rosparam_test.cpp +++ /dev/null @@ -1,200 +0,0 @@ -// Copyright 2020 PAL Robotics S.L. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -/// \author Adolfo Rodriguez Tsouroukdissian - -#include - -#include - -#include - -#include - -class JointLimitsRosParamTest : public ::testing::Test -{ -protected: - void SetUp() - { - rclcpp::NodeOptions node_options; - node_options.allow_undeclared_parameters(true).automatically_declare_parameters_from_overrides( - true); - - node_ = rclcpp::Node::make_shared("JointLimitsRosparamTestNode", node_options); - } - - rclcpp::Node::SharedPtr node_; -}; - -TEST_F(JointLimitsRosParamTest, GetJointLimits) -{ - // Invalid specification - { - joint_limits_interface::JointLimits limits; - EXPECT_FALSE(getJointLimits("bad_joint", node_, limits)); - EXPECT_FALSE(getJointLimits("unknown_joint", node_, limits)); - - EXPECT_FALSE(limits.has_position_limits); - EXPECT_FALSE(limits.has_velocity_limits); - EXPECT_FALSE(limits.has_acceleration_limits); - EXPECT_FALSE(limits.has_jerk_limits); - EXPECT_FALSE(limits.has_effort_limits); - } - - // Get full specification from parameter server - { - joint_limits_interface::JointLimits limits; - EXPECT_TRUE(getJointLimits("foo_joint", node_, limits)); - - EXPECT_TRUE(limits.has_position_limits); - EXPECT_EQ(0.0, limits.min_position); - EXPECT_EQ(1.0, limits.max_position); - - EXPECT_TRUE(limits.has_velocity_limits); - EXPECT_EQ(2.0, limits.max_velocity); - - EXPECT_TRUE(limits.has_acceleration_limits); - EXPECT_EQ(5.0, limits.max_acceleration); - - EXPECT_TRUE(limits.has_jerk_limits); - EXPECT_EQ(100.0, limits.max_jerk); - - EXPECT_TRUE(limits.has_effort_limits); - EXPECT_EQ(20.0, limits.max_effort); - - EXPECT_FALSE(limits.angle_wraparound); - } - - // Specifying flags but not values should set nothing - { - joint_limits_interface::JointLimits limits; - EXPECT_TRUE(getJointLimits("yinfoo_joint", node_, limits)); - - EXPECT_FALSE(limits.has_position_limits); - EXPECT_FALSE(limits.has_velocity_limits); - EXPECT_FALSE(limits.has_acceleration_limits); - EXPECT_FALSE(limits.has_jerk_limits); - EXPECT_FALSE(limits.has_effort_limits); - } - - // Specifying values but not flags should set nothing - { - joint_limits_interface::JointLimits limits; - EXPECT_TRUE(getJointLimits("yangfoo_joint", node_, limits)); - - EXPECT_FALSE(limits.has_position_limits); - EXPECT_FALSE(limits.has_velocity_limits); - EXPECT_FALSE(limits.has_acceleration_limits); - EXPECT_FALSE(limits.has_jerk_limits); - EXPECT_FALSE(limits.has_effort_limits); - } - - // Disable already set values - { - joint_limits_interface::JointLimits limits; - EXPECT_TRUE(getJointLimits("foo_joint", node_, limits)); - EXPECT_TRUE(limits.has_position_limits); - EXPECT_TRUE(limits.has_velocity_limits); - EXPECT_TRUE(limits.has_acceleration_limits); - EXPECT_TRUE(limits.has_jerk_limits); - EXPECT_TRUE(limits.has_effort_limits); - - EXPECT_TRUE(getJointLimits("antifoo_joint", node_, limits)); - EXPECT_FALSE(limits.has_position_limits); - EXPECT_FALSE(limits.has_velocity_limits); - EXPECT_FALSE(limits.has_acceleration_limits); - EXPECT_FALSE(limits.has_jerk_limits); - EXPECT_FALSE(limits.has_effort_limits); - EXPECT_TRUE(limits.angle_wraparound); - } - - // Incomplete position limits specification does not get loaded - { - joint_limits_interface::JointLimits limits; - EXPECT_TRUE(getJointLimits("baz_joint", node_, limits)); - - EXPECT_FALSE(limits.has_position_limits); - } - - // Override only one field, leave all others unchanged - { - joint_limits_interface::JointLimits limits, limits_ref; - EXPECT_TRUE(getJointLimits("bar_joint", node_, limits)); - - EXPECT_EQ(limits_ref.has_position_limits, limits.has_position_limits); - EXPECT_EQ(limits_ref.min_position, limits.min_position); - EXPECT_EQ(limits_ref.max_position, limits.max_position); - - // Max velocity is overridden - EXPECT_NE(limits_ref.has_velocity_limits, limits.has_velocity_limits); - EXPECT_NE(limits_ref.max_velocity, limits.max_velocity); - EXPECT_TRUE(limits.has_velocity_limits); - EXPECT_EQ(2.0, limits.max_velocity); - - EXPECT_EQ(limits_ref.has_acceleration_limits, limits.has_acceleration_limits); - EXPECT_EQ(limits_ref.max_acceleration, limits.max_acceleration); - - EXPECT_EQ(limits_ref.has_jerk_limits, limits.has_jerk_limits); - EXPECT_EQ(limits_ref.has_jerk_limits, limits.max_jerk); - - EXPECT_EQ(limits_ref.has_effort_limits, limits.has_effort_limits); - EXPECT_EQ(limits_ref.max_effort, limits.max_effort); - } -} - -TEST_F(JointLimitsRosParamTest, GetSoftJointLimits) -{ - // Invalid specification - { - joint_limits_interface::SoftJointLimits soft_limits; - EXPECT_FALSE(getSoftJointLimits("bad_joint", node_, soft_limits)); - EXPECT_FALSE(getSoftJointLimits("unknown_joint", node_, soft_limits)); - } - - // Get full specification from parameter server - { - joint_limits_interface::SoftJointLimits soft_limits; - EXPECT_TRUE(getSoftJointLimits("foo_joint", node_, soft_limits)); - - EXPECT_EQ(10.0, soft_limits.k_position); - EXPECT_EQ(20.0, soft_limits.k_velocity); - EXPECT_EQ(0.1, soft_limits.min_position); - EXPECT_EQ(0.9, soft_limits.max_position); - } - - // Skip parsing soft limits if has_soft_limits is false - { - joint_limits_interface::SoftJointLimits soft_limits, soft_limits_ref; - EXPECT_FALSE(getSoftJointLimits("foobar_joint", node_, soft_limits)); - } - - // Incomplete soft limits specification does not get loaded - { - joint_limits_interface::SoftJointLimits soft_limits, soft_limits_ref; - EXPECT_FALSE(getSoftJointLimits("barbaz_joint", node_, soft_limits)); - EXPECT_EQ(soft_limits.k_position, soft_limits_ref.k_position); - EXPECT_EQ(soft_limits.k_velocity, soft_limits_ref.k_velocity); - EXPECT_EQ(soft_limits.min_position, soft_limits_ref.min_position); - EXPECT_EQ(soft_limits.max_position, soft_limits_ref.max_position); - } -} - -int main(int argc, char ** argv) -{ - rclcpp::init(argc, argv); - testing::InitGoogleTest(&argc, argv); - int ret = RUN_ALL_TESTS(); - rclcpp::shutdown(); - return ret; -} diff --git a/ros2_control/package.xml b/ros2_control/package.xml index bb3078e65aa..57e0d85277e 100644 --- a/ros2_control/package.xml +++ b/ros2_control/package.xml @@ -14,6 +14,7 @@ controller_manager controller_manager_msgs hardware_interface + joint_limits ros2_control_test_assets ros2controlcli transmission_interface From abd5d6e6c8264706b58dddc877063f884cf34a58 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Fri, 8 Jul 2022 08:52:18 +0200 Subject: [PATCH 21/35] Deprecate and rename `start` and `stop` nomenclature toward user to `activate` and `deactivate` #ABI-breaking (#755) * Rename fileds and deprecated old nomenclature. * Remove new defines from SwitchController.srv * Deprecated start/stop nomenclature in all CLI commands. * Deprecate 'start_asap' too as other fields. --- .../controller_manager_services.py | 10 +-- .../controller_manager/launch_utils.py | 2 +- .../controller_manager/spawner.py | 24 ++++-- .../controller_manager/unspawner.py | 2 +- controller_manager/src/controller_manager.cpp | 51 ++++++++++-- .../srv/SwitchController.srv | 25 +++--- ros2controlcli/doc/userdoc.rst | 20 ++--- .../ros2controlcli/verb/load_controller.py | 9 ++- .../verb/set_controller_state.py | 81 ++++++++++++------- .../ros2controlcli/verb/switch_controllers.py | 33 +++++++- 10 files changed, 179 insertions(+), 78 deletions(-) diff --git a/controller_manager/controller_manager/controller_manager_services.py b/controller_manager/controller_manager/controller_manager_services.py index 8d89595d192..08f68fa663e 100644 --- a/controller_manager/controller_manager/controller_manager_services.py +++ b/controller_manager/controller_manager/controller_manager_services.py @@ -78,16 +78,16 @@ def reload_controller_libraries(node, controller_manager_name, force_kill): ReloadControllerLibraries, request) -def switch_controllers(node, controller_manager_name, stop_controllers, - start_controllers, strict, start_asap, timeout): +def switch_controllers(node, controller_manager_name, deactivate_controllers, + activate_controllers, strict, activate_asap, timeout): request = SwitchController.Request() - request.start_controllers = start_controllers - request.stop_controllers = stop_controllers + request.activate_controllers = activate_controllers + request.deactivate_controllers = deactivate_controllers if strict: request.strictness = SwitchController.Request.STRICT else: request.strictness = SwitchController.Request.BEST_EFFORT - request.start_asap = start_asap + request.activate_asap = activate_asap request.timeout = rclpy.duration.Duration(seconds=timeout).to_msg() return service_caller(node, f'{controller_manager_name}/switch_controller', SwitchController, request) diff --git a/controller_manager/controller_manager/launch_utils.py b/controller_manager/controller_manager/launch_utils.py index 0092675e7e9..d3f9356eae6 100644 --- a/controller_manager/controller_manager/launch_utils.py +++ b/controller_manager/controller_manager/launch_utils.py @@ -27,7 +27,7 @@ def generate_load_controller_launch_description(controller_name, Returns a list of LaunchDescription actions adding the 'controller_manager_name' and 'unload_on_kill' LaunchArguments and a Node action that runs the controller_manager - spawner node to load and start a controller + spawner node to load and activate a controller Examples # noqa: D416 -------- diff --git a/controller_manager/controller_manager/spawner.py b/controller_manager/controller_manager/spawner.py index 073f44638a8..5c478fc62c0 100644 --- a/controller_manager/controller_manager/spawner.py +++ b/controller_manager/controller_manager/spawner.py @@ -133,7 +133,10 @@ def main(args=None): '--load-only', help='Only load the controller and leave unconfigured.', action='store_true', required=False) parser.add_argument( - '--stopped', help='Load and configure the controller, however do not start them', + '--stopped', help='Load and configure the controller, however do not activate them', + action='store_true', required=False) + parser.add_argument( + '--inactive', help='Load and configure the controller, however do not activate them', action='store_true', required=False) parser.add_argument( '-t', '--controller-type', @@ -193,7 +196,7 @@ def main(args=None): node.get_logger().info('Failed to configure controller') return 1 - if not args.stopped: + if not args.stopped and not args.inactive: ret = switch_controllers( node, controller_manager_name, @@ -203,11 +206,13 @@ def main(args=None): True, 5.0) if not ret.ok: - node.get_logger().info('Failed to start controller') + node.get_logger().info('Failed to activate controller') return 1 - node.get_logger().info(bcolors.OKGREEN + 'Configured and started ' + + node.get_logger().info(bcolors.OKGREEN + 'Configured and activated ' + bcolors.OKCYAN + controller_name + bcolors.ENDC) + elif args.stopped: + node.get_logger.warn('"--stopped" flag is deprecated use "--inactive" instead') if not args.unload_on_kill: return 0 @@ -217,8 +222,8 @@ def main(args=None): while True: time.sleep(1) except KeyboardInterrupt: - if not args.stopped: - node.get_logger().info('Interrupt captured, stopping and unloading controller') + if not args.stopped and not args.inactive: + node.get_logger().info('Interrupt captured, deactivating and unloading controller') ret = switch_controllers( node, controller_manager_name, @@ -228,10 +233,13 @@ def main(args=None): True, 5.0) if not ret.ok: - node.get_logger().info('Failed to stop controller') + node.get_logger().info('Failed to deactivate controller') return 1 - node.get_logger().info('Stopped controller') + node.get_logger().info('Deactivated controller') + + elif args.stopped: + node.get_logger.warn('"--stopped" flag is deprecated use "--inactive" instead') ret = unload_controller( node, controller_manager_name, controller_name) diff --git a/controller_manager/controller_manager/unspawner.py b/controller_manager/controller_manager/unspawner.py index d2c44820100..4020e2ad06e 100644 --- a/controller_manager/controller_manager/unspawner.py +++ b/controller_manager/controller_manager/unspawner.py @@ -50,7 +50,7 @@ def main(args=None): True, True, 5.0) - node.get_logger().info('Stopped controller') + node.get_logger().info('Deactivated controller') ret = unload_controller(node, controller_manager_name, controller_name) if not ret.ok: diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 72239f18e1f..9e560bc9a34 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -639,9 +639,9 @@ controller_interface::return_type ControllerManager::switch_controller( { RCLCPP_WARN( get_logger(), - "Could not activate controller with name '%s'. (Check above warnings for more details.) " + "Could not activate controller with name '%s'. Check above warnings for more details. " "Check the state of the controllers and their required interfaces using " - "`ros2 control list_controllers -v` to get more information.", + "`ros2 control list_controllers -v` CLI to get more information.", (*ctrl_it).c_str()); if (strictness == controller_manager_msgs::srv::SwitchController::Request::BEST_EFFORT) { @@ -675,7 +675,7 @@ controller_interface::return_type ControllerManager::switch_controller( if (!is_controller_active(controller_it->c)) { RCLCPP_WARN( - get_logger(), "Controller with name '%s' can not be deactivated since is not active.", + get_logger(), "Controller with name '%s' can not be deactivated since it is not active.", controller_it->info.name.c_str()); ret = controller_interface::return_type::ERROR; } @@ -688,9 +688,9 @@ controller_interface::return_type ControllerManager::switch_controller( { RCLCPP_WARN( get_logger(), - "Could not deactivate controller with name '%s'. (Check above warnings for more details.)" + "Could not deactivate controller with name '%s'. Check above warnings for more details. " "Check the state of the controllers and their required interfaces using " - "`ros2 control list_controllers -v` to get more information.", + "`ros2 control list_controllers -v` CLI to get more information.", (*ctrl_it).c_str()); if (strictness == controller_manager_msgs::srv::SwitchController::Request::BEST_EFFORT) { @@ -1404,9 +1404,46 @@ void ControllerManager::switch_controller_service_cb( std::lock_guard guard(services_lock_); RCLCPP_DEBUG(get_logger(), "switching service locked"); + // response->ok = switch_controller( + // request->activate_controllers, request->deactivate_controllers, request->strictness, + // request->activate_asap, request->timeout) == controller_interface::return_type::OK; + // TODO(destogl): remove this after deprecated fields are removed from service and use the + // commented three lines above + // BEGIN: remove when deprecated removed + auto activate_controllers = request->activate_controllers; + auto deactivate_controllers = request->deactivate_controllers; + + if (!request->start_controllers.empty()) + { + RCLCPP_WARN( + get_logger(), + "'start_controllers' field is deprecated, use 'activate_controllers' field instead!"); + activate_controllers.insert( + activate_controllers.end(), request->start_controllers.begin(), + request->start_controllers.end()); + } + if (!request->stop_controllers.empty()) + { + RCLCPP_WARN( + get_logger(), + "'stop_controllers' field is deprecated, use 'deactivate_controllers' field instead!"); + deactivate_controllers.insert( + deactivate_controllers.end(), request->stop_controllers.begin(), + request->stop_controllers.end()); + } + + auto activate_asap = request->activate_asap; + if (request->start_asap) + { + RCLCPP_WARN( + get_logger(), "'start_asap' field is deprecated, use 'activate_asap' field instead!"); + activate_asap = request->start_asap; + } + response->ok = switch_controller( - request->start_controllers, request->stop_controllers, request->strictness, - request->start_asap, request->timeout) == controller_interface::return_type::OK; + activate_controllers, deactivate_controllers, request->strictness, activate_asap, + request->timeout) == controller_interface::return_type::OK; + // END: remove when deprecated removed RCLCPP_DEBUG(get_logger(), "switching service finished"); } diff --git a/controller_manager_msgs/srv/SwitchController.srv b/controller_manager_msgs/srv/SwitchController.srv index 3c68b0e7469..29cbb7c1bd4 100644 --- a/controller_manager_msgs/srv/SwitchController.srv +++ b/controller_manager_msgs/srv/SwitchController.srv @@ -1,16 +1,16 @@ -# The SwitchController service allows you stop a number of controllers -# and start a number of controllers, all in one single timestep of the -# controller_manager control loop. +# The SwitchController service allows you deactivate a number of controllers +# and activate a number of controllers, all in one single timestep of the +# controller manager's control loop. # To switch controllers, specify -# * the list of controller names to start, -# * the list of controller names to stop, and +# * the list of controller names to activate, +# * the list of controller names to deactivate, and # * the strictness (BEST_EFFORT or STRICT) # * STRICT means that switching will fail if anything goes wrong (an invalid -# controller name, a controller that failed to start, etc. ) +# controller name, a controller that failed to activate, etc. ) # * BEST_EFFORT means that even when something goes wrong with on controller, -# the service will still try to start/stop the remaining controllers -# * start the controllers as soon as their hardware dependencies are ready, will +# the service will still try to activate/stop the remaining controllers +# * activate the controllers as soon as their hardware dependencies are ready, will # wait for all interfaces to be ready otherwise # * the timeout before aborting pending controllers. Zero for infinite @@ -19,12 +19,15 @@ # specified strictness. -string[] start_controllers -string[] stop_controllers +string[] activate_controllers +string[] deactivate_controllers +string[] start_controllers # DEPRECATED: Use activate_controllers filed instead +string[] stop_controllers # DEPRECATED: Use deactivate_controllers filed instead int32 strictness int32 BEST_EFFORT=1 int32 STRICT=2 -bool start_asap +bool start_asap # DEPRECATED: Use activate_asap filed instead +bool activate_asap builtin_interfaces/Duration timeout --- bool ok diff --git a/ros2controlcli/doc/userdoc.rst b/ros2controlcli/doc/userdoc.rst index f3fc80190cc..9e13c9246a5 100644 --- a/ros2controlcli/doc/userdoc.rst +++ b/ros2controlcli/doc/userdoc.rst @@ -109,7 +109,7 @@ load_controller .. code-block:: console $ ros2 control load_controller -h - usage: ros2 control load_controller [-h] [--spin-time SPIN_TIME] [--set_state {configure,start}] [-c CONTROLLER_MANAGER] [--include-hidden-nodes] controller_name + usage: ros2 control load_controller [-h] [--spin-time SPIN_TIME] [--set_state {configure,activate}] [-c CONTROLLER_MANAGER] [--include-hidden-nodes] controller_name Load a controller in a controller manager @@ -120,7 +120,7 @@ load_controller -h, --help show this help message and exit --spin-time SPIN_TIME Spin time in seconds to wait for discovery (only applies when not using an already running daemon) - --set_state {configure,start} + --set_state {configured,active} Set the state of the loaded controller -c CONTROLLER_MANAGER, --controller-manager CONTROLLER_MANAGER Name of the controller manager ROS node @@ -153,13 +153,13 @@ set_controller_state .. code-block:: console $ ros2 control set_controller_state -h - usage: ros2 control set_controller_state [-h] [--spin-time SPIN_TIME] [-c CONTROLLER_MANAGER] [--include-hidden-nodes] controller_name {configure,start,stop} + usage: ros2 control set_controller_state [-h] [--spin-time SPIN_TIME] [-c CONTROLLER_MANAGER] [--include-hidden-nodes] controller_name {inactive,active} Adjust the state of the controller positional arguments: controller_name Name of the controller to be changed - {configure,start,stop} + {inactive,active} State in which the controller should be changed to optional arguments: @@ -177,7 +177,7 @@ switch_controllers .. code-block:: console $ ros2 control switch_controllers -h - usage: ros2 control switch_controllers [-h] [--spin-time SPIN_TIME] [--stop [STOP [STOP ...]]] [--start [START [START ...]]] [--strict] [--start-asap] [--switch-timeout SWITCH_TIMEOUT] [-c CONTROLLER_MANAGER] + usage: ros2 control switch_controllers [-h] [--spin-time SPIN_TIME] [--deactivate [CTRL1 [CTRL2 ...]]] [--activate [CTRL1 [CTRL2 ...]]] [--strict] [--activate-asap] [--switch-timeout SWITCH_TIMEOUT] [-c CONTROLLER_MANAGER] [--include-hidden-nodes] Switch controllers in a controller manager @@ -186,12 +186,12 @@ switch_controllers -h, --help show this help message and exit --spin-time SPIN_TIME Spin time in seconds to wait for discovery (only applies when not using an already running daemon) - --stop [STOP [STOP ...]] - Name of the controllers to be stopped - --start [START [START ...]] - Name of the controllers to be started + --deactivate [CTRL1 [CTRL2 ...]] + Name of the controllers to be deactivate + --activate [CTRL1 [CTRL2 ...]] + Name of the controllers to be activated --strict Strict switch - --start-asap Start asap controllers + --activate-asap Activate asap controllers --switch-timeout SWITCH_TIMEOUT Timeout for switching controllers -c CONTROLLER_MANAGER, --controller-manager CONTROLLER_MANAGER diff --git a/ros2controlcli/ros2controlcli/verb/load_controller.py b/ros2controlcli/ros2controlcli/verb/load_controller.py index 8354837bb77..e8fc47b903b 100644 --- a/ros2controlcli/ros2controlcli/verb/load_controller.py +++ b/ros2controlcli/ros2controlcli/verb/load_controller.py @@ -30,7 +30,7 @@ def add_arguments(self, parser, cli_name): arg.completer = ControllerNameCompleter() arg = parser.add_argument( '--set-state', - choices=['configure', 'start'], + choices=['configured', 'active'], help='Set the state of the loaded controller', ) add_controller_mgr_parsers(parser) @@ -50,12 +50,17 @@ def main(self, *, args): if not response.ok: return 'Error configuring controller' + # TODO(destogl): remove in humble+ if args.set_state == 'start': + print('Setting state "start" is deprecated "activate" instead!') + args.set_state == 'activate' + + if args.set_state == 'active': response = switch_controllers( node, args.controller_manager, [], [args.controller_name], True, True, 5.0 ) if not response.ok: - return 'Error starting controller, check controller_manager logs' + return 'Error activating controller, check controller_manager logs' print(f'Sucessfully loaded controller {args.controller_name} into ' f'state { "inactive" if args.set_state == "configure" else "active" }') diff --git a/ros2controlcli/ros2controlcli/verb/set_controller_state.py b/ros2controlcli/ros2controlcli/verb/set_controller_state.py index 8cf8932ab27..4ce6cdf7c7e 100644 --- a/ros2controlcli/ros2controlcli/verb/set_controller_state.py +++ b/ros2controlcli/ros2controlcli/verb/set_controller_state.py @@ -30,7 +30,8 @@ def add_arguments(self, parser, cli_name): arg.completer = LoadedControllerNameCompleter() arg = parser.add_argument( 'state', - choices=['configure', 'start', 'stop'], + # choices=['unconfigured', 'inactive', 'active'], TODO(destogl): when cleanup is impl + choices=['inactive', 'active'], help='State in which the controller should be changed to', ) add_controller_mgr_parsers(parser) @@ -44,43 +45,65 @@ def main(self, *, args): except IndexError: return f'controller {args.controller_name} does not seem to be loaded' - if args.state == 'configure': - if matched_controller.state != 'unconfigured': - return f'cannot configure {matched_controller.name} ' \ - f'from its current state {matched_controller.state}' + # TODO(destogl): This has to be implemented in CLI and controller manager + # if args.state == 'unconfigured': + # if matched_controller.state != 'inactive': + # return f'cannot cleanup {matched_controller.name} ' \ + # f'from its current state {matched_controller.state}' + # response = cleanup_controller( + # node, args.controller_manager, args.controller_name + # ) + # if not response.ok: + # return 'Error cleaning up controller, check controller_manager logs' + # # print(f'successfully cleaned up {args.controller_name}') + # return 0 - response = configure_controller( - node, args.controller_manager, args.controller_name - ) - if not response.ok: - return 'Error configuring controller, check controller_manager logs' + if args.state == 'configure': + args.state = 'inactive' + print('Setting state "configure" is deprecated, use "inactive" instead!') - print(f'successfully configured {args.controller_name}') - return 0 + if args.state == 'stop': + args.state = 'inactive' + print('Setting state "stop" is deprecated, use "inactive" instead!') + + if args.state == 'inactive': + if matched_controller.state == 'unconfigured': + response = configure_controller( + node, args.controller_manager, args.controller_name + ) + if not response.ok: + return 'Error configuring controller, check controller_manager logs' + + print(f'successfully configured {args.controller_name}') + return 0 + + elif matched_controller.state == 'active': + response = switch_controllers( + node, args.controller_manager, [args.controller_name], [], True, True, 5.0 + ) + if not response.ok: + return 'Error stopping controller, check controller_manager logs' + + print(f'successfully deactivated {args.controller_name}') + return 0 + + else: + return f'cannot put {matched_controller.name} in "inactive" state' \ + f'from its current state {matched_controller.state}' if args.state == 'start': + args.state = 'active' + print('Setting state "start" is deprecated, use "active" instead!') + + if args.state == 'active': if matched_controller.state != 'inactive': - return f'cannot start {matched_controller.name} ' \ + return f'cannot activate {matched_controller.name} ' \ f'from its current state {matched_controller.state}' response = switch_controllers( node, args.controller_manager, [], [args.controller_name], True, True, 5.0 ) if not response.ok: - return 'Error starting controller, check controller_manager logs' - - print(f'successfully started {args.controller_name}') - return 0 - - if args.state == 'stop': - if matched_controller.state != 'active': - return f'cannot stop {matched_controller.name} ' \ - f'from its current state {matched_controller.state}' - - response = switch_controllers( - node, args.controller_manager, [args.controller_name], [], True, True, 5.0 - ) - if not response.ok: - return 'Error stopping controller, check controller_manager logs' + return 'Error activating controller, check controller_manager logs' - print(f'successfully stopped {args.controller_name}') + print(f'successfully activated {args.controller_name}') return 0 diff --git a/ros2controlcli/ros2controlcli/verb/switch_controllers.py b/ros2controlcli/ros2controlcli/verb/switch_controllers.py index 558613bc79a..c6436635bc3 100644 --- a/ros2controlcli/ros2controlcli/verb/switch_controllers.py +++ b/ros2controlcli/ros2controlcli/verb/switch_controllers.py @@ -30,18 +30,33 @@ def add_arguments(self, parser, cli_name): '--stop', nargs='*', default=[], - help='Name of the controllers to be stopped', + help='Name of the controllers to be deactivated', + ) + arg.completer = LoadedControllerNameCompleter(['active']) + arg = parser.add_argument( + '--deactivate', + nargs='*', + default=[], + help='Name of the controllers to be deactivated', ) arg.completer = LoadedControllerNameCompleter(['active']) arg = parser.add_argument( '--start', nargs='*', default=[], - help='Name of the controllers to be started', + help='Name of the controllers to be activated', + ) + arg.completer = LoadedControllerNameCompleter(['inactive']) + arg = parser.add_argument( + '--activate', + nargs='*', + default=[], + help='Name of the controllers to be activated', ) arg.completer = LoadedControllerNameCompleter(['inactive']) parser.add_argument('--strict', action='store_true', help='Strict switch') parser.add_argument('--start-asap', action='store_true', help='Start asap controllers') + parser.add_argument('--activate-asap', action='store_true', help='Start asap controllers') parser.add_argument( '--switch-timeout', default=5.0, @@ -52,12 +67,22 @@ def add_arguments(self, parser, cli_name): add_controller_mgr_parsers(parser) def main(self, *, args): + if (args.stop): + print('"--stop" flag is deprecated, use "--deactivate" instead!') + args.deactivate = args.stop + if (args.start): + print('"--start" flag is deprecated, use "--activate" instead!') + args.activate = args.start + if (args.start_asap): + print('"--start-asap" flag is deprecated, use "--activate-asap" instead!') + args.activate_asap = args.start_asap + with NodeStrategy(args) as node: response = switch_controllers( node, args.controller_manager, - args.stop, - args.start, + args.deactivate, + args.activate, args.strict, args.start_asap, args.switch_timeout, From 51e59512b804aa531b266d0e8e0be0464f54121a Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Sat, 9 Jul 2022 08:53:57 +0100 Subject: [PATCH 22/35] Update changelogs --- controller_interface/CHANGELOG.rst | 3 + controller_manager/CHANGELOG.rst | 10 +++ controller_manager_msgs/CHANGELOG.rst | 9 +++ hardware_interface/CHANGELOG.rst | 6 ++ joint_limits/CHANGELOG.rst | 104 +++++++++++++++++++++++++ ros2_control/CHANGELOG.rst | 5 ++ ros2_control_test_assets/CHANGELOG.rst | 3 + ros2controlcli/CHANGELOG.rst | 9 +++ transmission_interface/CHANGELOG.rst | 3 + 9 files changed, 152 insertions(+) create mode 100644 joint_limits/CHANGELOG.rst diff --git a/controller_interface/CHANGELOG.rst b/controller_interface/CHANGELOG.rst index d52816691f3..62d1c79c870 100644 --- a/controller_interface/CHANGELOG.rst +++ b/controller_interface/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package controller_interface ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 2.11.0 (2022-07-03) ------------------- * [Interfaces] Improved ```get_name()``` method of hardware interfaces (soft) #api-breaking (`#737 `_) diff --git a/controller_manager/CHANGELOG.rst b/controller_manager/CHANGELOG.rst index 096b991ebf2..c10f9567b8b 100644 --- a/controller_manager/CHANGELOG.rst +++ b/controller_manager/CHANGELOG.rst @@ -2,6 +2,16 @@ Changelog for package controller_manager ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Deprecate and rename `start` and `stop` nomenclature toward user to `activate` and `deactivate` #ABI-breaking (`#755 `_) + * Rename fields and deprecate old nomenclature. + * Add new defines to SwitchController.srv + * Deprecated start/stop nomenclature in all CLI commands. + * Deprecate 'start_asap' too as other fields. +* [ros2_control_node] Automatically detect if RT kernel is used and opportunistically enable SCHED_FIFO (`#748 `_) +* Contributors: Denis Štogl, Tyler Weaver + 2.11.0 (2022-07-03) ------------------- * Remove hybrid services in controller manager. (`#761 `_) diff --git a/controller_manager_msgs/CHANGELOG.rst b/controller_manager_msgs/CHANGELOG.rst index a11f4a42ed4..b7fac7eb73e 100644 --- a/controller_manager_msgs/CHANGELOG.rst +++ b/controller_manager_msgs/CHANGELOG.rst @@ -2,6 +2,15 @@ Changelog for package controller_manager_msgs ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Deprecate and rename `start` and `stop` nomenclature toward user to `activate` and `deactivate` #ABI-breaking (`#755 `_) + * Rename fields and deprecate old nomenclature. + * Add new defines to SwitchController.srv + * Deprecated start/stop nomenclature in all CLI commands. + * Deprecate 'start_asap' too as other fields. +* Contributors: Denis Štogl + 2.11.0 (2022-07-03) ------------------- * Remove hybrid services in controller manager. They are just overhead. (`#761 `_) diff --git a/hardware_interface/CHANGELOG.rst b/hardware_interface/CHANGELOG.rst index 2a18135f5d0..f7fd396e003 100644 --- a/hardware_interface/CHANGELOG.rst +++ b/hardware_interface/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package hardware_interface ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Hardware interface specific update rate and best practices about it (`#716 `_) +* Deprecate fake components, long live mock components (`#762 `_) +* Contributors: Bence Magyar, Lovro Ivanov + 2.11.0 (2022-07-03) ------------------- * [Interfaces] Improved ```get_name()``` method of hardware interfaces #api-breaking (`#737 `_) diff --git a/joint_limits/CHANGELOG.rst b/joint_limits/CHANGELOG.rst new file mode 100644 index 00000000000..39ab06edea1 --- /dev/null +++ b/joint_limits/CHANGELOG.rst @@ -0,0 +1,104 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package joint_limits +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +Forthcoming +----------- +* Move Joint Limits structures for use in controllers (`#462 `_) +* Contributors: Denis Štogl, Andy Zelenak, Bence Magyar + +2.11.0 (2022-07-03) +------------------- + +2.10.0 (2022-06-18) +------------------- + +2.9.0 (2022-05-19) +------------------ + +2.8.0 (2022-05-13) +------------------ + +2.7.0 (2022-04-29) +------------------ + +2.6.0 (2022-04-20) +------------------ + +2.5.0 (2022-03-25) +------------------ + +2.4.0 (2022-02-23) +------------------ + +2.3.0 (2022-02-18) +------------------ + +2.2.0 (2022-01-24) +------------------ + +2.1.0 (2022-01-11) +------------------ + +2.0.0 (2021-12-29) +------------------ + +1.2.0 (2021-11-05) +------------------ + +1.1.0 (2021-10-25) +------------------ + +1.0.0 (2021-09-29) +------------------ + +0.8.0 (2021-08-28) +------------------ + +0.7.1 (2021-06-15) +------------------ + +0.7.0 (2021-06-06) +------------------ + +0.6.1 (2021-05-31) +------------------ + +0.6.0 (2021-05-23) +------------------ + +0.5.0 (2021-05-03) +------------------ + +0.4.0 (2021-04-07) +------------------ + +0.3.0 (2021-03-21) +------------------ + +0.2.1 (2021-03-02) +------------------ + +0.2.0 (2021-02-26) +------------------ + +0.1.6 (2021-02-05) +------------------ + +0.1.5 (2021-02-04) +------------------ + +0.1.4 (2021-02-03) +------------------ + +0.1.3 (2021-01-21) +------------------ + +0.1.2 (2021-01-06) +------------------ + +0.1.1 (2020-12-23) +------------------ + +0.1.0 (2020-12-22) +------------------ diff --git a/ros2_control/CHANGELOG.rst b/ros2_control/CHANGELOG.rst index cc64f91efe3..4c5151f027a 100644 --- a/ros2_control/CHANGELOG.rst +++ b/ros2_control/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package ros2_control ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Move Joint Limits structures for use in controllers (`#462 `_) +* Contributors: Denis Štogl, Andy Zelenak, Bence Magyar + 2.11.0 (2022-07-03) ------------------- * Update maintainers of packages (`#753 `_) diff --git a/ros2_control_test_assets/CHANGELOG.rst b/ros2_control_test_assets/CHANGELOG.rst index 5e96c4769f6..785131db944 100644 --- a/ros2_control_test_assets/CHANGELOG.rst +++ b/ros2_control_test_assets/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ros2_control_test_assets ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 2.11.0 (2022-07-03) ------------------- * Update maintainers of packages (`#753 `_) diff --git a/ros2controlcli/CHANGELOG.rst b/ros2controlcli/CHANGELOG.rst index 1ee32d8a005..729eeb9432e 100644 --- a/ros2controlcli/CHANGELOG.rst +++ b/ros2controlcli/CHANGELOG.rst @@ -2,6 +2,15 @@ Changelog for package ros2controlcli ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Deprecate and rename `start` and `stop` nomenclature toward user to `activate` and `deactivate` #ABI-breaking (`#755 `_) + * Rename fields and deprecate old nomenclature. + * Add new defines to SwitchController.srv + * Deprecated start/stop nomenclature in all CLI commands. + * Deprecate 'start_asap' too as other fields. +* Contributors: Denis Štogl + 2.11.0 (2022-07-03) ------------------- * Remove hybrid services in controller manager. They are just overhead. (`#761 `_) diff --git a/transmission_interface/CHANGELOG.rst b/transmission_interface/CHANGELOG.rst index 92394ad2270..38a4103dd8e 100644 --- a/transmission_interface/CHANGELOG.rst +++ b/transmission_interface/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package transmission_interface ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 2.11.0 (2022-07-03) ------------------- * [Interfaces] Improved ```get_name()``` method of hardware interfaces #api-breaking (`#737 `_) From 0ecadb3f38ee9c28e25185a8a079ad0d68b96ba3 Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Sat, 9 Jul 2022 08:55:25 +0100 Subject: [PATCH 23/35] Adjust package version for release --- joint_limits/package.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/joint_limits/package.xml b/joint_limits/package.xml index 12e11638d2f..4cc8b32dd04 100644 --- a/joint_limits/package.xml +++ b/joint_limits/package.xml @@ -1,6 +1,6 @@ joint_limits - 0.0.0 + 2.11.0 Interfaces for handling of joint limits for controllers or hardware. Bence Magyar From 601292093be6d309fc6bbb5bc879ecb80cf5a3bf Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Sat, 9 Jul 2022 08:55:50 +0100 Subject: [PATCH 24/35] 2.12.0 --- controller_interface/CHANGELOG.rst | 4 ++-- controller_interface/package.xml | 2 +- controller_manager/CHANGELOG.rst | 4 ++-- controller_manager/package.xml | 2 +- controller_manager_msgs/CHANGELOG.rst | 4 ++-- controller_manager_msgs/package.xml | 2 +- hardware_interface/CHANGELOG.rst | 4 ++-- hardware_interface/package.xml | 2 +- joint_limits/CHANGELOG.rst | 4 ++-- joint_limits/package.xml | 2 +- ros2_control/CHANGELOG.rst | 4 ++-- ros2_control/package.xml | 2 +- ros2_control_test_assets/CHANGELOG.rst | 4 ++-- ros2_control_test_assets/package.xml | 2 +- ros2controlcli/CHANGELOG.rst | 4 ++-- ros2controlcli/package.xml | 2 +- ros2controlcli/setup.py | 2 +- transmission_interface/CHANGELOG.rst | 4 ++-- transmission_interface/package.xml | 2 +- 19 files changed, 28 insertions(+), 28 deletions(-) diff --git a/controller_interface/CHANGELOG.rst b/controller_interface/CHANGELOG.rst index 62d1c79c870..55be59685f1 100644 --- a/controller_interface/CHANGELOG.rst +++ b/controller_interface/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package controller_interface ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.12.0 (2022-07-09) +------------------- 2.11.0 (2022-07-03) ------------------- diff --git a/controller_interface/package.xml b/controller_interface/package.xml index 639c4c14d59..7a0deecd5d8 100644 --- a/controller_interface/package.xml +++ b/controller_interface/package.xml @@ -2,7 +2,7 @@ controller_interface - 2.11.0 + 2.12.0 Description of controller_interface Bence Magyar Denis Štogl diff --git a/controller_manager/CHANGELOG.rst b/controller_manager/CHANGELOG.rst index c10f9567b8b..c44bdd6651c 100644 --- a/controller_manager/CHANGELOG.rst +++ b/controller_manager/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package controller_manager ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.12.0 (2022-07-09) +------------------- * Deprecate and rename `start` and `stop` nomenclature toward user to `activate` and `deactivate` #ABI-breaking (`#755 `_) * Rename fields and deprecate old nomenclature. * Add new defines to SwitchController.srv diff --git a/controller_manager/package.xml b/controller_manager/package.xml index c9ca543e63f..ce8d49f653a 100644 --- a/controller_manager/package.xml +++ b/controller_manager/package.xml @@ -2,7 +2,7 @@ controller_manager - 2.11.0 + 2.12.0 Description of controller_manager Bence Magyar Denis Štogl diff --git a/controller_manager_msgs/CHANGELOG.rst b/controller_manager_msgs/CHANGELOG.rst index b7fac7eb73e..d32b5be2dd8 100644 --- a/controller_manager_msgs/CHANGELOG.rst +++ b/controller_manager_msgs/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package controller_manager_msgs ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.12.0 (2022-07-09) +------------------- * Deprecate and rename `start` and `stop` nomenclature toward user to `activate` and `deactivate` #ABI-breaking (`#755 `_) * Rename fields and deprecate old nomenclature. * Add new defines to SwitchController.srv diff --git a/controller_manager_msgs/package.xml b/controller_manager_msgs/package.xml index 304c033a15e..4dae1eaefe3 100644 --- a/controller_manager_msgs/package.xml +++ b/controller_manager_msgs/package.xml @@ -2,7 +2,7 @@ controller_manager_msgs - 2.11.0 + 2.12.0 Messages and services for the controller manager. Bence Magyar Denis Štogl diff --git a/hardware_interface/CHANGELOG.rst b/hardware_interface/CHANGELOG.rst index f7fd396e003..4475e977601 100644 --- a/hardware_interface/CHANGELOG.rst +++ b/hardware_interface/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package hardware_interface ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.12.0 (2022-07-09) +------------------- * Hardware interface specific update rate and best practices about it (`#716 `_) * Deprecate fake components, long live mock components (`#762 `_) * Contributors: Bence Magyar, Lovro Ivanov diff --git a/hardware_interface/package.xml b/hardware_interface/package.xml index c1ceef11e6f..5038680087d 100644 --- a/hardware_interface/package.xml +++ b/hardware_interface/package.xml @@ -1,7 +1,7 @@ hardware_interface - 2.11.0 + 2.12.0 ros2_control hardware interface Bence Magyar Denis Štogl diff --git a/joint_limits/CHANGELOG.rst b/joint_limits/CHANGELOG.rst index 39ab06edea1..dcff615f662 100644 --- a/joint_limits/CHANGELOG.rst +++ b/joint_limits/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package joint_limits ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.12.0 (2022-07-09) +------------------- * Move Joint Limits structures for use in controllers (`#462 `_) * Contributors: Denis Štogl, Andy Zelenak, Bence Magyar diff --git a/joint_limits/package.xml b/joint_limits/package.xml index 4cc8b32dd04..ded7f001c8e 100644 --- a/joint_limits/package.xml +++ b/joint_limits/package.xml @@ -1,6 +1,6 @@ joint_limits - 2.11.0 + 2.12.0 Interfaces for handling of joint limits for controllers or hardware. Bence Magyar diff --git a/ros2_control/CHANGELOG.rst b/ros2_control/CHANGELOG.rst index 4c5151f027a..9984eeacd9a 100644 --- a/ros2_control/CHANGELOG.rst +++ b/ros2_control/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ros2_control ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.12.0 (2022-07-09) +------------------- * Move Joint Limits structures for use in controllers (`#462 `_) * Contributors: Denis Štogl, Andy Zelenak, Bence Magyar diff --git a/ros2_control/package.xml b/ros2_control/package.xml index 57e0d85277e..950f596fb12 100644 --- a/ros2_control/package.xml +++ b/ros2_control/package.xml @@ -1,7 +1,7 @@ ros2_control - 2.11.0 + 2.12.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 785131db944..ca31f420812 100644 --- a/ros2_control_test_assets/CHANGELOG.rst +++ b/ros2_control_test_assets/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ros2_control_test_assets ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.12.0 (2022-07-09) +------------------- 2.11.0 (2022-07-03) ------------------- diff --git a/ros2_control_test_assets/package.xml b/ros2_control_test_assets/package.xml index bc45ba9e13e..3afa30b42be 100644 --- a/ros2_control_test_assets/package.xml +++ b/ros2_control_test_assets/package.xml @@ -2,7 +2,7 @@ ros2_control_test_assets - 2.11.0 + 2.12.0 The package provides shared test resources for ros2_control stack Bence Magyar diff --git a/ros2controlcli/CHANGELOG.rst b/ros2controlcli/CHANGELOG.rst index 729eeb9432e..97c6b209885 100644 --- a/ros2controlcli/CHANGELOG.rst +++ b/ros2controlcli/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ros2controlcli ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.12.0 (2022-07-09) +------------------- * Deprecate and rename `start` and `stop` nomenclature toward user to `activate` and `deactivate` #ABI-breaking (`#755 `_) * Rename fields and deprecate old nomenclature. * Add new defines to SwitchController.srv diff --git a/ros2controlcli/package.xml b/ros2controlcli/package.xml index 193f42e5814..cc5e4515f01 100644 --- a/ros2controlcli/package.xml +++ b/ros2controlcli/package.xml @@ -2,7 +2,7 @@ ros2controlcli - 2.11.0 + 2.12.0 The ROS 2 command line tools for ROS2 Control. diff --git a/ros2controlcli/setup.py b/ros2controlcli/setup.py index 3c3f27c8603..ed34b82f33e 100644 --- a/ros2controlcli/setup.py +++ b/ros2controlcli/setup.py @@ -5,7 +5,7 @@ setup( name=package_name, - version='2.11.0', + version='2.12.0', packages=find_packages(exclude=['test']), data_files=[ ('share/' + package_name, ['package.xml']), diff --git a/transmission_interface/CHANGELOG.rst b/transmission_interface/CHANGELOG.rst index 38a4103dd8e..4af18fe412f 100644 --- a/transmission_interface/CHANGELOG.rst +++ b/transmission_interface/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package transmission_interface ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.12.0 (2022-07-09) +------------------- 2.11.0 (2022-07-03) ------------------- diff --git a/transmission_interface/package.xml b/transmission_interface/package.xml index c5c39fc934e..6be610c4c51 100644 --- a/transmission_interface/package.xml +++ b/transmission_interface/package.xml @@ -2,7 +2,7 @@ transmission_interface - 2.11.0 + 2.12.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 From 3e5d109c420b1d614b469b7b78a3be82221cb980 Mon Sep 17 00:00:00 2001 From: Tyler Weaver Date: Mon, 11 Jul 2022 11:14:43 -0600 Subject: [PATCH 25/35] Fix spelling in comment (#769) Signed-off-by: Tyler Weaver --- controller_manager/src/ros2_control_node.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/controller_manager/src/ros2_control_node.cpp b/controller_manager/src/ros2_control_node.cpp index 0c16e8d7a16..b8468318ace 100644 --- a/controller_manager/src/ros2_control_node.cpp +++ b/controller_manager/src/ros2_control_node.cpp @@ -60,12 +60,12 @@ int main(int argc, char ** argv) RCLCPP_INFO(cm->get_logger(), "RT kernel is recommended for better performance"); } - // for calcuating sleep time + // for calculating sleep time auto const period = std::chrono::nanoseconds(1'000'000'000 / cm->get_update_rate()); std::chrono::system_clock::time_point next_iteration_time = std::chrono::system_clock::time_point(std::chrono::nanoseconds(cm->now().nanoseconds())); - // for calclating the measured period of the loop + // for calculating the measured period of the loop rclcpp::Time previous_time = cm->now(); while (rclcpp::ok()) From a167eff99db024ad42542e0d76a81d9393c0a3ae Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Mon, 11 Jul 2022 19:17:56 +0200 Subject: [PATCH 26/35] Rename CM members from start/stop to activate/deactivate nomenclature. (#756) --- .../controller_manager/controller_manager.hpp | 15 +- controller_manager/src/controller_manager.cpp | 200 +++++++++--------- 2 files changed, 112 insertions(+), 103 deletions(-) diff --git a/controller_manager/include/controller_manager/controller_manager.hpp b/controller_manager/include/controller_manager/controller_manager.hpp index b922d1f935b..75a30ae99a9 100644 --- a/controller_manager/include/controller_manager/controller_manager.hpp +++ b/controller_manager/include/controller_manager/controller_manager.hpp @@ -134,7 +134,7 @@ class ControllerManager : public rclcpp::Node controller_interface::return_type switch_controller( const std::vector & start_controllers, const std::vector & stop_controllers, int strictness, - bool start_asap = kWaitForAllResources, + bool activate_asap = kWaitForAllResources, const rclcpp::Duration & timeout = rclcpp::Duration::from_nanoseconds(kInfiniteTimeout)); CONTROLLER_MANAGER_PUBLIC @@ -172,7 +172,7 @@ class ControllerManager : public rclcpp::Node void manage_switch(); CONTROLLER_MANAGER_PUBLIC - void stop_controllers(); + void deactivate_controllers(); /** * Switch chained mode for all the controllers with respect to the following cases: @@ -187,10 +187,10 @@ class ControllerManager : public rclcpp::Node const std::vector & chained_mode_switch_list, bool to_chained_mode); CONTROLLER_MANAGER_PUBLIC - void start_controllers(); + void activate_controllers(); CONTROLLER_MANAGER_PUBLIC - void start_controllers_asap(); + void activate_controllers_asap(); CONTROLLER_MANAGER_PUBLIC void list_controllers_srv_cb( @@ -428,9 +428,10 @@ class ControllerManager : public rclcpp::Node rclcpp::Service::SharedPtr set_hardware_component_state_service_; - std::vector start_request_, stop_request_; + std::vector activate_request_, deactivate_request_; std::vector to_chained_mode_request_, from_chained_mode_request_; - std::vector start_command_interface_request_, stop_command_interface_request_; + std::vector activate_command_interface_request_, + deactivate_command_interface_request_; struct SwitchParams { @@ -440,7 +441,7 @@ class ControllerManager : public rclcpp::Node // Switch options int strictness = {0}; - bool start_asap = {false}; + bool activate_asap = {false}; rclcpp::Duration timeout = rclcpp::Duration{0, 0}; }; diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 9e560bc9a34..0fa4bf7c854 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -482,34 +482,35 @@ controller_interface::return_type ControllerManager::configure_controller( void ControllerManager::clear_requests() { - stop_request_.clear(); - start_request_.clear(); + deactivate_request_.clear(); + activate_request_.clear(); to_chained_mode_request_.clear(); from_chained_mode_request_.clear(); - start_command_interface_request_.clear(); - stop_command_interface_request_.clear(); + activate_command_interface_request_.clear(); + deactivate_command_interface_request_.clear(); } controller_interface::return_type ControllerManager::switch_controller( - const std::vector & start_controllers, - const std::vector & stop_controllers, int strictness, bool start_asap, + const std::vector & activate_controllers, + const std::vector & deactivate_controllers, int strictness, bool activate_asap, const rclcpp::Duration & timeout) { switch_params_ = SwitchParams(); - if (!stop_request_.empty() || !start_request_.empty()) + if (!deactivate_request_.empty() || !activate_request_.empty()) { RCLCPP_FATAL( get_logger(), - "The internal stop and start request lists are not empty at the beginning of the " + "The internal deactivate and activate request lists are not empty at the beginning of the " "switch_controller() call. This should never happen."); throw std::runtime_error("CM's internal state is not correct. See the FATAL message above."); } - if (!stop_command_interface_request_.empty() || !start_command_interface_request_.empty()) + if ( + !deactivate_command_interface_request_.empty() || !activate_command_interface_request_.empty()) { RCLCPP_FATAL( get_logger(), - "The internal stop and start requests command interface lists are not empty at the " + "The internal deactivate and activat requests command interface lists are not empty at the " "switch_controller() call. This should never happen."); throw std::runtime_error("CM's internal state is not correct. See the FATAL message above."); } @@ -534,13 +535,13 @@ controller_interface::return_type ControllerManager::switch_controller( } RCLCPP_DEBUG(get_logger(), "Switching controllers:"); - for (const auto & controller : start_controllers) + for (const auto & controller : activate_controllers) { - RCLCPP_DEBUG(get_logger(), "- Starting controller '%s'", controller.c_str()); + RCLCPP_DEBUG(get_logger(), "- Activating controller '%s'", controller.c_str()); } - for (const auto & controller : stop_controllers) + for (const auto & controller : deactivate_controllers) { - RCLCPP_DEBUG(get_logger(), "- Stopping controller '%s'", controller.c_str()); + RCLCPP_DEBUG(get_logger(), "- Deactivating controller '%s'", controller.c_str()); } const auto list_controllers = [this, strictness]( @@ -551,7 +552,7 @@ controller_interface::return_type ControllerManager::switch_controller( // lock controllers std::lock_guard guard(rt_controllers_wrapper_.controllers_lock_); - // list all controllers to stop/start + // list all controllers to (de)activate for (const auto & controller : controller_list) { const auto & updated_controllers = rt_controllers_wrapper_.get_updated_list(guard); @@ -586,20 +587,20 @@ controller_interface::return_type ControllerManager::switch_controller( return controller_interface::return_type::OK; }; - // list all controllers to stop (check if all controllers exist) - auto ret = list_controllers(stop_controllers, stop_request_, "stop"); + // list all controllers to deactivate (check if all controllers exist) + auto ret = list_controllers(deactivate_controllers, deactivate_request_, "deactivate"); if (ret != controller_interface::return_type::OK) { - stop_request_.clear(); + deactivate_request_.clear(); return ret; } - // list all controllers to start (check if all controllers exist) - ret = list_controllers(start_controllers, start_request_, "start"); + // list all controllers to activate (check if all controllers exist) + ret = list_controllers(activate_controllers, activate_request_, "activate"); if (ret != controller_interface::return_type::OK) { - stop_request_.clear(); - start_request_.clear(); + deactivate_request_.clear(); + activate_request_.clear(); return ret; } @@ -613,7 +614,7 @@ controller_interface::return_type ControllerManager::switch_controller( propagate_deactivation_of_chained_mode(controllers); // check if controllers should be switched 'to' chained mode when controllers are activated - for (auto ctrl_it = start_request_.begin(); ctrl_it != start_request_.end(); ++ctrl_it) + for (auto ctrl_it = activate_request_.begin(); ctrl_it != activate_request_.end(); ++ctrl_it) { auto controller_it = std::find_if( controllers.begin(), controllers.end(), @@ -650,7 +651,7 @@ controller_interface::return_type ControllerManager::switch_controller( // controller_manager_msgs::srv::SwitchController::Request::MANIPULATE_CONTROLLERS_CHAIN); // remove controller that can not be activated from the activation request and step-back // iterator to correctly step to the next element in the list in the loop - start_request_.erase(ctrl_it); + activate_request_.erase(ctrl_it); --ctrl_it; } if (strictness == controller_manager_msgs::srv::SwitchController::Request::STRICT) @@ -664,7 +665,7 @@ controller_interface::return_type ControllerManager::switch_controller( } // check if controllers should be deactivated if used in chained mode - for (auto ctrl_it = stop_request_.begin(); ctrl_it != stop_request_.end(); ++ctrl_it) + for (auto ctrl_it = deactivate_request_.begin(); ctrl_it != deactivate_request_.end(); ++ctrl_it) { auto controller_it = std::find_if( controllers.begin(), controllers.end(), @@ -696,7 +697,7 @@ controller_interface::return_type ControllerManager::switch_controller( { // remove controller that can not be activated from the activation request and step-back // iterator to correctly step to the next element in the list in the loop - stop_request_.erase(ctrl_it); + deactivate_request_.erase(ctrl_it); --ctrl_it; } if (strictness == controller_manager_msgs::srv::SwitchController::Request::STRICT) @@ -719,39 +720,41 @@ controller_interface::return_type ControllerManager::switch_controller( from_chained_mode_request_.begin(), from_chained_mode_request_.end(), controller.info.name); bool in_from_chained_mode_list = from_chained_mode_list_it != from_chained_mode_request_.end(); - auto stop_list_it = std::find(stop_request_.begin(), stop_request_.end(), controller.info.name); - bool in_stop_list = stop_list_it != stop_request_.end(); + auto deactivate_list_it = + std::find(deactivate_request_.begin(), deactivate_request_.end(), controller.info.name); + bool in_deactivate_list = deactivate_list_it != deactivate_request_.end(); const bool is_active = is_controller_active(*controller.c); const bool is_inactive = is_controller_inactive(*controller.c); - // restart controllers that need to switch their 'chained mode' - add to stop/start lists + // restart controllers that need to switch their 'chained mode' - add to (de)activate lists if (in_to_chained_mode_list || in_from_chained_mode_list) { - if (is_active && !in_stop_list) + if (is_active && !in_deactivate_list) { - stop_request_.push_back(controller.info.name); - start_request_.push_back(controller.info.name); + deactivate_request_.push_back(controller.info.name); + activate_request_.push_back(controller.info.name); } } - // get pointers to places in stop and start lists (start/stop lists have changed) - stop_list_it = std::find(stop_request_.begin(), stop_request_.end(), controller.info.name); - in_stop_list = stop_list_it != stop_request_.end(); + // get pointers to places in deactivate and activate lists ((de)activate lists have changed) + deactivate_list_it = + std::find(deactivate_request_.begin(), deactivate_request_.end(), controller.info.name); + in_deactivate_list = deactivate_list_it != deactivate_request_.end(); - auto start_list_it = - std::find(start_request_.begin(), start_request_.end(), controller.info.name); - bool in_start_list = start_list_it != start_request_.end(); + auto activate_list_it = + std::find(activate_request_.begin(), activate_request_.end(), controller.info.name); + bool in_activate_list = activate_list_it != activate_request_.end(); auto handle_conflict = [&](const std::string & msg) { if (strictness == controller_manager_msgs::srv::SwitchController::Request::STRICT) { RCLCPP_ERROR(get_logger(), "%s", msg.c_str()); - stop_request_.clear(); - stop_command_interface_request_.clear(); - start_request_.clear(); - start_command_interface_request_.clear(); + deactivate_request_.clear(); + deactivate_command_interface_request_.clear(); + activate_request_.clear(); + activate_command_interface_request_.clear(); to_chained_mode_request_.clear(); from_chained_mode_request_.clear(); return controller_interface::return_type::ERROR; @@ -761,43 +764,43 @@ controller_interface::return_type ControllerManager::switch_controller( }; // check for double stop - if (!is_active && in_stop_list) + if (!is_active && in_deactivate_list) { auto ret = handle_conflict( - "Could not stop controller '" + controller.info.name + "' since it is not active"); + "Could not deactivate controller '" + controller.info.name + "' since it is not active"); if (ret != controller_interface::return_type::OK) { return ret; } - in_stop_list = false; - stop_request_.erase(stop_list_it); + in_deactivate_list = false; + deactivate_request_.erase(deactivate_list_it); } - // check for doubled start - if (is_active && !in_stop_list && in_start_list) + // check for doubled activation + if (is_active && !in_deactivate_list && in_activate_list) { auto ret = handle_conflict( - "Could not start controller '" + controller.info.name + "' since it is already active"); + "Could not activate controller '" + controller.info.name + "' since it is already active"); if (ret != controller_interface::return_type::OK) { return ret; } - in_start_list = false; - start_request_.erase(start_list_it); + in_activate_list = false; + activate_request_.erase(activate_list_it); } - // check for illegal start of an unconfigured/finalized controller - if (!is_inactive && !in_stop_list && in_start_list) + // check for illegal activation of an unconfigured/finalized controller + if (!is_inactive && !in_deactivate_list && in_activate_list) { auto ret = handle_conflict( - "Could not start controller '" + controller.info.name + + "Could not activate controller '" + controller.info.name + "' since it is not in inactive state"); if (ret != controller_interface::return_type::OK) { return ret; } - in_start_list = false; - start_request_.erase(start_list_it); + in_activate_list = false; + activate_request_.erase(activate_list_it); } const auto extract_interfaces_for_controller = @@ -820,27 +823,28 @@ controller_interface::return_type ControllerManager::switch_controller( command_interface_names.end()); }; - if (in_start_list) + if (in_activate_list) { - extract_interfaces_for_controller(controller, start_command_interface_request_); + extract_interfaces_for_controller(controller, activate_command_interface_request_); } - if (in_stop_list) + if (in_deactivate_list) { - extract_interfaces_for_controller(controller, stop_command_interface_request_); + extract_interfaces_for_controller(controller, deactivate_command_interface_request_); } } - if (start_request_.empty() && stop_request_.empty()) + if (activate_request_.empty() && deactivate_request_.empty()) { - RCLCPP_INFO(get_logger(), "Empty start and stop list, not requesting switch"); + RCLCPP_INFO(get_logger(), "Empty activate and deactivate list, not requesting switch"); clear_requests(); return controller_interface::return_type::OK; } - if (!start_command_interface_request_.empty() || !stop_command_interface_request_.empty()) + if ( + !activate_command_interface_request_.empty() || !deactivate_command_interface_request_.empty()) { if (!resource_manager_->prepare_command_mode_switch( - start_command_interface_request_, stop_command_interface_request_)) + activate_command_interface_request_, deactivate_command_interface_request_)) { RCLCPP_ERROR( get_logger(), @@ -851,7 +855,7 @@ controller_interface::return_type ControllerManager::switch_controller( } // start the atomic controller switching switch_params_.strictness = strictness; - switch_params_.start_asap = start_asap; + switch_params_.activate_asap = activate_asap; switch_params_.init_time = rclcpp::Clock().now(); switch_params_.timeout = timeout; switch_params_.do_switch = true; @@ -968,36 +972,36 @@ void ControllerManager::manage_switch() { // Ask hardware interfaces to change mode if (!resource_manager_->perform_command_mode_switch( - start_command_interface_request_, stop_command_interface_request_)) + activate_command_interface_request_, deactivate_command_interface_request_)) { RCLCPP_ERROR(get_logger(), "Error while performing mode switch."); } - stop_controllers(); + deactivate_controllers(); switch_chained_mode(to_chained_mode_request_, true); switch_chained_mode(from_chained_mode_request_, false); - // start controllers once the switch is fully complete - if (!switch_params_.start_asap) + // activate controllers once the switch is fully complete + if (!switch_params_.activate_asap) { - start_controllers(); + activate_controllers(); } else { - // start controllers as soon as their required joints are done switching - start_controllers_asap(); + // activate controllers as soon as their required joints are done switching + activate_controllers_asap(); } // TODO(destogl): move here "do_switch = false" } -void ControllerManager::stop_controllers() +void ControllerManager::deactivate_controllers() { std::vector & rt_controller_list = rt_controllers_wrapper_.update_and_get_used_by_rt_list(); // stop controllers - for (const auto & request : stop_request_) + for (const auto & request : deactivate_request_) { auto found_it = std::find_if( rt_controller_list.begin(), rt_controller_list.end(), @@ -1082,11 +1086,11 @@ void ControllerManager::switch_chained_mode( } } -void ControllerManager::start_controllers() +void ControllerManager::activate_controllers() { std::vector & rt_controller_list = rt_controllers_wrapper_.update_and_get_used_by_rt_list(); - for (const auto & request : start_request_) + for (const auto & request : activate_request_) { auto found_it = std::find_if( rt_controller_list.begin(), rt_controller_list.end(), @@ -1095,7 +1099,7 @@ void ControllerManager::start_controllers() { RCLCPP_ERROR( get_logger(), - "Got request to start controller '%s' but it is not in the realtime controller list", + "Got request to activate controller '%s' but it is not in the realtime controller list", request.c_str()); continue; } @@ -1189,14 +1193,14 @@ void ControllerManager::start_controllers() controller->get_node()->get_name(), new_state.label().c_str()); } } - // All controllers started, switching done + // All controllers activated, switching done switch_params_.do_switch = false; } -void ControllerManager::start_controllers_asap() +void ControllerManager::activate_controllers_asap() { // https://github.com/ros-controls/ros2_control/issues/263 - start_controllers(); + activate_controllers(); } void ControllerManager::list_controllers_srv_cb( @@ -1633,7 +1637,7 @@ controller_interface::return_type ControllerManager::update( } } - // there are controllers to start/stop + // there are controllers to (de)activate if (switch_params_.do_switch) { manage_switch(); @@ -1719,10 +1723,11 @@ void ControllerManager::propagate_deactivation_of_chained_mode( { for (const auto & controller : controllers) { - // get pointers to places in stop and start lists (start/stop lists have changed) - auto stop_list_it = std::find(stop_request_.begin(), stop_request_.end(), controller.info.name); + // get pointers to places in deactivate and activate lists ((de)activate lists have changed) + auto deactivate_list_it = + std::find(deactivate_request_.begin(), deactivate_request_.end(), controller.info.name); - if (stop_list_it != stop_request_.end()) + if (deactivate_list_it != deactivate_request_.end()) { // if controller is not active then skip adding following-controllers to "from" chained mode // request @@ -1806,8 +1811,9 @@ controller_interface::return_type ControllerManager::check_following_controllers { // will following controller be deactivated? if ( - std::find(stop_request_.begin(), stop_request_.end(), following_ctrl_it->info.name) != - stop_request_.end()) + std::find( + deactivate_request_.begin(), deactivate_request_.end(), following_ctrl_it->info.name) != + deactivate_request_.end()) { RCLCPP_WARN( get_logger(), "The following controller with name '%s' will be deactivated.", @@ -1817,8 +1823,8 @@ controller_interface::return_type ControllerManager::check_following_controllers } // check if following controller will not be activated else if ( - std::find(start_request_.begin(), start_request_.end(), following_ctrl_it->info.name) == - start_request_.end()) + std::find(activate_request_.begin(), activate_request_.end(), following_ctrl_it->info.name) == + activate_request_.end()) { RCLCPP_WARN( get_logger(), @@ -1840,8 +1846,8 @@ controller_interface::return_type ControllerManager::check_following_controllers // else if (strictness == // controller_manager_msgs::srv::SwitchController::Request::MANIPULATE_CONTROLLERS_CHAIN) // { - // // insert to the begin of start request list to be started before preceding controller - // start_request_.insert(start_request_.begin(), following_ctrl_name); + // // insert to the begin of activate request list to be activated before preceding controller + // activate_request_.insert(activate_request_.begin(), following_ctrl_name); // } if (!following_ctrl_it->c->is_in_chained_mode()) { @@ -1924,8 +1930,9 @@ controller_interface::return_type ControllerManager::check_preceeding_controller // check if preceding controller will be activated if ( is_controller_inactive(preceding_ctrl_it->c) && - std::find(start_request_.begin(), start_request_.end(), preceding_ctrl_it->info.name) != - start_request_.end()) + std::find( + activate_request_.begin(), activate_request_.end(), preceding_ctrl_it->info.name) != + activate_request_.end()) { RCLCPP_WARN( get_logger(), @@ -1937,8 +1944,9 @@ controller_interface::return_type ControllerManager::check_preceeding_controller // check if preceding controller will not be deactivated else if ( is_controller_active(preceding_ctrl_it->c) && - std::find(stop_request_.begin(), stop_request_.end(), preceding_ctrl_it->info.name) == - stop_request_.end()) + std::find( + deactivate_request_.begin(), deactivate_request_.end(), preceding_ctrl_it->info.name) == + deactivate_request_.end()) { RCLCPP_WARN( get_logger(), @@ -1952,8 +1960,8 @@ controller_interface::return_type ControllerManager::check_preceeding_controller // strictness == // controller_manager_msgs::srv::SwitchController::Request::MANIPULATE_CONTROLLERS_CHAIN) // { - // // insert to the begin of start request list to be started before preceding controller - // start_request_.insert(start_request_.begin(), preceding_ctrl_name); + // // insert to the begin of activate request list to be activated before preceding controller + // activate_request_.insert(activate_request_.begin(), preceding_ctrl_name); // } } } From 0db8322614ee4aaea066515c7e425052a1e94d57 Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Tue, 12 Jul 2022 14:09:17 +0100 Subject: [PATCH 27/35] Update dockerfile to fix docker build on tags (#770) * change default to humble --- .docker/release/Dockerfile | 2 +- .docker/source/Dockerfile | 4 ++-- .github/workflows/docker.yml | 2 +- 3 files changed, 4 insertions(+), 4 deletions(-) diff --git a/.docker/release/Dockerfile b/.docker/release/Dockerfile index 8a437de802b..6d0a6be6e4a 100644 --- a/.docker/release/Dockerfile +++ b/.docker/release/Dockerfile @@ -1,4 +1,4 @@ -ARG ROS_DISTRO="galactic" +ARG ROS_DISTRO="humble" FROM ros:${ROS_DISTRO} # Commands are combined in single RUN statement with "apt/lists" folder removal to reduce image size diff --git a/.docker/source/Dockerfile b/.docker/source/Dockerfile index 9ec7a0f2939..0ed76fbb9f7 100644 --- a/.docker/source/Dockerfile +++ b/.docker/source/Dockerfile @@ -1,11 +1,11 @@ -ARG ROS_DISTRO="galactic" +ARG ROS_DISTRO="rolling" FROM ros:$ROS_DISTRO ARG BRANCH="master" ENV ROS_UNDERLAY /root/ws_ros2_control/install WORKDIR $ROS_UNDERLAY/../src -ADD https://raw.githubusercontent.com/ros-controls/ros2_control/$BRANCH/ros2_control/ros2_control.repos ros2_control.repos +ADD https://raw.githubusercontent.com/ros-controls/ros2_control/$BRANCH/ros2_control.$ROS_DISTRO.repos ros2_control.repos RUN vcs import < ros2_control.repos RUN apt-get update && rosdep update && \ diff --git a/.github/workflows/docker.yml b/.github/workflows/docker.yml index b52926d60bb..3ea9a635414 100644 --- a/.github/workflows/docker.yml +++ b/.github/workflows/docker.yml @@ -7,7 +7,7 @@ on: env: REGISTRY: ghcr.io IMAGE_NAME: ${{ github.repository }} - ROS_DISTRO: galactic + ROS_DISTRO: humble BRANCH: master jobs: From 28554b7d19cfcff2b63980080b51b0714487b746 Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Wed, 13 Jul 2022 13:54:09 +0100 Subject: [PATCH 28/35] Fix fake components deprecation and add test for it (#771) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-authored-by: Denis Štogl --- hardware_interface/CMakeLists.txt | 1 + .../fake_components/generic_system.hpp | 7 ++- .../mock_components/fake_generic_system.cpp | 19 +++++++ .../mock_components/test_generic_system.cpp | 52 +++++++++++++++++++ 4 files changed, 75 insertions(+), 4 deletions(-) create mode 100644 hardware_interface/src/mock_components/fake_generic_system.cpp diff --git a/hardware_interface/CMakeLists.txt b/hardware_interface/CMakeLists.txt index 7fec4293dd8..9af1a359cd9 100644 --- a/hardware_interface/CMakeLists.txt +++ b/hardware_interface/CMakeLists.txt @@ -73,6 +73,7 @@ add_library( fake_components SHARED src/mock_components/generic_system.cpp + src/mock_components/fake_generic_system.cpp ) target_include_directories( fake_components diff --git a/hardware_interface/include/fake_components/generic_system.hpp b/hardware_interface/include/fake_components/generic_system.hpp index c671f165be2..3dfb2b9b703 100644 --- a/hardware_interface/include/fake_components/generic_system.hpp +++ b/hardware_interface/include/fake_components/generic_system.hpp @@ -17,14 +17,13 @@ #ifndef FAKE_COMPONENTS__GENERIC_SYSTEM_HPP_ #define FAKE_COMPONENTS__GENERIC_SYSTEM_HPP_ -#include "hardware_interface/mock_components/generic_system.hpp" +#include "mock_components/generic_system.hpp" namespace fake_components { -[[deprecated]] using GenericSystem = mock_components::GenericSystem; - -[[deprecated]] using GenericSystem = GenericRobot; +using GenericSystem [[deprecated]] = mock_components::GenericSystem; +using GenericSystem [[deprecated]] = mock_components::GenericRobot; } // namespace fake_components #endif // FAKE_COMPONENTS__GENERIC_SYSTEM_HPP_ diff --git a/hardware_interface/src/mock_components/fake_generic_system.cpp b/hardware_interface/src/mock_components/fake_generic_system.cpp new file mode 100644 index 00000000000..736575c8a68 --- /dev/null +++ b/hardware_interface/src/mock_components/fake_generic_system.cpp @@ -0,0 +1,19 @@ +// Copyright (c) 2022 PickNik, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// +// Author: Jafar Abdi, Denis Stogl + +#include "fake_components/generic_system.hpp" +#include "pluginlib/class_list_macros.hpp" +PLUGINLIB_EXPORT_CLASS(fake_components::GenericSystem, hardware_interface::SystemInterface) diff --git a/hardware_interface/test/mock_components/test_generic_system.cpp b/hardware_interface/test/mock_components/test_generic_system.cpp index fae841fdc25..52518a52e89 100644 --- a/hardware_interface/test/mock_components/test_generic_system.cpp +++ b/hardware_interface/test/mock_components/test_generic_system.cpp @@ -41,6 +41,26 @@ class TestGenericSystem : public ::testing::Test protected: void SetUp() override { + // REMOVE THIS MEMBER ONCE FAKE COMPONENTS ARE REMOVED + hardware_fake_system_2dof_ = + R"( + + + fake_components/GenericSystem + + + + + 1.57 + + + + + 0.7854 + + +)"; + hardware_system_2dof_ = R"( @@ -394,6 +414,7 @@ class TestGenericSystem : public ::testing::Test std::string hardware_robot_2dof_; std::string hardware_system_2dof_; + std::string hardware_fake_system_2dof_; std::string hardware_system_2dof_asymetric_; std::string hardware_system_2dof_standard_interfaces_; std::string hardware_system_2dof_with_other_interface_; @@ -453,6 +474,37 @@ TEST_F(TestGenericSystem, load_generic_system_2dof) ASSERT_NO_THROW(hardware_interface::ResourceManager rm(urdf)); } +// REMOVE THIS TEST ONCE FAKE COMPONENTS ARE REMOVED +TEST_F(TestGenericSystem, generic_fake_system_2dof_symetric_interfaces) +{ + auto urdf = ros2_control_test_assets::urdf_head + hardware_fake_system_2dof_ + + ros2_control_test_assets::urdf_tail; + hardware_interface::ResourceManager rm(urdf); + // Activate components to get all interfaces available + activate_components(rm); + + // Check interfaces + EXPECT_EQ(1u, rm.system_components_size()); + ASSERT_EQ(2u, rm.state_interface_keys().size()); + EXPECT_TRUE(rm.state_interface_exists("joint1/position")); + EXPECT_TRUE(rm.state_interface_exists("joint2/position")); + + ASSERT_EQ(2u, rm.command_interface_keys().size()); + EXPECT_TRUE(rm.command_interface_exists("joint1/position")); + EXPECT_TRUE(rm.command_interface_exists("joint2/position")); + + // Check initial values + hardware_interface::LoanedStateInterface j1p_s = rm.claim_state_interface("joint1/position"); + hardware_interface::LoanedStateInterface j2p_s = rm.claim_state_interface("joint2/position"); + hardware_interface::LoanedCommandInterface j1p_c = rm.claim_command_interface("joint1/position"); + hardware_interface::LoanedCommandInterface j2p_c = rm.claim_command_interface("joint2/position"); + + ASSERT_EQ(1.57, j1p_s.get_value()); + ASSERT_EQ(0.7854, j2p_s.get_value()); + ASSERT_TRUE(std::isnan(j1p_c.get_value())); + ASSERT_TRUE(std::isnan(j2p_c.get_value())); +} + // Test inspired by hardware_interface/test_resource_manager.cpp TEST_F(TestGenericSystem, generic_system_2dof_symetric_interfaces) { From 43e6ff61aecb3269309a87a123a26ccfd8f27fe9 Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Thu, 14 Jul 2022 08:26:19 +0100 Subject: [PATCH 29/35] Update changelogs --- controller_interface/CHANGELOG.rst | 3 +++ controller_manager/CHANGELOG.rst | 6 ++++++ controller_manager_msgs/CHANGELOG.rst | 3 +++ hardware_interface/CHANGELOG.rst | 5 +++++ joint_limits/CHANGELOG.rst | 3 +++ ros2_control/CHANGELOG.rst | 3 +++ ros2_control_test_assets/CHANGELOG.rst | 3 +++ ros2controlcli/CHANGELOG.rst | 3 +++ transmission_interface/CHANGELOG.rst | 3 +++ 9 files changed, 32 insertions(+) diff --git a/controller_interface/CHANGELOG.rst b/controller_interface/CHANGELOG.rst index 55be59685f1..4643def4bec 100644 --- a/controller_interface/CHANGELOG.rst +++ b/controller_interface/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package controller_interface ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 2.12.0 (2022-07-09) ------------------- diff --git a/controller_manager/CHANGELOG.rst b/controller_manager/CHANGELOG.rst index c44bdd6651c..e5fd3439ec0 100644 --- a/controller_manager/CHANGELOG.rst +++ b/controller_manager/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package controller_manager ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Rename CM members from start/stop to activate/deactivate nomenclature. (`#756 `_) +* Fix spelling in comment (`#769 `_) +* Contributors: Denis Štogl, Tyler Weaver + 2.12.0 (2022-07-09) ------------------- * Deprecate and rename `start` and `stop` nomenclature toward user to `activate` and `deactivate` #ABI-breaking (`#755 `_) diff --git a/controller_manager_msgs/CHANGELOG.rst b/controller_manager_msgs/CHANGELOG.rst index d32b5be2dd8..b62b0c72137 100644 --- a/controller_manager_msgs/CHANGELOG.rst +++ b/controller_manager_msgs/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package controller_manager_msgs ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 2.12.0 (2022-07-09) ------------------- * Deprecate and rename `start` and `stop` nomenclature toward user to `activate` and `deactivate` #ABI-breaking (`#755 `_) diff --git a/hardware_interface/CHANGELOG.rst b/hardware_interface/CHANGELOG.rst index 4475e977601..b70d019c767 100644 --- a/hardware_interface/CHANGELOG.rst +++ b/hardware_interface/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package hardware_interface ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Fix fake components deprecation and add test for it (`#771 `_) +* Contributors: Bence Magyar + 2.12.0 (2022-07-09) ------------------- * Hardware interface specific update rate and best practices about it (`#716 `_) diff --git a/joint_limits/CHANGELOG.rst b/joint_limits/CHANGELOG.rst index dcff615f662..998c8159151 100644 --- a/joint_limits/CHANGELOG.rst +++ b/joint_limits/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package joint_limits ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 2.12.0 (2022-07-09) ------------------- * Move Joint Limits structures for use in controllers (`#462 `_) diff --git a/ros2_control/CHANGELOG.rst b/ros2_control/CHANGELOG.rst index 9984eeacd9a..354fa8afff6 100644 --- a/ros2_control/CHANGELOG.rst +++ b/ros2_control/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ros2_control ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 2.12.0 (2022-07-09) ------------------- * Move Joint Limits structures for use in controllers (`#462 `_) diff --git a/ros2_control_test_assets/CHANGELOG.rst b/ros2_control_test_assets/CHANGELOG.rst index ca31f420812..fcad0d5e541 100644 --- a/ros2_control_test_assets/CHANGELOG.rst +++ b/ros2_control_test_assets/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ros2_control_test_assets ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 2.12.0 (2022-07-09) ------------------- diff --git a/ros2controlcli/CHANGELOG.rst b/ros2controlcli/CHANGELOG.rst index 97c6b209885..85fb1ac6c96 100644 --- a/ros2controlcli/CHANGELOG.rst +++ b/ros2controlcli/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ros2controlcli ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 2.12.0 (2022-07-09) ------------------- * Deprecate and rename `start` and `stop` nomenclature toward user to `activate` and `deactivate` #ABI-breaking (`#755 `_) diff --git a/transmission_interface/CHANGELOG.rst b/transmission_interface/CHANGELOG.rst index 4af18fe412f..3c865e5f5be 100644 --- a/transmission_interface/CHANGELOG.rst +++ b/transmission_interface/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package transmission_interface ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 2.12.0 (2022-07-09) ------------------- From 300328516752c4020c561fd1a2dc96e9f52d3033 Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Thu, 14 Jul 2022 08:26:39 +0100 Subject: [PATCH 30/35] 2.12.1 --- controller_interface/CHANGELOG.rst | 4 ++-- controller_interface/package.xml | 2 +- controller_manager/CHANGELOG.rst | 4 ++-- controller_manager/package.xml | 2 +- controller_manager_msgs/CHANGELOG.rst | 4 ++-- controller_manager_msgs/package.xml | 2 +- hardware_interface/CHANGELOG.rst | 4 ++-- hardware_interface/package.xml | 2 +- joint_limits/CHANGELOG.rst | 4 ++-- joint_limits/package.xml | 2 +- ros2_control/CHANGELOG.rst | 4 ++-- ros2_control/package.xml | 2 +- ros2_control_test_assets/CHANGELOG.rst | 4 ++-- ros2_control_test_assets/package.xml | 2 +- ros2controlcli/CHANGELOG.rst | 4 ++-- ros2controlcli/package.xml | 2 +- ros2controlcli/setup.py | 2 +- transmission_interface/CHANGELOG.rst | 4 ++-- transmission_interface/package.xml | 2 +- 19 files changed, 28 insertions(+), 28 deletions(-) diff --git a/controller_interface/CHANGELOG.rst b/controller_interface/CHANGELOG.rst index 4643def4bec..23ad3b38fe8 100644 --- a/controller_interface/CHANGELOG.rst +++ b/controller_interface/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package controller_interface ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.12.1 (2022-07-14) +------------------- 2.12.0 (2022-07-09) ------------------- diff --git a/controller_interface/package.xml b/controller_interface/package.xml index 7a0deecd5d8..bdbca6c77e1 100644 --- a/controller_interface/package.xml +++ b/controller_interface/package.xml @@ -2,7 +2,7 @@ controller_interface - 2.12.0 + 2.12.1 Description of controller_interface Bence Magyar Denis Štogl diff --git a/controller_manager/CHANGELOG.rst b/controller_manager/CHANGELOG.rst index e5fd3439ec0..d0803a53d5c 100644 --- a/controller_manager/CHANGELOG.rst +++ b/controller_manager/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package controller_manager ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.12.1 (2022-07-14) +------------------- * Rename CM members from start/stop to activate/deactivate nomenclature. (`#756 `_) * Fix spelling in comment (`#769 `_) * Contributors: Denis Štogl, Tyler Weaver diff --git a/controller_manager/package.xml b/controller_manager/package.xml index ce8d49f653a..84acbaef179 100644 --- a/controller_manager/package.xml +++ b/controller_manager/package.xml @@ -2,7 +2,7 @@ controller_manager - 2.12.0 + 2.12.1 Description of controller_manager Bence Magyar Denis Štogl diff --git a/controller_manager_msgs/CHANGELOG.rst b/controller_manager_msgs/CHANGELOG.rst index b62b0c72137..3336dbebb96 100644 --- a/controller_manager_msgs/CHANGELOG.rst +++ b/controller_manager_msgs/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package controller_manager_msgs ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.12.1 (2022-07-14) +------------------- 2.12.0 (2022-07-09) ------------------- diff --git a/controller_manager_msgs/package.xml b/controller_manager_msgs/package.xml index 4dae1eaefe3..e3d6c428189 100644 --- a/controller_manager_msgs/package.xml +++ b/controller_manager_msgs/package.xml @@ -2,7 +2,7 @@ controller_manager_msgs - 2.12.0 + 2.12.1 Messages and services for the controller manager. Bence Magyar Denis Štogl diff --git a/hardware_interface/CHANGELOG.rst b/hardware_interface/CHANGELOG.rst index b70d019c767..c34666e5cfc 100644 --- a/hardware_interface/CHANGELOG.rst +++ b/hardware_interface/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package hardware_interface ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.12.1 (2022-07-14) +------------------- * Fix fake components deprecation and add test for it (`#771 `_) * Contributors: Bence Magyar diff --git a/hardware_interface/package.xml b/hardware_interface/package.xml index 5038680087d..58b8c1ebffa 100644 --- a/hardware_interface/package.xml +++ b/hardware_interface/package.xml @@ -1,7 +1,7 @@ hardware_interface - 2.12.0 + 2.12.1 ros2_control hardware interface Bence Magyar Denis Štogl diff --git a/joint_limits/CHANGELOG.rst b/joint_limits/CHANGELOG.rst index 998c8159151..9ea7214da6c 100644 --- a/joint_limits/CHANGELOG.rst +++ b/joint_limits/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package joint_limits ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.12.1 (2022-07-14) +------------------- 2.12.0 (2022-07-09) ------------------- diff --git a/joint_limits/package.xml b/joint_limits/package.xml index ded7f001c8e..2db5a070338 100644 --- a/joint_limits/package.xml +++ b/joint_limits/package.xml @@ -1,6 +1,6 @@ joint_limits - 2.12.0 + 2.12.1 Interfaces for handling of joint limits for controllers or hardware. Bence Magyar diff --git a/ros2_control/CHANGELOG.rst b/ros2_control/CHANGELOG.rst index 354fa8afff6..b9cf4a5591b 100644 --- a/ros2_control/CHANGELOG.rst +++ b/ros2_control/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ros2_control ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.12.1 (2022-07-14) +------------------- 2.12.0 (2022-07-09) ------------------- diff --git a/ros2_control/package.xml b/ros2_control/package.xml index 950f596fb12..41a2bef8110 100644 --- a/ros2_control/package.xml +++ b/ros2_control/package.xml @@ -1,7 +1,7 @@ ros2_control - 2.12.0 + 2.12.1 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 fcad0d5e541..7526cb1c7c5 100644 --- a/ros2_control_test_assets/CHANGELOG.rst +++ b/ros2_control_test_assets/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ros2_control_test_assets ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.12.1 (2022-07-14) +------------------- 2.12.0 (2022-07-09) ------------------- diff --git a/ros2_control_test_assets/package.xml b/ros2_control_test_assets/package.xml index 3afa30b42be..be3074c2155 100644 --- a/ros2_control_test_assets/package.xml +++ b/ros2_control_test_assets/package.xml @@ -2,7 +2,7 @@ ros2_control_test_assets - 2.12.0 + 2.12.1 The package provides shared test resources for ros2_control stack Bence Magyar diff --git a/ros2controlcli/CHANGELOG.rst b/ros2controlcli/CHANGELOG.rst index 85fb1ac6c96..e7165e15631 100644 --- a/ros2controlcli/CHANGELOG.rst +++ b/ros2controlcli/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ros2controlcli ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.12.1 (2022-07-14) +------------------- 2.12.0 (2022-07-09) ------------------- diff --git a/ros2controlcli/package.xml b/ros2controlcli/package.xml index cc5e4515f01..42db799d278 100644 --- a/ros2controlcli/package.xml +++ b/ros2controlcli/package.xml @@ -2,7 +2,7 @@ ros2controlcli - 2.12.0 + 2.12.1 The ROS 2 command line tools for ROS2 Control. diff --git a/ros2controlcli/setup.py b/ros2controlcli/setup.py index ed34b82f33e..cf1cc6c901e 100644 --- a/ros2controlcli/setup.py +++ b/ros2controlcli/setup.py @@ -5,7 +5,7 @@ setup( name=package_name, - version='2.12.0', + version='2.12.1', packages=find_packages(exclude=['test']), data_files=[ ('share/' + package_name, ['package.xml']), diff --git a/transmission_interface/CHANGELOG.rst b/transmission_interface/CHANGELOG.rst index 3c865e5f5be..c2555353a15 100644 --- a/transmission_interface/CHANGELOG.rst +++ b/transmission_interface/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package transmission_interface ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.12.1 (2022-07-14) +------------------- 2.12.0 (2022-07-09) ------------------- diff --git a/transmission_interface/package.xml b/transmission_interface/package.xml index 6be610c4c51..05ce647cf51 100644 --- a/transmission_interface/package.xml +++ b/transmission_interface/package.xml @@ -2,7 +2,7 @@ transmission_interface - 2.12.0 + 2.12.1 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 From 2c459d2362b257cb3634c8cbb084e078d0799840 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Fri, 15 Jul 2022 08:47:27 +0200 Subject: [PATCH 31/35] Fix readme links to branches (#772) --- README.md | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/README.md b/README.md index 0d060c76f58..f19f01cc102 100644 --- a/README.md +++ b/README.md @@ -10,8 +10,8 @@ For more, please check the [documentation](https://ros-controls.github.io/contro ROS2 Distro | Branch | Build status | Documentation | Released packages :---------: | :----: | :----------: | :-----------: | :---------------: -**Rolling** | [`rolling`](https://github.com/ros-controls/ros2_control/tree/rolling) | [![Rolling Binary Build](https://github.com/ros-controls/ros2_control/actions/workflows/rolling-binary-build-main.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_control/actions/workflows/rolling-binary-build-main.yml?branch=master)
[![Rolling Semi-Binary Build](https://github.com/ros-controls/ros2_control/actions/workflows/rolling-semi-binary-build-main.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_control/actions/workflows/rolling-semi-binary-build-main.yml?branch=master) | [Documentation](https://control.ros.org/master/index.html)
[API Reference](https://control.ros.org/master/doc/api/index.html) | [ros2_control](https://index.ros.org/p/ros2_control/#rolling) -**Humble** | [`humble`](https://github.com/ros-controls/ros2_control/tree/humble) | [![Humble Binary Build](https://github.com/ros-controls/ros2_control/actions/workflows/humble-binary-build-main.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_control/actions/workflows/humble-binary-build-main.yml?branch=master)
[![Humble Semi-Binary Build](https://github.com/ros-controls/ros2_control/actions/workflows/humble-semi-binary-build-main.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_control/actions/workflows/humble-semi-binary-build-main.yml?branch=master) | [Documentation](https://control.ros.org/master/index.html)
[API Reference](https://control.ros.org/master/doc/api/index.html) | [ros2_control](https://index.ros.org/p/ros2_control/#humble) +**Rolling** | [`rolling`](https://github.com/ros-controls/ros2_control/tree/master) | [![Rolling Binary Build](https://github.com/ros-controls/ros2_control/actions/workflows/rolling-binary-build-main.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_control/actions/workflows/rolling-binary-build-main.yml?branch=master)
[![Rolling Semi-Binary Build](https://github.com/ros-controls/ros2_control/actions/workflows/rolling-semi-binary-build-main.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_control/actions/workflows/rolling-semi-binary-build-main.yml?branch=master) | [Documentation](https://control.ros.org/master/index.html)
[API Reference](https://control.ros.org/master/doc/api/index.html) | [ros2_control](https://index.ros.org/p/ros2_control/#rolling) +**Humble** | [`humble`](https://github.com/ros-controls/ros2_control/tree/master) | [![Humble Binary Build](https://github.com/ros-controls/ros2_control/actions/workflows/humble-binary-build-main.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_control/actions/workflows/humble-binary-build-main.yml?branch=master)
[![Humble Semi-Binary Build](https://github.com/ros-controls/ros2_control/actions/workflows/humble-semi-binary-build-main.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_control/actions/workflows/humble-semi-binary-build-main.yml?branch=master) | [Documentation](https://control.ros.org/master/index.html)
[API Reference](https://control.ros.org/master/doc/api/index.html) | [ros2_control](https://index.ros.org/p/ros2_control/#humble) **Galactic** | [`galactic`](https://github.com/ros-controls/ros2_control/tree/galactic) | [![Galactic Binary Build](https://github.com/ros-controls/ros2_control/actions/workflows/galactic-binary-build-main.yml/badge.svg?branch=galactic)](https://github.com/ros-controls/ros2_control/actions/workflows/galactic-binary-build-main.yml?branch=galactic)
[![Galactic Semi-Binary Build](https://github.com/ros-controls/ros2_control/actions/workflows/galactic-semi-binary-build-main.yml/badge.svg?branch=galactic)](https://github.com/ros-controls/ros2_control/actions/workflows/galactic-semi-binary-build-main.yml?branch=galactic) | [Documentation](https://control.ros.org/galactic/index.html)
[API Reference](https://control.ros.org/galactic/doc/api/index.html) | [ros2_control](https://index.ros.org/p/ros2_control/#galactic) **Foxy** | [`foxy`](https://github.com/ros-controls/ros2_control/tree/foxy) | [![Foxy Binary Build](https://github.com/ros-controls/ros2_control/actions/workflows/foxy-binary-build-main.yml/badge.svg?branch=foxy)](https://github.com/ros-controls/ros2_control/actions/workflows/foxy-binary-build-main.yml?branch=foxy)
[![Foxy Semi-Binary Build](https://github.com/ros-controls/ros2_control/actions/workflows/foxy-semi-binary-build-main.yml/badge.svg?branch=foxy)](https://github.com/ros-controls/ros2_control/actions/workflows/foxy-semi-binary-build-main.yml?branch=foxy) | [Documentation](https://control.ros.org/foxy/index.html)
[API Reference](https://control.ros.org/foxy/doc/api/index.html) | [ros2_control](https://index.ros.org/p/ros2_control/#foxy) From 777f24fff7473f3197d7140a4c71511199e28df4 Mon Sep 17 00:00:00 2001 From: Leander Stephen D'Souza Date: Fri, 15 Jul 2022 12:30:44 +0530 Subject: [PATCH 32/35] Added spawner colors to command interfaces based on availablity and claimed status (#754) --- .../verb/list_hardware_interfaces.py | 17 ++++++++++++----- 1 file changed, 12 insertions(+), 5 deletions(-) diff --git a/ros2controlcli/ros2controlcli/verb/list_hardware_interfaces.py b/ros2controlcli/ros2controlcli/verb/list_hardware_interfaces.py index 0bde1876cee..6065e080b1a 100644 --- a/ros2controlcli/ros2controlcli/verb/list_hardware_interfaces.py +++ b/ros2controlcli/ros2controlcli/verb/list_hardware_interfaces.py @@ -13,6 +13,7 @@ # limitations under the License. from controller_manager import list_hardware_interfaces +from controller_manager.spawner import bcolors from ros2cli.node.direct import add_arguments from ros2cli.node.strategy import NodeStrategy @@ -38,11 +39,17 @@ def main(self, *, args): ) print('command interfaces') for command_interface in command_interfaces: - print( - f'\t{command_interface.name} ' - f'{"[available]" if command_interface.is_available else "[unavailable]"} ' - f'{"[claimed]" if command_interface.is_claimed else "[unclaimed]"}' - ) + if command_interface.is_available: + if command_interface.is_claimed: + print(f'\t{bcolors.OKBLUE}{command_interface.name} ' + f'[available] [claimed]{bcolors.ENDC}') + else: + print(f'\t{bcolors.OKCYAN}{command_interface.name} ' + f'[available] [unclaimed]{bcolors.ENDC}') + else: + print(f'\t{bcolors.WARNING}{command_interface.name} ' + f'[unavailable] [unclaimed]{bcolors.ENDC}') + print('state interfaces') for state_interface in state_interfaces: print(f'\t{state_interface.name}') From b3e601a535ff2c875af0d1f2aa3c8ea01472176f Mon Sep 17 00:00:00 2001 From: bijoua29 <73511637+bijoua29@users.noreply.github.com> Date: Sat, 16 Jul 2022 07:17:14 -0700 Subject: [PATCH 33/35] Fix bug in spawner with getter for node's logger (#776) --- controller_manager/controller_manager/spawner.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/controller_manager/controller_manager/spawner.py b/controller_manager/controller_manager/spawner.py index 5c478fc62c0..d612a42e776 100644 --- a/controller_manager/controller_manager/spawner.py +++ b/controller_manager/controller_manager/spawner.py @@ -212,7 +212,7 @@ def main(args=None): node.get_logger().info(bcolors.OKGREEN + 'Configured and activated ' + bcolors.OKCYAN + controller_name + bcolors.ENDC) elif args.stopped: - node.get_logger.warn('"--stopped" flag is deprecated use "--inactive" instead') + node.get_logger().warn('"--stopped" flag is deprecated use "--inactive" instead') if not args.unload_on_kill: return 0 From 9c4c40e732f07f7dbc0c6881bd7462c72c2c179e Mon Sep 17 00:00:00 2001 From: AndyZe Date: Wed, 20 Jul 2022 13:40:29 -0700 Subject: [PATCH 34/35] Add documentation for realtime permission (#781) * Add documentation for realtime permission * rst syntax --- controller_manager/doc/userdoc.rst | 26 +++++++++++++++++++++++--- 1 file changed, 23 insertions(+), 3 deletions(-) diff --git a/controller_manager/doc/userdoc.rst b/controller_manager/doc/userdoc.rst index b086c90a8a5..98c93e07a0d 100644 --- a/controller_manager/doc/userdoc.rst +++ b/controller_manager/doc/userdoc.rst @@ -10,9 +10,29 @@ Determinism For best performance when controlling hardware you want the controller manager to have as little jitter as possible in the main control loop. The normal linux kernel is optimized for computational throughput and therefore is not well suited for hardware control. -The main thread of Controller Manager attempts to configure ``SCHED_FIFO`` with a priority of ``50``. -To enable this functionality install a RT kernel and run the Controller Manager with permissions to make syscalls to set its thread priorities. -The two easiest options for this are using the [Real-time Ubuntu 22.04 LTS Beta](https://ubuntu.com/blog/real-time-ubuntu-released) or [linux-image-rt-amd64](https://packages.debian.org/bullseye/linux-image-rt-amd64) on Debian Bullseye. +The two easiest kernel options are the `Real-time Ubuntu 22.04 LTS Beta `_ or `linux-image-rt-amd64 `_ on Debian Bullseye. + +If you have a realtime kernel installed, the main thread of Controller Manager attempts to configure ``SCHED_FIFO`` with a priority of ``50``. +By default, the user does not have permission to set such a high priority. +To give the user such permissions, add a group named realtime and add the user controlling your robot to this group: + +.. code-block:: console + + $ sudo addgroup realtime + $ sudo usermod -a -G realtime $(whoami) + +Afterwards, add the following limits to the realtime group in ``/etc/security/limits.conf``: + +.. code-block:: console + + @realtime soft rtprio 99 + @realtime soft priority 99 + @realtime soft memlock 102400 + @realtime hard rtprio 99 + @realtime hard priority 99 + @realtime hard memlock 102400 + +The limits will be applied after you log out and in again. Parameters ----------- From 65a3a823e7f006c033af19f604e385394666e42f Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Wed, 27 Jul 2022 12:43:00 +0200 Subject: [PATCH 35/35] spawner.py: Fix python logging on deprecation warning (#787) This was an error introduced in abd5d6e6c8264706b58dddc877063f884cf34a58 --- controller_manager/controller_manager/spawner.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/controller_manager/controller_manager/spawner.py b/controller_manager/controller_manager/spawner.py index d612a42e776..43a02865e1d 100644 --- a/controller_manager/controller_manager/spawner.py +++ b/controller_manager/controller_manager/spawner.py @@ -239,7 +239,7 @@ def main(args=None): node.get_logger().info('Deactivated controller') elif args.stopped: - node.get_logger.warn('"--stopped" flag is deprecated use "--inactive" instead') + node.get_logger().warn('"--stopped" flag is deprecated use "--inactive" instead') ret = unload_controller( node, controller_manager_name, controller_name)