diff --git a/controller_manager/include/controller_manager/controller_manager.hpp b/controller_manager/include/controller_manager/controller_manager.hpp index 0866280eb7..a81b5f0ad9 100644 --- a/controller_manager/include/controller_manager/controller_manager.hpp +++ b/controller_manager/include/controller_manager/controller_manager.hpp @@ -28,11 +28,13 @@ #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" #include "controller_manager_msgs/srv/unload_controller.hpp" @@ -67,6 +69,9 @@ class ControllerManager : public rclcpp::Node CONTROLLER_MANAGER_PUBLIC virtual ~ControllerManager() = default; + CONTROLLER_MANAGER_PUBLIC + void init_resource_manager(const std::string & robot_description); + CONTROLLER_MANAGER_PUBLIC controller_interface::ControllerInterfaceSharedPtr load_controller( const std::string & controller_name, const std::string & controller_type); @@ -173,11 +178,6 @@ class ControllerManager : public rclcpp::Node const std::shared_ptr request, std::shared_ptr response); - CONTROLLER_MANAGER_PUBLIC - void list_controller_types_srv_cb( - const std::shared_ptr request, - std::shared_ptr response); - CONTROLLER_MANAGER_PUBLIC void list_hardware_interfaces_srv_cb( const std::shared_ptr request, @@ -223,6 +223,21 @@ class ControllerManager : public rclcpp::Node const std::shared_ptr request, std::shared_ptr response); + CONTROLLER_MANAGER_PUBLIC + void list_controller_types_srv_cb( + const std::shared_ptr request, + std::shared_ptr response); + + CONTROLLER_MANAGER_PUBLIC + void list_hardware_components_srv_cb( + const std::shared_ptr request, + std::shared_ptr response); + + CONTROLLER_MANAGER_PUBLIC + void set_hardware_component_state_srv_cb( + const std::shared_ptr request, + std::shared_ptr response); + // Per controller update rate support unsigned int update_loop_counter_ = 0; unsigned int update_rate_ = 100; @@ -323,8 +338,6 @@ class ControllerManager : public rclcpp::Node list_controllers_service_; rclcpp::Service::SharedPtr list_controller_types_service_; - rclcpp::Service::SharedPtr - list_hardware_interfaces_service_; rclcpp::Service::SharedPtr load_controller_service_; rclcpp::Service::SharedPtr configure_controller_service_; @@ -341,6 +354,13 @@ class ControllerManager : public rclcpp::Node rclcpp::Service::SharedPtr unload_controller_service_; + rclcpp::Service::SharedPtr + list_hardware_components_service_; + rclcpp::Service::SharedPtr + list_hardware_interfaces_service_; + rclcpp::Service::SharedPtr + set_hardware_component_state_service_; + std::vector start_request_, stop_request_; std::vector start_command_interface_request_, stop_command_interface_request_; diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 71a62bba6a..d99e474041 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -112,11 +112,6 @@ void ControllerManager::init_services() "~/list_controller_types", std::bind(&ControllerManager::list_controller_types_srv_cb, this, _1, _2), rmw_qos_profile_services_default, best_effort_callback_group_); - list_hardware_interfaces_service_ = - create_service( - "~/list_hardware_interfaces", - std::bind(&ControllerManager::list_hardware_interfaces_srv_cb, this, _1, _2), - rmw_qos_profile_services_default, best_effort_callback_group_); load_controller_service_ = create_service( "~/load_controller", std::bind(&ControllerManager::load_controller_service_cb, this, _1, _2), rmw_qos_profile_services_default, best_effort_callback_group_); @@ -152,6 +147,21 @@ void ControllerManager::init_services() "~/unload_controller", std::bind(&ControllerManager::unload_controller_service_cb, this, _1, _2), rmw_qos_profile_services_default, best_effort_callback_group_); + list_hardware_components_service_ = + create_service( + "~/list_hardware_components", + std::bind(&ControllerManager::list_hardware_components_srv_cb, this, _1, _2), + rmw_qos_profile_services_default, best_effort_callback_group_); + list_hardware_interfaces_service_ = + create_service( + "~/list_hardware_interfaces", + std::bind(&ControllerManager::list_hardware_interfaces_srv_cb, this, _1, _2), + rmw_qos_profile_services_default, best_effort_callback_group_); + set_hardware_component_state_service_ = + create_service( + "~/set_hardware_component_state", + std::bind(&ControllerManager::set_hardware_component_state_srv_cb, this, _1, _2), + rmw_qos_profile_services_default, best_effort_callback_group_); } controller_interface::ControllerInterfaceSharedPtr ControllerManager::load_controller( @@ -909,28 +919,6 @@ void ControllerManager::list_controller_types_srv_cb( RCLCPP_DEBUG(get_logger(), "list types service finished"); } -void ControllerManager::list_hardware_interfaces_srv_cb( - const std::shared_ptr, - std::shared_ptr response) -{ - auto state_interface_names = resource_manager_->state_interface_keys(); - for (const auto & state_interface_name : state_interface_names) - { - controller_manager_msgs::msg::HardwareInterface hwi; - hwi.name = state_interface_name; - hwi.is_claimed = false; - response->state_interfaces.push_back(hwi); - } - auto command_interface_names = resource_manager_->command_interface_keys(); - for (const auto & command_interface_name : command_interface_names) - { - controller_manager_msgs::msg::HardwareInterface hwi; - hwi.name = command_interface_name; - hwi.is_claimed = resource_manager_->command_interface_is_claimed(command_interface_name); - response->command_interfaces.push_back(hwi); - } -} - void ControllerManager::load_controller_service_cb( const std::shared_ptr request, std::shared_ptr response) @@ -1159,6 +1147,58 @@ void ControllerManager::unload_controller_service_cb( get_logger(), "unloading service finished for controller '%s' ", request->name.c_str()); } +void ControllerManager::list_hardware_components_srv_cb( + const std::shared_ptr, + std::shared_ptr response) +{ + RCLCPP_DEBUG(get_logger(), "list hardware components service called"); + std::lock_guard guard(services_lock_); + RCLCPP_DEBUG(get_logger(), "list hardware components service locked"); + + RCLCPP_DEBUG(get_logger(), "list hardware components service finished"); +} + +void ControllerManager::list_hardware_interfaces_srv_cb( + const std::shared_ptr, + std::shared_ptr response) +{ + RCLCPP_DEBUG(get_logger(), "list hardware interfaces service called"); + std::lock_guard guard(services_lock_); + RCLCPP_DEBUG(get_logger(), "list hardware interfaces service locked"); + + auto state_interface_names = resource_manager_->state_interface_keys(); + for (const auto & state_interface_name : state_interface_names) + { + controller_manager_msgs::msg::HardwareInterface hwi; + hwi.name = state_interface_name; + hwi.is_claimed = false; + response->state_interfaces.push_back(hwi); + } + auto command_interface_names = resource_manager_->command_interface_keys(); + for (const auto & command_interface_name : command_interface_names) + { + controller_manager_msgs::msg::HardwareInterface hwi; + hwi.name = command_interface_name; + hwi.is_claimed = resource_manager_->command_interface_is_claimed(command_interface_name); + response->command_interfaces.push_back(hwi); + } + + RCLCPP_DEBUG(get_logger(), "list hardware interfaces service finished"); +} + +void ControllerManager::set_hardware_component_state_srv_cb( + const std::shared_ptr request, + std::shared_ptr response) +{ + RCLCPP_DEBUG(get_logger(), "set hardware component state service called"); + std::lock_guard guard(services_lock_); + RCLCPP_DEBUG(get_logger(), "set hardware component state service locked"); + + RCLCPP_DEBUG(get_logger(), "set hardware component state '%s'", request->name.c_str()); + + RCLCPP_DEBUG(get_logger(), "set hardware component state service finished"); +} + std::vector ControllerManager::get_controller_names() { std::vector names; diff --git a/controller_manager_msgs/CMakeLists.txt b/controller_manager_msgs/CMakeLists.txt index a23311cfc6..3f0ce8a0b2 100644 --- a/controller_manager_msgs/CMakeLists.txt +++ b/controller_manager_msgs/CMakeLists.txt @@ -7,10 +7,12 @@ endif() find_package(ament_cmake REQUIRED) find_package(builtin_interfaces REQUIRED) +find_package(lifecycle_msgs REQUIRED) find_package(rosidl_default_generators REQUIRED) set(msg_files msg/ControllerState.msg + msg/HardwareComponentState.msg msg/HardwareInterface.msg ) set(srv_files @@ -18,11 +20,13 @@ set(srv_files 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 srv/UnloadController.srv ) @@ -35,7 +39,7 @@ endif() rosidl_generate_interfaces(${PROJECT_NAME} ${msg_files} ${srv_files} - DEPENDENCIES builtin_interfaces + DEPENDENCIES builtin_interfaces lifecycle_msgs ADD_LINTER_TESTS ) ament_export_dependencies(rosidl_default_runtime) diff --git a/controller_manager_msgs/msg/HardwareComponentState.msg b/controller_manager_msgs/msg/HardwareComponentState.msg new file mode 100644 index 0000000000..51fd170701 --- /dev/null +++ b/controller_manager_msgs/msg/HardwareComponentState.msg @@ -0,0 +1,6 @@ +string name +string type +string class_type +lifecycle_msgs/State state +HardwareInterface[] command_interfaces +HardwareInterface[] state_interfaces diff --git a/controller_manager_msgs/msg/HardwareInterface.msg b/controller_manager_msgs/msg/HardwareInterface.msg index b6ce4bab24..80807ef859 100644 --- a/controller_manager_msgs/msg/HardwareInterface.msg +++ b/controller_manager_msgs/msg/HardwareInterface.msg @@ -1,2 +1,3 @@ string name +bool is_available bool is_claimed diff --git a/controller_manager_msgs/package.xml b/controller_manager_msgs/package.xml index 2eaca93509..ab87eab96b 100644 --- a/controller_manager_msgs/package.xml +++ b/controller_manager_msgs/package.xml @@ -13,8 +13,10 @@ rosidl_default_generators builtin_interfaces + lifecycle_msgs builtin_interfaces + lifecycle_msgs rosidl_default_runtime ament_lint_auto diff --git a/controller_manager_msgs/srv/ListHardwareComponents.srv b/controller_manager_msgs/srv/ListHardwareComponents.srv new file mode 100644 index 0000000000..393db0eaf5 --- /dev/null +++ b/controller_manager_msgs/srv/ListHardwareComponents.srv @@ -0,0 +1,6 @@ +# The ListHardwareComponents service returns a list of hardware HardwareComponentsState. +# This will convey name, component_type, state and type of the components +# that are loaded inside the resource_manager. + +--- +HardwareComponentState[] component diff --git a/controller_manager_msgs/srv/SetHardwareComponentState.srv b/controller_manager_msgs/srv/SetHardwareComponentState.srv new file mode 100644 index 0000000000..470c809972 --- /dev/null +++ b/controller_manager_msgs/srv/SetHardwareComponentState.srv @@ -0,0 +1,14 @@ +# The SetHardwareComponentState service allows to control life-cycle of a single hardware component. +# Supported states are defined in the design document of LifecycleNodes available at: +# https://design.ros2.org/articles/node_lifecycle.html +# To control life-cycle of a hardware component, specify its "name" and "target_state". +# Target state may be defined by "id" using a constant from `lifecycle_msgs/msg/State` or a label +# using definitions from `hardware_interface/types/lifecycle_state_names.hpp` file. +# The return value "ok" indicates if the component has successfully changed its state to "target_state". +# The return value "state" returns current state of the hardware component. + +string name +lifecycle_msgs/State target_state +--- +bool ok +lifecycle_msgs/State state