diff --git a/controller_interface/test/test_force_torque_sensor.cpp b/controller_interface/test/test_force_torque_sensor.cpp index dd1e55e126..c4cef85162 100644 --- a/controller_interface/test/test_force_torque_sensor.cpp +++ b/controller_interface/test/test_force_torque_sensor.cpp @@ -47,20 +47,20 @@ TEST_F(ForceTorqueSensorTest, validate_all_with_default_names) std::vector interface_names = force_torque_sensor_->get_state_interface_names(); // assign values to force - hardware_interface::StateInterface force_x{ - sensor_name_, fts_interface_names_[0], &force_values_[0]}; - hardware_interface::StateInterface force_y{ - sensor_name_, fts_interface_names_[1], &force_values_[1]}; - hardware_interface::StateInterface force_z{ - sensor_name_, fts_interface_names_[2], &force_values_[2]}; + auto force_x = std::make_shared( + sensor_name_, fts_interface_names_[0], &force_values_[0]); + auto force_y = std::make_shared( + sensor_name_, fts_interface_names_[1], &force_values_[1]); + auto force_z = std::make_shared( + sensor_name_, fts_interface_names_[2], &force_values_[2]); // assign values to torque - hardware_interface::StateInterface torque_x{ - sensor_name_, fts_interface_names_[3], &torque_values_[0]}; - hardware_interface::StateInterface torque_y{ - sensor_name_, fts_interface_names_[4], &torque_values_[1]}; - hardware_interface::StateInterface torque_z{ - sensor_name_, fts_interface_names_[5], &torque_values_[2]}; + auto torque_x = std::make_shared( + sensor_name_, fts_interface_names_[3], &torque_values_[0]); + auto torque_y = std::make_shared( + sensor_name_, fts_interface_names_[4], &torque_values_[1]); + auto torque_z = std::make_shared( + sensor_name_, fts_interface_names_[5], &torque_values_[2]); // create local state interface vector std::vector temp_state_interfaces; @@ -131,16 +131,16 @@ TEST_F(ForceTorqueSensorTest, validate_all_with_custom_names) std::vector interface_names = force_torque_sensor_->get_state_interface_names(); // assign values to force.x and force.z - hardware_interface::StateInterface force_x{ - sensor_name_, fts_interface_names_[0], &force_values_[0]}; - hardware_interface::StateInterface force_z{ - sensor_name_, fts_interface_names_[2], &force_values_[2]}; + auto force_x = std::make_shared( + sensor_name_, fts_interface_names_[0], &force_values_[0]); + auto force_z = std::make_shared( + sensor_name_, fts_interface_names_[2], &force_values_[2]); // assign values to torque.y and torque.z - hardware_interface::StateInterface torque_y{ - sensor_name_, fts_interface_names_[4], &torque_values_[1]}; - hardware_interface::StateInterface torque_z{ - sensor_name_, fts_interface_names_[5], &torque_values_[2]}; + auto torque_y = std::make_shared( + sensor_name_, fts_interface_names_[4], &torque_values_[1]); + auto torque_z = std::make_shared( + sensor_name_, fts_interface_names_[5], &torque_values_[2]); // create local state interface vector std::vector temp_state_interfaces; @@ -213,20 +213,20 @@ TEST_F(ForceTorqueSensorTest, validate_all_custom_names) ASSERT_EQ(force_torque_sensor_->interface_names_[5], sensor_name_ + "/" + "torque.z"); // assign values to force - hardware_interface::StateInterface force_x{ - sensor_name_, fts_interface_names_[0], &force_values_[0]}; - hardware_interface::StateInterface force_y{ - sensor_name_, fts_interface_names_[1], &force_values_[1]}; - hardware_interface::StateInterface force_z{ - sensor_name_, fts_interface_names_[2], &force_values_[2]}; + auto force_x = std::make_shared( + sensor_name_, fts_interface_names_[0], &force_values_[0]); + auto force_y = std::make_shared( + sensor_name_, fts_interface_names_[1], &force_values_[1]); + auto force_z = std::make_shared( + sensor_name_, fts_interface_names_[2], &force_values_[2]); // assign values to torque - hardware_interface::StateInterface torque_x{ - sensor_name_, fts_interface_names_[3], &torque_values_[0]}; - hardware_interface::StateInterface torque_y{ - sensor_name_, fts_interface_names_[4], &torque_values_[1]}; - hardware_interface::StateInterface torque_z{ - sensor_name_, fts_interface_names_[5], &torque_values_[2]}; + auto torque_x = std::make_shared( + sensor_name_, fts_interface_names_[3], &torque_values_[0]); + auto torque_y = std::make_shared( + sensor_name_, fts_interface_names_[4], &torque_values_[1]); + auto torque_z = std::make_shared( + sensor_name_, fts_interface_names_[5], &torque_values_[2]); // create local state interface vector std::vector temp_state_interfaces; diff --git a/controller_interface/test/test_imu_sensor.cpp b/controller_interface/test/test_imu_sensor.cpp index 1fbf205238..4a1575b1e1 100644 --- a/controller_interface/test/test_imu_sensor.cpp +++ b/controller_interface/test/test_imu_sensor.cpp @@ -46,30 +46,30 @@ TEST_F(IMUSensorTest, validate_all) std::vector interface_names = imu_sensor_->get_state_interface_names(); // assign values to orientation - hardware_interface::StateInterface orientation_x{ - sensor_name_, imu_interface_names_[0], &orientation_values_[0]}; - hardware_interface::StateInterface orientation_y{ - sensor_name_, imu_interface_names_[1], &orientation_values_[1]}; - hardware_interface::StateInterface orientation_z{ - sensor_name_, imu_interface_names_[2], &orientation_values_[2]}; - hardware_interface::StateInterface orientation_w{ - sensor_name_, imu_interface_names_[3], &orientation_values_[4]}; + auto orientation_x = std::make_shared( + sensor_name_, imu_interface_names_[0], &orientation_values_[0]); + auto orientation_y = std::make_shared( + sensor_name_, imu_interface_names_[1], &orientation_values_[1]); + auto orientation_z = std::make_shared( + sensor_name_, imu_interface_names_[2], &orientation_values_[2]); + auto orientation_w = std::make_shared( + sensor_name_, imu_interface_names_[3], &orientation_values_[4]); // assign values to angular velocity - hardware_interface::StateInterface angular_velocity_x{ - sensor_name_, imu_interface_names_[4], &angular_velocity_values_[0]}; - hardware_interface::StateInterface angular_velocity_y{ - sensor_name_, imu_interface_names_[5], &angular_velocity_values_[1]}; - hardware_interface::StateInterface angular_velocity_z{ - sensor_name_, imu_interface_names_[6], &angular_velocity_values_[2]}; + auto angular_velocity_x = std::make_shared( + sensor_name_, imu_interface_names_[4], &angular_velocity_values_[0]); + auto angular_velocity_y = std::make_shared( + sensor_name_, imu_interface_names_[5], &angular_velocity_values_[1]); + auto angular_velocity_z = std::make_shared( + sensor_name_, imu_interface_names_[6], &angular_velocity_values_[2]); // assign values to linear acceleration - hardware_interface::StateInterface linear_acceleration_x{ - sensor_name_, imu_interface_names_[7], &linear_acceleration_values_[0]}; - hardware_interface::StateInterface linear_acceleration_y{ - sensor_name_, imu_interface_names_[8], &linear_acceleration_values_[1]}; - hardware_interface::StateInterface linear_acceleration_z{ - sensor_name_, imu_interface_names_[9], &linear_acceleration_values_[2]}; + auto linear_acceleration_x = std::make_shared( + sensor_name_, imu_interface_names_[7], &linear_acceleration_values_[0]); + auto linear_acceleration_y = std::make_shared( + sensor_name_, imu_interface_names_[8], &linear_acceleration_values_[1]); + auto linear_acceleration_z = std::make_shared( + sensor_name_, imu_interface_names_[9], &linear_acceleration_values_[2]); // create local state interface vector std::vector temp_state_interfaces; diff --git a/controller_interface/test/test_semantic_component_interface.cpp b/controller_interface/test/test_semantic_component_interface.cpp index f81b4f64fe..23171fa288 100644 --- a/controller_interface/test/test_semantic_component_interface.cpp +++ b/controller_interface/test/test_semantic_component_interface.cpp @@ -94,9 +94,12 @@ TEST_F(SemanticComponentInterfaceTest, validate_state_interfaces) std::vector interface_values = {1.1, 3.3, 5.5}; // assign 1.1 to interface_1, 3.3 to interface_3 and 5.5 to interface_5 - hardware_interface::StateInterface interface_1{component_name_, "1", &interface_values[0]}; - hardware_interface::StateInterface interface_3{component_name_, "3", &interface_values[1]}; - hardware_interface::StateInterface interface_5{component_name_, "5", &interface_values[2]}; + auto interface_1 = std::make_shared( + component_name_, "1", &interface_values[0]); + auto interface_3 = std::make_shared( + component_name_, "3", &interface_values[1]); + auto interface_5 = std::make_shared( + component_name_, "5", &interface_values[2]); // create local state interface vector std::vector temp_state_interfaces; diff --git a/controller_manager/CMakeLists.txt b/controller_manager/CMakeLists.txt index 6905679ac3..0089b906e1 100644 --- a/controller_manager/CMakeLists.txt +++ b/controller_manager/CMakeLists.txt @@ -27,12 +27,12 @@ endforeach() add_library(${PROJECT_NAME} SHARED src/controller_manager.cpp ) + target_include_directories(${PROJECT_NAME} PRIVATE include) ament_target_dependencies(${PROJECT_NAME} ${THIS_PACKAGE_INCLUDE_DEPENDS}) # Causes the visibility macros to use dllexport rather than dllimport, # which is appropriate when building the dll but not consuming it. target_compile_definitions(${PROJECT_NAME} PRIVATE "CONTROLLER_MANAGER_BUILDING_DLL") - add_executable(ros2_control_node src/ros2_control_node.cpp) target_include_directories(ros2_control_node PRIVATE include) target_link_libraries(ros2_control_node diff --git a/controller_manager/include/controller_manager/controller_manager.hpp b/controller_manager/include/controller_manager/controller_manager.hpp index e6b377570a..fa67321421 100644 --- a/controller_manager/include/controller_manager/controller_manager.hpp +++ b/controller_manager/include/controller_manager/controller_manager.hpp @@ -35,6 +35,7 @@ #include "controller_manager_msgs/srv/list_hardware_components.hpp" #include "controller_manager_msgs/srv/list_hardware_interfaces.hpp" #include "controller_manager_msgs/srv/load_controller.hpp" +#include "controller_manager_msgs/srv/register_sub_controller_manager.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" @@ -51,7 +52,6 @@ #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; @@ -190,6 +190,27 @@ class ControllerManager : public rclcpp::Node CONTROLLER_MANAGER_PUBLIC void init_services(); + CONTROLLER_MANAGER_PUBLIC + void configure_controller_manager(); + + CONTROLLER_MANAGER_PUBLIC + void init_distributed_main_controller_services(); + + CONTROLLER_MANAGER_PUBLIC + void register_sub_controller_manager_srv_cb( + const std::shared_ptr + request, + std::shared_ptr response); + + CONTROLLER_MANAGER_PUBLIC + void add_hardware_state_publishers(); + + CONTROLLER_MANAGER_PUBLIC + void add_hardware_command_forwarders(); + + CONTROLLER_MANAGER_PUBLIC + void register_sub_controller_manager(); + CONTROLLER_MANAGER_PUBLIC controller_interface::ControllerInterfaceBaseSharedPtr add_controller_impl( const ControllerSpec & controller); @@ -395,6 +416,10 @@ class ControllerManager : public rclcpp::Node */ rclcpp::CallbackGroup::SharedPtr best_effort_callback_group_; + bool distributed_ = false; + bool sub_controller_manager_ = false; + + rclcpp::CallbackGroup::SharedPtr distributed_system_srv_callback_group_; /** * The RTControllerListWrapper class wraps a double-buffered list of controllers * to avoid needing to lock the real-time thread when switching controllers in @@ -492,6 +517,11 @@ class ControllerManager : public rclcpp::Node rclcpp::Service::SharedPtr set_hardware_component_state_service_; + // services for distributed control + std::mutex central_controller_manager_srv_lock_; + rclcpp::Service::SharedPtr + register_sub_controller_manager_srv_; + std::vector activate_request_, deactivate_request_; std::vector to_chained_mode_request_, from_chained_mode_request_; std::vector activate_command_interface_request_, diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 8ce3be22a6..d2c071b497 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -14,6 +14,7 @@ #include "controller_manager/controller_manager.hpp" +#include #include #include #include @@ -22,7 +23,12 @@ #include "controller_interface/controller_interface_base.hpp" #include "controller_manager_msgs/msg/hardware_component_state.hpp" + +#include "hardware_interface/distributed_control_interface/command_forwarder.hpp" +#include "hardware_interface/distributed_control_interface/state_publisher.hpp" +#include "hardware_interface/handle.hpp" #include "hardware_interface/types/lifecycle_state_names.hpp" + #include "lifecycle_msgs/msg/state.hpp" #include "rclcpp/rclcpp.hpp" #include "rclcpp_lifecycle/state.hpp" @@ -41,6 +47,10 @@ rclcpp::QoS qos_services = .reliable() .durability_volatile(); +rclcpp::QoSInitialization qos_profile_services_keep_all_persist_init( + RMW_QOS_POLICY_HISTORY_KEEP_ALL, 1); +rclcpp::QoS qos_profile_services_keep_all(qos_profile_services_keep_all_persist_init); + inline bool is_controller_inactive(const controller_interface::ControllerInterfaceBase & controller) { return controller.get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE; @@ -122,6 +132,7 @@ bool command_interface_is_reference_interface_of_controller( namespace controller_manager { + rclcpp::NodeOptions get_cm_node_options() { rclcpp::NodeOptions node_options; @@ -146,7 +157,8 @@ ControllerManager::ControllerManager( { if (!get_parameter("update_rate", update_rate_)) { - RCLCPP_WARN(get_logger(), "'update_rate' parameter not set, using default value."); + RCLCPP_WARN( + get_logger(), "'update_rate' parameter not set, using default value:%iHz", update_rate_); } std::string robot_description = ""; @@ -162,6 +174,7 @@ ControllerManager::ControllerManager( diagnostics_updater_.add( "Controllers Activity", this, &ControllerManager::controller_activity_diagnostic_callback); init_services(); + configure_controller_manager(); } ControllerManager::ControllerManager( @@ -182,6 +195,7 @@ ControllerManager::ControllerManager( diagnostics_updater_.add( "Controllers Activity", this, &ControllerManager::controller_activity_diagnostic_callback); init_services(); + configure_controller_manager(); } void ControllerManager::init_resource_manager(const std::string & robot_description) @@ -271,6 +285,284 @@ void ControllerManager::init_services() qos_services, best_effort_callback_group_); } +// TODO(Manuel) don't like this, this is for fast poc +// probably better to create factory and handle creation of correct controller manager type +// there. Since asynchronous control should be supported im the future as well and we don't +// want dozen of ifs. +void ControllerManager::configure_controller_manager() +{ + if (!get_parameter("distributed", distributed_)) + { + RCLCPP_WARN( + get_logger(), "'distributed' parameter not set, using default value:%s", + distributed_ ? "true" : "false"); + } + + if (!get_parameter("sub_controller_manager", sub_controller_manager_)) + { + RCLCPP_WARN( + get_logger(), "'sub_controller_manager' parameter not set, using default value:%s", + sub_controller_manager_ ? "true" : "false"); + } + + bool std_controller_manager = !distributed_ && !sub_controller_manager_; + bool distributed_sub_controller_manager = distributed_ && sub_controller_manager_; + bool central_controller_manager = distributed_ && !sub_controller_manager_; + if (distributed_sub_controller_manager) + { + add_hardware_state_publishers(); + add_hardware_command_forwarders(); + register_sub_controller_manager(); + } + // This means we are the central controller manager + else if (central_controller_manager) + { + init_distributed_main_controller_services(); + } + // std controller manager or error. std controller manager needs no special setup. + else + { + if (!std_controller_manager) + { + throw std::logic_error( + "Controller manager configured with: distributed:false and sub_controller_manager:true. " + "Only distributed controller manager can be a sub controller manager."); + } + } +} + +void ControllerManager::init_distributed_main_controller_services() +{ + distributed_system_srv_callback_group_ = + create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + + register_sub_controller_manager_srv_ = + create_service( + "register_sub_controller_manager", + std::bind( + &ControllerManager::register_sub_controller_manager_srv_cb, this, std::placeholders::_1, + std::placeholders::_2), + qos_profile_services_keep_all, distributed_system_srv_callback_group_); +} + +void ControllerManager::register_sub_controller_manager_srv_cb( + const std::shared_ptr + request, + std::shared_ptr response) +{ + std::lock_guard guard(central_controller_manager_srv_lock_); + + auto sub_ctrl_mng_wrapper = std::make_shared( + request->sub_controller_manager_namespace, request->sub_controller_manager_name, + request->state_publishers, request->command_state_publishers); + + resource_manager_->register_sub_controller_manager(sub_ctrl_mng_wrapper); + + std::vector> + distributed_state_interfaces; + distributed_state_interfaces.reserve(sub_ctrl_mng_wrapper->get_state_publisher_count()); + distributed_state_interfaces = + resource_manager_->import_state_interfaces_of_sub_controller_manager(sub_ctrl_mng_wrapper); + + for (const auto & state_interface : distributed_state_interfaces) + { + try + { + executor_->add_node(state_interface->get_node()->get_node_base_interface()); + } + catch (const std::runtime_error & e) + { + response->ok = false; + RCLCPP_WARN_STREAM( + get_logger(), + "ControllerManager: Caught exception while trying to register sub controller manager. " + "Exception:" + << e.what()); + } + } + + std::vector> + distributed_command_interfaces; + distributed_command_interfaces.reserve(sub_ctrl_mng_wrapper->get_command_forwarder_count()); + distributed_command_interfaces = + resource_manager_->import_command_interfaces_of_sub_controller_manager(sub_ctrl_mng_wrapper); + + for (const auto & command_interface : distributed_command_interfaces) + { + try + { + executor_->add_node(command_interface->get_node()->get_node_base_interface()); + } + catch (const std::runtime_error & e) + { + response->ok = false; + RCLCPP_WARN_STREAM( + get_logger(), + "ControllerManager: Caught exception while trying to register sub controller manager. " + "Exception:" + << e.what()); + } + auto msg = controller_manager_msgs::msg::PublisherDescription(); + msg.ns = get_namespace(); + msg.name.prefix_name = command_interface->get_prefix_name(); + msg.name.interface_name = command_interface->get_interface_name(); + // TODO(Manuel) want topic name relative to namespace, but have to treat "root" namespace separate + msg.publisher_topic = std::string("/") + command_interface->forward_command_topic_name(); + response->command_state_publishers.push_back(msg); + } + + response->ok = true; + RCLCPP_INFO_STREAM( + get_logger(), "ControllerManager: Registered sub controller manager <" + << sub_ctrl_mng_wrapper->get_name() << ">."); +} + +void ControllerManager::add_hardware_state_publishers() +{ + std::vector> state_publishers_vec; + state_publishers_vec.reserve(resource_manager_->available_state_interfaces().size()); + state_publishers_vec = resource_manager_->create_hardware_state_publishers(get_namespace()); + + for (auto const & state_publisher : state_publishers_vec) + { + try + { + executor_->add_node(state_publisher->get_node()->get_node_base_interface()); + } + catch (const std::runtime_error & e) + { + RCLCPP_WARN_STREAM( + get_logger(), "ControllerManager: Can't create StatePublishers<" + << state_publisher->state_interface_name() << ">." << e.what()); + } + } +} + +void ControllerManager::add_hardware_command_forwarders() +{ + std::vector> command_forwarder_vec; + command_forwarder_vec.reserve(resource_manager_->available_command_interfaces().size()); + command_forwarder_vec = resource_manager_->create_hardware_command_forwarders(get_namespace()); + + for (auto const & command_forwarder : command_forwarder_vec) + { + try + { + executor_->add_node(command_forwarder->get_node()->get_node_base_interface()); + } + catch (const std::runtime_error & e) + { + RCLCPP_WARN_STREAM( + get_logger(), "ControllerManager: Can't create CommandForwarder<" + << command_forwarder->command_interface_name() << ">." << e.what()); + } + } +} + +void ControllerManager::register_sub_controller_manager() +{ + RCLCPP_INFO_STREAM( + get_logger(), + "SubControllerManager:<" << get_namespace() << "/" << get_name() << "> trying to register."); + rclcpp::Client::SharedPtr client = + create_client( + "/register_sub_controller_manager"); + + auto request = + std::make_shared(); + request->sub_controller_manager_namespace = get_namespace(); + request->sub_controller_manager_name = get_name(); + + // export the provided StatePublishers + for (auto const & state_publisher : resource_manager_->get_state_publishers()) + { + // create description of StatePublisher including: prefix_name, interface_name and topic. + // So that receiver is able to create a DistributedStateInterface which subscribes to the + // topics provided by this sub controller manager + request->state_publishers.push_back(state_publisher->create_publisher_description_msg()); + } + + // export the provided CommandForwarders + for (auto const & command_forwarders : resource_manager_->get_command_forwarders()) + { + // create description of StatePublisher including: prefix_name, interface_name and topic. + // So that receiver is able to create a DistributedStateInterface which subscribes to the + // topics provided by this sub controller manager + request->command_state_publishers.push_back( + command_forwarders->create_publisher_description_msg()); + } + + using namespace std::chrono_literals; + while (!client->wait_for_service(1s)) + { + if (!rclcpp::ok()) + { + RCLCPP_ERROR_STREAM( + get_logger(), "SubControllerManager:<" + << get_namespace() << "/" << get_name() + << ">. Interrupted while waiting for central controller managers " + "registration service. Exiting."); + return; + } + RCLCPP_INFO_STREAM( + get_logger(), "SubControllerManager:<" + << get_namespace() << "/" << get_name() + << ">. Central controller managers registration service not available, " + "waiting again..."); + } + + auto result = client->async_send_request(request); + if ( + rclcpp::spin_until_future_complete(this->get_node_base_interface(), result) == + rclcpp::FutureReturnCode::SUCCESS) + { + // can call get only once + auto res = result.get(); + if (res->ok) + { + auto command_state_publishers = res->command_state_publishers; + // TODO(Manuel) we should probably make the keys explicit (add key_generation function to handles) + // send keys with request + for (const auto & command_state_publisher : command_state_publishers) + { + std::string key = command_state_publisher.name.prefix_name + "/" + + command_state_publisher.name.interface_name; + auto [found, command_forwarder] = resource_manager_->find_command_forwarder(key); + if (found) + { + command_forwarder->subscribe_to_command_publisher( + command_state_publisher.publisher_topic); + } + else + { + RCLCPP_WARN_STREAM( + get_logger(), "SubControllerManager:<" + << get_namespace() << "/" << get_name() + << ">. Could not find a CommandForwarder for key[" << key + << "]. No subscription to command state possible."); + } + } + RCLCPP_INFO_STREAM( + get_logger(), "SubControllerManager:<" << get_namespace() << "/" << get_name() + << ">. Successfully registered."); + } + else + { + RCLCPP_WARN_STREAM( + get_logger(), "SubControllerManager: <" + << get_namespace() << "/" << get_name() + << ">. Registration of StatePublishers failed. Central ControllerManager " + "returned error code."); + } + } + else + { + RCLCPP_WARN_STREAM( + get_logger(), "SubControllerManager: <" << get_namespace() << "/" << get_name() + << ">. Registration of StatePublishers failed."); + } +} + controller_interface::ControllerInterfaceBaseSharedPtr ControllerManager::load_controller( const std::string & controller_name, const std::string & controller_type) { diff --git a/controller_manager_msgs/CMakeLists.txt b/controller_manager_msgs/CMakeLists.txt index 2ba8f23194..0225d91881 100644 --- a/controller_manager_msgs/CMakeLists.txt +++ b/controller_manager_msgs/CMakeLists.txt @@ -13,8 +13,10 @@ find_package(rosidl_default_generators REQUIRED) set(msg_files msg/ControllerState.msg msg/ChainConnection.msg + msg/HandleName.msg msg/HardwareComponentState.msg msg/HardwareInterface.msg + msg/PublisherDescription.msg ) set(srv_files srv/ConfigureController.srv @@ -23,6 +25,7 @@ set(srv_files srv/ListHardwareComponents.srv srv/ListHardwareInterfaces.srv srv/LoadController.srv + srv/RegisterSubControllerManager.srv srv/ReloadControllerLibraries.srv srv/SetHardwareComponentState.srv srv/SwitchController.srv diff --git a/controller_manager_msgs/msg/HandleName.msg b/controller_manager_msgs/msg/HandleName.msg new file mode 100644 index 0000000000..bae7974d77 --- /dev/null +++ b/controller_manager_msgs/msg/HandleName.msg @@ -0,0 +1,2 @@ +string prefix_name # prefix name of the handle (ReadOnly or ReadWrite) +string interface_name # interface name of the handle (ReadOnly or ReadWrite) \ No newline at end of file diff --git a/controller_manager_msgs/msg/PublisherDescription.msg b/controller_manager_msgs/msg/PublisherDescription.msg new file mode 100644 index 0000000000..be19be5aad --- /dev/null +++ b/controller_manager_msgs/msg/PublisherDescription.msg @@ -0,0 +1,3 @@ +string ns # the namespace the Publisher is in +controller_manager_msgs/HandleName name # the full qualified name (prefix_name + interface_name) of the Publishers interface +string publisher_topic # the topic via which the values/commands are published. \ No newline at end of file diff --git a/controller_manager_msgs/srv/RegisterSubControllerManager.srv b/controller_manager_msgs/srv/RegisterSubControllerManager.srv new file mode 100644 index 0000000000..97aaf1f0cf --- /dev/null +++ b/controller_manager_msgs/srv/RegisterSubControllerManager.srv @@ -0,0 +1,20 @@ +# The RegisterSubControllerManager service allows a distributed controller +# manager to register itself at the central controller manager + +# Req: +# To register a controller the distributed sub controller manager has to +# define: +# * the full namespace it operates in +# * all statePublishers the sub controller provides + +# Res: +# The response includes: +# * a bool if the registration succeeded + +string sub_controller_manager_namespace +string sub_controller_manager_name +controller_manager_msgs/PublisherDescription[] state_publishers +controller_manager_msgs/PublisherDescription[] command_state_publishers +--- +controller_manager_msgs/PublisherDescription[] command_state_publishers +bool ok \ No newline at end of file diff --git a/hardware_interface/CMakeLists.txt b/hardware_interface/CMakeLists.txt index 9af1a359cd..cd86620afd 100644 --- a/hardware_interface/CMakeLists.txt +++ b/hardware_interface/CMakeLists.txt @@ -7,11 +7,14 @@ endif() set(THIS_PACKAGE_INCLUDE_DEPENDS control_msgs + controller_manager_msgs lifecycle_msgs pluginlib + rclcpp rclcpp_lifecycle rcpputils rcutils + std_msgs tinyxml2_vendor TinyXML2 ) @@ -26,6 +29,8 @@ add_library( SHARED src/actuator.cpp src/component_parser.cpp + src/hardware_interface/distributed_control_interface/command_forwarder.cpp + src/hardware_interface/distributed_control_interface/state_publisher.cpp src/resource_manager.cpp src/sensor.cpp src/system.cpp diff --git a/hardware_interface/include/hardware_interface/distributed_control_interface/command_forwarder.hpp b/hardware_interface/include/hardware_interface/distributed_control_interface/command_forwarder.hpp new file mode 100644 index 0000000000..3597dbaf00 --- /dev/null +++ b/hardware_interface/include/hardware_interface/distributed_control_interface/command_forwarder.hpp @@ -0,0 +1,66 @@ +#ifndef DISTRIBUTED_CONTROL__COMMAND_FORWARDER_HPP_ +#define DISTRIBUTED_CONTROL__COMMAND_FORWARDER_HPP_ + +#include +#include +#include + +#include "hardware_interface/distributed_control_interface/publisher_description.hpp" +#include "hardware_interface/loaned_command_interface.hpp" + +#include "controller_manager_msgs/msg/publisher_description.hpp" + +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_lifecycle/lifecycle_node.hpp" +#include "rclcpp_lifecycle/state.hpp" +#include "std_msgs/msg/float64.hpp" + +namespace distributed_control +{ + +class CommandForwarder final +{ +public: + explicit CommandForwarder( + std::unique_ptr loaned_command_interface_ptr_, + const std::string & ns = ""); + + CommandForwarder() = delete; + + ~CommandForwarder() {} + + std::shared_ptr get_node() const; + + std::string get_namespace() const; + + std::string topic_name() const; + + std::string topic_name_relative_to_namespace() const; + + std::string command_interface_name() const; + + std::string command_interface_prefix_name() const; + + std::string command_interface_interface_name() const; + + controller_manager_msgs::msg::PublisherDescription create_publisher_description_msg() const; + + void subscribe_to_command_publisher(const std::string & topic_name); + +private: + void publish_value_on_timer(); + void forward_command(const std_msgs::msg::Float64 & msg); + + std::unique_ptr loaned_command_interface_ptr_; + const std::string namespace_; + const std::string topic_name_; + std::string subscription_topic_name_; + std::shared_ptr node_; + rclcpp::Publisher::SharedPtr state_value_pub_; + rclcpp::Subscription::SharedPtr command_subscription_; + rclcpp::TimerBase::SharedPtr timer_; +}; + +} // namespace distributed_control + +#endif // DISTRIBUTED_CONTROL__COMMAND_FORWARDER_HPP_ \ No newline at end of file diff --git a/hardware_interface/include/hardware_interface/distributed_control_interface/publisher_description.hpp b/hardware_interface/include/hardware_interface/distributed_control_interface/publisher_description.hpp new file mode 100644 index 0000000000..2397d47a1b --- /dev/null +++ b/hardware_interface/include/hardware_interface/distributed_control_interface/publisher_description.hpp @@ -0,0 +1,47 @@ +#ifndef DISTRIBUTED_CONTROL__STATE_PUBLISHER_DESCRIPTION_HPP_ +#define DISTRIBUTED_CONTROL__STATE_PUBLISHER_DESCRIPTION_HPP_ + +#include "controller_manager_msgs/msg/publisher_description.hpp" + +#include "rclcpp/rclcpp.hpp" + +namespace distributed_control +{ + +class PublisherDescription final +{ +public: + explicit PublisherDescription( + const controller_manager_msgs::msg::PublisherDescription & description) + : namespace_(description.ns), + prefix_name_(description.name.prefix_name), + interface_name_(description.name.interface_name), + topic_name_(description.publisher_topic) + { + } + + PublisherDescription() = delete; + + PublisherDescription(const PublisherDescription & other) = default; + + PublisherDescription(PublisherDescription && other) = default; + + ~PublisherDescription() {} + + std::string get_namespace() const { return namespace_; } + + std::string prefix_name() const { return prefix_name_; } + + std::string interface_name() const { return interface_name_; } + + std::string topic_name() const { return topic_name_; } + +private: + std::string namespace_; + std::string prefix_name_; + std::string interface_name_; + std::string topic_name_; +}; + +} // namespace distributed_control +#endif // DISTRIBUTED_CONTROL__STATE_PUBLISHER_DESCRIPTION_HPP_ \ No newline at end of file diff --git a/hardware_interface/include/hardware_interface/distributed_control_interface/state_publisher.hpp b/hardware_interface/include/hardware_interface/distributed_control_interface/state_publisher.hpp new file mode 100644 index 0000000000..6fe316c4b1 --- /dev/null +++ b/hardware_interface/include/hardware_interface/distributed_control_interface/state_publisher.hpp @@ -0,0 +1,60 @@ +#ifndef DISTRIBUTED_CONTROL__STATE_PUBLISHER_HPP_ +#define DISTRIBUTED_CONTROL__STATE_PUBLISHER_HPP_ + +#include +#include +#include + +#include "hardware_interface/loaned_state_interface.hpp" + +#include "controller_manager_msgs/msg/publisher_description.hpp" + +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_lifecycle/lifecycle_node.hpp" +#include "rclcpp_lifecycle/state.hpp" +#include "std_msgs/msg/float64.hpp" + +namespace distributed_control +{ + +class StatePublisher final +{ +public: + explicit StatePublisher( + std::unique_ptr loaned_state_interface_ptr, + const std::string & ns = ""); + + StatePublisher() = delete; + + ~StatePublisher() {} + + std::shared_ptr get_node() const; + + std::string get_namespace() const; + + std::string topic_name() const; + + std::string topic_name_relative_to_namespace() const; + + std::string state_interface_name() const; + + std::string state_interface_prefix_name() const; + + std::string state_interface_interface_name() const; + + controller_manager_msgs::msg::PublisherDescription create_publisher_description_msg() const; + +private: + void publish_value_on_timer(); + + std::unique_ptr loaned_state_interface_ptr_; + const std::string namespace_; + const std::string topic_name_; + std::shared_ptr node_; + rclcpp::Publisher::SharedPtr state_value_pub_; + rclcpp::TimerBase::SharedPtr timer_; +}; + +} // namespace distributed_control + +#endif // DISTRIBUTED_CONTROL__STATE_PUBLISHER_HPP_ \ No newline at end of file diff --git a/hardware_interface/include/hardware_interface/distributed_control_interface/sub_controller_manager_wrapper.hpp b/hardware_interface/include/hardware_interface/distributed_control_interface/sub_controller_manager_wrapper.hpp new file mode 100644 index 0000000000..24fb64a5ff --- /dev/null +++ b/hardware_interface/include/hardware_interface/distributed_control_interface/sub_controller_manager_wrapper.hpp @@ -0,0 +1,64 @@ +#ifndef DISTRIBUTED_CONTROL__SUB_CONTROLLER_MANAGER_WRAPPER_HPP_ +#define DISTRIBUTED_CONTROL__SUB_CONTROLLER_MANAGER_WRAPPER_HPP_ + +#include +#include +#include + +#include "controller_manager_msgs/msg/publisher_description.hpp" + +#include "hardware_interface/distributed_control_interface/publisher_description.hpp" + +namespace distributed_control +{ + +class SubControllerManagerWrapper final +{ +public: + explicit SubControllerManagerWrapper( + const std::string & ns, const std::string & name, + const std::vector & state_publishers, + std::vector & command_forwarders) + : NAMESPACE_(ns), + NAME_(name), + state_publisher_descriptions_({state_publishers.begin(), state_publishers.end()}), + command_forwarder_descriptions_({command_forwarders.begin(), command_forwarders.end()}) + { + } + + SubControllerManagerWrapper(const SubControllerManagerWrapper & other) = default; + + SubControllerManagerWrapper(SubControllerManagerWrapper && other) = default; + + ~SubControllerManagerWrapper() {} + + std::string get_namespace() const { return NAMESPACE_; } + + std::string get_controller_manager_name() const { return NAME_; } + + std::string get_name() const { return get_namespace() + "/" + get_controller_manager_name(); } + + std::vector get_state_publisher_descriptions() const + { + return state_publisher_descriptions_; + } + + std::vector get_command_forwarder_descriptions() const + { + return command_forwarder_descriptions_; + } + + size_t get_state_publisher_count() const { return state_publisher_descriptions_.size(); } + + size_t get_command_forwarder_count() const { return command_forwarder_descriptions_.size(); } + +private: + const std::string NAMESPACE_; + const std::string NAME_; + std::vector state_publisher_descriptions_; + std::vector command_forwarder_descriptions_; +}; + +} // namespace distributed_control + +#endif // DISTRIBUTED_CONTROL__SUB_CONTROLLER_MANAGER_WRAPPER_HPP_ \ No newline at end of file diff --git a/hardware_interface/include/hardware_interface/handle.hpp b/hardware_interface/include/hardware_interface/handle.hpp index 1aea017754..1e506bf0b2 100644 --- a/hardware_interface/include/hardware_interface/handle.hpp +++ b/hardware_interface/include/hardware_interface/handle.hpp @@ -18,48 +18,62 @@ #include #include +#include "hardware_interface/distributed_control_interface/publisher_description.hpp" #include "hardware_interface/macros.hpp" #include "hardware_interface/visibility_control.h" +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_lifecycle/lifecycle_node.hpp" +#include "std_msgs/msg/float64.hpp" namespace hardware_interface { /// A handle used to get and set a value on a given interface. -class ReadOnlyHandle +class ReadHandleInterface { public: - ReadOnlyHandle( + virtual double get_value() const = 0; +}; + +class WriteHandleInterface +{ +public: + virtual void set_value(double value) = 0; +}; + +class HandleInterface +{ +public: + HandleInterface( 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) + double * value_ptr = nullptr, std::shared_ptr node = nullptr) + : prefix_name_(prefix_name), interface_name_(interface_name), value_ptr_(value_ptr), node_(node) { } - explicit ReadOnlyHandle(const std::string & interface_name) - : interface_name_(interface_name), value_ptr_(nullptr) + explicit HandleInterface(const std::string & interface_name) + : interface_name_(interface_name), value_ptr_(nullptr), node_(nullptr) { } - explicit ReadOnlyHandle(const char * interface_name) - : interface_name_(interface_name), value_ptr_(nullptr) + explicit HandleInterface(const char * interface_name) + : interface_name_(interface_name), value_ptr_(nullptr), node_(nullptr) { } - ReadOnlyHandle(const ReadOnlyHandle & other) = default; + HandleInterface(const HandleInterface & other) = default; - ReadOnlyHandle(ReadOnlyHandle && other) = default; + HandleInterface(HandleInterface && other) = default; - ReadOnlyHandle & operator=(const ReadOnlyHandle & other) = default; + HandleInterface & operator=(const HandleInterface & other) = default; - ReadOnlyHandle & operator=(ReadOnlyHandle && other) = default; + HandleInterface & operator=(HandleInterface && other) = default; - virtual ~ReadOnlyHandle() = default; + virtual ~HandleInterface() = default; /// Returns true if handle references a value. inline operator bool() const { return value_ptr_ != nullptr; } - const std::string get_name() const { return prefix_name_ + "/" + interface_name_; } - - const std::string & get_interface_name() const { return interface_name_; } + std::string get_interface_name() const { return interface_name_; } [[deprecated( "Replaced by get_name method, which is semantically more correct")]] const std::string @@ -68,48 +82,95 @@ class ReadOnlyHandle return get_name(); } - const std::string & get_prefix_name() const { return prefix_name_; } + virtual std::string get_name() const { return prefix_name_ + "/" + interface_name_; } - double get_value() const + // TODO(Manuel): Maybe not the best place to put... + std::shared_ptr get_node() const { - THROW_ON_NULLPTR(value_ptr_); - return *value_ptr_; + THROW_ON_NULLPTR(node_); + if (!node_.get()) + { + throw std::runtime_error("Node not initialized!"); + } + return node_; + } + + std::string get_prefix_name() const { return prefix_name_; } + + /** + * @brief Create the full name consisting of prefix and interface name separated by an underscore. + * Used for e.g. name generation of nodes, where "/" are not allowed. + * + * @return std::string prefix_name + _ + interface_name. + */ + virtual std::string get_underscore_separated_name() const + { + return append_char(get_prefix_name(), '_') + get_interface_name(); } protected: + std::string append_char(std::string str, const char & char_to_append) const + { + if (!str.empty()) + { + return str + char_to_append; + } + return str; + } + + std::string erase_slash_at_start(std::string str) const + { + if (!str.empty()) + { + if (str.at(0) == '/') + { + return str.erase(0, 1); + } + } + return str; + } + + std::string replace_all_chars_from_string( + std::string str, const char & char_to_replace, const char & replace_with_char) const + { + std::replace(str.begin(), str.end(), char_to_replace, replace_with_char); + return str; + } + std::string prefix_name_; std::string interface_name_; double * value_ptr_; + std::shared_ptr node_; }; -class ReadWriteHandle : public ReadOnlyHandle +class ReadOnlyHandle : public HandleInterface, public ReadHandleInterface { public: - ReadWriteHandle( + ReadOnlyHandle( const std::string & prefix_name, const std::string & interface_name, double * value_ptr = nullptr) - : ReadOnlyHandle(prefix_name, interface_name, value_ptr) + : HandleInterface(prefix_name, interface_name, value_ptr) { } - explicit ReadWriteHandle(const std::string & interface_name) : ReadOnlyHandle(interface_name) {} + explicit ReadOnlyHandle(const std::string & interface_name) : HandleInterface(interface_name) {} - explicit ReadWriteHandle(const char * interface_name) : ReadOnlyHandle(interface_name) {} + explicit ReadOnlyHandle(const char * interface_name) : HandleInterface(interface_name) {} - ReadWriteHandle(const ReadWriteHandle & other) = default; + ReadOnlyHandle(const ReadOnlyHandle & other) = default; - ReadWriteHandle(ReadWriteHandle && other) = default; + ReadOnlyHandle(ReadOnlyHandle && other) = default; - ReadWriteHandle & operator=(const ReadWriteHandle & other) = default; + ReadOnlyHandle & operator=(const ReadOnlyHandle & other) = default; - ReadWriteHandle & operator=(ReadWriteHandle && other) = default; + ReadOnlyHandle & operator=(ReadOnlyHandle && other) = default; - virtual ~ReadWriteHandle() = default; + virtual ~ReadOnlyHandle() = default; - void set_value(double value) + double get_value() const override { - THROW_ON_NULLPTR(this->value_ptr_); - *this->value_ptr_ = value; + THROW_ON_NULLPTR(value_ptr_); + return *value_ptr_; } }; @@ -123,6 +184,134 @@ class StateInterface : public ReadOnlyHandle using ReadOnlyHandle::ReadOnlyHandle; }; +class DistributedReadOnlyHandle : public ReadOnlyHandle +{ +public: + // TODO(Manuel): We should pass the initial value via service call, so that the value_ of ReadOnlyHandle + // is initialized with a feasible value. + DistributedReadOnlyHandle( + const distributed_control::PublisherDescription & description, const std::string & ns = "/") + : ReadOnlyHandle(description.prefix_name(), description.interface_name(), &value_), + get_value_topic_name_(description.topic_name()), + namespace_(ns), + interface_namespace_(description.get_namespace()) + { + rclcpp::NodeOptions node_options; + // create node for subscribing to StatePublisher described in StatePublisherDescription + node_ = std::make_shared( + get_underscore_separated_name() + "_state_interface_subscriber", namespace_, node_options, + false); + + // subscribe to topic provided by StatePublisher + state_value_sub_ = node_->create_subscription( + get_value_topic_name_, 10, + std::bind(&DistributedReadOnlyHandle::get_value_cb, this, std::placeholders::_1)); + } + + explicit DistributedReadOnlyHandle(const std::string & interface_name) + : ReadOnlyHandle(interface_name) + { + } + + explicit DistributedReadOnlyHandle(const char * interface_name) : ReadOnlyHandle(interface_name) + { + } + + DistributedReadOnlyHandle() = delete; + + DistributedReadOnlyHandle(const DistributedReadOnlyHandle & other) = delete; + + DistributedReadOnlyHandle(DistributedReadOnlyHandle && other) = default; + + DistributedReadOnlyHandle & operator=(const DistributedReadOnlyHandle & other) = default; + + DistributedReadOnlyHandle & operator=(DistributedReadOnlyHandle && other) = default; + + virtual ~DistributedReadOnlyHandle() = default; + + virtual std::string get_name() const override + { + // concatenate: interface_namespace/prefix_name/interface_name to obtain + // a unique name. + return append_char(interface_namespace_, '/') + append_char(get_prefix_name(), '/') + + get_interface_name(); + } + + virtual std::string get_underscore_separated_name() const override + { + // remove first "/" from namespace and replace all follow occurrences of "/" with "_" + std::string ns = + replace_all_chars_from_string(erase_slash_at_start(interface_namespace_), '/', '_'); + // concatenate: interface_namespace + _ + namespace_prefix + _ + name_interface_name + return append_char(ns, '_') + append_char(get_prefix_name(), '_') + get_interface_name(); + } + +protected: + void get_value_cb(const std_msgs::msg::Float64 & msg) + { + value_ = msg.data; + RCLCPP_WARN_STREAM(node_->get_logger(), "Receiving:[" << value_ << "]."); + } + + std::string get_value_topic_name_; + // the current namespace we are in. Needed to create the node in the correct namespace + std::string namespace_; + // the namespace the actual StateInterface we subscribe to is in. + // We need this to create unique names for the StateInterface. + std::string interface_namespace_; + rclcpp::Subscription::SharedPtr state_value_sub_; + double value_; +}; + +class DistributedStateInterface : public DistributedReadOnlyHandle +{ +public: + DistributedStateInterface(const DistributedStateInterface & other) = default; + + DistributedStateInterface(DistributedStateInterface && other) = default; + + using DistributedReadOnlyHandle::DistributedReadOnlyHandle; +}; + +class ReadWriteHandle : public HandleInterface, + public ReadHandleInterface, + public WriteHandleInterface +{ +public: + ReadWriteHandle( + const std::string & prefix_name, const std::string & interface_name, + double * value_ptr = nullptr) + : HandleInterface(prefix_name, interface_name, value_ptr) + { + } + + explicit ReadWriteHandle(const std::string & interface_name) : HandleInterface(interface_name) {} + + explicit ReadWriteHandle(const char * interface_name) : HandleInterface(interface_name) {} + + ReadWriteHandle(const ReadWriteHandle & other) = default; + + ReadWriteHandle(ReadWriteHandle && other) = default; + + ReadWriteHandle & operator=(const ReadWriteHandle & other) = default; + + ReadWriteHandle & operator=(ReadWriteHandle && other) = default; + + virtual ~ReadWriteHandle() = default; + + virtual double get_value() const override + { + THROW_ON_NULLPTR(value_ptr_); + return *value_ptr_; + } + + virtual void set_value(double value) override + { + THROW_ON_NULLPTR(this->value_ptr_); + *this->value_ptr_ = value; + } +}; + class CommandInterface : public ReadWriteHandle { public: @@ -139,6 +328,118 @@ class CommandInterface : public ReadWriteHandle using ReadWriteHandle::ReadWriteHandle; }; +class DistributedReadWriteHandle : public ReadWriteHandle +{ +public: + DistributedReadWriteHandle( + const distributed_control::PublisherDescription & description, const std::string & ns = "/") + : ReadWriteHandle(description.prefix_name(), description.interface_name(), &value_), + get_value_topic_name_(description.topic_name()), + namespace_(ns), + interface_namespace_(description.get_namespace()), + forward_command_topic_name_(get_underscore_separated_name() + "_command_forwarding") + { + // create node for subscribing to StatePublisher described in StatePublisherDescription + rclcpp::NodeOptions node_options; + node_ = std::make_shared( + get_underscore_separated_name() + "_distributed_command_interface", namespace_, node_options, + false); + + // subscribe to topic provided by StatePublisher + command_value_sub_ = node_->create_subscription( + get_value_topic_name_, 10, + std::bind(&DistributedReadWriteHandle::get_value_cb, this, std::placeholders::_1)); + + // create publisher so that we can forward the commands + command_value_pub_ = + node_->create_publisher(forward_command_topic_name_, 10); + } + + explicit DistributedReadWriteHandle(const std::string & interface_name) + : ReadWriteHandle(interface_name) + { + } + + explicit DistributedReadWriteHandle(const char * interface_name) : ReadWriteHandle(interface_name) + { + } + + DistributedReadWriteHandle(const DistributedReadWriteHandle & other) = default; + + DistributedReadWriteHandle(DistributedReadWriteHandle && other) = default; + + DistributedReadWriteHandle & operator=(const DistributedReadWriteHandle & other) = default; + + DistributedReadWriteHandle & operator=(DistributedReadWriteHandle && other) = default; + + virtual ~DistributedReadWriteHandle() = default; + + virtual std::string get_name() const override + { + // concatenate: interface_namespace/prefix_name/interface_name to obtain + // a unique name. + return append_char(interface_namespace_, '/') + append_char(get_prefix_name(), '/') + + get_interface_name(); + } + + virtual std::string get_underscore_separated_name() const override + { + // remove first "/" from namespace and replace all follow occurrences with "_" + std::string ns = + replace_all_chars_from_string(erase_slash_at_start(interface_namespace_), '/', '_'); + // concatenate: interface_namespace + _ + namespace_prefix + _ + name_interface_name + return append_char(ns, '_') + append_char(get_prefix_name(), '_') + get_interface_name(); + } + + void set_value(double value) override + { + auto msg = std::make_unique(); + msg->data = value; + + RCLCPP_WARN(node_->get_logger(), "DistributedCommandInterface Publishing: '%.7lf'", msg->data); + std::flush(std::cout); + + command_value_pub_->publish(std::move(msg)); + } + + std::string forward_command_topic_name() const { return forward_command_topic_name_; } + +protected: + void get_value_cb(const std_msgs::msg::Float64 & msg) + { + value_ = msg.data; + RCLCPP_WARN_STREAM( + node_->get_logger(), "DistributedCommandInterface Receiving:[" << value_ << "]."); + } + + std::string get_value_topic_name_; + // the current namespace we are in. Needed to create the node in the correct namespace + std::string namespace_; + // the namespace the actual CommandInterface we subscribe to is in. + // We need this to create unique names for the CommandInterface. + std::string interface_namespace_; + rclcpp::Subscription::SharedPtr command_value_sub_; + std::string forward_command_topic_name_; + rclcpp::Publisher::SharedPtr command_value_pub_; + double value_; +}; + +class DistributedCommandInterface : public DistributedReadWriteHandle +{ +public: + /// CommandInterface copy constructor is actively deleted. + /** + * Command interfaces are having a unique ownership and thus + * can't be copied in order to avoid simultaneous writes to + * the same resource. + */ + DistributedCommandInterface(const DistributedCommandInterface & other) = delete; + + DistributedCommandInterface(DistributedCommandInterface && other) = default; + + using DistributedReadWriteHandle::DistributedReadWriteHandle; +}; + } // namespace hardware_interface #endif // HARDWARE_INTERFACE__HANDLE_HPP_ diff --git a/hardware_interface/include/hardware_interface/loaned_command_interface.hpp b/hardware_interface/include/hardware_interface/loaned_command_interface.hpp index bb5807c398..1658261541 100644 --- a/hardware_interface/include/hardware_interface/loaned_command_interface.hpp +++ b/hardware_interface/include/hardware_interface/loaned_command_interface.hpp @@ -28,12 +28,12 @@ class LoanedCommandInterface public: using Deleter = std::function; - explicit LoanedCommandInterface(CommandInterface & command_interface) + explicit LoanedCommandInterface(std::shared_ptr command_interface) : LoanedCommandInterface(command_interface, nullptr) { } - LoanedCommandInterface(CommandInterface & command_interface, Deleter && deleter) + LoanedCommandInterface(std::shared_ptr command_interface, Deleter && deleter) : command_interface_(command_interface), deleter_(std::forward(deleter)) { } @@ -50,25 +50,30 @@ class LoanedCommandInterface } } - const std::string get_name() const { return command_interface_.get_name(); } + std::string get_name() const { return command_interface_->get_name(); } - const std::string & get_interface_name() const { return command_interface_.get_interface_name(); } + std::string get_interface_name() const { return command_interface_->get_interface_name(); } [[deprecated( "Replaced by get_name method, which is semantically more correct")]] const std::string get_full_name() const { - return command_interface_.get_name(); + return command_interface_->get_name(); } - const std::string & get_prefix_name() const { return command_interface_.get_prefix_name(); } + std::string get_prefix_name() const { return command_interface_->get_prefix_name(); } - void set_value(double val) { command_interface_.set_value(val); } + void set_value(double val) { command_interface_->set_value(val); } - double get_value() const { return command_interface_.get_value(); } + double get_value() const { return command_interface_->get_value(); } + + std::string get_underscore_separated_name() const + { + return command_interface_->get_underscore_separated_name(); + } protected: - CommandInterface & command_interface_; + std::shared_ptr command_interface_; Deleter deleter_; }; diff --git a/hardware_interface/include/hardware_interface/loaned_state_interface.hpp b/hardware_interface/include/hardware_interface/loaned_state_interface.hpp index 4fe67d1290..ed4d3bcbd5 100644 --- a/hardware_interface/include/hardware_interface/loaned_state_interface.hpp +++ b/hardware_interface/include/hardware_interface/loaned_state_interface.hpp @@ -20,7 +20,6 @@ #include #include "hardware_interface/handle.hpp" - namespace hardware_interface { class LoanedStateInterface @@ -28,12 +27,12 @@ class LoanedStateInterface public: using Deleter = std::function; - explicit LoanedStateInterface(StateInterface & state_interface) + explicit LoanedStateInterface(std::shared_ptr state_interface) : LoanedStateInterface(state_interface, nullptr) { } - LoanedStateInterface(StateInterface & state_interface, Deleter && deleter) + LoanedStateInterface(std::shared_ptr state_interface, Deleter && deleter) : state_interface_(state_interface), deleter_(std::forward(deleter)) { } @@ -50,23 +49,28 @@ class LoanedStateInterface } } - const std::string get_name() const { return state_interface_.get_name(); } + std::string get_interface_name() const { return state_interface_->get_interface_name(); } - const std::string & get_interface_name() const { return state_interface_.get_interface_name(); } + std::string get_name() const { return state_interface_->get_name(); } [[deprecated( "Replaced by get_name method, which is semantically more correct")]] const std::string get_full_name() const { - return state_interface_.get_name(); + return state_interface_->get_name(); } - const std::string & get_prefix_name() const { return state_interface_.get_prefix_name(); } + std::string get_prefix_name() const { return state_interface_->get_prefix_name(); } + + std::string get_underscore_separated_name() const + { + return state_interface_->get_underscore_separated_name(); + } - double get_value() const { return state_interface_.get_value(); } + double get_value() const { return state_interface_->get_value(); } protected: - StateInterface & state_interface_; + std::shared_ptr state_interface_; Deleter deleter_; }; diff --git a/hardware_interface/include/hardware_interface/resource_manager.hpp b/hardware_interface/include/hardware_interface/resource_manager.hpp index 1693e85574..e5591899ba 100644 --- a/hardware_interface/include/hardware_interface/resource_manager.hpp +++ b/hardware_interface/include/hardware_interface/resource_manager.hpp @@ -21,11 +21,16 @@ #include #include +#include "hardware_interface/distributed_control_interface/command_forwarder.hpp" +#include "hardware_interface/distributed_control_interface/state_publisher.hpp" +#include "hardware_interface/distributed_control_interface/sub_controller_manager_wrapper.hpp" +#include "hardware_interface/handle.hpp" #include "hardware_interface/hardware_component_info.hpp" #include "hardware_interface/hardware_info.hpp" #include "hardware_interface/loaned_command_interface.hpp" #include "hardware_interface/loaned_state_interface.hpp" #include "hardware_interface/types/hardware_interface_return_values.hpp" + #include "rclcpp/duration.hpp" #include "rclcpp/time.hpp" @@ -114,6 +119,31 @@ class HARDWARE_INTERFACE_PUBLIC ResourceManager */ bool state_interface_is_available(const std::string & name) const; + void register_sub_controller_manager( + std::shared_ptr sub_controller_manager); + + std::vector> + import_state_interfaces_of_sub_controller_manager( + std::shared_ptr sub_controller_manager); + + std::vector> + import_command_interfaces_of_sub_controller_manager( + std::shared_ptr sub_controller_manager); + + std::vector> + create_hardware_state_publishers(const std::string & ns); + + std::vector> + create_hardware_command_forwarders(const std::string & ns); + + std::pair> find_command_forwarder( + const std::string & key); + + std::vector> get_state_publishers() const; + + std::vector> get_command_forwarders() + const; + /// Add controllers' reference interfaces to resource manager. /** * Interface for transferring management of reference interfaces to resource manager. diff --git a/hardware_interface/package.xml b/hardware_interface/package.xml index 0339900ff7..fc1779f601 100644 --- a/hardware_interface/package.xml +++ b/hardware_interface/package.xml @@ -10,11 +10,14 @@ ament_cmake control_msgs + controller_manager_msgs lifecycle_msgs pluginlib + rclcpp rclcpp_lifecycle rcpputils tinyxml2_vendor + std_msgs rcutils rcutils diff --git a/hardware_interface/src/hardware_interface/distributed_control_interface/command_forwarder.cpp b/hardware_interface/src/hardware_interface/distributed_control_interface/command_forwarder.cpp new file mode 100644 index 0000000000..a8a1eefa98 --- /dev/null +++ b/hardware_interface/src/hardware_interface/distributed_control_interface/command_forwarder.cpp @@ -0,0 +1,114 @@ +#include "hardware_interface/distributed_control_interface/command_forwarder.hpp" + +#include +#include +#include +#include +#include + +using namespace std::chrono_literals; + +namespace distributed_control +{ + +CommandForwarder::CommandForwarder( + std::unique_ptr loaned_command_interface_ptr, + const std::string & ns) +: loaned_command_interface_ptr_(std::move(loaned_command_interface_ptr)), + namespace_(ns), + topic_name_(loaned_command_interface_ptr_->get_underscore_separated_name() + "_command_state") +{ + rclcpp::NodeOptions node_options; + node_ = std::make_shared( + loaned_command_interface_ptr_->get_underscore_separated_name() + "_command_forwarder", + namespace_, node_options, false); + + state_value_pub_ = node_->create_publisher(topic_name_, 10); + // TODO(Manuel): We should check if we cannot detect changes to LoanedStateInterface's value and only publish then + timer_ = + node_->create_wall_timer(50ms, std::bind(&CommandForwarder::publish_value_on_timer, this)); + RCLCPP_INFO(node_->get_logger(), "Creating CommandForwarder<%s>.", topic_name_.c_str()); +} + +std::shared_ptr CommandForwarder::get_node() const +{ + if (!node_.get()) + { + std::string msg( + "CommandForwarder<" + command_interface_name() + ">: Node hasn't been configured yet!"); + throw std::runtime_error(msg); + } + return node_; +} + +std::string CommandForwarder::get_namespace() const { return namespace_; } + +std::string CommandForwarder::topic_name() const { return topic_name_; } + +std::string CommandForwarder::topic_name_relative_to_namespace() const +{ + return get_namespace() + "/" + topic_name(); +} + +std::string CommandForwarder::command_interface_name() const +{ + return loaned_command_interface_ptr_->get_name(); +} + +std::string CommandForwarder::command_interface_prefix_name() const +{ + return loaned_command_interface_ptr_->get_prefix_name(); +} + +std::string CommandForwarder::command_interface_interface_name() const +{ + return loaned_command_interface_ptr_->get_interface_name(); +} + +controller_manager_msgs::msg::PublisherDescription +CommandForwarder::create_publisher_description_msg() const +{ + auto msg = controller_manager_msgs::msg::PublisherDescription(); + + msg.ns = get_namespace(); + msg.name.prefix_name = command_interface_prefix_name(); + msg.name.interface_name = command_interface_interface_name(); + msg.publisher_topic = topic_name_relative_to_namespace(); + + return msg; +} + +void CommandForwarder::subscribe_to_command_publisher(const std::string & topic_name) +{ + subscription_topic_name_ = topic_name; + command_subscription_ = node_->create_subscription( + subscription_topic_name_, 10, + std::bind(&CommandForwarder::forward_command, this, std::placeholders::_1)); +} + +void CommandForwarder::publish_value_on_timer() +{ + // Todo(Manuel) create custom msg and return success or failure not just nan. + auto msg = std::make_unique(); + try + { + msg->data = loaned_command_interface_ptr_->get_value(); + } + catch (const std::runtime_error & e) + { + msg->data = std::numeric_limits::quiet_NaN(); + } + RCLCPP_WARN(node_->get_logger(), "Publishing: '%.7lf'", msg->data); + std::flush(std::cout); + + // Put the message into a queue to be processed by the middleware. + // This call is non-blocking. + state_value_pub_->publish(std::move(msg)); +} + +void CommandForwarder::forward_command(const std_msgs::msg::Float64 & msg) +{ + loaned_command_interface_ptr_->set_value(msg.data); +} + +} // namespace distributed_control \ No newline at end of file diff --git a/hardware_interface/src/hardware_interface/distributed_control_interface/state_publisher.cpp b/hardware_interface/src/hardware_interface/distributed_control_interface/state_publisher.cpp new file mode 100644 index 0000000000..c84bfee1e5 --- /dev/null +++ b/hardware_interface/src/hardware_interface/distributed_control_interface/state_publisher.cpp @@ -0,0 +1,100 @@ +#include "hardware_interface/distributed_control_interface/state_publisher.hpp" + +#include +#include +#include +#include +#include + +using namespace std::chrono_literals; + +namespace distributed_control +{ + +StatePublisher::StatePublisher( + std::unique_ptr loaned_state_interface_ptr, + const std::string & ns) +: loaned_state_interface_ptr_(std::move(loaned_state_interface_ptr)), + namespace_(ns), + topic_name_(loaned_state_interface_ptr_->get_underscore_separated_name() + "_state") +{ + rclcpp::NodeOptions node_options; + node_ = std::make_shared( + loaned_state_interface_ptr_->get_underscore_separated_name() + "_state_publisher", namespace_, + node_options, false); + + state_value_pub_ = node_->create_publisher(topic_name_, 10); + // TODO(Manuel): We should check if we cannot detect changes to LoanedStateInterface's value and only publish then + timer_ = node_->create_wall_timer(50ms, std::bind(&StatePublisher::publish_value_on_timer, this)); + RCLCPP_INFO(node_->get_logger(), "Creating StatePublisher<%s>.", topic_name_.c_str()); +} + +std::shared_ptr StatePublisher::get_node() const +{ + if (!node_.get()) + { + std::string msg( + "StatePublisher<" + state_interface_name() + ">: Node hasn't been configured yet!"); + throw std::runtime_error(msg); + } + return node_; +} + +std::string StatePublisher::get_namespace() const { return namespace_; } + +std::string StatePublisher::topic_name() const { return topic_name_; } + +std::string StatePublisher::topic_name_relative_to_namespace() const +{ + return get_namespace() + "/" + topic_name(); +} + +std::string StatePublisher::state_interface_name() const +{ + return loaned_state_interface_ptr_->get_name(); +} + +std::string StatePublisher::state_interface_prefix_name() const +{ + return loaned_state_interface_ptr_->get_prefix_name(); +} + +std::string StatePublisher::state_interface_interface_name() const +{ + return loaned_state_interface_ptr_->get_interface_name(); +} + +controller_manager_msgs::msg::PublisherDescription +StatePublisher::create_publisher_description_msg() const +{ + auto msg = controller_manager_msgs::msg::PublisherDescription(); + msg.ns = get_namespace(); + msg.name.prefix_name = state_interface_prefix_name(); + msg.name.interface_name = state_interface_interface_name(); + msg.publisher_topic = topic_name_relative_to_namespace(); + + return msg; +} + +void StatePublisher::publish_value_on_timer() +{ + auto msg = std::make_unique(); + try + { + msg->data = loaned_state_interface_ptr_->get_value(); + } + catch (const std::runtime_error & e) + { + // Todo(Manuel) create custom msg and return success or failure not just nan. + // Make explicit note implicit!!! + msg->data = std::numeric_limits::quiet_NaN(); + } + RCLCPP_WARN(node_->get_logger(), "Publishing: '%.7lf'", msg->data); + std::flush(std::cout); + + // Put the message into a queue to be processed by the middleware. + // This call is non-blocking. + state_value_pub_->publish(std::move(msg)); +} + +} // namespace distributed_control \ No newline at end of file diff --git a/hardware_interface/src/resource_manager.cpp b/hardware_interface/src/resource_manager.cpp index 7e88cca9b5..b423526a29 100644 --- a/hardware_interface/src/resource_manager.cpp +++ b/hardware_interface/src/resource_manager.cpp @@ -31,8 +31,10 @@ #include "hardware_interface/system.hpp" #include "hardware_interface/system_interface.hpp" #include "hardware_interface/types/hardware_interface_type_values.hpp" + #include "lifecycle_msgs/msg/state.hpp" #include "pluginlib/class_loader.hpp" +#include "rclcpp/rclcpp.hpp" #include "rcutils/logging_macros.h" namespace hardware_interface @@ -407,17 +409,42 @@ class ResourceStorage return result; } + void add_state_interface(std::shared_ptr state_interface) + { + const auto [it, success] = + state_interface_map_.insert(std::make_pair(state_interface->get_name(), state_interface)); + if (!success) + { + std::string msg( + "ResourceStorage: Tried to insert StateInterface with already existing key. Insert[" + + state_interface->get_name() + "]"); + throw std::runtime_error(msg); + } + } + + void add_state_interface(const StateInterface & state_interface) + { + const auto [it, success] = state_interface_map_.emplace(std::make_pair( + state_interface.get_name(), std::make_shared(state_interface))); + if (!success) + { + std::string msg( + "ResourceStorage: Tried to insert StateInterface with already existing key. Insert[" + + state_interface.get_name() + "]"); + throw std::runtime_error(msg); + } + } + template void import_state_interfaces(HardwareT & hardware) { auto interfaces = hardware.export_state_interfaces(); std::vector interface_names; interface_names.reserve(interfaces.size()); - for (auto & interface : interfaces) + for (auto const & interface : interfaces) { - auto key = interface.get_name(); - state_interface_map_.emplace(std::make_pair(key, std::move(interface))); - interface_names.push_back(key); + add_state_interface(interface); + interface_names.push_back(interface.get_name()); } hardware_info_map_[hardware.get_name()].state_interfaces = interface_names; available_state_interfaces_.reserve( @@ -431,6 +458,33 @@ class ResourceStorage hardware_info_map_[hardware.get_name()].command_interfaces = add_command_interfaces(interfaces); } + void add_command_interface(std::shared_ptr command_interface) + { + const auto [it, success] = command_interface_map_.insert( + std::make_pair(command_interface->get_name(), command_interface)); + if (!success) + { + std::string msg( + "ResourceStorage: Tried to insert CommandInterface with already existing key. Insert[" + + command_interface->get_name() + "]"); + throw std::runtime_error(msg); + } + } + + void add_command_interface(CommandInterface && command_interface) + { + std::string key = command_interface.get_name(); + const auto [it, success] = command_interface_map_.emplace( + std::make_pair(key, std::make_shared(std::move(command_interface)))); + if (!success) + { + std::string msg( + "ResourceStorage: Tried to insert CommandInterface with already existing key. Insert[" + + key + "]"); + throw std::runtime_error(msg); + } + } + /// Adds exported command interfaces into internal storage. /** * Add command interfaces to the internal storage. Command interfaces exported from hardware or @@ -449,7 +503,7 @@ class ResourceStorage for (auto & interface : interfaces) { auto key = interface.get_name(); - command_interface_map_.emplace(std::make_pair(key, std::move(interface))); + add_command_interface(std::move(interface)); claimed_command_interface_map_.emplace(std::make_pair(key, false)); interface_names.push_back(key); } @@ -474,6 +528,75 @@ class ResourceStorage } } + void add_state_publisher(std::shared_ptr state_publisher) + { + const auto [it, success] = state_interface_state_publisher_map_.insert( + std::pair{state_publisher->state_interface_name(), state_publisher}); + if (!success) + { + std::string msg( + "ResourceStorage: Tried to insert StatePublisher with already existing key. Insert[" + + state_publisher->state_interface_name() + "]"); + throw std::runtime_error(msg); + } + } + + void add_command_forwarder( + std::shared_ptr command_forwarder) + { + const auto [it, success] = command_interface_command_forwarder_map_.insert( + std::pair{command_forwarder->command_interface_name(), command_forwarder}); + if (!success) + { + std::string msg( + "ResourceStorage: Tried to insert CommandForwarder with already existing key. Insert[" + + command_forwarder->command_interface_name() + "]"); + throw std::runtime_error(msg); + } + } + + std::shared_ptr get_state_publisher( + std::string state_interface_name) const + { + return state_interface_state_publisher_map_.at(state_interface_name); + } + + std::vector> get_state_publishers() const + { + std::vector> state_publishers_vec; + state_publishers_vec.reserve(state_interface_state_publisher_map_.size()); + + for (auto state_publisher : state_interface_state_publisher_map_) + { + state_publishers_vec.push_back(state_publisher.second); + } + return state_publishers_vec; + } + + std::vector> get_command_forwarders() const + { + std::vector> command_forwarders_vec; + command_forwarders_vec.reserve(command_interface_command_forwarder_map_.size()); + + for (auto command_forwarder : command_interface_command_forwarder_map_) + { + command_forwarders_vec.push_back(command_forwarder.second); + } + return command_forwarders_vec; + } + + std::pair> find_command_forwarder( + const std::string & key) + { + auto command_forwarder = command_interface_command_forwarder_map_.find(key); + // we could not find a command forwarder for the provided key + if (command_forwarder == command_interface_command_forwarder_map_.end()) + { + return std::make_pair(false, nullptr); + } + return std::make_pair(true, command_forwarder->second); + } + void check_for_duplicates(const HardwareInfo & hardware_info) { // Check for identical names @@ -538,6 +661,128 @@ class ResourceStorage import_command_interfaces(systems_.back()); } + void add_sub_controller_manager( + std::shared_ptr sub_controller_manager) + { + const auto [it, success] = sub_controller_manager_map_.insert( + std::pair{sub_controller_manager->get_name(), sub_controller_manager}); + if (!success) + { + std::string msg( + "ResourceStorage: Tried to add sub ControllerManager with already existing key. Insert[" + + sub_controller_manager->get_name() + "]"); + throw std::runtime_error(msg); + } + } + + std::vector> import_distributed_state_interfaces( + std::shared_ptr sub_controller_manager) + { + std::vector> distributed_state_interfaces; + distributed_state_interfaces.reserve(sub_controller_manager->get_state_publisher_count()); + std::vector interface_names; + interface_names.reserve(sub_controller_manager->get_state_publisher_count()); + + for (const auto & state_publisher_description : + sub_controller_manager->get_state_publisher_descriptions()) + { + // create StateInterface from the Description and store in ResourceStorage. + auto state_interface = + std::make_shared(state_publisher_description); + + add_state_interface(state_interface); + // add to return vector, node needs to added to executor. + distributed_state_interfaces.push_back(state_interface); + interface_names.push_back(state_interface->get_name()); + } + // TODO(Manuel) this should be handled at one point DRY (adding, hardware_info_map, make available...), key should be made explicit + hardware_info_map_[sub_controller_manager->get_name()].state_interfaces = interface_names; + available_state_interfaces_.reserve( + available_state_interfaces_.capacity() + interface_names.size()); + + for (const auto & interface : + hardware_info_map_[sub_controller_manager->get_name()].state_interfaces) + { + // add all state interfaces to available list + auto found_it = std::find( + available_state_interfaces_.begin(), available_state_interfaces_.end(), interface); + + if (found_it == available_state_interfaces_.end()) + { + available_state_interfaces_.emplace_back(interface); + RCUTILS_LOG_DEBUG_NAMED( + "resource_manager", "(hardware '%s'): '%s' state interface added into available list", + sub_controller_manager->get_name().c_str(), interface.c_str()); + } + else + { + // TODO(destogl): do here error management if interfaces are only partially added into + // "available" list - this should never be the case! + RCUTILS_LOG_WARN_NAMED( + "resource_manager", + "(hardware '%s'): '%s' state interface already in available list." + " This can happen due to multiple calls to 'configure'", + sub_controller_manager->get_name().c_str(), interface.c_str()); + } + } + return distributed_state_interfaces; + } + + std::vector> import_distributed_command_interfaces( + std::shared_ptr sub_controller_manager) + { + std::vector> distributed_command_interfaces; + distributed_command_interfaces.reserve(sub_controller_manager->get_command_forwarder_count()); + std::vector interface_names; + interface_names.reserve(sub_controller_manager->get_command_forwarder_count()); + + for (const auto & command_forwarder_description : + sub_controller_manager->get_command_forwarder_descriptions()) + { + // create StateInterface from the Description and store in ResourceStorage. + auto command_interface = + std::make_shared(command_forwarder_description); + add_command_interface(command_interface); + // add to return vector, node needs to added to executor. + distributed_command_interfaces.push_back(command_interface); + // TODO(Manuel) this should be handled at one point DRY (adding, claimed ....), key should be made explicit + claimed_command_interface_map_.emplace(std::make_pair(command_interface->get_name(), false)); + interface_names.push_back(command_interface->get_name()); + } + // TODO(Manuel) this should be handled at one point DRY(adding, claimed,make available....), key should be made explicit + available_command_interfaces_.reserve( + available_command_interfaces_.capacity() + interface_names.size()); + hardware_info_map_[sub_controller_manager->get_name()].command_interfaces = interface_names; + + for (const auto & interface : + hardware_info_map_[sub_controller_manager->get_name()].command_interfaces) + { + // TODO(destogl): check if interface should be available on configure + auto found_it = std::find( + available_command_interfaces_.begin(), available_command_interfaces_.end(), interface); + + if (found_it == available_command_interfaces_.end()) + { + available_command_interfaces_.emplace_back(interface); + RCUTILS_LOG_DEBUG_NAMED( + "resource_manager", "(hardware '%s'): '%s' command interface added into available list", + sub_controller_manager->get_name().c_str(), interface.c_str()); + } + else + { + // TODO(destogl): do here error management if interfaces are only partially added into + // "available" list - this should never be the case! + RCUTILS_LOG_WARN_NAMED( + "resource_manager", + "(hardware '%s'): '%s' command interface already in available list." + " This can happen due to multiple calls to 'configure'", + sub_controller_manager->get_name().c_str(), interface.c_str()); + } + } + + return distributed_command_interfaces; + } + // hardware plugins pluginlib::ClassLoader actuator_loader_; pluginlib::ClassLoader sensor_loader_; @@ -556,9 +801,9 @@ class ResourceStorage std::unordered_map> controllers_reference_interfaces_map_; /// Storage of all available state interfaces - std::map state_interface_map_; + std::map> state_interface_map_; /// Storage of all available command interfaces - std::map command_interface_map_; + std::map> command_interface_map_; /// Vectors with interfaces available to controllers (depending on hardware component state) std::vector available_state_interfaces_; @@ -566,6 +811,16 @@ class ResourceStorage /// List of all claimed command interfaces std::unordered_map claimed_command_interface_map_; + +private: + std::map> + state_interface_state_publisher_map_; + + std::map> + command_interface_command_forwarder_map_; + + std::map> + sub_controller_manager_map_; }; ResourceManager::ResourceManager() : resource_storage_(std::make_unique()) {} @@ -671,6 +926,91 @@ bool ResourceManager::state_interface_is_available(const std::string & name) con name) != resource_storage_->available_state_interfaces_.end(); } +void ResourceManager::register_sub_controller_manager( + std::shared_ptr sub_controller_manager) +{ + std::lock_guard guard(resource_interfaces_lock_); + resource_storage_->add_sub_controller_manager(sub_controller_manager); +} + +std::vector> +ResourceManager::import_state_interfaces_of_sub_controller_manager( + std::shared_ptr sub_controller_manager) +{ + std::lock_guard guard(resource_interfaces_lock_); + return resource_storage_->import_distributed_state_interfaces(sub_controller_manager); +} + +std::vector> +ResourceManager::import_command_interfaces_of_sub_controller_manager( + std::shared_ptr sub_controller_manager) +{ + std::lock_guard guard(resource_interfaces_lock_); + return resource_storage_->import_distributed_command_interfaces(sub_controller_manager); +} + +std::vector> +ResourceManager::create_hardware_state_publishers(const std::string & ns) +{ + std::lock_guard guard(resource_interfaces_lock_); + std::vector> state_publishers_vec; + state_publishers_vec.reserve(available_state_interfaces().size()); + + for (const auto & state_interface : available_state_interfaces()) + { + auto state_publisher = std::make_shared( + std::move(std::make_unique( + claim_state_interface(state_interface))), + ns); + + resource_storage_->add_state_publisher(state_publisher); + state_publishers_vec.push_back(state_publisher); + } + + return state_publishers_vec; +} + +std::vector> +ResourceManager::create_hardware_command_forwarders(const std::string & ns) +{ + std::lock_guard guard(resource_interfaces_lock_); + std::vector> command_forwarders_vec; + command_forwarders_vec.reserve(available_command_interfaces().size()); + + for (const auto & command_interface : available_command_interfaces()) + { + auto command_forwarder = std::make_shared( + std::move(std::make_unique( + claim_command_interface(command_interface))), + ns); + + resource_storage_->add_command_forwarder(command_forwarder); + command_forwarders_vec.push_back(command_forwarder); + } + + return command_forwarders_vec; +} + +std::vector> +ResourceManager::get_state_publishers() const +{ + std::lock_guard guard(resource_interfaces_lock_); + return resource_storage_->get_state_publishers(); +} + +std::vector> +ResourceManager::get_command_forwarders() const +{ + std::lock_guard guard(resource_interfaces_lock_); + return resource_storage_->get_command_forwarders(); +} + +std::pair> +ResourceManager::find_command_forwarder(const std::string & key) +{ + return resource_storage_->find_command_forwarder(key); +} + // CM API: Called in "callback/slow"-thread void ResourceManager::import_controller_reference_interfaces( const std::string & controller_name, std::vector & interfaces)