diff --git a/controller_interface/include/controller_interface/chainable_controller_interface.hpp b/controller_interface/include/controller_interface/chainable_controller_interface.hpp index 37b784011c..d5f1192613 100644 --- a/controller_interface/include/controller_interface/chainable_controller_interface.hpp +++ b/controller_interface/include/controller_interface/chainable_controller_interface.hpp @@ -57,6 +57,10 @@ class ChainableControllerInterface : public ControllerInterfaceBase CONTROLLER_INTERFACE_PUBLIC std::vector export_reference_interfaces() final; + CONTROLLER_INTERFACE_PUBLIC + std::vector + export_distributed_reference_interfaces() final; + CONTROLLER_INTERFACE_PUBLIC bool set_chained_mode(bool chained_mode) final; @@ -75,6 +79,15 @@ class ChainableControllerInterface : public ControllerInterfaceBase */ virtual std::vector on_export_reference_interfaces() = 0; + // Only for fast poc should be pure virtual + /** + * Default returns emtpy list, so that not every chainable controller has to be updated for poc. + * + * \returns empty list. + */ + virtual std::vector + on_export_distributed_reference_interfaces(); + /// Virtual method that each chainable controller should implement to switch chained mode. /** * Each chainable controller implements this methods to switch between "chained" and "external" diff --git a/controller_interface/include/controller_interface/controller_interface.hpp b/controller_interface/include/controller_interface/controller_interface.hpp index a3d006755f..71d9c4f4f7 100644 --- a/controller_interface/include/controller_interface/controller_interface.hpp +++ b/controller_interface/include/controller_interface/controller_interface.hpp @@ -43,13 +43,22 @@ class ControllerInterface : public controller_interface::ControllerInterfaceBase bool is_chainable() const final; /** - * Controller has no reference interfaces. + * Controller has no distributed reference interfaces. * * \returns empty list. */ CONTROLLER_INTERFACE_PUBLIC std::vector export_reference_interfaces() final; + /** + * Controller has no reference interfaces. + * + * \returns empty list. + */ + CONTROLLER_INTERFACE_PUBLIC + std::vector + export_distributed_reference_interfaces() final; + /** * Controller is not chainable, therefore no chained mode can be set. * diff --git a/controller_interface/include/controller_interface/controller_interface_base.hpp b/controller_interface/include/controller_interface/controller_interface_base.hpp index fdaffac06f..5966e0758b 100644 --- a/controller_interface/include/controller_interface/controller_interface_base.hpp +++ b/controller_interface/include/controller_interface/controller_interface_base.hpp @@ -197,6 +197,16 @@ class ControllerInterfaceBase : public rclcpp_lifecycle::node_interfaces::Lifecy CONTROLLER_INTERFACE_PUBLIC virtual std::vector export_reference_interfaces() = 0; + /** + * Export interfaces for a chainable controller that can be used as command interface of other + * controllers if controller is distributed. + * + * \returns list of distributed command interfaces for preceding controllers. + */ + CONTROLLER_INTERFACE_PUBLIC + virtual std::vector + export_distributed_reference_interfaces() = 0; + /** * Set chained mode of a chainable controller. This method triggers internal processes to switch * a chainable controller to "chained" mode and vice-versa. Setting controller to "chained" mode diff --git a/controller_interface/src/chainable_controller_interface.cpp b/controller_interface/src/chainable_controller_interface.cpp index 2f7c67741e..56539950f9 100644 --- a/controller_interface/src/chainable_controller_interface.cpp +++ b/controller_interface/src/chainable_controller_interface.cpp @@ -14,6 +14,7 @@ #include "controller_interface/chainable_controller_interface.hpp" +#include #include #include "hardware_interface/types/lifecycle_state_names.hpp" @@ -44,6 +45,46 @@ return_type ChainableControllerInterface::update( return ret; } +std::vector +ChainableControllerInterface::export_distributed_reference_interfaces() +{ + auto reference_interfaces = on_export_distributed_reference_interfaces(); + + // check if the "reference_interfaces_" variable is resized to number of interfaces + if (reference_interfaces_.size() != reference_interfaces.size()) + { + // TODO(destogl): Should here be "FATAL"? It is fatal in terms of controller but not for the + // framework + RCLCPP_FATAL( + get_node()->get_logger(), + "The internal storage for reference values 'reference_interfaces_' variable has size '%zu', " + "but it is expected to have the size '%zu' equal to the number of exported reference " + "interfaces. No reference interface will be exported. Please correct and recompile " + "the controller with name '%s' and try again.", + reference_interfaces_.size(), reference_interfaces.size(), get_node()->get_name()); + reference_interfaces.clear(); + } + + // check if the names of the reference interfaces begin with the controller's name + for (const auto & interface : reference_interfaces) + { + if (interface.get_prefix_name() != get_node()->get_name()) + { + RCLCPP_FATAL( + get_node()->get_logger(), + "The name of the interface '%s' does not begin with the controller's name. This is " + "mandatory " + " for reference interfaces. No reference interface will be exported. Please correct and " + "recompile the controller with name '%s' and try again.", + interface.get_name().c_str(), get_node()->get_name()); + reference_interfaces.clear(); + break; + } + } + + return reference_interfaces; +} + std::vector ChainableControllerInterface::export_reference_interfaces() { @@ -111,6 +152,12 @@ bool ChainableControllerInterface::set_chained_mode(bool chained_mode) bool ChainableControllerInterface::is_in_chained_mode() const { return in_chained_mode_; } +std::vector +ChainableControllerInterface::on_export_distributed_reference_interfaces() +{ + return {}; +} + bool ChainableControllerInterface::on_set_chained_mode(bool /*chained_mode*/) { return true; } } // namespace controller_interface diff --git a/controller_interface/src/controller_interface.cpp b/controller_interface/src/controller_interface.cpp index 392a48ffa4..2c3d87e597 100644 --- a/controller_interface/src/controller_interface.cpp +++ b/controller_interface/src/controller_interface.cpp @@ -33,6 +33,12 @@ std::vector ControllerInterface::export_re return {}; } +std::vector +ControllerInterface::export_distributed_reference_interfaces() +{ + return {}; +} + bool ControllerInterface::set_chained_mode(bool /*chained_mode*/) { return false; } bool ControllerInterface::is_in_chained_mode() const { return false; } 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 1d9b062efa..b1a6e336ee 100644 --- a/controller_manager/CMakeLists.txt +++ b/controller_manager/CMakeLists.txt @@ -14,6 +14,7 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS pluginlib rclcpp realtime_tools + std_msgs ) find_package(ament_cmake REQUIRED) @@ -37,7 +38,6 @@ ament_target_dependencies(controller_manager PUBLIC ${THIS_PACKAGE_INCLUDE_DEPEN # Causes the visibility macros to use dllexport rather than dllimport, # which is appropriate when building the dll but not consuming it. target_compile_definitions(controller_manager PRIVATE "CONTROLLER_MANAGER_BUILDING_DLL") - add_executable(ros2_control_node src/ros2_control_node.cpp) target_link_libraries(ros2_control_node PRIVATE controller_manager @@ -143,6 +143,16 @@ if(BUILD_TESTING) ament_target_dependencies(test_controller_manager_srvs controller_manager_msgs ) + ament_add_gmock(test_controller_manager_urdf_passing + test/test_controller_manager_urdf_passing.cpp + ) + target_link_libraries(test_controller_manager_urdf_passing + controller_manager + ros2_control_test_assets::ros2_control_test_assets + ) + ament_target_dependencies(test_controller_manager_urdf_passing + controller_manager_msgs + ) add_library(test_controller_with_interfaces SHARED test/test_controller_with_interfaces/test_controller_with_interfaces.cpp diff --git a/controller_manager/include/controller_manager/controller_manager.hpp b/controller_manager/include/controller_manager/controller_manager.hpp index 537f0447be..d0dddc3ebb 100644 --- a/controller_manager/include/controller_manager/controller_manager.hpp +++ b/controller_manager/include/controller_manager/controller_manager.hpp @@ -35,6 +35,8 @@ #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/register_sub_controller_manager_references.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 +53,8 @@ #include "rclcpp/node_interfaces/node_logging_interface.hpp" #include "rclcpp/node_interfaces/node_parameters_interface.hpp" #include "rclcpp/parameter.hpp" - +#include "rclcpp/rclcpp.hpp" +#include "std_msgs/msg/string.hpp" namespace controller_manager { using ControllersListIterator = std::vector::const_iterator; @@ -60,6 +63,8 @@ rclcpp::NodeOptions get_cm_node_options(); class ControllerManager : public rclcpp::Node { + enum class controller_manager_type : std::uint8_t; + public: static constexpr bool kWaitForAllResources = false; static constexpr auto kInfiniteTimeout = 0; @@ -82,6 +87,12 @@ class ControllerManager : public rclcpp::Node CONTROLLER_MANAGER_PUBLIC virtual ~ControllerManager() = default; + CONTROLLER_MANAGER_PUBLIC + void subscribe_to_robot_description_topic(); + + CONTROLLER_MANAGER_PUBLIC + void robot_description_callback(const std_msgs::msg::String & msg); + CONTROLLER_MANAGER_PUBLIC void init_resource_manager(const std::string & robot_description); @@ -190,10 +201,70 @@ class ControllerManager : public rclcpp::Node CONTROLLER_MANAGER_PUBLIC unsigned int get_update_rate() const; + CONTROLLER_MANAGER_PUBLIC + rmw_qos_profile_t determine_qos_profile(const std::string & key) const; + + CONTROLLER_MANAGER_PUBLIC + bool is_central_controller_manager() const; + + CONTROLLER_MANAGER_PUBLIC + bool is_sub_controller_manager() const; + + CONTROLLER_MANAGER_PUBLIC + bool use_multiple_nodes() const; + + CONTROLLER_MANAGER_PUBLIC + std::chrono::milliseconds distributed_interfaces_publish_period() const; + protected: CONTROLLER_MANAGER_PUBLIC void init_services(); + CONTROLLER_MANAGER_PUBLIC + void get_and_initialize_distributed_parameters(); + + CONTROLLER_MANAGER_PUBLIC + controller_manager_type determine_controller_manager_type(); + + CONTROLLER_MANAGER_PUBLIC + void configure_controller_manager(); + + CONTROLLER_MANAGER_PUBLIC + void init_distributed_sub_controller_manager(); + + CONTROLLER_MANAGER_PUBLIC + void init_distributed_central_controller_manager(); + + CONTROLLER_MANAGER_PUBLIC void init_distributed_central_controller_manager_services(); + + CONTROLLER_MANAGER_PUBLIC + void register_sub_controller_manager_srv_cb( + const std::shared_ptr + request, + std::shared_ptr response); + + CONTROLLER_MANAGER_PUBLIC + void register_sub_controller_manager_references_srv_cb( + const std::shared_ptr< + controller_manager_msgs::srv::RegisterSubControllerManagerReferences::Request> + request, + std::shared_ptr + response); + + CONTROLLER_MANAGER_PUBLIC + void create_hardware_state_publishers( + const std::vector & state_interfaces_to_export); + + CONTROLLER_MANAGER_PUBLIC + void create_hardware_command_forwarders( + const std::vector & command_interfaces_to_export); + + CONTROLLER_MANAGER_PUBLIC + void register_sub_controller_manager(); + + CONTROLLER_MANAGER_PUBLIC + void register_reference_interfaces(const std::vector & reference_interfaces_names); + CONTROLLER_MANAGER_PUBLIC controller_interface::ControllerInterfaceBaseSharedPtr add_controller_impl( const ControllerSpec & controller); @@ -399,6 +470,32 @@ class ControllerManager : public rclcpp::Node */ rclcpp::CallbackGroup::SharedPtr best_effort_callback_group_; + enum class controller_manager_type : std::uint8_t + { + standard_controller_manager, + distributed_central_controller_manager, + distributed_sub_controller_manager, + unkown_type // indicating something went wrong and type could not be determined + }; + + bool sub_controller_manager_ = false; + bool central_controller_manager_ = false; + bool use_multiple_nodes_ = false; + std::shared_ptr qos_helper_; + std::string handles_qos_key_ = "system_default"; + bool publish_evaluation_msg_ = true; + std::string evaluation_qos_key_ = "system_default"; + std::vector command_interfaces_to_export_ = std::vector({}); + std::vector state_interfaces_to_export_ = std::vector({}); + + // TODO(Manuel): weak_ptr would probably be a better choice. This way has to be checked + // if pointer points to an object. Don't like the nullptr thing and implicit checks + // associated with it ... (create on distributed Handles and StatePublisher/CommandForwarder) + // needs to be checked if is nullptr before usage + std::shared_ptr distributed_pub_sub_node_ = nullptr; + std::chrono::milliseconds distributed_interfaces_publish_period_ = std::chrono::milliseconds(12); + + 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 @@ -496,11 +593,20 @@ 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_; + rclcpp::Service::SharedPtr + register_sub_controller_manager_references_srv_; + std::vector activate_request_, deactivate_request_; std::vector to_chained_mode_request_, from_chained_mode_request_; std::vector activate_command_interface_request_, deactivate_command_interface_request_; + rclcpp::Subscription::SharedPtr robot_description_subscription_; + struct SwitchParams { bool do_switch = {false}; diff --git a/controller_manager/package.xml b/controller_manager/package.xml index 65291cd864..d8d3a88c7c 100644 --- a/controller_manager/package.xml +++ b/controller_manager/package.xml @@ -26,6 +26,7 @@ ros2_control_test_assets ros2param ros2run + std_msgs ament_cmake_gmock ros2_control_test_assets diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index b912b622e2..bd89445aa5 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,14 @@ #include "controller_interface/controller_interface_base.hpp" #include "controller_manager_msgs/msg/hardware_component_state.hpp" +#include "controller_manager_msgs/msg/publisher_description.hpp" + +#include "hardware_interface/distributed_control_interface/command_forwarder.hpp" +#include "hardware_interface/distributed_control_interface/evaluation_helper.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" @@ -122,6 +130,7 @@ bool command_interface_is_reference_interface_of_controller( namespace controller_manager { + rclcpp::NodeOptions get_cm_node_options() { rclcpp::NodeOptions node_options; @@ -146,22 +155,32 @@ 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 = ""; + // TODO(Manuel): robot_description parameter is deprecated and should be removed. get_parameter("robot_description", robot_description); if (robot_description.empty()) { - throw std::runtime_error("Unable to initialize resource manager, no robot description found."); + subscribe_to_robot_description_topic(); + } + else + { + RCLCPP_WARN( + get_logger(), + "[Deprecated] Passing the robot description file directly to the control_manager node is " + "deprecated. Use robot_state_publisher instead."); + init_resource_manager(robot_description); } - - init_resource_manager(robot_description); diagnostics_updater_.setHardwareID("ros2_control"); diagnostics_updater_.add( "Controllers Activity", this, &ControllerManager::controller_activity_diagnostic_callback); init_services(); + get_and_initialize_distributed_parameters(); + configure_controller_manager(); } ControllerManager::ControllerManager( @@ -182,10 +201,55 @@ ControllerManager::ControllerManager( { RCLCPP_WARN(get_logger(), "'update_rate' parameter not set, using default value."); } + + subscribe_to_robot_description_topic(); + diagnostics_updater_.setHardwareID("ros2_control"); diagnostics_updater_.add( "Controllers Activity", this, &ControllerManager::controller_activity_diagnostic_callback); init_services(); + get_and_initialize_distributed_parameters(); + configure_controller_manager(); +} + +void ControllerManager::subscribe_to_robot_description_topic() +{ + // set QoS to transient local to get messages that have already been published + // (if robot state publisher starts before controller manager) + RCLCPP_INFO_STREAM( + get_logger(), "Subscribing to '~/robot_description' topic for robot description file."); + robot_description_subscription_ = create_subscription( + "~/robot_description", rclcpp::QoS(1).transient_local(), + std::bind(&ControllerManager::robot_description_callback, this, std::placeholders::_1)); +} + +void ControllerManager::robot_description_callback(const std_msgs::msg::String & robot_description) +{ + RCLCPP_INFO(get_logger(), "Received robot description file."); + RCLCPP_DEBUG( + get_logger(), "'Content of robot description file: %s", robot_description.data.c_str()); + // TODO(Manuel): errors should probably be caught since we don't want controller_manager node + // to die if a non valid urdf is passed. However, should maybe be fine tuned. + try + { + if (resource_manager_->load_urdf_called()) + { + RCLCPP_WARN( + get_logger(), + "ResourceManager has already loaded an urdf file. Ignoring attempt to reload a robot " + "description file."); + return; + } + init_resource_manager(robot_description.data.c_str()); + } + catch (std::runtime_error & e) + { + RCLCPP_ERROR_STREAM( + get_logger(), + "The published robot description file (urdf) seems not to be genuine. The following error " + "was caught:" + << e.what()); + } } void ControllerManager::init_resource_manager(const std::string & robot_description) @@ -290,6 +354,698 @@ void ControllerManager::init_services() qos_services, best_effort_callback_group_); } +void ControllerManager::get_and_initialize_distributed_parameters() +{ + if (!get_parameter("central_controller_manager", central_controller_manager_)) + { + RCLCPP_WARN( + get_logger(), "'central_controller_manager' parameter not set, using default value:%s", + central_controller_manager_ ? "true" : "false"); + } + + int64_t distributed_interfaces_publish_period; + if (get_parameter("distributed_interfaces_publish_period", distributed_interfaces_publish_period)) + { + distributed_interfaces_publish_period_ = + std::chrono::milliseconds(distributed_interfaces_publish_period); + } + else + { + RCLCPP_WARN( + get_logger(), + "'distributed_interfaces_publish_period' parameter not set, using default value."); + } + + if (!get_parameter("export_command_interfaces", command_interfaces_to_export_)) + { + RCLCPP_WARN( + get_logger(), + "'export_command_interfaces' parameter not set, going to export all available command " + "interfaces"); + } + + if (!get_parameter("export_state_interfaces", state_interfaces_to_export_)) + { + RCLCPP_WARN( + get_logger(), + "'export_state_interfaces' parameter not set, going to export all available command " + "interfaces"); + } + + 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"); + } + + if (!get_parameter("use_multiple_nodes", use_multiple_nodes_)) + { + RCLCPP_WARN( + get_logger(), "'use_multiple_nodes' parameter not set, using default value:%s", + use_multiple_nodes_ ? "true" : "false"); + } + if (!get_parameter("handles_qos_key", handles_qos_key_)) + { + RCLCPP_WARN( + get_logger(), "'handles_qos_key' parameter not set, using default value:%s", + handles_qos_key_.c_str()); + } + if (!get_parameter("publish_evaluation_msg", publish_evaluation_msg_)) + { + RCLCPP_WARN( + get_logger(), "'publish_evaluation_msg' parameter not set, using default value:%s", + publish_evaluation_msg_ ? "true" : "false"); + } + if (!get_parameter("evaluation_qos_key", evaluation_qos_key_)) + { + RCLCPP_WARN( + get_logger(), "'evaluation_qos_key' parameter not set, using default value:%s", + evaluation_qos_key_.c_str()); + } +} + +void ControllerManager::configure_controller_manager() +{ + auto cm_type = determine_controller_manager_type(); + + switch (cm_type) + { + case controller_manager_type::distributed_central_controller_manager: + init_distributed_central_controller_manager(); + break; + case controller_manager_type::distributed_sub_controller_manager: + init_distributed_sub_controller_manager(); + break; + case controller_manager_type::standard_controller_manager: + //nothing special to configure + break; + default: + throw std::logic_error( + "Controller manager configuration not possible. Not a known controller manager type." + "Did you maybe set `central_controller_manager:true` and `sub_controller_manager:true`?" + "Note:Distributed controller manager can only be central_controller_manager or " + "sub_controller_manager, not both."); + break; + } +} + +// 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. +ControllerManager::controller_manager_type ControllerManager::determine_controller_manager_type() +{ + bool std_controller_manager = !central_controller_manager_ && !sub_controller_manager_; + bool distributed_sub_controller_manager = !central_controller_manager_ && sub_controller_manager_; + bool distributed_central_controller_manager = + central_controller_manager_ && !sub_controller_manager_; + if (distributed_sub_controller_manager) + { + return controller_manager_type::distributed_sub_controller_manager; + } + // This means we are the central controller manager + else if (distributed_central_controller_manager) + { + return controller_manager_type::distributed_central_controller_manager; + } + // std controller manager or error. std controller manager needs no special setup. + else if (std_controller_manager) + { + return controller_manager_type::standard_controller_manager; + } + return controller_manager_type::unkown_type; +} + +rmw_qos_profile_t ControllerManager::determine_qos_profile(const std::string & key) const +{ + if (key == "sensor_data") + { + return evaluation_helper::rmw_qos_profile_sensor_data; + } + else if (key == "sensor_data_1") + { + return evaluation_helper::rmw_qos_profile_sensor_data_1; + } + else if (key == "sensor_data_100") + { + return evaluation_helper::rmw_qos_profile_sensor_data_100; + } + else if (key == "reliable") + { + return evaluation_helper::rmw_qos_profile_reliable; + } + else if (key == "reliable_100") + { + return evaluation_helper::rmw_qos_profile_reliable_100; + } + else if (key == "system_default") + { + return evaluation_helper::rmw_qos_profile_system_default; + } + throw std::runtime_error("Given qos profile not know"); +} + +void ControllerManager::init_distributed_sub_controller_manager() +{ + // just for evaluation of concept + auto handle_qos_profile = determine_qos_profile(handles_qos_key_); + auto evaluation_qos_profile = determine_qos_profile(evaluation_qos_key_); + qos_helper_ = evaluation_helper::Evaluation_Helper::create_instance( + handle_qos_profile, publish_evaluation_msg_, evaluation_qos_profile); + // if only one node per sub controller manager is used + if (!use_multiple_nodes()) + { + // create node for publishing/subscribing + rclcpp::NodeOptions node_options; + node_options.clock_type(rcl_clock_type_t::RCL_STEADY_TIME); + distributed_pub_sub_node_ = std::make_shared( + std::string(get_name()) + "_pub_sub_node", get_namespace(), node_options, false); + //try to add to executor + try + { + executor_->add_node(distributed_pub_sub_node_->get_node_base_interface()); + } + catch (const std::runtime_error & e) + { + RCLCPP_WARN_STREAM( + get_logger(), + "ControllerManager: can not add node for distributed publishing/subscribing to executor:" + << e.what()); + } + } + + // export every interface by default + if (state_interfaces_to_export_.empty()) + { + // get all available state interfaces and export + create_hardware_state_publishers(resource_manager_->available_state_interfaces()); + } + else + { + create_hardware_state_publishers(state_interfaces_to_export_); + } + // export every interface by default + if (command_interfaces_to_export_.empty()) + { + // get all available command interfaces and export + create_hardware_command_forwarders(resource_manager_->available_command_interfaces()); + } + else + { + create_hardware_command_forwarders(command_interfaces_to_export_); + } + register_sub_controller_manager(); +} + +void ControllerManager::init_distributed_central_controller_manager() +{ + // just for evaluation of concept + auto handle_qos_profile = determine_qos_profile(handles_qos_key_); + auto evaluation_qos_profile = determine_qos_profile(evaluation_qos_key_); + qos_helper_ = evaluation_helper::Evaluation_Helper::create_instance( + handle_qos_profile, publish_evaluation_msg_, evaluation_qos_profile); + if (!use_multiple_nodes()) + { + rclcpp::NodeOptions node_options; + node_options.clock_type(rcl_clock_type_t::RCL_STEADY_TIME); + distributed_pub_sub_node_ = std::make_shared( + std::string(get_name()) + "_pub_sub_node", get_namespace(), node_options, false); + //try to add to executor + try + { + executor_->add_node(distributed_pub_sub_node_->get_node_base_interface()); + } + catch (const std::runtime_error & e) + { + RCLCPP_WARN_STREAM( + get_logger(), + "ControllerManager: can not add node for distributed publishing/subscribing to executor:" + << e.what()); + } + } + init_distributed_central_controller_manager_services(); +} + +void ControllerManager::init_distributed_central_controller_manager_services() +{ + rclcpp::QoS qos_distributed_services_keep_10 = + rclcpp::QoS(rclcpp::QoSInitialization(RMW_QOS_POLICY_HISTORY_KEEP_ALL, 10)) + .reliable() + .durability_volatile(); + + 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_distributed_services_keep_10, distributed_system_srv_callback_group_); + + register_sub_controller_manager_references_srv_ = + create_service( + "register_sub_controller_manager_references", + std::bind( + &ControllerManager::register_sub_controller_manager_references_srv_cb, this, + std::placeholders::_1, std::placeholders::_2), + qos_distributed_services_keep_10, 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()); + // create distributed state interface and import into resource storage. + distributed_state_interfaces = + resource_manager_->import_state_interfaces_of_sub_controller_manager( + sub_ctrl_mng_wrapper, get_namespace(), distributed_pub_sub_node_); + + // register every node of state_interface at executor only if multiple nodes + // are used. Otherwise the single nodes has already been added + if (use_multiple_nodes()) + { + 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 node of distributed state " + "interface of sub controller manager. Exception:" + << e.what()); + } + } + } + + std::vector> + distributed_command_interfaces; + distributed_command_interfaces.reserve(sub_ctrl_mng_wrapper->get_command_forwarder_count()); + // create distributed command interface and import into resource storage. + distributed_command_interfaces = + resource_manager_->import_command_interfaces_of_sub_controller_manager( + sub_ctrl_mng_wrapper, get_namespace(), distributed_pub_sub_node_); + + for (const auto & command_interface : distributed_command_interfaces) + { + // register every node of command_interface at executor only if multiple nodes + // are used. Otherwise the single nodes has already been added + if (use_multiple_nodes()) + { + 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 node of distributed " + "command_interface of 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::register_sub_controller_manager_references_srv_cb( + const std::shared_ptr< + controller_manager_msgs::srv::RegisterSubControllerManagerReferences::Request> + request, + std::shared_ptr + response) +{ + std::lock_guard guard(central_controller_manager_srv_lock_); + + // only command interfaces can be state publishers. We initialize state interfaces to empty list. + std::vector empty_state_publishers{}; + auto sub_ctrl_mng_wrapper = std::make_shared( + request->sub_controller_manager_namespace, request->sub_controller_manager_name, + empty_state_publishers, request->command_state_publishers); + + std::vector> + distributed_command_interfaces; + distributed_command_interfaces.reserve(sub_ctrl_mng_wrapper->get_command_forwarder_count()); + // create distributed command interface and import into resource storage. + distributed_command_interfaces = + resource_manager_->import_reference_interfaces_of_sub_controller_manager( + sub_ctrl_mng_wrapper, get_namespace(), distributed_pub_sub_node_); + + for (const auto & command_interface : distributed_command_interfaces) + { + // register every node of command_interface at executor only if multiple nodes + // are used. Otherwise the single nodes has already been added + if (use_multiple_nodes()) + { + 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 node of reference " + "interface of 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 reference interfaces of sub_controller_manager <" + << sub_ctrl_mng_wrapper->get_name() << ">."); +} + +void ControllerManager::create_hardware_state_publishers( + const std::vector & state_interfaces_to_export) +{ + for (const auto & state_interface : state_interfaces_to_export) + { + std::shared_ptr state_publisher; + try + { + state_publisher = std::make_shared( + std::move(std::make_unique( + resource_manager_->claim_state_interface(state_interface))), + get_namespace(), distributed_interfaces_publish_period(), distributed_pub_sub_node_); + } + catch (const std::exception & e) + { + RCLCPP_ERROR( + get_logger(), "Can't create StatePublisher for state interface<'%s'>: %s", + state_interface.c_str(), e.what()); + continue; + } + + resource_manager_->add_hardware_state_publishers(state_publisher); + + if (use_multiple_nodes()) + { + 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::create_hardware_command_forwarders( + const std::vector & command_interfaces_to_export) +{ + for (auto const & command_interface : command_interfaces_to_export) + { + std::shared_ptr command_forwarder; + try + { + command_forwarder = std::make_shared( + std::move(std::make_unique( + resource_manager_->claim_command_interface(command_interface))), + get_namespace(), distributed_interfaces_publish_period(), distributed_pub_sub_node_); + } + catch (const std::exception & e) + { + RCLCPP_ERROR( + get_logger(), "Can't create CommandForwarder for command interface<'%s'>: %s", + command_interface.c_str(), e.what()); + continue; + } + + resource_manager_->add_hardware_command_forwarders(command_forwarder); + + if (use_multiple_nodes()) + { + 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."); + } +} + +void ControllerManager::register_reference_interfaces( + const std::vector & reference_interfaces_names) +{ + RCLCPP_INFO_STREAM( + get_logger(), "SubControllerManager:<" << get_namespace() << "/" << get_name() + << "> trying to register reference interfaces."); + rclcpp::Client::SharedPtr + client = create_client( + "/register_sub_controller_manager_references"); + + auto request = std::make_shared< + controller_manager_msgs::srv::RegisterSubControllerManagerReferences::Request>(); + request->sub_controller_manager_namespace = get_namespace(); + request->sub_controller_manager_name = get_name(); + + // export the provided CommandForwarders + for (auto const & reference_interface_name : reference_interfaces_names) + { + auto [found, command_forwarder] = + resource_manager_->find_command_forwarder(reference_interface_name); + if (found) + { + // 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_forwarder->create_publisher_description_msg()); + } + else + { + RCLCPP_WARN_STREAM( + get_logger(), "SubControllerManager: <" + << get_namespace() << "/" << get_name() + << "> could not find command_forwarder for reference interfaces:" + << reference_interface_name); + } + } + + 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 " + "register_sub_controller_manager_references service. Exiting."); + return; + } + RCLCPP_INFO_STREAM( + get_logger(), "SubControllerManager:<" + << get_namespace() << "/" << get_name() + << ">. Central controller managers " + "register_sub_controller_manager_references service not available, " + "waiting again..."); + } + + auto result = client->async_send_request(request); + // TODO(Manuel): first try to wait synchronous. If this doesn't work we might have to create a + // queue or something similar, add the future and check in update periodically if finished. + + // This blocks... which might be bad... + result.wait(); + // 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) + { + RCLCPP_WARN_STREAM( + get_logger(), "SubControllerManager: <" << get_namespace() << "/" << get_name() + << "> found commad forwarder for" << key); + 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."); + } +} + controller_interface::ControllerInterfaceBaseSharedPtr ControllerManager::load_controller( const std::string & controller_name, const std::string & controller_type) { @@ -518,8 +1274,33 @@ controller_interface::return_type ControllerManager::configure_controller( controller_name.c_str()); return controller_interface::return_type::ERROR; } + // safe the name for later. Interfaces get moved so they are no longer available after they have been imported. + std::vector reference_interfaces_names; + reference_interfaces_names.reserve(interfaces.size()); + for (const auto & interface : interfaces) + { + reference_interfaces_names.push_back(interface.get_name()); + } + resource_manager_->import_controller_reference_interfaces(controller_name, interfaces); + if (is_sub_controller_manager()) + { + // TODO(Manuel); This is only for fast poc, chaining of multiples in sub controller + // is most likely going to lead to issues if handled this way. + // We should only allow the first controller in the chain to be distributed in each + // sub controller manager and chain the successor locally in sub controller manager + // instead of exporting for every. + + // Set chained mode as default true and make references available so that + // hardware_command_forwarders can be created. + controller->set_chained_mode(true); + resource_manager_->make_controller_reference_interfaces_available(controller_name); + // export all of the just created reference interfaces by default + create_hardware_command_forwarders(reference_interfaces_names); + register_reference_interfaces(reference_interfaces_names); + } + // TODO(destogl): check and resort controllers in the vector } @@ -1893,6 +2674,20 @@ std::pair ControllerManager::split_command_interface( unsigned int ControllerManager::get_update_rate() const { return update_rate_; } +bool ControllerManager::is_central_controller_manager() const +{ + return central_controller_manager_; +} + +bool ControllerManager::is_sub_controller_manager() const { return sub_controller_manager_; } + +bool ControllerManager::use_multiple_nodes() const { return use_multiple_nodes_; } + +std::chrono::milliseconds ControllerManager::distributed_interfaces_publish_period() const +{ + return distributed_interfaces_publish_period_; +} + void ControllerManager::propagate_deactivation_of_chained_mode( const std::vector & controllers) { diff --git a/controller_manager/test/controller_manager_test_common.hpp b/controller_manager/test/controller_manager_test_common.hpp index b6e6acac0e..78c3fcb06b 100644 --- a/controller_manager/test/controller_manager_test_common.hpp +++ b/controller_manager/test/controller_manager_test_common.hpp @@ -22,15 +22,20 @@ #include #include #include +#include #include #include "controller_interface/controller_interface.hpp" #include "controller_manager/controller_manager.hpp" +#include "controller_manager_msgs/srv/list_hardware_interfaces.hpp" #include "controller_manager_msgs/srv/switch_controller.hpp" +#include "rclcpp/rclcpp.hpp" #include "rclcpp/utilities.hpp" +#include "std_msgs/msg/string.hpp" + #include "ros2_control_test_assets/descriptions.hpp" #include "test_controller_failed_init/test_controller_failed_init.hpp" @@ -60,21 +65,51 @@ template class ControllerManagerFixture : public ::testing::Test { public: + explicit ControllerManagerFixture( + const std::string & robot_description = ros2_control_test_assets::minimal_robot_urdf, + const bool & pass_urdf_as_parameter = false) + : robot_description_(robot_description), pass_urdf_as_parameter_(pass_urdf_as_parameter) + { + executor_ = std::make_shared(); + // We want to be able to create a ResourceManager where no urdf file has been passed to + if (robot_description_.empty()) + { + cm_ = std::make_shared( + std::make_unique(), executor_, TEST_CM_NAME); + } + else + { + // can be removed later, needed if we want to have the deprecated way of passing the robot + // description file to the controller manager covered by tests + if (pass_urdf_as_parameter_) + { + cm_ = std::make_shared( + std::make_unique(robot_description_, true, true), + executor_, TEST_CM_NAME); + } + else + { + // TODO(Manuel) : passing via topic not working in test setup, tested cm does + // not receive msg. Have to check this... + + // this is just a workaround to skip passing + cm_ = std::make_shared( + std::make_unique(), executor_, TEST_CM_NAME); + // mimic topic call + auto msg = std_msgs::msg::String(); + msg.data = robot_description_; + cm_->robot_description_callback(msg); + } + } + } + static void SetUpTestCase() { rclcpp::init(0, nullptr); } static void TearDownTestCase() { rclcpp::shutdown(); } - void SetUp() - { - executor_ = std::make_shared(); - cm_ = std::make_shared( - std::make_unique( - ros2_control_test_assets::minimal_robot_urdf, true, true), - executor_, TEST_CM_NAME); - run_updater_ = false; - } + void SetUp() override { run_updater_ = false; } - void TearDown() { stopCmUpdater(); } + void TearDown() override { stopCmUpdater(); } void startCmUpdater() { @@ -120,6 +155,8 @@ class ControllerManagerFixture : public ::testing::Test std::thread updater_; bool run_updater_; + const std::string robot_description_; + const bool pass_urdf_as_parameter_; }; class TestControllerManagerSrvs diff --git a/controller_manager/test/test_controller_manager_urdf_passing.cpp b/controller_manager/test/test_controller_manager_urdf_passing.cpp new file mode 100644 index 0000000000..c73a45e291 --- /dev/null +++ b/controller_manager/test/test_controller_manager_urdf_passing.cpp @@ -0,0 +1,88 @@ +// Copyright 2020 Open Source Robotics Foundation, 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 +#include +#include +#include +#include + +#include "controller_manager/controller_manager.hpp" +#include "controller_manager_test_common.hpp" + +#include "ros2_control_test_assets/descriptions.hpp" + +class TestControllerManagerWithTestableCM; + +class TestableControllerManager : public controller_manager::ControllerManager +{ + friend TestControllerManagerWithTestableCM; + + FRIEND_TEST(TestControllerManagerWithTestableCM, initial_no_load_urdf_called); + FRIEND_TEST(TestControllerManagerWithTestableCM, load_urdf_called_after_callback); + FRIEND_TEST(TestControllerManagerWithTestableCM, load_urdf_called_after_invalid_urdf_passed); + FRIEND_TEST(TestControllerManagerWithTestableCM, load_urdf_called_after_callback); + FRIEND_TEST(TestControllerManagerWithTestableCM, load_urdf_called_after_callback); + +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 TestControllerManagerWithTestableCM +: public ControllerManagerFixture, + public testing::WithParamInterface +{ +public: + // create cm with no urdf + TestControllerManagerWithTestableCM() + : ControllerManagerFixture("", false) + { + } +}; + +TEST_P(TestControllerManagerWithTestableCM, initial_no_load_urdf_called) +{ + ASSERT_FALSE(cm_->resource_manager_->load_urdf_called()); +} + +TEST_P(TestControllerManagerWithTestableCM, load_urdf_called_after_callback) +{ + ASSERT_FALSE(cm_->resource_manager_->load_urdf_called()); + // mimic callback + auto msg = std_msgs::msg::String(); + msg.data = ros2_control_test_assets::minimal_robot_urdf; + cm_->robot_description_callback(msg); + ASSERT_TRUE(cm_->resource_manager_->load_urdf_called()); +} + +TEST_P(TestControllerManagerWithTestableCM, load_urdf_called_after_invalid_urdf_passed) +{ + ASSERT_FALSE(cm_->resource_manager_->load_urdf_called()); + // mimic callback + auto msg = std_msgs::msg::String(); + msg.data = ros2_control_test_assets::minimal_robot_missing_command_keys_urdf; + cm_->robot_description_callback(msg); + ASSERT_TRUE(cm_->resource_manager_->load_urdf_called()); +} + +INSTANTIATE_TEST_SUITE_P( + test_best_effort, TestControllerManagerWithTestableCM, testing::Values(best_effort)); diff --git a/controller_manager_msgs/CMakeLists.txt b/controller_manager_msgs/CMakeLists.txt index 2a863c29dd..cd51c6ab22 100644 --- a/controller_manager_msgs/CMakeLists.txt +++ b/controller_manager_msgs/CMakeLists.txt @@ -9,8 +9,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 @@ -19,6 +21,8 @@ set(srv_files srv/ListHardwareComponents.srv srv/ListHardwareInterfaces.srv srv/LoadController.srv + srv/RegisterSubControllerManager.srv + srv/RegisterSubControllerManagerReferences.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/controller_manager_msgs/srv/RegisterSubControllerManagerReferences.srv b/controller_manager_msgs/srv/RegisterSubControllerManagerReferences.srv new file mode 100644 index 0000000000..6963b086a2 --- /dev/null +++ b/controller_manager_msgs/srv/RegisterSubControllerManagerReferences.srv @@ -0,0 +1,19 @@ +# The RegisterSubControllerManagerReferences service allows a distributed controller +# manager to register it's reference interfaces 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[] 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 77eec3ec86..9be78cbc6b 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 tinyxml2_vendor ) @@ -24,6 +27,9 @@ endforeach() add_library(hardware_interface 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/hardware_interface/distributed_control_interface/evaluation_helper.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..fb94a09450 --- /dev/null +++ b/hardware_interface/include/hardware_interface/distributed_control_interface/command_forwarder.hpp @@ -0,0 +1,71 @@ +#ifndef DISTRIBUTED_CONTROL__COMMAND_FORWARDER_HPP_ +#define DISTRIBUTED_CONTROL__COMMAND_FORWARDER_HPP_ + +#include +#include +#include +#include + +#include "hardware_interface/distributed_control_interface/evaluation_helper.hpp" +#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, std::chrono::milliseconds period_in_ms, + std::shared_ptr node); + + 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::chrono::milliseconds period_in_ms_; + + 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/evaluation_helper.hpp b/hardware_interface/include/hardware_interface/distributed_control_interface/evaluation_helper.hpp new file mode 100644 index 0000000000..84905fd9f6 --- /dev/null +++ b/hardware_interface/include/hardware_interface/distributed_control_interface/evaluation_helper.hpp @@ -0,0 +1,156 @@ +// Copyright 2020 - 2021 ros2_control Development Team +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef DISTRIBUTED_CONTROL__QOS_PROFILES_HELPER_HPP_ +#define DISTRIBUTED_CONTROL__QOS_PROFILES_HELPER_HPP_ + +#include +#include + +#include "rmw/qos_profiles.h" +#include "rmw/types.h" + +namespace evaluation_helper +{ + +// All profiles used for evaluation, copy them to make sure that exactly those params are used. +static const rmw_qos_profile_t rmw_qos_profile_sensor_data = { + RMW_QOS_POLICY_HISTORY_KEEP_LAST, + 5, + RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT, + RMW_QOS_POLICY_DURABILITY_VOLATILE, + RMW_QOS_DEADLINE_DEFAULT, + RMW_QOS_LIFESPAN_DEFAULT, + RMW_QOS_POLICY_LIVELINESS_SYSTEM_DEFAULT, + RMW_QOS_LIVELINESS_LEASE_DURATION_DEFAULT, + false}; + +static const rmw_qos_profile_t rmw_qos_profile_sensor_data_1 = { + RMW_QOS_POLICY_HISTORY_KEEP_LAST, + 1, + RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT, + RMW_QOS_POLICY_DURABILITY_VOLATILE, + RMW_QOS_DEADLINE_DEFAULT, + RMW_QOS_LIFESPAN_DEFAULT, + RMW_QOS_POLICY_LIVELINESS_SYSTEM_DEFAULT, + RMW_QOS_LIVELINESS_LEASE_DURATION_DEFAULT, + false}; + +static const rmw_qos_profile_t rmw_qos_profile_sensor_data_100 = { + RMW_QOS_POLICY_HISTORY_KEEP_LAST, + 100, + RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT, + RMW_QOS_POLICY_DURABILITY_VOLATILE, + RMW_QOS_DEADLINE_DEFAULT, + RMW_QOS_LIFESPAN_DEFAULT, + RMW_QOS_POLICY_LIVELINESS_SYSTEM_DEFAULT, + RMW_QOS_LIVELINESS_LEASE_DURATION_DEFAULT, + false}; + +static const rmw_qos_profile_t rmw_qos_profile_reliable = { + RMW_QOS_POLICY_HISTORY_KEEP_LAST, + 1, + RMW_QOS_POLICY_RELIABILITY_RELIABLE, + RMW_QOS_POLICY_DURABILITY_VOLATILE, + RMW_QOS_DEADLINE_DEFAULT, + RMW_QOS_LIFESPAN_DEFAULT, + RMW_QOS_POLICY_LIVELINESS_SYSTEM_DEFAULT, + RMW_QOS_LIVELINESS_LEASE_DURATION_DEFAULT, + false}; + +static const rmw_qos_profile_t rmw_qos_profile_reliable_100 = { + RMW_QOS_POLICY_HISTORY_KEEP_LAST, + 100, + RMW_QOS_POLICY_RELIABILITY_RELIABLE, + RMW_QOS_POLICY_DURABILITY_VOLATILE, + RMW_QOS_DEADLINE_DEFAULT, + RMW_QOS_LIFESPAN_DEFAULT, + RMW_QOS_POLICY_LIVELINESS_SYSTEM_DEFAULT, + RMW_QOS_LIVELINESS_LEASE_DURATION_DEFAULT, + false}; + +static const rmw_qos_profile_t rmw_qos_profile_system_default = { + RMW_QOS_POLICY_HISTORY_SYSTEM_DEFAULT, + RMW_QOS_POLICY_DEPTH_SYSTEM_DEFAULT, + RMW_QOS_POLICY_RELIABILITY_SYSTEM_DEFAULT, + RMW_QOS_POLICY_DURABILITY_SYSTEM_DEFAULT, + RMW_QOS_DEADLINE_DEFAULT, + RMW_QOS_LIFESPAN_DEFAULT, + RMW_QOS_POLICY_LIVELINESS_SYSTEM_DEFAULT, + RMW_QOS_LIVELINESS_LEASE_DURATION_DEFAULT, + false}; + +class Evaluation_Helper +{ +protected: + Evaluation_Helper( + const rmw_qos_profile_t & qos_profile, const bool & publish_evaluation_msg, + const rmw_qos_profile_t & evaluation_qos_profile) + : qos_profile_(qos_profile), + publish_evaluation_msg_(publish_evaluation_msg), + evaluation_qos_profile_(evaluation_qos_profile) + { + } + +public: + /** + * Evaluation_Helpers should not be cloneable. + */ + Evaluation_Helper(Evaluation_Helper & other) = delete; + /** + * Evaluation_Helpers should not be assignable. + */ + void operator=(const Evaluation_Helper &) = delete; + + static std::shared_ptr create_instance( + const rmw_qos_profile_t & qos_profile, const bool & publish_evaluation_msg, + const rmw_qos_profile_t & evaluation_qos_profile) + { + std::lock_guard lock(mutex_); + if (qos_profile_instance_ == nullptr) + { + qos_profile_instance_ = std::shared_ptr( + new Evaluation_Helper(qos_profile, publish_evaluation_msg, evaluation_qos_profile)); + } + return qos_profile_instance_; + } + + static std::shared_ptr get_instance() + { + std::lock_guard lock(mutex_); + if (qos_profile_instance_ == nullptr) + { + throw std::runtime_error("Evaluation_Helper not initialized!"); + } + return qos_profile_instance_; + } + + rmw_qos_profile_t get_qos_profile() const { return qos_profile_; } + + rmw_qos_profile_t get_evaluation_qos_profile() const { return evaluation_qos_profile_; } + + bool publish_evaluation_msg() const { return publish_evaluation_msg_; } + +protected: + static std::shared_ptr qos_profile_instance_; + static std::mutex mutex_; + + const rmw_qos_profile_t qos_profile_; + const bool publish_evaluation_msg_; + const rmw_qos_profile_t evaluation_qos_profile_; +}; + +} // namespace evaluation_helper + +#endif // DISTRIBUTED_CONTROL__EVALUATION_HELPER_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..65a166658a --- /dev/null +++ b/hardware_interface/include/hardware_interface/distributed_control_interface/state_publisher.hpp @@ -0,0 +1,63 @@ +#ifndef DISTRIBUTED_CONTROL__STATE_PUBLISHER_HPP_ +#define DISTRIBUTED_CONTROL__STATE_PUBLISHER_HPP_ + +#include +#include +#include + +#include "hardware_interface/distributed_control_interface/evaluation_helper.hpp" +#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, std::chrono::milliseconds period_in_ms, + std::shared_ptr node); + 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::chrono::milliseconds period_in_ms_; + + 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..ec7e359da2 --- /dev/null +++ b/hardware_interface/include/hardware_interface/distributed_control_interface/sub_controller_manager_wrapper.hpp @@ -0,0 +1,67 @@ +#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(); } + + std::string type() const { return TYPE_; } + +private: + const std::string NAMESPACE_; + const std::string NAME_; + const std::string TYPE_ = "sub_controller_manager"; + 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..0b4aaba12d 100644 --- a/hardware_interface/include/hardware_interface/handle.hpp +++ b/hardware_interface/include/hardware_interface/handle.hpp @@ -18,48 +18,63 @@ #include #include +#include "hardware_interface/distributed_control_interface/evaluation_helper.hpp" +#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 +83,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) + double * value_ptr = nullptr, std::shared_ptr node = nullptr) + : HandleInterface(prefix_name, interface_name, value_ptr, node) { } - 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 +185,143 @@ 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, + std::shared_ptr node) + : ReadOnlyHandle(description.prefix_name(), description.interface_name(), &value_, node), + get_value_topic_name_(description.topic_name()), + namespace_(ns), + interface_namespace_(description.get_namespace()) + { + // if no node has been passed + // create node for subscribing to StatePublisher described in StatePublisherDescription + if (!node_.get()) + { + rclcpp::NodeOptions node_options; + node_options.clock_type(rcl_clock_type_t::RCL_STEADY_TIME); + node_ = std::make_shared( + get_underscore_separated_name() + "_state_interface_subscriber", namespace_, node_options, + false); + } + + auto evaluation_helper = evaluation_helper::Evaluation_Helper::get_instance(); + rclcpp::QoS qos_profile( + rclcpp::QoSInitialization::from_rmw(evaluation_helper->get_qos_profile())); + // subscribe to topic provided by StatePublisher + state_value_sub_ = node_->create_subscription( + get_value_topic_name_, qos_profile, + 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_DEBUG_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, std::shared_ptr node = nullptr) + : HandleInterface(prefix_name, interface_name, value_ptr, node) + { + } + + 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 +338,127 @@ class CommandInterface : public ReadWriteHandle using ReadWriteHandle::ReadWriteHandle; }; +class DistributedReadWriteHandle : public ReadWriteHandle +{ +public: + DistributedReadWriteHandle( + const distributed_control::PublisherDescription & description, const std::string & ns, + std::shared_ptr node) + : ReadWriteHandle(description.prefix_name(), description.interface_name(), &value_, node), + get_value_topic_name_(description.topic_name()), + namespace_(ns), + interface_namespace_(description.get_namespace()), + forward_command_topic_name_(get_underscore_separated_name() + "_command_forwarding") + { + // if no node has been passed + // create node for subscribing to CommandForwarder described in CommandForwarderDescription + if (!node_.get()) + { + rclcpp::NodeOptions node_options; + node_options.clock_type(rcl_clock_type_t::RCL_STEADY_TIME); + node_ = std::make_shared( + get_underscore_separated_name() + "_distributed_command_interface", namespace_, + node_options, false); + } + + auto evaluation_helper = evaluation_helper::Evaluation_Helper::get_instance(); + rclcpp::QoS qos_profile( + rclcpp::QoSInitialization::from_rmw(evaluation_helper->get_qos_profile())); + // subscribe to topic provided by CommandForwarder + command_value_sub_ = node_->create_subscription( + get_value_topic_name_, qos_profile, + 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_, qos_profile); + } + + 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_DEBUG(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_DEBUG_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/hardware_component_info.hpp b/hardware_interface/include/hardware_interface/hardware_component_info.hpp index 45afebdb34..44bfafcbc3 100644 --- a/hardware_interface/include/hardware_interface/hardware_component_info.hpp +++ b/hardware_interface/include/hardware_interface/hardware_component_info.hpp @@ -36,7 +36,7 @@ struct HardwareComponentInfo /// Component name. std::string name; - /// Component "classification": "actuator", "sensor" or "system" + /// Component "classification": "actuator", "sensor", "system" or "sub_controller_manager" std::string type; /// Component pluginlib plugin name. 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..eb1aa1d79d 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" @@ -83,6 +88,16 @@ class HARDWARE_INTERFACE_PUBLIC ResourceManager */ void load_urdf(const std::string & urdf, bool validate_interfaces = true); + /** + * @brief if the resource manager load_urdf(...) function has been called this returns true. + * We want to permit to load the urdf later on but we currently don't want to permit multiple + * calls to load_urdf (reloading/loading different urdf). + * + * @return true if resource manager's load_urdf() has been already called. + * @return false if resource manager's load_urdf() has not been yet called. + */ + bool load_urdf_called() const; + /// Claim a state interface given its key. /** * The resource is claimed as long as being in scope. @@ -114,6 +129,38 @@ 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, + const std::string & ns, std::shared_ptr node); + + std::vector> + import_command_interfaces_of_sub_controller_manager( + std::shared_ptr sub_controller_manager, + const std::string & ns, std::shared_ptr node); + + std::vector> + import_reference_interfaces_of_sub_controller_manager( + std::shared_ptr sub_controller_manager, + const std::string & ns, std::shared_ptr node); + + void add_hardware_state_publishers( + std::shared_ptr state_publisher); + + void add_hardware_command_forwarders( + std::shared_ptr command_forwarder); + + 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. @@ -405,6 +452,8 @@ class HARDWARE_INTERFACE_PUBLIC ResourceManager // Structure to store read and write status so it is not initialized in the real-time loop HardwareReadWriteStatus read_write_status; + + bool load_urdf_called_ = false; }; } // namespace hardware_interface diff --git a/hardware_interface/package.xml b/hardware_interface/package.xml index dda55a198b..1a1dbd275d 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..203be40c00 --- /dev/null +++ b/hardware_interface/src/hardware_interface/distributed_control_interface/command_forwarder.cpp @@ -0,0 +1,130 @@ +#include "hardware_interface/distributed_control_interface/command_forwarder.hpp" + +#include +#include +#include +#include +#include + +#include "rclcpp/rclcpp.hpp" + +using namespace std::chrono_literals; + +namespace distributed_control +{ + +CommandForwarder::CommandForwarder( + std::unique_ptr loaned_command_interface_ptr, + const std::string & ns, std::chrono::milliseconds period_in_ms, + std::shared_ptr node) +: loaned_command_interface_ptr_(std::move(loaned_command_interface_ptr)), + namespace_(ns), + period_in_ms_(period_in_ms), + node_(node), + topic_name_(loaned_command_interface_ptr_->get_underscore_separated_name() + "_command_state") +{ + // if we did not get a node passed, we create one ourselves + if (!node_.get()) + { + rclcpp::NodeOptions node_options; + node_options.clock_type(rcl_clock_type_t::RCL_STEADY_TIME); + node_ = std::make_shared( + loaned_command_interface_ptr_->get_underscore_separated_name() + "_command_forwarder", + namespace_, node_options, false); + } + + auto evaluation_helper = evaluation_helper::Evaluation_Helper::get_instance(); + rclcpp::QoS qos_profile( + rclcpp::QoSInitialization::from_rmw(evaluation_helper->get_qos_profile())); + state_value_pub_ = node_->create_publisher(topic_name_, qos_profile); + // TODO(Manuel): We should check if we cannot detect changes to LoanedStateInterface's value and only publish then + timer_ = node_->create_wall_timer( + period_in_ms_, 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) +{ + auto evaluation_helper = evaluation_helper::Evaluation_Helper::get_instance(); + rclcpp::QoS qos_profile( + rclcpp::QoSInitialization::from_rmw(evaluation_helper->get_qos_profile())); + subscription_topic_name_ = topic_name; + command_subscription_ = node_->create_subscription( + subscription_topic_name_, qos_profile, + 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_DEBUG(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/evaluation_helper.cpp b/hardware_interface/src/hardware_interface/distributed_control_interface/evaluation_helper.cpp new file mode 100644 index 0000000000..35c7f213bd --- /dev/null +++ b/hardware_interface/src/hardware_interface/distributed_control_interface/evaluation_helper.cpp @@ -0,0 +1,5 @@ +#include "hardware_interface/distributed_control_interface/evaluation_helper.hpp" + +std::shared_ptr + evaluation_helper::Evaluation_Helper::qos_profile_instance_{nullptr}; +std::mutex evaluation_helper::Evaluation_Helper::mutex_; \ 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..bec3564d89 --- /dev/null +++ b/hardware_interface/src/hardware_interface/distributed_control_interface/state_publisher.cpp @@ -0,0 +1,114 @@ +#include "hardware_interface/distributed_control_interface/state_publisher.hpp" + +#include +#include +#include +#include +#include + +#include "rclcpp/rclcpp.hpp" + +using namespace std::chrono_literals; + +namespace distributed_control +{ + +StatePublisher::StatePublisher( + std::unique_ptr loaned_state_interface_ptr, + const std::string & ns, std::chrono::milliseconds period_in_ms, + std::shared_ptr node) +: loaned_state_interface_ptr_(std::move(loaned_state_interface_ptr)), + namespace_(ns), + period_in_ms_(period_in_ms), + node_(node), + topic_name_(loaned_state_interface_ptr_->get_underscore_separated_name() + "_state") +{ + // if we did not get a node passed, we create one ourselves + if (!node_.get()) + { + rclcpp::NodeOptions node_options; + node_options.clock_type(rcl_clock_type_t::RCL_STEADY_TIME); + node_ = std::make_shared( + loaned_state_interface_ptr_->get_underscore_separated_name() + "_state_publisher", namespace_, + node_options, false); + } + + auto evaluation_helper = evaluation_helper::Evaluation_Helper::get_instance(); + rclcpp::QoS qos_profile( + rclcpp::QoSInitialization::from_rmw(evaluation_helper->get_qos_profile())); + state_value_pub_ = node_->create_publisher(topic_name_, qos_profile); + // TODO(Manuel): We should check if we cannot detect changes to LoanedStateInterface's value and only publish then + timer_ = node_->create_wall_timer( + period_in_ms_, 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_DEBUG(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 33236afd1c..e2cad29e83 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 @@ -408,17 +410,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( @@ -432,6 +459,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 @@ -450,7 +504,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); } @@ -475,6 +529,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 @@ -539,6 +662,193 @@ class ResourceStorage import_command_interfaces(systems_.back()); } + void add_sub_controller_manager( + std::shared_ptr sub_controller_manager) + { + HardwareComponentInfo component_info; + component_info.name = sub_controller_manager->get_name(); + component_info.type = sub_controller_manager->type(); + component_info.state_interfaces = {}; + component_info.command_interfaces = {}; + const auto [it, success] = + hardware_info_map_.insert(std::pair{component_info.name, component_info}); + 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, + const std::string & ns, std::shared_ptr node) + { + 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, ns, node); + + 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, + const std::string & ns, std::shared_ptr node) + { + 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, ns, node); + 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; + } + + std::vector> import_distributed_reference_interfaces( + std::shared_ptr sub_controller_manager, + const std::string & ns, std::shared_ptr node) + { + 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, ns, node); + 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()); + auto command_interfaces_end = + hardware_info_map_[sub_controller_manager->get_name()].command_interfaces.end(); + hardware_info_map_[sub_controller_manager->get_name()].command_interfaces.insert( + command_interfaces_end, interface_names.begin(), interface_names.end()); + + for (const auto & interface : interface_names) + { + // 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_; @@ -557,9 +867,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_; @@ -567,6 +877,13 @@ 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_; }; ResourceManager::ResourceManager() : resource_storage_(std::make_unique()) {} @@ -593,6 +910,7 @@ ResourceManager::ResourceManager( // CM API: Called in "callback/slow"-thread void ResourceManager::load_urdf(const std::string & urdf, bool validate_interfaces) { + load_urdf_called_ = true; const std::string system_type = "system"; const std::string sensor_type = "sensor"; const std::string actuator_type = "actuator"; @@ -631,6 +949,8 @@ void ResourceManager::load_urdf(const std::string & urdf, bool validate_interfac resource_storage_->systems_.size()); } +bool ResourceManager::load_urdf_called() const { return load_urdf_called_; } + // CM API: Called in "update"-thread LoanedStateInterface ResourceManager::claim_state_interface(const std::string & key) { @@ -672,6 +992,76 @@ 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, + const std::string & ns, std::shared_ptr node) +{ + std::lock_guard guard(resource_interfaces_lock_); + return resource_storage_->import_distributed_state_interfaces(sub_controller_manager, ns, node); +} + +std::vector> +ResourceManager::import_command_interfaces_of_sub_controller_manager( + std::shared_ptr sub_controller_manager, + const std::string & ns, std::shared_ptr node) +{ + std::lock_guard guard(resource_interfaces_lock_); + return resource_storage_->import_distributed_command_interfaces(sub_controller_manager, ns, node); +} + +std::vector> +ResourceManager::import_reference_interfaces_of_sub_controller_manager( + std::shared_ptr sub_controller_manager, + const std::string & ns, std::shared_ptr node) +{ + std::lock_guard guard(resource_interfaces_lock_); + return resource_storage_->import_distributed_reference_interfaces( + sub_controller_manager, ns, node); +} + +void ResourceManager::add_hardware_state_publishers( + std::shared_ptr state_publisher) +{ + std::lock_guard guard(resource_interfaces_lock_); + resource_storage_->add_state_publisher(state_publisher); +} + +void ResourceManager::add_hardware_command_forwarders( + std::shared_ptr command_forwarder) +{ + std::lock_guard guard(resource_interfaces_lock_); + resource_storage_->add_command_forwarder(command_forwarder); +} + +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) +{ + std::lock_guard guard(resource_interfaces_lock_); + 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) diff --git a/hardware_interface/test/test_resource_manager.cpp b/hardware_interface/test/test_resource_manager.cpp index 59257f4edc..f7d1962316 100644 --- a/hardware_interface/test/test_resource_manager.cpp +++ b/hardware_interface/test/test_resource_manager.cpp @@ -212,6 +212,34 @@ TEST_F(ResourceManagerTest, initialization_with_urdf_unclaimed) } } +TEST_F(ResourceManagerTest, no_load_urdf_function_called) +{ + TestableResourceManager rm; + ASSERT_FALSE(rm.load_urdf_called()); +} + +TEST_F(ResourceManagerTest, load_urdf_called_if_urdf_is_invalid) +{ + TestableResourceManager rm; + EXPECT_THROW( + rm.load_urdf(ros2_control_test_assets::minimal_robot_missing_state_keys_urdf), std::exception); + ASSERT_TRUE(rm.load_urdf_called()); +} + +TEST_F(ResourceManagerTest, load_urdf_called_if_urdf_is_valid) +{ + TestableResourceManager rm(ros2_control_test_assets::minimal_robot_urdf); + ASSERT_TRUE(rm.load_urdf_called()); +} + +TEST_F(ResourceManagerTest, can_load_urdf_later) +{ + TestableResourceManager rm; + ASSERT_FALSE(rm.load_urdf_called()); + rm.load_urdf(ros2_control_test_assets::minimal_robot_urdf); + ASSERT_TRUE(rm.load_urdf_called()); +} + TEST_F(ResourceManagerTest, resource_claiming) { TestableResourceManager rm(ros2_control_test_assets::minimal_robot_urdf);