From 2dbe4822dcbe3d7f0003be5a0798b891366ec943 Mon Sep 17 00:00:00 2001 From: Manuel M Date: Thu, 10 Nov 2022 08:42:43 +0100 Subject: [PATCH 01/19] create topic for publishing StateInterface's value via loan --- controller_manager/CMakeLists.txt | 24 +++++++++++- .../controller_manager/controller_manager.hpp | 12 +++++- .../state_publisher.hpp | 30 +++++++++++++++ controller_manager/package.xml | 2 + controller_manager/src/controller_manager.cpp | 34 +++++++++++++++-- .../state_publisher.cpp | 38 +++++++++++++++++++ .../loaned_state_interface.hpp | 12 ++++++ 7 files changed, 145 insertions(+), 7 deletions(-) create mode 100644 controller_manager/include/controller_manager/controller_manager_components/state_publisher.hpp create mode 100644 controller_manager/src/controller_manager_components/state_publisher.cpp diff --git a/controller_manager/CMakeLists.txt b/controller_manager/CMakeLists.txt index 6905679ac3..163b0a52a8 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) @@ -24,15 +25,35 @@ foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) find_package(${Dependency} REQUIRED) endforeach() +# components + +add_library(state_publisher SHARED + src/controller_manager_components/state_publisher.cpp) + +target_include_directories( + state_publisher + PUBLIC + include) + +ament_target_dependencies(state_publisher ${THIS_PACKAGE_INCLUDE_DEPENDS}) + +target_compile_definitions(state_publisher + PRIVATE "COMPOSITION_BUILDING_DLL") + +rclcpp_components_register_nodes(state_publisher "controller_manager_components::StatePublisher") + +set(node_plugins "${node_plugins}controller_manager_components::StatePublisher;$\n") + add_library(${PROJECT_NAME} SHARED src/controller_manager.cpp ) + target_include_directories(${PROJECT_NAME} PRIVATE include) ament_target_dependencies(${PROJECT_NAME} ${THIS_PACKAGE_INCLUDE_DEPENDS}) # Causes the visibility macros to use dllexport rather than dllimport, # which is appropriate when building the dll but not consuming it. target_compile_definitions(${PROJECT_NAME} PRIVATE "CONTROLLER_MANAGER_BUILDING_DLL") - +target_link_libraries(${PROJECT_NAME} state_publisher) add_executable(ros2_control_node src/ros2_control_node.cpp) target_include_directories(ros2_control_node PRIVATE include) target_link_libraries(ros2_control_node @@ -41,6 +62,7 @@ target_link_libraries(ros2_control_node ament_target_dependencies(ros2_control_node ${THIS_PACKAGE_INCLUDE_DEPENDS}) install(TARGETS ${PROJECT_NAME} + state_publisher RUNTIME DESTINATION bin LIBRARY DESTINATION lib ARCHIVE DESTINATION lib diff --git a/controller_manager/include/controller_manager/controller_manager.hpp b/controller_manager/include/controller_manager/controller_manager.hpp index e6b377570a..38c2559ed3 100644 --- a/controller_manager/include/controller_manager/controller_manager.hpp +++ b/controller_manager/include/controller_manager/controller_manager.hpp @@ -67,13 +67,15 @@ class ControllerManager : public rclcpp::Node std::unique_ptr resource_manager, std::shared_ptr executor, const std::string & manager_node_name = "controller_manager", - const std::string & namespace_ = ""); + const std::string & namespace_ = "", + const bool & is_distributed = false); CONTROLLER_MANAGER_PUBLIC ControllerManager( std::shared_ptr executor, const std::string & manager_node_name = "controller_manager", - const std::string & namespace_ = ""); + const std::string & namespace_ = "", + const bool & is_distributed = false); CONTROLLER_MANAGER_PUBLIC virtual ~ControllerManager() = default; @@ -190,6 +192,9 @@ class ControllerManager : public rclcpp::Node CONTROLLER_MANAGER_PUBLIC void init_services(); + CONTROLLER_MANAGER_PUBLIC + void create_hardware_state_publisher(); + CONTROLLER_MANAGER_PUBLIC controller_interface::ControllerInterfaceBaseSharedPtr add_controller_impl( const ControllerSpec & controller); @@ -395,6 +400,9 @@ class ControllerManager : public rclcpp::Node */ rclcpp::CallbackGroup::SharedPtr best_effort_callback_group_; + rclcpp::CallbackGroup::SharedPtr components_callback_group_; + const bool is_distributed_; + /** * The RTControllerListWrapper class wraps a double-buffered list of controllers * to avoid needing to lock the real-time thread when switching controllers in diff --git a/controller_manager/include/controller_manager/controller_manager_components/state_publisher.hpp b/controller_manager/include/controller_manager/controller_manager_components/state_publisher.hpp new file mode 100644 index 0000000000..85911962b2 --- /dev/null +++ b/controller_manager/include/controller_manager/controller_manager_components/state_publisher.hpp @@ -0,0 +1,30 @@ +#ifndef CONTROLLER_MAMAGER_COMPONENTS__STATE_PUBLISHER +#define CONTROLLER_MAMAGER_COMPONENTS__STATE_PUBLISHER + +#include + +#include "hardware_interface/loaned_state_interface.hpp" +#include "rclcpp/rclcpp.hpp" +#include "std_msgs/msg/float64.hpp" + +namespace controller_manager_components +{ + +class StatePublisher final : public rclcpp::Node +{ +public: + explicit StatePublisher(const rclcpp::NodeOptions & options, std::unique_ptr loaned_state_interface_ptr = nullptr); + + ~StatePublisher(){} + +private: + void on_timer(); + + std::unique_ptr loaned_state_interface_ptr_; + rclcpp::Publisher::SharedPtr state_value_pub_; + rclcpp::TimerBase::SharedPtr timer_; +}; + +} // namespace controller_manager_components + +#endif // CONTROLLER_MAMAGER_COMPONENTS__STATE_PUBLISHER \ No newline at end of file diff --git a/controller_manager/package.xml b/controller_manager/package.xml index 401e99ebe0..d09b5839d9 100644 --- a/controller_manager/package.xml +++ b/controller_manager/package.xml @@ -21,11 +21,13 @@ launch_ros pluginlib rclcpp + rclcpp_components rcpputils realtime_tools ros2_control_test_assets ros2param ros2run + std_msgs ament_cmake_gmock diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 8ce3be22a6..36e0119202 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -22,6 +22,7 @@ #include "controller_interface/controller_interface_base.hpp" #include "controller_manager_msgs/msg/hardware_component_state.hpp" +#include "controller_manager/controller_manager_components/state_publisher.hpp" #include "hardware_interface/types/lifecycle_state_names.hpp" #include "lifecycle_msgs/msg/state.hpp" #include "rclcpp/rclcpp.hpp" @@ -133,7 +134,7 @@ rclcpp::NodeOptions get_cm_node_options() ControllerManager::ControllerManager( std::shared_ptr executor, const std::string & manager_node_name, - const std::string & namespace_) + const std::string & namespace_, const bool & is_distributed) : rclcpp::Node(manager_node_name, namespace_, get_cm_node_options()), resource_manager_(std::make_unique()), diagnostics_updater_(this), @@ -142,7 +143,8 @@ ControllerManager::ControllerManager( kControllerInterfaceNamespace, kControllerInterfaceClassName)), chainable_loader_( std::make_shared>( - kControllerInterfaceNamespace, kChainableControllerInterfaceClassName)) + kControllerInterfaceNamespace, kChainableControllerInterfaceClassName)), + is_distributed_(is_distributed_) { if (!get_parameter("update_rate", update_rate_)) { @@ -162,12 +164,13 @@ ControllerManager::ControllerManager( diagnostics_updater_.add( "Controllers Activity", this, &ControllerManager::controller_activity_diagnostic_callback); init_services(); + create_hardware_state_publisher(); } ControllerManager::ControllerManager( std::unique_ptr resource_manager, std::shared_ptr executor, const std::string & manager_node_name, - const std::string & namespace_) + const std::string & namespace_, const bool & is_distributed) : rclcpp::Node(manager_node_name, namespace_, get_cm_node_options()), resource_manager_(std::move(resource_manager)), diagnostics_updater_(this), @@ -176,12 +179,14 @@ ControllerManager::ControllerManager( kControllerInterfaceNamespace, kControllerInterfaceClassName)), chainable_loader_( std::make_shared>( - kControllerInterfaceNamespace, kChainableControllerInterfaceClassName)) + kControllerInterfaceNamespace, kChainableControllerInterfaceClassName)), + is_distributed_(is_distributed) { diagnostics_updater_.setHardwareID("ros2_control"); diagnostics_updater_.add( "Controllers Activity", this, &ControllerManager::controller_activity_diagnostic_callback); init_services(); + create_hardware_state_publisher(); } void ControllerManager::init_resource_manager(const std::string & robot_description) @@ -271,6 +276,27 @@ void ControllerManager::init_services() qos_services, best_effort_callback_group_); } +void ControllerManager::create_hardware_state_publisher() +{ + + auto state_interface_names = resource_manager_->available_state_interfaces(); + + for (const auto & state_interface : state_interface_names) + { + try + { + rclcpp::NodeOptions node_options; + auto state_publisher_node = std::make_shared(node_options, std::make_unique(resource_manager_->claim_state_interface(state_interface))); + executor_->add_node(state_publisher_node); + } + catch (const std::exception & e) + { + RCLCPP_ERROR(get_logger(), "Can't create StatePublisher<'%s'> : %s", state_interface.c_str(), e.what()); + break; + } + } +} + controller_interface::ControllerInterfaceBaseSharedPtr ControllerManager::load_controller( const std::string & controller_name, const std::string & controller_type) { diff --git a/controller_manager/src/controller_manager_components/state_publisher.cpp b/controller_manager/src/controller_manager_components/state_publisher.cpp new file mode 100644 index 0000000000..96e7748a98 --- /dev/null +++ b/controller_manager/src/controller_manager_components/state_publisher.cpp @@ -0,0 +1,38 @@ +#include "controller_manager/controller_manager_components/state_publisher.hpp" + +#include + +using namespace std::chrono_literals; + +namespace controller_manager_components +{ + +StatePublisher::StatePublisher(const rclcpp::NodeOptions & options, std::unique_ptr loaned_state_interface_ptr) +: Node(loaned_state_interface_ptr->get_underscore_separated_name() + "_node", options), loaned_state_interface_ptr_(std::move(loaned_state_interface_ptr)) +{ + assert(loaned_state_interface_ptr_); + RCLCPP_INFO(this->get_logger(), "Creating StatePublisher<%s>.", loaned_state_interface_ptr->get_underscore_separated_name().c_str()); + state_value_pub_ = create_publisher(loaned_state_interface_ptr->get_underscore_separated_name(), 10); + timer_ = create_wall_timer(1s, std::bind(&StatePublisher::on_timer, this)); +} + +void StatePublisher::on_timer() +{ + auto msg = std::make_unique(); + msg->data = loaned_state_interface_ptr_->get_value(); + RCLCPP_INFO(this->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 controller_manager_components + +#include "rclcpp_components/register_node_macro.hpp" + +// Register the component with class_loader. +// This acts as a sort of entry point, allowing the component to be discoverable when its library +// is being loaded into a running process. +RCLCPP_COMPONENTS_REGISTER_NODE(controller_manager_components::StatePublisher) \ No newline at end of file diff --git a/hardware_interface/include/hardware_interface/loaned_state_interface.hpp b/hardware_interface/include/hardware_interface/loaned_state_interface.hpp index 4fe67d1290..f08763a0e4 100644 --- a/hardware_interface/include/hardware_interface/loaned_state_interface.hpp +++ b/hardware_interface/include/hardware_interface/loaned_state_interface.hpp @@ -61,6 +61,18 @@ class LoanedStateInterface return state_interface_.get_name(); } + const std::string get_underscore_separated_name() const + { + std::string prefix = get_prefix_name(); + std::string interface = get_interface_name(); + + if(prefix.empty()) + { + return interface; + } + return prefix + "_" + interface; + } + const std::string & get_prefix_name() const { return state_interface_.get_prefix_name(); } double get_value() const { return state_interface_.get_value(); } From 9f0d1b0c67c9d37954f903da3dbc3569e6e19fee Mon Sep 17 00:00:00 2001 From: Manuel M Date: Fri, 11 Nov 2022 09:27:41 +0100 Subject: [PATCH 02/19] working now * fix typo in StatePublisher (used the pointer which has been moved...) * not a component anymore * collect StatePublishers in map --- controller_manager/CMakeLists.txt | 23 +--------- .../controller_manager/controller_manager.hpp | 2 + .../state_publisher.hpp | 9 +++- controller_manager/package.xml | 2 +- controller_manager/src/controller_manager.cpp | 10 +++-- .../state_publisher.cpp | 45 ++++++++++++------- 6 files changed, 47 insertions(+), 44 deletions(-) diff --git a/controller_manager/CMakeLists.txt b/controller_manager/CMakeLists.txt index 163b0a52a8..468d27fb90 100644 --- a/controller_manager/CMakeLists.txt +++ b/controller_manager/CMakeLists.txt @@ -13,6 +13,7 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS hardware_interface pluginlib rclcpp + rclcpp_lifecycle realtime_tools std_msgs ) @@ -25,26 +26,8 @@ foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) find_package(${Dependency} REQUIRED) endforeach() -# components - -add_library(state_publisher SHARED - src/controller_manager_components/state_publisher.cpp) - -target_include_directories( - state_publisher - PUBLIC - include) - -ament_target_dependencies(state_publisher ${THIS_PACKAGE_INCLUDE_DEPENDS}) - -target_compile_definitions(state_publisher - PRIVATE "COMPOSITION_BUILDING_DLL") - -rclcpp_components_register_nodes(state_publisher "controller_manager_components::StatePublisher") - -set(node_plugins "${node_plugins}controller_manager_components::StatePublisher;$\n") - add_library(${PROJECT_NAME} SHARED + src/controller_manager_components/state_publisher.cpp src/controller_manager.cpp ) @@ -53,7 +36,6 @@ ament_target_dependencies(${PROJECT_NAME} ${THIS_PACKAGE_INCLUDE_DEPENDS}) # Causes the visibility macros to use dllexport rather than dllimport, # which is appropriate when building the dll but not consuming it. target_compile_definitions(${PROJECT_NAME} PRIVATE "CONTROLLER_MANAGER_BUILDING_DLL") -target_link_libraries(${PROJECT_NAME} state_publisher) add_executable(ros2_control_node src/ros2_control_node.cpp) target_include_directories(ros2_control_node PRIVATE include) target_link_libraries(ros2_control_node @@ -62,7 +44,6 @@ target_link_libraries(ros2_control_node ament_target_dependencies(ros2_control_node ${THIS_PACKAGE_INCLUDE_DEPENDS}) install(TARGETS ${PROJECT_NAME} - state_publisher RUNTIME DESTINATION bin LIBRARY DESTINATION lib ARCHIVE DESTINATION lib diff --git a/controller_manager/include/controller_manager/controller_manager.hpp b/controller_manager/include/controller_manager/controller_manager.hpp index 38c2559ed3..78fb10f795 100644 --- a/controller_manager/include/controller_manager/controller_manager.hpp +++ b/controller_manager/include/controller_manager/controller_manager.hpp @@ -27,6 +27,7 @@ #include "controller_interface/controller_interface.hpp" #include "controller_interface/controller_interface_base.hpp" +#include "controller_manager/controller_manager_components/state_publisher.hpp" #include "controller_manager/controller_spec.hpp" #include "controller_manager/visibility_control.h" #include "controller_manager_msgs/srv/configure_controller.hpp" @@ -402,6 +403,7 @@ class ControllerManager : public rclcpp::Node rclcpp::CallbackGroup::SharedPtr components_callback_group_; const bool is_distributed_; + std::map> state_interface_state_publisher_map_; /** * The RTControllerListWrapper class wraps a double-buffered list of controllers diff --git a/controller_manager/include/controller_manager/controller_manager_components/state_publisher.hpp b/controller_manager/include/controller_manager/controller_manager_components/state_publisher.hpp index 85911962b2..76b2941266 100644 --- a/controller_manager/include/controller_manager/controller_manager_components/state_publisher.hpp +++ b/controller_manager/include/controller_manager/controller_manager_components/state_publisher.hpp @@ -5,21 +5,26 @@ #include "hardware_interface/loaned_state_interface.hpp" #include "rclcpp/rclcpp.hpp" +#include "rclcpp_lifecycle/lifecycle_node.hpp" #include "std_msgs/msg/float64.hpp" namespace controller_manager_components { -class StatePublisher final : public rclcpp::Node +class StatePublisher final { public: - explicit StatePublisher(const rclcpp::NodeOptions & options, std::unique_ptr loaned_state_interface_ptr = nullptr); + explicit StatePublisher(const std::string & ns = "" , std::unique_ptr loaned_state_interface_ptr = nullptr); ~StatePublisher(){} + std::shared_ptr get_node(); + private: void on_timer(); + const std::string & namespace_; + std::shared_ptr node_; std::unique_ptr loaned_state_interface_ptr_; rclcpp::Publisher::SharedPtr state_value_pub_; rclcpp::TimerBase::SharedPtr timer_; diff --git a/controller_manager/package.xml b/controller_manager/package.xml index d09b5839d9..cee9d5b8c2 100644 --- a/controller_manager/package.xml +++ b/controller_manager/package.xml @@ -21,7 +21,7 @@ launch_ros pluginlib rclcpp - rclcpp_components + rclcpp_lifecycle rcpputils realtime_tools ros2_control_test_assets diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 36e0119202..81b194f188 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -22,7 +22,6 @@ #include "controller_interface/controller_interface_base.hpp" #include "controller_manager_msgs/msg/hardware_component_state.hpp" -#include "controller_manager/controller_manager_components/state_publisher.hpp" #include "hardware_interface/types/lifecycle_state_names.hpp" #include "lifecycle_msgs/msg/state.hpp" #include "rclcpp/rclcpp.hpp" @@ -123,6 +122,7 @@ bool command_interface_is_reference_interface_of_controller( namespace controller_manager { + rclcpp::NodeOptions get_cm_node_options() { rclcpp::NodeOptions node_options; @@ -285,9 +285,11 @@ void ControllerManager::create_hardware_state_publisher() { try { - rclcpp::NodeOptions node_options; - auto state_publisher_node = std::make_shared(node_options, std::make_unique(resource_manager_->claim_state_interface(state_interface))); - executor_->add_node(state_publisher_node); + + RCLCPP_INFO(this->get_logger() , "Creating StatePublisher for interface:<%s>.", state_interface.c_str()); + auto state_publisher = std::make_shared(get_namespace() , std::move(std::make_unique(resource_manager_->claim_state_interface(state_interface)))); + state_interface_state_publisher_map_.insert(std::pair{state_interface, state_publisher}); + executor_->add_node(state_publisher->get_node()->get_node_base_interface()); } catch (const std::exception & e) { diff --git a/controller_manager/src/controller_manager_components/state_publisher.cpp b/controller_manager/src/controller_manager_components/state_publisher.cpp index 96e7748a98..d387157321 100644 --- a/controller_manager/src/controller_manager_components/state_publisher.cpp +++ b/controller_manager/src/controller_manager_components/state_publisher.cpp @@ -1,26 +1,46 @@ #include "controller_manager/controller_manager_components/state_publisher.hpp" #include +#include +#include using namespace std::chrono_literals; namespace controller_manager_components { -StatePublisher::StatePublisher(const rclcpp::NodeOptions & options, std::unique_ptr loaned_state_interface_ptr) -: Node(loaned_state_interface_ptr->get_underscore_separated_name() + "_node", options), loaned_state_interface_ptr_(std::move(loaned_state_interface_ptr)) +StatePublisher::StatePublisher(const std::string & ns, std::unique_ptr loaned_state_interface_ptr) +: namespace_(ns), loaned_state_interface_ptr_(std::move(loaned_state_interface_ptr)) { - assert(loaned_state_interface_ptr_); - RCLCPP_INFO(this->get_logger(), "Creating StatePublisher<%s>.", loaned_state_interface_ptr->get_underscore_separated_name().c_str()); - state_value_pub_ = create_publisher(loaned_state_interface_ptr->get_underscore_separated_name(), 10); - timer_ = create_wall_timer(1s, std::bind(&StatePublisher::on_timer, this)); + rclcpp::NodeOptions node_options; + node_ = std::make_shared(loaned_state_interface_ptr_->get_underscore_separated_name() + "_node", namespace_, node_options, false); + RCLCPP_INFO(node_->get_logger(), "Creating StatePublisher<%s>.", loaned_state_interface_ptr_->get_underscore_separated_name().c_str()); + state_value_pub_ = node_->create_publisher(loaned_state_interface_ptr_->get_underscore_separated_name(), 10); + timer_ = node_->create_wall_timer(1s, std::bind(&StatePublisher::on_timer, this)); +} + +std::shared_ptr StatePublisher::get_node() +{ + if (!node_.get()) + { + throw std::runtime_error("Lifecycle node hasn't been initialized yet!"); + } + return node_; } void StatePublisher::on_timer() { + // Todo(Manuel) create custom msg and return success or failure not just nan. auto msg = std::make_unique(); - msg->data = loaned_state_interface_ptr_->get_value(); - RCLCPP_INFO(this->get_logger(), "Publishing: '%.7lf'", msg->data); + try + { + msg->data = loaned_state_interface_ptr_->get_value(); + } + catch ( const std::runtime_error & e) + { + msg->data = std::numeric_limits::quiet_NaN(); + } + RCLCPP_INFO(node_->get_logger(), "Publishing: '%.7lf'", msg->data); std::flush(std::cout); // Put the message into a queue to be processed by the middleware. @@ -28,11 +48,4 @@ void StatePublisher::on_timer() state_value_pub_->publish(std::move(msg)); } -} // namespace controller_manager_components - -#include "rclcpp_components/register_node_macro.hpp" - -// Register the component with class_loader. -// This acts as a sort of entry point, allowing the component to be discoverable when its library -// is being loaded into a running process. -RCLCPP_COMPONENTS_REGISTER_NODE(controller_manager_components::StatePublisher) \ No newline at end of file +} // namespace controller_manager_components \ No newline at end of file From 3d1e985058b773eb8cf95b51b736eb7ff801b232 Mon Sep 17 00:00:00 2001 From: Manuel M Date: Fri, 11 Nov 2022 10:15:53 +0100 Subject: [PATCH 03/19] add visibility control, rename to distributed since no component anymore --- controller_manager/CMakeLists.txt | 2 +- .../controller_manager/controller_manager.hpp | 4 +- .../state_publisher.hpp | 17 ++++--- .../distributed_control/visibility_control.h | 49 +++++++++++++++++++ controller_manager/src/controller_manager.cpp | 6 +-- .../state_publisher.cpp | 6 +-- 6 files changed, 68 insertions(+), 16 deletions(-) rename controller_manager/include/controller_manager/{controller_manager_components => distributed_control}/state_publisher.hpp (68%) create mode 100644 controller_manager/include/controller_manager/distributed_control/visibility_control.h rename controller_manager/src/{controller_manager_components => distributed_control}/state_publisher.cpp (91%) diff --git a/controller_manager/CMakeLists.txt b/controller_manager/CMakeLists.txt index 468d27fb90..55ba772d9b 100644 --- a/controller_manager/CMakeLists.txt +++ b/controller_manager/CMakeLists.txt @@ -27,7 +27,7 @@ foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) endforeach() add_library(${PROJECT_NAME} SHARED - src/controller_manager_components/state_publisher.cpp + src/distributed_control/state_publisher.cpp src/controller_manager.cpp ) diff --git a/controller_manager/include/controller_manager/controller_manager.hpp b/controller_manager/include/controller_manager/controller_manager.hpp index 78fb10f795..600f3f0eb4 100644 --- a/controller_manager/include/controller_manager/controller_manager.hpp +++ b/controller_manager/include/controller_manager/controller_manager.hpp @@ -27,7 +27,7 @@ #include "controller_interface/controller_interface.hpp" #include "controller_interface/controller_interface_base.hpp" -#include "controller_manager/controller_manager_components/state_publisher.hpp" +#include "controller_manager/distributed_control/state_publisher.hpp" #include "controller_manager/controller_spec.hpp" #include "controller_manager/visibility_control.h" #include "controller_manager_msgs/srv/configure_controller.hpp" @@ -403,7 +403,7 @@ class ControllerManager : public rclcpp::Node rclcpp::CallbackGroup::SharedPtr components_callback_group_; const bool is_distributed_; - std::map> state_interface_state_publisher_map_; + std::map> state_interface_state_publisher_map_; /** * The RTControllerListWrapper class wraps a double-buffered list of controllers diff --git a/controller_manager/include/controller_manager/controller_manager_components/state_publisher.hpp b/controller_manager/include/controller_manager/distributed_control/state_publisher.hpp similarity index 68% rename from controller_manager/include/controller_manager/controller_manager_components/state_publisher.hpp rename to controller_manager/include/controller_manager/distributed_control/state_publisher.hpp index 76b2941266..bcd646924b 100644 --- a/controller_manager/include/controller_manager/controller_manager_components/state_publisher.hpp +++ b/controller_manager/include/controller_manager/distributed_control/state_publisher.hpp @@ -1,23 +1,26 @@ -#ifndef CONTROLLER_MAMAGER_COMPONENTS__STATE_PUBLISHER -#define CONTROLLER_MAMAGER_COMPONENTS__STATE_PUBLISHER +#ifndef DISTRIBUTED_CONTROL__STATE_PUBLISHER +#define DISTRIBUTED_CONTROL__STATE_PUBLISHER #include #include "hardware_interface/loaned_state_interface.hpp" +#include "visibility_control.h" + #include "rclcpp/rclcpp.hpp" #include "rclcpp_lifecycle/lifecycle_node.hpp" #include "std_msgs/msg/float64.hpp" -namespace controller_manager_components +namespace distributed_control { -class StatePublisher final +class StatePublisher final : public rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface { public: explicit StatePublisher(const std::string & ns = "" , std::unique_ptr loaned_state_interface_ptr = nullptr); ~StatePublisher(){} - + + STATE_PUBLISHER_PUBLIC std::shared_ptr get_node(); private: @@ -30,6 +33,6 @@ class StatePublisher final rclcpp::TimerBase::SharedPtr timer_; }; -} // namespace controller_manager_components +} // namespace distributed_control -#endif // CONTROLLER_MAMAGER_COMPONENTS__STATE_PUBLISHER \ No newline at end of file +#endif // DISTRIBUTED_CONTROL__STATE_PUBLISHER \ No newline at end of file diff --git a/controller_manager/include/controller_manager/distributed_control/visibility_control.h b/controller_manager/include/controller_manager/distributed_control/visibility_control.h new file mode 100644 index 0000000000..86d509e6b2 --- /dev/null +++ b/controller_manager/include/controller_manager/distributed_control/visibility_control.h @@ -0,0 +1,49 @@ +// 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. + +#ifndef STATE_PUBLISHER__VISIBILITY_CONTROL_H_ +#define STATE_PUBLISHER__VISIBILITY_CONTROL_H_ + +// This logic was borrowed (then namespaced) from the examples on the gcc wiki: +// https://gcc.gnu.org/wiki/Visibility + +#if defined _WIN32 || defined __CYGWIN__ +#ifdef __GNUC__ +#define STATE_PUBLISHER_EXPORT __attribute__((dllexport)) +#define STATE_PUBLISHER_IMPORT __attribute__((dllimport)) +#else +#define STATE_PUBLISHER_EXPORT __declspec(dllexport) +#define STATE_PUBLISHER_IMPORT __declspec(dllimport) +#endif +#ifdef STATE_PUBLISHER_BUILDING_DLL +#define STATE_PUBLISHER_PUBLIC STATE_PUBLISHER_EXPORT +#else +#define STATE_PUBLISHER_PUBLIC STATE_PUBLISHER_IMPORT +#endif +#define STATE_PUBLISHER_PUBLIC_TYPE STATE_PUBLISHER_PUBLIC +#define STATE_PUBLISHER_LOCAL +#else +#define STATE_PUBLISHER_EXPORT __attribute__((visibility("default"))) +#define STATE_PUBLISHER_IMPORT +#if __GNUC__ >= 4 +#define STATE_PUBLISHER_PUBLIC __attribute__((visibility("default"))) +#define STATE_PUBLISHER_LOCAL __attribute__((visibility("hidden"))) +#else +#define STATE_PUBLISHER_PUBLIC +#define STATE_PUBLISHER_LOCAL +#endif +#define STATE_PUBLISHER_PUBLIC_TYPE +#endif + +#endif // STATE_PUBLISHER__VISIBILITY_CONTROL_H_ diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 81b194f188..031ea6a7d2 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -286,14 +286,14 @@ void ControllerManager::create_hardware_state_publisher() try { - RCLCPP_INFO(this->get_logger() , "Creating StatePublisher for interface:<%s>.", state_interface.c_str()); - auto state_publisher = std::make_shared(get_namespace() , std::move(std::make_unique(resource_manager_->claim_state_interface(state_interface)))); + RCLCPP_INFO(get_logger() , "Creating StatePublisher for interface:<%s>.", state_interface.c_str()); + auto state_publisher = std::make_shared(get_namespace() , std::move(std::make_unique(resource_manager_->claim_state_interface(state_interface)))); state_interface_state_publisher_map_.insert(std::pair{state_interface, state_publisher}); executor_->add_node(state_publisher->get_node()->get_node_base_interface()); } catch (const std::exception & e) { - RCLCPP_ERROR(get_logger(), "Can't create StatePublisher<'%s'> : %s", state_interface.c_str(), e.what()); + RCLCPP_ERROR(get_logger(), "Can't create StatePublisher<%s> : %s", state_interface.c_str(), e.what()); break; } } diff --git a/controller_manager/src/controller_manager_components/state_publisher.cpp b/controller_manager/src/distributed_control/state_publisher.cpp similarity index 91% rename from controller_manager/src/controller_manager_components/state_publisher.cpp rename to controller_manager/src/distributed_control/state_publisher.cpp index d387157321..4c4fa66e48 100644 --- a/controller_manager/src/controller_manager_components/state_publisher.cpp +++ b/controller_manager/src/distributed_control/state_publisher.cpp @@ -1,4 +1,4 @@ -#include "controller_manager/controller_manager_components/state_publisher.hpp" +#include "controller_manager/distributed_control/state_publisher.hpp" #include #include @@ -6,7 +6,7 @@ using namespace std::chrono_literals; -namespace controller_manager_components +namespace distributed_control { StatePublisher::StatePublisher(const std::string & ns, std::unique_ptr loaned_state_interface_ptr) @@ -48,4 +48,4 @@ void StatePublisher::on_timer() state_value_pub_->publish(std::move(msg)); } -} // namespace controller_manager_components \ No newline at end of file +} // namespace distributed_control \ No newline at end of file From c54ff02d85a872ec6171b4e2e54c67680d4a33c7 Mon Sep 17 00:00:00 2001 From: Manuel M Date: Mon, 14 Nov 2022 08:23:00 +0100 Subject: [PATCH 04/19] rename include guard --- .../distributed_control/state_publisher.hpp | 14 ++++++++------ .../src/distributed_control/state_publisher.cpp | 12 ++++++------ 2 files changed, 14 insertions(+), 12 deletions(-) diff --git a/controller_manager/include/controller_manager/distributed_control/state_publisher.hpp b/controller_manager/include/controller_manager/distributed_control/state_publisher.hpp index bcd646924b..7cde5d9f27 100644 --- a/controller_manager/include/controller_manager/distributed_control/state_publisher.hpp +++ b/controller_manager/include/controller_manager/distributed_control/state_publisher.hpp @@ -1,13 +1,15 @@ -#ifndef DISTRIBUTED_CONTROL__STATE_PUBLISHER -#define DISTRIBUTED_CONTROL__STATE_PUBLISHER +#ifndef DISTRIBUTED_CONTROL__STATE_PUBLISHER_HPP_ +#define DISTRIBUTED_CONTROL__STATE_PUBLISHER_HPP_ #include -#include "hardware_interface/loaned_state_interface.hpp" #include "visibility_control.h" +#include "hardware_interface/loaned_state_interface.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 @@ -22,17 +24,17 @@ class StatePublisher final : public rclcpp_lifecycle::node_interfaces::Lifecycle STATE_PUBLISHER_PUBLIC std::shared_ptr get_node(); - + private: void on_timer(); const std::string & namespace_; - std::shared_ptr node_; std::unique_ptr loaned_state_interface_ptr_; + std::shared_ptr node_; rclcpp::Publisher::SharedPtr state_value_pub_; rclcpp::TimerBase::SharedPtr timer_; }; } // namespace distributed_control -#endif // DISTRIBUTED_CONTROL__STATE_PUBLISHER \ No newline at end of file +#endif // DISTRIBUTED_CONTROL__STATE_PUBLISHER_HPP_ \ No newline at end of file diff --git a/controller_manager/src/distributed_control/state_publisher.cpp b/controller_manager/src/distributed_control/state_publisher.cpp index 4c4fa66e48..8f0eb98855 100644 --- a/controller_manager/src/distributed_control/state_publisher.cpp +++ b/controller_manager/src/distributed_control/state_publisher.cpp @@ -12,18 +12,18 @@ namespace distributed_control StatePublisher::StatePublisher(const std::string & ns, std::unique_ptr loaned_state_interface_ptr) : namespace_(ns), loaned_state_interface_ptr_(std::move(loaned_state_interface_ptr)) { - rclcpp::NodeOptions node_options; - node_ = std::make_shared(loaned_state_interface_ptr_->get_underscore_separated_name() + "_node", namespace_, node_options, false); - RCLCPP_INFO(node_->get_logger(), "Creating StatePublisher<%s>.", loaned_state_interface_ptr_->get_underscore_separated_name().c_str()); - state_value_pub_ = node_->create_publisher(loaned_state_interface_ptr_->get_underscore_separated_name(), 10); - timer_ = node_->create_wall_timer(1s, std::bind(&StatePublisher::on_timer, this)); + rclcpp::NodeOptions node_options; + node_ = std::make_shared(loaned_state_interface_ptr_->get_underscore_separated_name() + "_node", namespace_, node_options, false); + state_value_pub_ = node_->create_publisher(loaned_state_interface_ptr_->get_underscore_separated_name(), 10); + timer_ = node_->create_wall_timer(1ms, std::bind(&StatePublisher::on_timer, this)); + RCLCPP_INFO(node_->get_logger(), "Creating StatePublisher<%s>.", loaned_state_interface_ptr_->get_underscore_separated_name().c_str()); } std::shared_ptr StatePublisher::get_node() { if (!node_.get()) { - throw std::runtime_error("Lifecycle node hasn't been initialized yet!"); + throw std::runtime_error("Lifecycle node hasn't been configured yet!"); } return node_; } From 6feda989e8cd0269d89bb9a0b67ee1dd66b2ba15 Mon Sep 17 00:00:00 2001 From: Manuel M Date: Thu, 17 Nov 2022 16:22:44 +0100 Subject: [PATCH 05/19] crude registration of StatePublishers at central controller manager --- .../controller_manager/controller_manager.hpp | 41 +++- .../distributed_control/state_publisher.hpp | 41 ++-- .../distributed_control/state_subscriber.hpp | 0 .../sub_controller_manager_wrapper.hpp | 50 +++++ controller_manager/src/controller_manager.cpp | 175 +++++++++++++++--- .../distributed_control/state_publisher.cpp | 37 ++-- .../distributed_control/state_subscriber.cpp | 0 controller_manager_msgs/CMakeLists.txt | 1 + .../srv/RegisterSubControllerManager.srv | 22 +++ 9 files changed, 307 insertions(+), 60 deletions(-) create mode 100644 controller_manager/include/controller_manager/distributed_control/state_subscriber.hpp create mode 100644 controller_manager/include/controller_manager/distributed_control/sub_controller_manager_wrapper.hpp create mode 100644 controller_manager/src/distributed_control/state_subscriber.cpp create mode 100644 controller_manager_msgs/srv/RegisterSubControllerManager.srv diff --git a/controller_manager/include/controller_manager/controller_manager.hpp b/controller_manager/include/controller_manager/controller_manager.hpp index 600f3f0eb4..074afb73e4 100644 --- a/controller_manager/include/controller_manager/controller_manager.hpp +++ b/controller_manager/include/controller_manager/controller_manager.hpp @@ -26,9 +26,10 @@ #include "controller_interface/chainable_controller_interface.hpp" #include "controller_interface/controller_interface.hpp" #include "controller_interface/controller_interface_base.hpp" +#include "distributed_control/sub_controller_manager_wrapper.hpp" -#include "controller_manager/distributed_control/state_publisher.hpp" #include "controller_manager/controller_spec.hpp" +#include "controller_manager/distributed_control/state_publisher.hpp" #include "controller_manager/visibility_control.h" #include "controller_manager_msgs/srv/configure_controller.hpp" #include "controller_manager_msgs/srv/list_controller_types.hpp" @@ -36,6 +37,7 @@ #include "controller_manager_msgs/srv/list_hardware_components.hpp" #include "controller_manager_msgs/srv/list_hardware_interfaces.hpp" #include "controller_manager_msgs/srv/load_controller.hpp" +#include "controller_manager_msgs/srv/register_sub_controller_manager.hpp" #include "controller_manager_msgs/srv/reload_controller_libraries.hpp" #include "controller_manager_msgs/srv/set_hardware_component_state.hpp" #include "controller_manager_msgs/srv/switch_controller.hpp" @@ -68,15 +70,13 @@ class ControllerManager : public rclcpp::Node std::unique_ptr resource_manager, std::shared_ptr executor, const std::string & manager_node_name = "controller_manager", - const std::string & namespace_ = "", - const bool & is_distributed = false); + const std::string & namespace_ = ""); CONTROLLER_MANAGER_PUBLIC ControllerManager( std::shared_ptr executor, const std::string & manager_node_name = "controller_manager", - const std::string & namespace_ = "", - const bool & is_distributed = false); + const std::string & namespace_ = ""); CONTROLLER_MANAGER_PUBLIC virtual ~ControllerManager() = default; @@ -193,9 +193,24 @@ class ControllerManager : public rclcpp::Node CONTROLLER_MANAGER_PUBLIC void init_services(); + CONTROLLER_MANAGER_PUBLIC + void configure_controller_manager(); + + CONTROLLER_MANAGER_PUBLIC + void init_distributed_main_controller_services(); + + CONTROLLER_MANAGER_PUBLIC + void register_sub_controller_manager_srv_cb( + const std::shared_ptr + request, + std::shared_ptr response); + CONTROLLER_MANAGER_PUBLIC void create_hardware_state_publisher(); + CONTROLLER_MANAGER_PUBLIC + void register_sub_controller_manager(); + CONTROLLER_MANAGER_PUBLIC controller_interface::ControllerInterfaceBaseSharedPtr add_controller_impl( const ControllerSpec & controller); @@ -401,10 +416,13 @@ class ControllerManager : public rclcpp::Node */ rclcpp::CallbackGroup::SharedPtr best_effort_callback_group_; - rclcpp::CallbackGroup::SharedPtr components_callback_group_; - const bool is_distributed_; - std::map> state_interface_state_publisher_map_; - + bool distributed_ = false; + bool sub_controller_manager_ = false; + std::map> + state_interface_state_publisher_map_; + std::map> + sub_controller_manager_map_; + 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 @@ -502,6 +520,11 @@ class ControllerManager : public rclcpp::Node rclcpp::Service::SharedPtr set_hardware_component_state_service_; + // services for distributed control + std::mutex central_controller_manager_srv_lock_; + rclcpp::Service::SharedPtr + register_sub_controller_manager_srv_; + std::vector activate_request_, deactivate_request_; std::vector to_chained_mode_request_, from_chained_mode_request_; std::vector activate_command_interface_request_, diff --git a/controller_manager/include/controller_manager/distributed_control/state_publisher.hpp b/controller_manager/include/controller_manager/distributed_control/state_publisher.hpp index 7cde5d9f27..f6318e3cf9 100644 --- a/controller_manager/include/controller_manager/distributed_control/state_publisher.hpp +++ b/controller_manager/include/controller_manager/distributed_control/state_publisher.hpp @@ -2,6 +2,8 @@ #define DISTRIBUTED_CONTROL__STATE_PUBLISHER_HPP_ #include +#include +#include #include "visibility_control.h" @@ -18,23 +20,32 @@ namespace distributed_control class StatePublisher final : public rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface { public: - explicit StatePublisher(const std::string & ns = "" , std::unique_ptr loaned_state_interface_ptr = nullptr); - - ~StatePublisher(){} - - STATE_PUBLISHER_PUBLIC - std::shared_ptr get_node(); + explicit StatePublisher( + const std::string & ns = "", + std::unique_ptr loaned_state_interface_ptr = nullptr); -private: - void on_timer(); + ~StatePublisher() {} + + STATE_PUBLISHER_PUBLIC + std::shared_ptr get_node() const; + + STATE_PUBLISHER_PUBLIC + std::string get_topic_name() const; - const std::string & namespace_; - std::unique_ptr loaned_state_interface_ptr_; - std::shared_ptr node_; - rclcpp::Publisher::SharedPtr state_value_pub_; - rclcpp::TimerBase::SharedPtr timer_; + STATE_PUBLISHER_PUBLIC + std::string get_state_interface_name() const; + +private: + void publish_value_on_timer(); + + const std::string namespace_; + std::unique_ptr loaned_state_interface_ptr_; + const std::string topic_name_; + std::shared_ptr node_; + rclcpp::Publisher::SharedPtr state_value_pub_; + rclcpp::TimerBase::SharedPtr timer_; }; -} // namespace distributed_control +} // namespace distributed_control -#endif // DISTRIBUTED_CONTROL__STATE_PUBLISHER_HPP_ \ No newline at end of file +#endif // DISTRIBUTED_CONTROL__STATE_PUBLISHER_HPP_ \ No newline at end of file diff --git a/controller_manager/include/controller_manager/distributed_control/state_subscriber.hpp b/controller_manager/include/controller_manager/distributed_control/state_subscriber.hpp new file mode 100644 index 0000000000..e69de29bb2 diff --git a/controller_manager/include/controller_manager/distributed_control/sub_controller_manager_wrapper.hpp b/controller_manager/include/controller_manager/distributed_control/sub_controller_manager_wrapper.hpp new file mode 100644 index 0000000000..ea375a14d1 --- /dev/null +++ b/controller_manager/include/controller_manager/distributed_control/sub_controller_manager_wrapper.hpp @@ -0,0 +1,50 @@ +#ifndef DISTRIBUTED_CONTROL__SUB_CONTROLLER_MANAGER_WRAPPER_HPP_ +#define DISTRIBUTED_CONTROL__SUB_CONTROLLER_MANAGER_WRAPPER_HPP_ + +#include +#include +#include + +namespace distributed_control +{ + +class SubControllerManagerWrapper final +{ +public: + explicit SubControllerManagerWrapper( + const std::string & ns, const std::string & name, + const std::vector state_interface_names, std::vector topic_names) + : NAMESPACE_(ns), NAME_(name) + { + if (state_interface_names.size() != topic_names.size()) + { + throw std::logic_error( + "SubControllerManagerWrapper: state_interfaces_names.size() != topic_names.size(). They " + "must be equal."); + } + for (size_t i = 0; i < state_interface_names.size(); ++i) + { + state_interface_topic_name_map_.insert( + std::pair{state_interface_names.at(i), topic_names.at(i)}); + } + } + + ~SubControllerManagerWrapper() {} + + std::string get_namespace() const { return NAMESPACE_; } + + std::string get_name() const { return NAME_; } + + std::string get_full_qualified_name() const { return "/" + get_namespace() + "/" + get_name(); } + + std::vector get_topic_names() const {} + +private: + const std::string NAMESPACE_; + const std::string NAME_; + std::map state_interface_topic_name_map_; +}; + +} // namespace distributed_control + +#endif // DISTRIBUTED_CONTROL__SUB_CONTROLLER_MANAGER_WRAPPER_HPP_ \ No newline at end of file diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 031ea6a7d2..ac10b92711 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 @@ -134,7 +135,7 @@ rclcpp::NodeOptions get_cm_node_options() ControllerManager::ControllerManager( std::shared_ptr executor, const std::string & manager_node_name, - const std::string & namespace_, const bool & is_distributed) + const std::string & namespace_) : rclcpp::Node(manager_node_name, namespace_, get_cm_node_options()), resource_manager_(std::make_unique()), diagnostics_updater_(this), @@ -143,12 +144,12 @@ ControllerManager::ControllerManager( kControllerInterfaceNamespace, kControllerInterfaceClassName)), chainable_loader_( std::make_shared>( - kControllerInterfaceNamespace, kChainableControllerInterfaceClassName)), - is_distributed_(is_distributed_) + kControllerInterfaceNamespace, kChainableControllerInterfaceClassName)) { 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 = ""; @@ -164,13 +165,13 @@ ControllerManager::ControllerManager( diagnostics_updater_.add( "Controllers Activity", this, &ControllerManager::controller_activity_diagnostic_callback); init_services(); - create_hardware_state_publisher(); + configure_controller_manager(); } ControllerManager::ControllerManager( std::unique_ptr resource_manager, std::shared_ptr executor, const std::string & manager_node_name, - const std::string & namespace_, const bool & is_distributed) + const std::string & namespace_) : rclcpp::Node(manager_node_name, namespace_, get_cm_node_options()), resource_manager_(std::move(resource_manager)), diagnostics_updater_(this), @@ -179,14 +180,13 @@ ControllerManager::ControllerManager( kControllerInterfaceNamespace, kControllerInterfaceClassName)), chainable_loader_( std::make_shared>( - kControllerInterfaceNamespace, kChainableControllerInterfaceClassName)), - is_distributed_(is_distributed) + kControllerInterfaceNamespace, kChainableControllerInterfaceClassName)) { diagnostics_updater_.setHardwareID("ros2_control"); diagnostics_updater_.add( "Controllers Activity", this, &ControllerManager::controller_activity_diagnostic_callback); init_services(); - create_hardware_state_publisher(); + configure_controller_manager(); } void ControllerManager::init_resource_manager(const std::string & robot_description) @@ -276,27 +276,152 @@ void ControllerManager::init_services() qos_services, best_effort_callback_group_); } +void ControllerManager::configure_controller_manager() +{ + if (!get_parameter("distributed", distributed_)) + { + RCLCPP_WARN( + get_logger(), "'distributed' parameter not set, using default value:%s", + distributed_ ? "true" : "false"); + } + + if (!get_parameter("sub_controller_manager", sub_controller_manager_)) + { + RCLCPP_WARN( + get_logger(), "'sub_controller_manager' parameter not set, using default value:%s", + sub_controller_manager_ ? "true" : "false"); + } + + // 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. + + // This means we are a sub controller manager + bool std_controller_manager = !distributed_ && !sub_controller_manager_; + bool distributed_sub_controller_manager = distributed_ && sub_controller_manager_; + bool central_controller_manager = distributed_ && !sub_controller_manager_; + if (distributed_sub_controller_manager) + { + create_hardware_state_publisher(); + register_sub_controller_manager(); + } + // This means we are the central controller manager + else if (central_controller_manager) + { + init_distributed_main_controller_services(); + } + // std controller manager or error. std controller manager needs no special setup. + else + { + if (!std_controller_manager) + { + throw std::logic_error( + "Controller manager configured with: distributed:false and sub_controller_manager:true. " + "Only distributed controller manager can be a sub controller manager."); + } + } +} + +void ControllerManager::init_distributed_main_controller_services() +{ + distributed_system_srv_callback_group_ = + create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + + register_sub_controller_manager_srv_ = + create_service( + "register_sub_controller_manager", + std::bind( + &ControllerManager::register_sub_controller_manager_srv_cb, this, std::placeholders::_1, + std::placeholders::_2), + rmw_qos_profile_services_hist_keep_all, distributed_system_srv_callback_group_); +} + +void ControllerManager::register_sub_controller_manager_srv_cb( + const std::shared_ptr + request, + std::shared_ptr response) +{ + std::lock_guard guard(central_controller_manager_srv_lock_); + try + { + auto sub_ctrl_mng_wrapper = std::make_shared( + request->sub_controller_manager_namespace, request->sub_controller_manager_name, + request->state_interface_names, request->state_publisher_topics); + sub_controller_manager_map_.insert( + std::pair{sub_ctrl_mng_wrapper->get_full_qualified_name(), sub_ctrl_mng_wrapper}); + response->ok = true; + + RCLCPP_INFO( + get_logger(), "ControllerManager: Registered sub controller manager '%s'.", + sub_ctrl_mng_wrapper->get_full_qualified_name()); + } + catch (const std::runtime_error & e) + { + response->ok = false; + RCLCPP_ERROR( + get_logger(), "ControllerManager:Cannot register sub controller manager:.: %s", + request->sub_controller_manager_namespace, request->sub_controller_manager_name, e.what()); + } +} + void ControllerManager::create_hardware_state_publisher() { + auto state_interface_names = resource_manager_->available_state_interfaces(); - auto state_interface_names = resource_manager_->available_state_interfaces(); - - for (const auto & state_interface : state_interface_names) + for (const auto & state_interface : state_interface_names) + { + try { - try - { - - RCLCPP_INFO(get_logger() , "Creating StatePublisher for interface:<%s>.", state_interface.c_str()); - auto state_publisher = std::make_shared(get_namespace() , std::move(std::make_unique(resource_manager_->claim_state_interface(state_interface)))); - state_interface_state_publisher_map_.insert(std::pair{state_interface, state_publisher}); - executor_->add_node(state_publisher->get_node()->get_node_base_interface()); - } - catch (const std::exception & e) - { - RCLCPP_ERROR(get_logger(), "Can't create StatePublisher<%s> : %s", state_interface.c_str(), e.what()); - break; - } + RCLCPP_INFO( + get_logger(), "Creating StatePublisher for interface:<%s>.", state_interface.c_str()); + auto state_publisher = std::make_shared( + get_namespace(), std::move(std::make_unique( + resource_manager_->claim_state_interface(state_interface)))); + state_interface_state_publisher_map_.insert(std::pair{state_interface, state_publisher}); + executor_->add_node(state_publisher->get_node()->get_node_base_interface()); + } + catch (const std::exception & e) + { + RCLCPP_ERROR( + get_logger(), "Can't create StatePublisher<%s> : %s", state_interface.c_str(), e.what()); + break; } + } +} + +void ControllerManager::register_sub_controller_manager() +{ + rclcpp::Client::SharedPtr client = + create_client( + "/register_sub_controller_manager"); + + std::vector state_interface_names, topic_names; + for (auto it = state_interface_state_publisher_map_.begin(); + it != state_interface_state_publisher_map_.end(); ++it) + { + state_interface_names.push_back(it->second->get_state_interface_name()); + topic_names.push_back(it->second->get_topic_name()); + } + + auto request = + std::make_shared(); + request->sub_controller_manager_namespace = get_namespace(); + request->sub_controller_manager_name = get_name(); + request->state_interface_names = std::move(state_interface_names); + request->state_publisher_topics = std::move(topic_names); + + using namespace std::chrono_literals; + while (!client->wait_for_service(1s)) + { + if (!rclcpp::ok()) + { + RCLCPP_ERROR( + rclcpp::get_logger("rclcpp"), "Interrupted while waiting for the service. Exiting."); + return; + } + RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "service not available, waiting again..."); + } } controller_interface::ControllerInterfaceBaseSharedPtr ControllerManager::load_controller( diff --git a/controller_manager/src/distributed_control/state_publisher.cpp b/controller_manager/src/distributed_control/state_publisher.cpp index 8f0eb98855..7976833b5f 100644 --- a/controller_manager/src/distributed_control/state_publisher.cpp +++ b/controller_manager/src/distributed_control/state_publisher.cpp @@ -1,7 +1,9 @@ #include "controller_manager/distributed_control/state_publisher.hpp" +#include #include #include +#include #include using namespace std::chrono_literals; @@ -9,17 +11,23 @@ using namespace std::chrono_literals; namespace distributed_control { -StatePublisher::StatePublisher(const std::string & ns, std::unique_ptr loaned_state_interface_ptr) -: namespace_(ns), loaned_state_interface_ptr_(std::move(loaned_state_interface_ptr)) +StatePublisher::StatePublisher( + const std::string & ns, + std::unique_ptr loaned_state_interface_ptr) +: namespace_(ns), + loaned_state_interface_ptr_(std::move(loaned_state_interface_ptr)), + topic_name_(loaned_state_interface_ptr_->get_underscore_separated_name()) { rclcpp::NodeOptions node_options; - node_ = std::make_shared(loaned_state_interface_ptr_->get_underscore_separated_name() + "_node", namespace_, node_options, false); - state_value_pub_ = node_->create_publisher(loaned_state_interface_ptr_->get_underscore_separated_name(), 10); - timer_ = node_->create_wall_timer(1ms, std::bind(&StatePublisher::on_timer, this)); - RCLCPP_INFO(node_->get_logger(), "Creating StatePublisher<%s>.", loaned_state_interface_ptr_->get_underscore_separated_name().c_str()); + node_ = std::make_shared( + loaned_state_interface_ptr_->get_underscore_separated_name() + "_node", namespace_, + node_options, false); + state_value_pub_ = node_->create_publisher(topic_name_, 10); + timer_ = node_->create_wall_timer(50ms, std::bind(&StatePublisher::publish_value_on_timer, this)); + RCLCPP_INFO(node_->get_logger(), "Creating StatePublisher<%s>.", topic_name_.c_str()); } -std::shared_ptr StatePublisher::get_node() +std::shared_ptr StatePublisher::get_node() const { if (!node_.get()) { @@ -28,15 +36,22 @@ std::shared_ptr StatePublisher::get_node() return node_; } -void StatePublisher::on_timer() +std::string StatePublisher::get_topic_name() const { return topic_name_; } + +std::string StatePublisher::get_state_interface_name() const +{ + return loaned_state_interface_ptr_->get_name(); +} + +void StatePublisher::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_state_interface_ptr_->get_value(); - } - catch ( const std::runtime_error & e) + } + catch (const std::runtime_error & e) { msg->data = std::numeric_limits::quiet_NaN(); } @@ -48,4 +63,4 @@ void StatePublisher::on_timer() state_value_pub_->publish(std::move(msg)); } -} // namespace distributed_control \ No newline at end of file +} // namespace distributed_control \ No newline at end of file diff --git a/controller_manager/src/distributed_control/state_subscriber.cpp b/controller_manager/src/distributed_control/state_subscriber.cpp new file mode 100644 index 0000000000..e69de29bb2 diff --git a/controller_manager_msgs/CMakeLists.txt b/controller_manager_msgs/CMakeLists.txt index 2ba8f23194..e5f92a8eae 100644 --- a/controller_manager_msgs/CMakeLists.txt +++ b/controller_manager_msgs/CMakeLists.txt @@ -23,6 +23,7 @@ set(srv_files srv/ListHardwareComponents.srv srv/ListHardwareInterfaces.srv srv/LoadController.srv + srv/RegisterSubControllerManager.srv srv/ReloadControllerLibraries.srv srv/SetHardwareComponentState.srv srv/SwitchController.srv diff --git a/controller_manager_msgs/srv/RegisterSubControllerManager.srv b/controller_manager_msgs/srv/RegisterSubControllerManager.srv new file mode 100644 index 0000000000..96c7c43e1b --- /dev/null +++ b/controller_manager_msgs/srv/RegisterSubControllerManager.srv @@ -0,0 +1,22 @@ +# 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 +# * the StatePublisher it has + +# Res: +# The response includes: +# * a bool if the registration succeeded +# * a the CommandPublishers the central controller manager provides + +string sub_controller_manager_namespace +string sub_controller_manager_name +string[] state_interface_names +string[] state_publisher_topics +--- +bool ok +string[] command_interface_names +string[] command_publishers_topics \ No newline at end of file From 8f200fb84958cbfc682bfe9072f96243a0c77f27 Mon Sep 17 00:00:00 2001 From: Manuel M Date: Fri, 18 Nov 2022 14:36:46 +0100 Subject: [PATCH 06/19] move StatePublisher to Hardware side --- controller_manager/CMakeLists.txt | 3 -- .../controller_manager/controller_manager.hpp | 2 +- .../sub_controller_manager_wrapper.hpp | 3 +- controller_manager/package.xml | 2 - controller_manager/src/controller_manager.cpp | 42 ++++++++++++++----- hardware_interface/CMakeLists.txt | 2 + .../state_publisher.hpp | 0 .../state_subscriber.hpp | 0 .../visibility_control.h | 0 hardware_interface/package.xml | 1 + .../state_publisher.cpp | 2 +- .../state_subscriber.cpp | 0 12 files changed, 39 insertions(+), 18 deletions(-) rename {controller_manager/include/controller_manager/distributed_control => hardware_interface/include/hardware_interface/distributed_control_interface}/state_publisher.hpp (100%) rename {controller_manager/include/controller_manager/distributed_control => hardware_interface/include/hardware_interface/distributed_control_interface}/state_subscriber.hpp (100%) rename {controller_manager/include/controller_manager/distributed_control => hardware_interface/include/hardware_interface/distributed_control_interface}/visibility_control.h (100%) rename {controller_manager/src/distributed_control => hardware_interface/src/hardware_interface/distributed_control_interface}/state_publisher.cpp (96%) rename {controller_manager/src/distributed_control => hardware_interface/src/hardware_interface/distributed_control_interface}/state_subscriber.cpp (100%) diff --git a/controller_manager/CMakeLists.txt b/controller_manager/CMakeLists.txt index 55ba772d9b..0089b906e1 100644 --- a/controller_manager/CMakeLists.txt +++ b/controller_manager/CMakeLists.txt @@ -13,9 +13,7 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS hardware_interface pluginlib rclcpp - rclcpp_lifecycle realtime_tools - std_msgs ) find_package(ament_cmake REQUIRED) @@ -27,7 +25,6 @@ foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) endforeach() add_library(${PROJECT_NAME} SHARED - src/distributed_control/state_publisher.cpp src/controller_manager.cpp ) diff --git a/controller_manager/include/controller_manager/controller_manager.hpp b/controller_manager/include/controller_manager/controller_manager.hpp index 074afb73e4..add822c72d 100644 --- a/controller_manager/include/controller_manager/controller_manager.hpp +++ b/controller_manager/include/controller_manager/controller_manager.hpp @@ -29,7 +29,6 @@ #include "distributed_control/sub_controller_manager_wrapper.hpp" #include "controller_manager/controller_spec.hpp" -#include "controller_manager/distributed_control/state_publisher.hpp" #include "controller_manager/visibility_control.h" #include "controller_manager_msgs/srv/configure_controller.hpp" #include "controller_manager_msgs/srv/list_controller_types.hpp" @@ -44,6 +43,7 @@ #include "controller_manager_msgs/srv/unload_controller.hpp" #include "diagnostic_updater/diagnostic_updater.hpp" +#include "hardware_interface/distributed_control_interface/state_publisher.hpp" #include "hardware_interface/handle.hpp" #include "hardware_interface/resource_manager.hpp" diff --git a/controller_manager/include/controller_manager/distributed_control/sub_controller_manager_wrapper.hpp b/controller_manager/include/controller_manager/distributed_control/sub_controller_manager_wrapper.hpp index ea375a14d1..ba6cbc14d7 100644 --- a/controller_manager/include/controller_manager/distributed_control/sub_controller_manager_wrapper.hpp +++ b/controller_manager/include/controller_manager/distributed_control/sub_controller_manager_wrapper.hpp @@ -2,6 +2,7 @@ #define DISTRIBUTED_CONTROL__SUB_CONTROLLER_MANAGER_WRAPPER_HPP_ #include +#include #include #include @@ -35,7 +36,7 @@ class SubControllerManagerWrapper final std::string get_name() const { return NAME_; } - std::string get_full_qualified_name() const { return "/" + get_namespace() + "/" + get_name(); } + std::string get_full_qualified_name() const { return get_namespace() + "/" + get_name(); } std::vector get_topic_names() const {} diff --git a/controller_manager/package.xml b/controller_manager/package.xml index cee9d5b8c2..401e99ebe0 100644 --- a/controller_manager/package.xml +++ b/controller_manager/package.xml @@ -21,13 +21,11 @@ launch_ros pluginlib rclcpp - rclcpp_lifecycle rcpputils realtime_tools ros2_control_test_assets ros2param ros2run - std_msgs ament_cmake_gmock diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index ac10b92711..1489a51ef1 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -342,6 +342,7 @@ void ControllerManager::register_sub_controller_manager_srv_cb( request, std::shared_ptr response) { + RCLCPP_WARN(get_logger(), "ControllerManager: Got registration callback."); std::lock_guard guard(central_controller_manager_srv_lock_); try { @@ -352,16 +353,15 @@ void ControllerManager::register_sub_controller_manager_srv_cb( std::pair{sub_ctrl_mng_wrapper->get_full_qualified_name(), sub_ctrl_mng_wrapper}); response->ok = true; - RCLCPP_INFO( - get_logger(), "ControllerManager: Registered sub controller manager '%s'.", - sub_ctrl_mng_wrapper->get_full_qualified_name()); + RCLCPP_WARN_STREAM( + get_logger(), "ControllerManager: Registered sub controller manager <" + << sub_ctrl_mng_wrapper->get_full_qualified_name() << ">."); } - catch (const std::runtime_error & e) + catch (const std::logic_error & e) { response->ok = false; RCLCPP_ERROR( - get_logger(), "ControllerManager:Cannot register sub controller manager:.: %s", - request->sub_controller_manager_namespace, request->sub_controller_manager_name, e.what()); + get_logger(), "ControllerManager: Cannot register sub controller manager. %s", e.what()); } } @@ -374,7 +374,8 @@ void ControllerManager::create_hardware_state_publisher() try { RCLCPP_INFO( - get_logger(), "Creating StatePublisher for interface:<%s>.", state_interface.c_str()); + get_logger(), "ControllerManager: Creating StatePublisher for interface:<%s>.", + state_interface.c_str()); auto state_publisher = std::make_shared( get_namespace(), std::move(std::make_unique( resource_manager_->claim_state_interface(state_interface)))); @@ -384,7 +385,8 @@ void ControllerManager::create_hardware_state_publisher() catch (const std::exception & e) { RCLCPP_ERROR( - get_logger(), "Can't create StatePublisher<%s> : %s", state_interface.c_str(), e.what()); + get_logger(), "ControllerManager: Can't create StatePublisher<%s> : %s", + state_interface.c_str(), e.what()); break; } } @@ -392,6 +394,7 @@ void ControllerManager::create_hardware_state_publisher() void ControllerManager::register_sub_controller_manager() { + RCLCPP_INFO(get_logger(), "SubControllerManager:Trying to register StatePublishers."); rclcpp::Client::SharedPtr client = create_client( "/register_sub_controller_manager"); @@ -417,10 +420,29 @@ void ControllerManager::register_sub_controller_manager() if (!rclcpp::ok()) { RCLCPP_ERROR( - rclcpp::get_logger("rclcpp"), "Interrupted while waiting for the service. Exiting."); + get_logger(), + "SubControllerManager: Interrupted while waiting for central controller managers " + "registration service. Exiting."); return; } - RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "service not available, waiting again..."); + RCLCPP_INFO( + get_logger(), + "SubControllerManager:Central controller managers registration service not available, " + "waiting again..."); + } + + RCLCPP_WARN(get_logger(), "SubControllerManager: Send request."); + auto result = client->async_send_request(request); + if ( + (rclcpp::spin_until_future_complete(this->get_node_base_interface(), result) == + rclcpp::FutureReturnCode::SUCCESS) && + result.get()->ok) + { + RCLCPP_INFO(get_logger(), "SubControllerManager: Successfully registered StatePublishers."); + } + else + { + RCLCPP_WARN(get_logger(), "SubControllerManager: Registration of StatePublishers failed."); } } diff --git a/hardware_interface/CMakeLists.txt b/hardware_interface/CMakeLists.txt index 9af1a359cd..7529510cf0 100644 --- a/hardware_interface/CMakeLists.txt +++ b/hardware_interface/CMakeLists.txt @@ -9,6 +9,7 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS control_msgs lifecycle_msgs pluginlib + rclcpp rclcpp_lifecycle rcpputils rcutils @@ -26,6 +27,7 @@ add_library( SHARED src/actuator.cpp src/component_parser.cpp + src/hardware_interface/distributed_control_interface/state_publisher.cpp src/resource_manager.cpp src/sensor.cpp src/system.cpp diff --git a/controller_manager/include/controller_manager/distributed_control/state_publisher.hpp b/hardware_interface/include/hardware_interface/distributed_control_interface/state_publisher.hpp similarity index 100% rename from controller_manager/include/controller_manager/distributed_control/state_publisher.hpp rename to hardware_interface/include/hardware_interface/distributed_control_interface/state_publisher.hpp diff --git a/controller_manager/include/controller_manager/distributed_control/state_subscriber.hpp b/hardware_interface/include/hardware_interface/distributed_control_interface/state_subscriber.hpp similarity index 100% rename from controller_manager/include/controller_manager/distributed_control/state_subscriber.hpp rename to hardware_interface/include/hardware_interface/distributed_control_interface/state_subscriber.hpp diff --git a/controller_manager/include/controller_manager/distributed_control/visibility_control.h b/hardware_interface/include/hardware_interface/distributed_control_interface/visibility_control.h similarity index 100% rename from controller_manager/include/controller_manager/distributed_control/visibility_control.h rename to hardware_interface/include/hardware_interface/distributed_control_interface/visibility_control.h diff --git a/hardware_interface/package.xml b/hardware_interface/package.xml index 0339900ff7..0cb61def09 100644 --- a/hardware_interface/package.xml +++ b/hardware_interface/package.xml @@ -12,6 +12,7 @@ control_msgs lifecycle_msgs pluginlib + rclcpp rclcpp_lifecycle rcpputils tinyxml2_vendor diff --git a/controller_manager/src/distributed_control/state_publisher.cpp b/hardware_interface/src/hardware_interface/distributed_control_interface/state_publisher.cpp similarity index 96% rename from controller_manager/src/distributed_control/state_publisher.cpp rename to hardware_interface/src/hardware_interface/distributed_control_interface/state_publisher.cpp index 7976833b5f..174d3bbfc9 100644 --- a/controller_manager/src/distributed_control/state_publisher.cpp +++ b/hardware_interface/src/hardware_interface/distributed_control_interface/state_publisher.cpp @@ -1,4 +1,4 @@ -#include "controller_manager/distributed_control/state_publisher.hpp" +#include "hardware_interface/distributed_control_interface/state_publisher.hpp" #include #include diff --git a/controller_manager/src/distributed_control/state_subscriber.cpp b/hardware_interface/src/hardware_interface/distributed_control_interface/state_subscriber.cpp similarity index 100% rename from controller_manager/src/distributed_control/state_subscriber.cpp rename to hardware_interface/src/hardware_interface/distributed_control_interface/state_subscriber.cpp From 1e156a52f20c6ff35a2b63763c6db5c9e3244212 Mon Sep 17 00:00:00 2001 From: Manuel M Date: Fri, 18 Nov 2022 14:39:22 +0100 Subject: [PATCH 07/19] remove not needed rclcpp warn and info outputs --- controller_manager/src/controller_manager.cpp | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 1489a51ef1..303519fa7b 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -342,7 +342,6 @@ void ControllerManager::register_sub_controller_manager_srv_cb( request, std::shared_ptr response) { - RCLCPP_WARN(get_logger(), "ControllerManager: Got registration callback."); std::lock_guard guard(central_controller_manager_srv_lock_); try { @@ -353,7 +352,7 @@ void ControllerManager::register_sub_controller_manager_srv_cb( std::pair{sub_ctrl_mng_wrapper->get_full_qualified_name(), sub_ctrl_mng_wrapper}); response->ok = true; - RCLCPP_WARN_STREAM( + RCLCPP_INFO_STREAM( get_logger(), "ControllerManager: Registered sub controller manager <" << sub_ctrl_mng_wrapper->get_full_qualified_name() << ">."); } @@ -431,7 +430,6 @@ void ControllerManager::register_sub_controller_manager() "waiting again..."); } - RCLCPP_WARN(get_logger(), "SubControllerManager: Send request."); auto result = client->async_send_request(request); if ( (rclcpp::spin_until_future_complete(this->get_node_base_interface(), result) == From 02970999f13fe80b2d2e54baa607de27dc34e9d2 Mon Sep 17 00:00:00 2001 From: Manuel M Date: Mon, 21 Nov 2022 14:58:37 +0100 Subject: [PATCH 08/19] add DistributedStateInterface and adjusted LoanedStateInterface --- .../test/test_force_torque_sensor.cpp | 64 ++++++++--------- controller_interface/test/test_imu_sensor.cpp | 40 +++++------ .../test_semantic_component_interface.cpp | 9 ++- .../sub_controller_manager_wrapper.hpp | 36 ++++++++-- hardware_interface/CMakeLists.txt | 1 + .../include/hardware_interface/handle.hpp | 72 ++++++++++++++++++- .../loaned_state_interface.hpp | 33 ++++++--- hardware_interface/package.xml | 1 + hardware_interface/src/resource_manager.cpp | 5 +- 9 files changed, 185 insertions(+), 76 deletions(-) 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/include/controller_manager/distributed_control/sub_controller_manager_wrapper.hpp b/controller_manager/include/controller_manager/distributed_control/sub_controller_manager_wrapper.hpp index ba6cbc14d7..14bad015e9 100644 --- a/controller_manager/include/controller_manager/distributed_control/sub_controller_manager_wrapper.hpp +++ b/controller_manager/include/controller_manager/distributed_control/sub_controller_manager_wrapper.hpp @@ -1,6 +1,7 @@ #ifndef DISTRIBUTED_CONTROL__SUB_CONTROLLER_MANAGER_WRAPPER_HPP_ #define DISTRIBUTED_CONTROL__SUB_CONTROLLER_MANAGER_WRAPPER_HPP_ +#include #include #include #include @@ -15,18 +16,27 @@ class SubControllerManagerWrapper final explicit SubControllerManagerWrapper( const std::string & ns, const std::string & name, const std::vector state_interface_names, std::vector topic_names) - : NAMESPACE_(ns), NAME_(name) + : NAMESPACE_(ns), + NAME_(name), + state_interface_names_(std::move(state_interface_names)), + topic_names_(std::move(topic_names)) { - if (state_interface_names.size() != topic_names.size()) + if (state_interface_names_.size() != topic_names_.size()) { throw std::logic_error( "SubControllerManagerWrapper: state_interfaces_names.size() != topic_names.size(). They " "must be equal."); } - for (size_t i = 0; i < state_interface_names.size(); ++i) + for (size_t i = 0; i < state_interface_names_.size(); ++i) { - state_interface_topic_name_map_.insert( - std::pair{state_interface_names.at(i), topic_names.at(i)}); + const auto [it, success] = state_interface_topic_name_map_.insert( + std::pair{state_interface_names_.at(i), topic_names_.at(i)}); + if (!success) + { + throw std::logic_error( + "SubControllerManagerWrapper: Duplicate of given state_interface_name:'" + it->first + + "'. They must be unique."); + } } } @@ -38,11 +48,25 @@ class SubControllerManagerWrapper final std::string get_full_qualified_name() const { return get_namespace() + "/" + get_name(); } - std::vector get_topic_names() const {} + std::vector get_state_interface_names() const { return state_interface_names_; } + + std::vector get_topic_names() const { return topic_names_; } + + std::string get_topic_for_state_interface(const std::string & state_interface_name) + { + if (auto element = state_interface_topic_name_map_.find(state_interface_name); + element != state_interface_topic_name_map_.end()) + { + return element->second; + } + throw std::runtime_error("SubControllerManagerWrapper: No state_interface found."); + } private: const std::string NAMESPACE_; const std::string NAME_; + std::vector state_interface_names_; + std::vector topic_names_; std::map state_interface_topic_name_map_; }; diff --git a/hardware_interface/CMakeLists.txt b/hardware_interface/CMakeLists.txt index 7529510cf0..ed48a5e852 100644 --- a/hardware_interface/CMakeLists.txt +++ b/hardware_interface/CMakeLists.txt @@ -13,6 +13,7 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS rclcpp_lifecycle rcpputils rcutils + std_msgs tinyxml2_vendor TinyXML2 ) diff --git a/hardware_interface/include/hardware_interface/handle.hpp b/hardware_interface/include/hardware_interface/handle.hpp index 1aea017754..46121739ce 100644 --- a/hardware_interface/include/hardware_interface/handle.hpp +++ b/hardware_interface/include/hardware_interface/handle.hpp @@ -20,7 +20,9 @@ #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. @@ -113,6 +115,64 @@ class ReadWriteHandle : public 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 std::string & prefix_name, const std::string & interface_name, + const std::string & topic_name) + : ReadOnlyHandle(prefix_name, interface_name, &value_), topic_name_(topic_name) + { + rclcpp::NodeOptions node_options; + node_ = std::make_shared( + get_interface_name() + "_node", get_prefix_name(), node_options, false); + state_value_subscription_ = node_->create_subscription( + topic_name, 10, + std::bind(&DistributedReadOnlyHandle::set_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(const DistributedReadOnlyHandle & other) = default; + + DistributedReadOnlyHandle(DistributedReadOnlyHandle && other) = default; + + DistributedReadOnlyHandle & operator=(const DistributedReadOnlyHandle & other) = default; + + DistributedReadOnlyHandle & operator=(DistributedReadOnlyHandle && other) = default; + + virtual ~DistributedReadOnlyHandle() = default; + + /// Returns true if handle references a value. + inline operator bool() const { return value_ptr_ != nullptr; } + + std::shared_ptr get_node() const + { + if (!node_.get()) + { + throw std::runtime_error("DistributedReadOnlyHandle: Node not initialized!"); + } + return node_; + } + +protected: + void set_value_cb(const std_msgs::msg::Float64 & msg) { value_ = msg.data; } + + std::string topic_name_; + std::shared_ptr node_; + rclcpp::Subscription::SharedPtr state_value_subscription_; + double value_; +}; + class StateInterface : public ReadOnlyHandle { public: @@ -123,6 +183,15 @@ class StateInterface : public ReadOnlyHandle using ReadOnlyHandle::ReadOnlyHandle; }; +class DistributedStateInterface : public DistributedReadOnlyHandle +{ + DistributedStateInterface(const DistributedStateInterface & other) = default; + + DistributedStateInterface(DistributedStateInterface && other) = default; + + using DistributedReadOnlyHandle::DistributedReadOnlyHandle; +}; + class CommandInterface : public ReadWriteHandle { public: @@ -138,7 +207,6 @@ class CommandInterface : public ReadWriteHandle using ReadWriteHandle::ReadWriteHandle; }; - } // namespace hardware_interface #endif // HARDWARE_INTERFACE__HANDLE_HPP_ diff --git a/hardware_interface/include/hardware_interface/loaned_state_interface.hpp b/hardware_interface/include/hardware_interface/loaned_state_interface.hpp index f08763a0e4..6491e0d79e 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,16 +27,28 @@ 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)) { } + explicit LoanedStateInterface( + std::shared_ptr & distributed_state_interface) + : LoanedStateInterface(distributed_state_interface, nullptr) + { + } + + LoanedStateInterface( + std::shared_ptr & distributed_state_interface, Deleter && deleter) + : state_interface_(distributed_state_interface), deleter_(std::forward(deleter)) + { + } + LoanedStateInterface(const LoanedStateInterface & other) = delete; LoanedStateInterface(LoanedStateInterface && other) = default; @@ -50,15 +61,15 @@ class LoanedStateInterface } } - const std::string get_name() const { return state_interface_.get_name(); } + const std::string get_name() const { return state_interface_->get_name(); } - const std::string & get_interface_name() const { return state_interface_.get_interface_name(); } + const std::string & get_interface_name() const { return state_interface_->get_interface_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_underscore_separated_name() const @@ -66,19 +77,19 @@ class LoanedStateInterface std::string prefix = get_prefix_name(); std::string interface = get_interface_name(); - if(prefix.empty()) + if (prefix.empty()) { return interface; } return prefix + "_" + interface; - } + } - const std::string & get_prefix_name() const { return state_interface_.get_prefix_name(); } + const std::string & get_prefix_name() const { return state_interface_->get_prefix_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/package.xml b/hardware_interface/package.xml index 0cb61def09..88f5e3ea13 100644 --- a/hardware_interface/package.xml +++ b/hardware_interface/package.xml @@ -16,6 +16,7 @@ rclcpp_lifecycle rcpputils tinyxml2_vendor + std_msgs rcutils rcutils diff --git a/hardware_interface/src/resource_manager.cpp b/hardware_interface/src/resource_manager.cpp index 7e88cca9b5..85245b43c4 100644 --- a/hardware_interface/src/resource_manager.cpp +++ b/hardware_interface/src/resource_manager.cpp @@ -416,7 +416,8 @@ class ResourceStorage for (auto & interface : interfaces) { auto key = interface.get_name(); - state_interface_map_.emplace(std::make_pair(key, std::move(interface))); + state_interface_map_.emplace( + std::make_pair(key, std::make_shared(interface))); interface_names.push_back(key); } hardware_info_map_[hardware.get_name()].state_interfaces = interface_names; @@ -556,7 +557,7 @@ 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_; From e52740bbdb984dbd7c82c74dd9d88ff944c60f6b Mon Sep 17 00:00:00 2001 From: Manuel M Date: Wed, 23 Nov 2022 12:08:47 +0100 Subject: [PATCH 09/19] restructure handles --- .../include/hardware_interface/handle.hpp | 113 ++++++++++++------ hardware_interface/src/resource_manager.cpp | 2 +- 2 files changed, 78 insertions(+), 37 deletions(-) diff --git a/hardware_interface/include/hardware_interface/handle.hpp b/hardware_interface/include/hardware_interface/handle.hpp index 46121739ce..35ec5d64da 100644 --- a/hardware_interface/include/hardware_interface/handle.hpp +++ b/hardware_interface/include/hardware_interface/handle.hpp @@ -26,35 +26,47 @@ 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; } @@ -70,33 +82,69 @@ class ReadOnlyHandle return get_name(); } - const std::string & get_prefix_name() const { return prefix_name_; } - - double get_value() const + 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("DistributedReadOnlyHandle: Node not initialized!"); + } + return node_; } + const std::string & get_prefix_name() const { return prefix_name_; } + protected: std::string prefix_name_; std::string interface_name_; double * value_ptr_; + std::shared_ptr node_; +}; + +class ReadOnlyHandle : public HandleInterface, ReadHandleInterface +{ +public: + ReadOnlyHandle( + const std::string & prefix_name, const std::string & interface_name, + double * value_ptr = nullptr) + : HandleInterface(prefix_name, interface_name, value_ptr) + { + } + + explicit ReadOnlyHandle(const std::string & interface_name) : HandleInterface(interface_name) {} + + explicit ReadOnlyHandle(const char * interface_name) : HandleInterface(interface_name) {} + + ReadOnlyHandle(const ReadOnlyHandle & other) = default; + + ReadOnlyHandle(ReadOnlyHandle && other) = default; + + ReadOnlyHandle & operator=(const ReadOnlyHandle & other) = default; + + ReadOnlyHandle & operator=(ReadOnlyHandle && other) = default; + + virtual ~ReadOnlyHandle() = default; + + double get_value() const override + { + THROW_ON_NULLPTR(value_ptr_); + return *value_ptr_; + } }; -class ReadWriteHandle : public ReadOnlyHandle +class ReadWriteHandle : public HandleInterface, ReadHandleInterface, WriteHandleInterface { public: ReadWriteHandle( const std::string & prefix_name, const std::string & interface_name, double * value_ptr = nullptr) - : ReadOnlyHandle(prefix_name, interface_name, value_ptr) + : HandleInterface(prefix_name, interface_name, value_ptr) { } - explicit ReadWriteHandle(const std::string & interface_name) : ReadOnlyHandle(interface_name) {} + explicit ReadWriteHandle(const std::string & interface_name) : HandleInterface(interface_name) {} - explicit ReadWriteHandle(const char * interface_name) : ReadOnlyHandle(interface_name) {} + explicit ReadWriteHandle(const char * interface_name) : HandleInterface(interface_name) {} ReadWriteHandle(const ReadWriteHandle & other) = default; @@ -108,7 +156,13 @@ class ReadWriteHandle : public ReadOnlyHandle virtual ~ReadWriteHandle() = default; - void set_value(double value) + double get_value() const override + { + THROW_ON_NULLPTR(value_ptr_); + return *value_ptr_; + } + + void set_value(double value) override { THROW_ON_NULLPTR(this->value_ptr_); *this->value_ptr_ = value; @@ -152,23 +206,10 @@ class DistributedReadOnlyHandle : public ReadOnlyHandle virtual ~DistributedReadOnlyHandle() = default; - /// Returns true if handle references a value. - inline operator bool() const { return value_ptr_ != nullptr; } - - std::shared_ptr get_node() const - { - if (!node_.get()) - { - throw std::runtime_error("DistributedReadOnlyHandle: Node not initialized!"); - } - return node_; - } - protected: void set_value_cb(const std_msgs::msg::Float64 & msg) { value_ = msg.data; } std::string topic_name_; - std::shared_ptr node_; rclcpp::Subscription::SharedPtr state_value_subscription_; double value_; }; diff --git a/hardware_interface/src/resource_manager.cpp b/hardware_interface/src/resource_manager.cpp index 85245b43c4..3e3a160eca 100644 --- a/hardware_interface/src/resource_manager.cpp +++ b/hardware_interface/src/resource_manager.cpp @@ -557,7 +557,7 @@ 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_; From 48069e3bc8341b19d415a9fa57e9f6f915c40631 Mon Sep 17 00:00:00 2001 From: Manuel M Date: Wed, 23 Nov 2022 12:09:11 +0100 Subject: [PATCH 10/19] move creation of publishers into resource manager --- .../controller_manager/controller_manager.hpp | 5 +- .../sub_controller_manager_wrapper.hpp | 10 ++-- controller_manager/src/controller_manager.cpp | 46 ++++++--------- .../srv/RegisterSubControllerManager.srv | 5 +- .../state_subscriber.hpp | 0 .../loaned_state_interface.hpp | 16 +---- .../hardware_interface/resource_manager.hpp | 5 ++ hardware_interface/package.xml | 2 +- .../state_publisher.cpp | 4 +- .../state_subscriber.cpp | 0 hardware_interface/src/resource_manager.cpp | 58 +++++++++++++++++++ 11 files changed, 96 insertions(+), 55 deletions(-) delete mode 100644 hardware_interface/include/hardware_interface/distributed_control_interface/state_subscriber.hpp delete mode 100644 hardware_interface/src/hardware_interface/distributed_control_interface/state_subscriber.cpp diff --git a/controller_manager/include/controller_manager/controller_manager.hpp b/controller_manager/include/controller_manager/controller_manager.hpp index add822c72d..aa02117e7c 100644 --- a/controller_manager/include/controller_manager/controller_manager.hpp +++ b/controller_manager/include/controller_manager/controller_manager.hpp @@ -206,7 +206,7 @@ class ControllerManager : public rclcpp::Node std::shared_ptr response); CONTROLLER_MANAGER_PUBLIC - void create_hardware_state_publisher(); + void add_hardware_state_publishers(); CONTROLLER_MANAGER_PUBLIC void register_sub_controller_manager(); @@ -418,8 +418,7 @@ class ControllerManager : public rclcpp::Node bool distributed_ = false; bool sub_controller_manager_ = false; - std::map> - state_interface_state_publisher_map_; + std::map> sub_controller_manager_map_; rclcpp::CallbackGroup::SharedPtr distributed_system_srv_callback_group_; diff --git a/controller_manager/include/controller_manager/distributed_control/sub_controller_manager_wrapper.hpp b/controller_manager/include/controller_manager/distributed_control/sub_controller_manager_wrapper.hpp index 14bad015e9..18cee4f5ac 100644 --- a/controller_manager/include/controller_manager/distributed_control/sub_controller_manager_wrapper.hpp +++ b/controller_manager/include/controller_manager/distributed_control/sub_controller_manager_wrapper.hpp @@ -24,8 +24,8 @@ class SubControllerManagerWrapper final if (state_interface_names_.size() != topic_names_.size()) { throw std::logic_error( - "SubControllerManagerWrapper: state_interfaces_names.size() != topic_names.size(). They " - "must be equal."); + "SubControllerManagerWrapper: state_interfaces_names.size() != topic_names.size(). " + "Must be equal."); } for (size_t i = 0; i < state_interface_names_.size(); ++i) { @@ -35,7 +35,7 @@ class SubControllerManagerWrapper final { throw std::logic_error( "SubControllerManagerWrapper: Duplicate of given state_interface_name:'" + it->first + - "'. They must be unique."); + "'. Must be unique."); } } } @@ -44,9 +44,9 @@ class SubControllerManagerWrapper final std::string get_namespace() const { return NAMESPACE_; } - std::string get_name() const { return NAME_; } + std::string get_controller_manager_name() const { return NAME_; } - std::string get_full_qualified_name() const { return get_namespace() + "/" + get_name(); } + std::string get_name() const { return get_namespace() + "/" + get_name(); } std::vector get_state_interface_names() const { return state_interface_names_; } diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 303519fa7b..18b9350fb6 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -276,6 +276,10 @@ void ControllerManager::init_services() qos_services, best_effort_callback_group_); } +// TODO(Manuel) don't like this, this is for fast poc +// probably better to create factory and handle creation of correct controller manager type +// there. Since asynchronous control should be supported im the future as well and we don't +// want dozen of ifs. void ControllerManager::configure_controller_manager() { if (!get_parameter("distributed", distributed_)) @@ -292,18 +296,12 @@ void ControllerManager::configure_controller_manager() sub_controller_manager_ ? "true" : "false"); } - // 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. - - // This means we are a sub controller manager bool std_controller_manager = !distributed_ && !sub_controller_manager_; bool distributed_sub_controller_manager = distributed_ && sub_controller_manager_; bool central_controller_manager = distributed_ && !sub_controller_manager_; if (distributed_sub_controller_manager) { - create_hardware_state_publisher(); + add_hardware_state_publishers(); register_sub_controller_manager(); } // This means we are the central controller manager @@ -349,12 +347,12 @@ void ControllerManager::register_sub_controller_manager_srv_cb( request->sub_controller_manager_namespace, request->sub_controller_manager_name, request->state_interface_names, request->state_publisher_topics); sub_controller_manager_map_.insert( - std::pair{sub_ctrl_mng_wrapper->get_full_qualified_name(), sub_ctrl_mng_wrapper}); + std::pair{sub_ctrl_mng_wrapper->get_name(), sub_ctrl_mng_wrapper}); response->ok = true; RCLCPP_INFO_STREAM( get_logger(), "ControllerManager: Registered sub controller manager <" - << sub_ctrl_mng_wrapper->get_full_qualified_name() << ">."); + << sub_ctrl_mng_wrapper->get_name() << ">."); } catch (const std::logic_error & e) { @@ -364,29 +362,23 @@ void ControllerManager::register_sub_controller_manager_srv_cb( } } -void ControllerManager::create_hardware_state_publisher() +void ControllerManager::add_hardware_state_publishers() { - auto state_interface_names = resource_manager_->available_state_interfaces(); + std::vector> state_publishers_vec; + state_publishers_vec.reserve(resource_manager_->available_state_interfaces().size()); + state_publishers_vec = resource_manager_->create_hardware_state_publishers(get_namespace()); - for (const auto & state_interface : state_interface_names) + for (auto const & state_publisher : state_publishers_vec) { try { - RCLCPP_INFO( - get_logger(), "ControllerManager: Creating StatePublisher for interface:<%s>.", - state_interface.c_str()); - auto state_publisher = std::make_shared( - get_namespace(), std::move(std::make_unique( - resource_manager_->claim_state_interface(state_interface)))); - state_interface_state_publisher_map_.insert(std::pair{state_interface, state_publisher}); executor_->add_node(state_publisher->get_node()->get_node_base_interface()); } - catch (const std::exception & e) + catch (const std::runtime_error & e) { RCLCPP_ERROR( - get_logger(), "ControllerManager: Can't create StatePublisher<%s> : %s", - state_interface.c_str(), e.what()); - break; + get_logger(), "ControllerManager: Can't create StatePublishers<%s>", + state_publisher->get_state_interface_name(), e.what()); } } } @@ -399,11 +391,11 @@ void ControllerManager::register_sub_controller_manager() "/register_sub_controller_manager"); std::vector state_interface_names, topic_names; - for (auto it = state_interface_state_publisher_map_.begin(); - it != state_interface_state_publisher_map_.end(); ++it) + + for (auto const & state_publsiher : resource_manager_->get_state_publishers()) { - state_interface_names.push_back(it->second->get_state_interface_name()); - topic_names.push_back(it->second->get_topic_name()); + state_interface_names.push_back(state_publsiher->get_state_interface_name()); + topic_names.push_back(state_publsiher->get_topic_name()); } auto request = diff --git a/controller_manager_msgs/srv/RegisterSubControllerManager.srv b/controller_manager_msgs/srv/RegisterSubControllerManager.srv index 96c7c43e1b..1092226c5e 100644 --- a/controller_manager_msgs/srv/RegisterSubControllerManager.srv +++ b/controller_manager_msgs/srv/RegisterSubControllerManager.srv @@ -10,13 +10,10 @@ # Res: # The response includes: # * a bool if the registration succeeded -# * a the CommandPublishers the central controller manager provides string sub_controller_manager_namespace string sub_controller_manager_name string[] state_interface_names string[] state_publisher_topics --- -bool ok -string[] command_interface_names -string[] command_publishers_topics \ No newline at end of file +bool ok \ No newline at end of file diff --git a/hardware_interface/include/hardware_interface/distributed_control_interface/state_subscriber.hpp b/hardware_interface/include/hardware_interface/distributed_control_interface/state_subscriber.hpp deleted file mode 100644 index e69de29bb2..0000000000 diff --git a/hardware_interface/include/hardware_interface/loaned_state_interface.hpp b/hardware_interface/include/hardware_interface/loaned_state_interface.hpp index 6491e0d79e..524da5c636 100644 --- a/hardware_interface/include/hardware_interface/loaned_state_interface.hpp +++ b/hardware_interface/include/hardware_interface/loaned_state_interface.hpp @@ -27,28 +27,16 @@ class LoanedStateInterface public: using Deleter = std::function; - explicit LoanedStateInterface(std::shared_ptr state_interface) + explicit LoanedStateInterface(std::shared_ptr state_interface) : LoanedStateInterface(state_interface, nullptr) { } - LoanedStateInterface(std::shared_ptr state_interface, Deleter && deleter) + LoanedStateInterface(std::shared_ptr state_interface, Deleter && deleter) : state_interface_(state_interface), deleter_(std::forward(deleter)) { } - explicit LoanedStateInterface( - std::shared_ptr & distributed_state_interface) - : LoanedStateInterface(distributed_state_interface, nullptr) - { - } - - LoanedStateInterface( - std::shared_ptr & distributed_state_interface, Deleter && deleter) - : state_interface_(distributed_state_interface), deleter_(std::forward(deleter)) - { - } - LoanedStateInterface(const LoanedStateInterface & other) = delete; LoanedStateInterface(LoanedStateInterface && other) = default; diff --git a/hardware_interface/include/hardware_interface/resource_manager.hpp b/hardware_interface/include/hardware_interface/resource_manager.hpp index 1693e85574..0be4fac031 100644 --- a/hardware_interface/include/hardware_interface/resource_manager.hpp +++ b/hardware_interface/include/hardware_interface/resource_manager.hpp @@ -21,6 +21,7 @@ #include #include +#include "hardware_interface/distributed_control_interface/state_publisher.hpp" #include "hardware_interface/hardware_component_info.hpp" #include "hardware_interface/hardware_info.hpp" #include "hardware_interface/loaned_command_interface.hpp" @@ -114,6 +115,10 @@ class HARDWARE_INTERFACE_PUBLIC ResourceManager */ bool state_interface_is_available(const std::string & name) const; + std::vector> + create_hardware_state_publishers(const std::string & ns); + + std::vector> get_state_publishers() const; /// Add controllers' reference interfaces to resource manager. /** * Interface for transferring management of reference interfaces to resource manager. diff --git a/hardware_interface/package.xml b/hardware_interface/package.xml index 88f5e3ea13..2f5329ad9c 100644 --- a/hardware_interface/package.xml +++ b/hardware_interface/package.xml @@ -16,7 +16,7 @@ rclcpp_lifecycle rcpputils tinyxml2_vendor - std_msgs + std_msgs rcutils rcutils 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 index 174d3bbfc9..3f870f77ad 100644 --- a/hardware_interface/src/hardware_interface/distributed_control_interface/state_publisher.cpp +++ b/hardware_interface/src/hardware_interface/distributed_control_interface/state_publisher.cpp @@ -31,7 +31,9 @@ std::shared_ptr StatePublisher::get_node() cons { if (!node_.get()) { - throw std::runtime_error("Lifecycle node hasn't been configured yet!"); + std::string msg( + "StatePublisher<" + get_state_interface_name() + ">: Node hasn't been configured yet!"); + throw std::runtime_error(msg); } return node_; } diff --git a/hardware_interface/src/hardware_interface/distributed_control_interface/state_subscriber.cpp b/hardware_interface/src/hardware_interface/distributed_control_interface/state_subscriber.cpp deleted file mode 100644 index e69de29bb2..0000000000 diff --git a/hardware_interface/src/resource_manager.cpp b/hardware_interface/src/resource_manager.cpp index 3e3a160eca..34fd7c0e5b 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 @@ -475,6 +477,30 @@ class ResourceStorage } } + void add_state_publisher(std::shared_ptr state_publisher) + { + state_interface_state_publisher_map_.insert( + std::pair{state_publisher->get_state_interface_name(), state_publisher}); + } + + 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; + } + void check_for_duplicates(const HardwareInfo & hardware_info) { // Check for identical names @@ -567,6 +593,10 @@ class ResourceStorage /// List of all claimed command interfaces std::unordered_map claimed_command_interface_map_; + +protected: + std::map> + state_interface_state_publisher_map_; }; ResourceManager::ResourceManager() : resource_storage_(std::make_unique()) {} @@ -672,6 +702,34 @@ bool ResourceManager::state_interface_is_available(const std::string & name) con name) != resource_storage_->available_state_interfaces_.end(); } +std::vector> +ResourceManager::create_hardware_state_publishers(const std::string & ns) +{ + std::vector> state_publishers_vec; + state_publishers_vec.reserve(available_state_interfaces().size()); + + for (const auto & state_interface : available_state_interfaces()) + { + RCLCPP_INFO( + rclcpp::get_logger("ResourceManager"), "Creating StatePublisher for interface:<%s>.", + state_interface.c_str()); + auto state_publisher = std::make_shared( + ns, std::move(std::make_unique( + claim_state_interface(state_interface)))); + + resource_storage_->add_state_publisher(state_publisher); + state_publishers_vec.push_back(state_publisher); + } + + return state_publishers_vec; +} + +std::vector> +ResourceManager::get_state_publishers() const +{ + return resource_storage_->get_state_publishers(); +} + // CM API: Called in "callback/slow"-thread void ResourceManager::import_controller_reference_interfaces( const std::string & controller_name, std::vector & interfaces) From 513201ae8ce2735ddbeae496e80d9112a6c05994 Mon Sep 17 00:00:00 2001 From: Manuel M Date: Wed, 30 Nov 2022 09:16:17 +0100 Subject: [PATCH 11/19] subscription in central controller * export and imprt statepublisher description * initialize DistributedHandle through description * automated generation of node and topic names --- .../controller_manager/controller_manager.hpp | 4 - .../sub_controller_manager_wrapper.hpp | 75 ------------ controller_manager/src/controller_manager.cpp | 71 ++++++----- controller_manager_msgs/CMakeLists.txt | 2 + controller_manager_msgs/msg/HandleName.msg | 2 + .../msg/StatePublisherDescription.msg | 3 + .../srv/RegisterSubControllerManager.srv | 5 +- hardware_interface/CMakeLists.txt | 1 + .../state_publisher.hpp | 23 +++- .../state_publisher_description.hpp | 47 ++++++++ .../sub_controller_manager_wrapper.hpp | 54 +++++++++ .../include/hardware_interface/handle.hpp | 113 ++++++++++++++++-- .../loaned_command_interface.hpp | 6 +- .../loaned_state_interface.hpp | 19 +-- .../hardware_interface/resource_manager.hpp | 7 ++ hardware_interface/package.xml | 1 + .../state_publisher.cpp | 40 ++++++- hardware_interface/src/resource_manager.cpp | 98 +++++++++++++-- 18 files changed, 416 insertions(+), 155 deletions(-) delete mode 100644 controller_manager/include/controller_manager/distributed_control/sub_controller_manager_wrapper.hpp create mode 100644 controller_manager_msgs/msg/HandleName.msg create mode 100644 controller_manager_msgs/msg/StatePublisherDescription.msg create mode 100644 hardware_interface/include/hardware_interface/distributed_control_interface/state_publisher_description.hpp create mode 100644 hardware_interface/include/hardware_interface/distributed_control_interface/sub_controller_manager_wrapper.hpp diff --git a/controller_manager/include/controller_manager/controller_manager.hpp b/controller_manager/include/controller_manager/controller_manager.hpp index aa02117e7c..8eeef6c713 100644 --- a/controller_manager/include/controller_manager/controller_manager.hpp +++ b/controller_manager/include/controller_manager/controller_manager.hpp @@ -26,7 +26,6 @@ #include "controller_interface/chainable_controller_interface.hpp" #include "controller_interface/controller_interface.hpp" #include "controller_interface/controller_interface_base.hpp" -#include "distributed_control/sub_controller_manager_wrapper.hpp" #include "controller_manager/controller_spec.hpp" #include "controller_manager/visibility_control.h" @@ -54,7 +53,6 @@ #include "rclcpp/node_interfaces/node_logging_interface.hpp" #include "rclcpp/node_interfaces/node_parameters_interface.hpp" #include "rclcpp/parameter.hpp" - namespace controller_manager { using ControllersListIterator = std::vector::const_iterator; @@ -419,8 +417,6 @@ class ControllerManager : public rclcpp::Node bool distributed_ = false; bool sub_controller_manager_ = false; - std::map> - sub_controller_manager_map_; rclcpp::CallbackGroup::SharedPtr distributed_system_srv_callback_group_; /** * The RTControllerListWrapper class wraps a double-buffered list of controllers diff --git a/controller_manager/include/controller_manager/distributed_control/sub_controller_manager_wrapper.hpp b/controller_manager/include/controller_manager/distributed_control/sub_controller_manager_wrapper.hpp deleted file mode 100644 index 18cee4f5ac..0000000000 --- a/controller_manager/include/controller_manager/distributed_control/sub_controller_manager_wrapper.hpp +++ /dev/null @@ -1,75 +0,0 @@ -#ifndef DISTRIBUTED_CONTROL__SUB_CONTROLLER_MANAGER_WRAPPER_HPP_ -#define DISTRIBUTED_CONTROL__SUB_CONTROLLER_MANAGER_WRAPPER_HPP_ - -#include -#include -#include -#include -#include - -namespace distributed_control -{ - -class SubControllerManagerWrapper final -{ -public: - explicit SubControllerManagerWrapper( - const std::string & ns, const std::string & name, - const std::vector state_interface_names, std::vector topic_names) - : NAMESPACE_(ns), - NAME_(name), - state_interface_names_(std::move(state_interface_names)), - topic_names_(std::move(topic_names)) - { - if (state_interface_names_.size() != topic_names_.size()) - { - throw std::logic_error( - "SubControllerManagerWrapper: state_interfaces_names.size() != topic_names.size(). " - "Must be equal."); - } - for (size_t i = 0; i < state_interface_names_.size(); ++i) - { - const auto [it, success] = state_interface_topic_name_map_.insert( - std::pair{state_interface_names_.at(i), topic_names_.at(i)}); - if (!success) - { - throw std::logic_error( - "SubControllerManagerWrapper: Duplicate of given state_interface_name:'" + it->first + - "'. Must be unique."); - } - } - } - - ~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_name(); } - - std::vector get_state_interface_names() const { return state_interface_names_; } - - std::vector get_topic_names() const { return topic_names_; } - - std::string get_topic_for_state_interface(const std::string & state_interface_name) - { - if (auto element = state_interface_topic_name_map_.find(state_interface_name); - element != state_interface_topic_name_map_.end()) - { - return element->second; - } - throw std::runtime_error("SubControllerManagerWrapper: No state_interface found."); - } - -private: - const std::string NAMESPACE_; - const std::string NAME_; - std::vector state_interface_names_; - std::vector topic_names_; - std::map state_interface_topic_name_map_; -}; - -} // namespace distributed_control - -#endif // DISTRIBUTED_CONTROL__SUB_CONTROLLER_MANAGER_WRAPPER_HPP_ \ No newline at end of file diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 18b9350fb6..6a5a239a79 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -23,7 +23,10 @@ #include "controller_interface/controller_interface_base.hpp" #include "controller_manager_msgs/msg/hardware_component_state.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" @@ -341,25 +344,36 @@ void ControllerManager::register_sub_controller_manager_srv_cb( std::shared_ptr response) { std::lock_guard guard(central_controller_manager_srv_lock_); - try - { - auto sub_ctrl_mng_wrapper = std::make_shared( - request->sub_controller_manager_namespace, request->sub_controller_manager_name, - request->state_interface_names, request->state_publisher_topics); - sub_controller_manager_map_.insert( - std::pair{sub_ctrl_mng_wrapper->get_name(), sub_ctrl_mng_wrapper}); - response->ok = true; - RCLCPP_INFO_STREAM( - get_logger(), "ControllerManager: Registered sub controller manager <" - << sub_ctrl_mng_wrapper->get_name() << ">."); - } - catch (const std::logic_error & e) + auto sub_ctrl_mng_wrapper = std::make_shared( + request->sub_controller_manager_namespace, request->sub_controller_manager_name, + request->state_publishers); + + std::vector> state_interfaces; + state_interfaces.reserve(sub_ctrl_mng_wrapper->get_state_publisher_count()); + state_interfaces = resource_manager_->register_sub_controller_manager(sub_ctrl_mng_wrapper); + + for (const auto & state_interface : state_interfaces) { - response->ok = false; - RCLCPP_ERROR( - get_logger(), "ControllerManager: Cannot register sub controller manager. %s", e.what()); + try + { + executor_->add_node(state_interface->get_node()->get_node_base_interface()); + } + catch (const std::runtime_error & e) + { + response->ok = false; + RCLCPP_WARN_STREAM( + get_logger(), + "ControllerManager: Caught exception while trying to register sub controller manager. " + "Exception:" + << e.what()); + } } + + response->ok = true; + RCLCPP_INFO_STREAM( + get_logger(), "ControllerManager: Registered sub controller manager <" + << sub_ctrl_mng_wrapper->get_name() << ">."); } void ControllerManager::add_hardware_state_publishers() @@ -376,9 +390,9 @@ void ControllerManager::add_hardware_state_publishers() } catch (const std::runtime_error & e) { - RCLCPP_ERROR( - get_logger(), "ControllerManager: Can't create StatePublishers<%s>", - state_publisher->get_state_interface_name(), e.what()); + RCLCPP_WARN_STREAM( + get_logger(), "ControllerManager: Can't create StatePublishers<" + << state_publisher->state_interface_name() << ">." << e.what()); } } } @@ -390,20 +404,19 @@ void ControllerManager::register_sub_controller_manager() create_client( "/register_sub_controller_manager"); - std::vector state_interface_names, topic_names; - - for (auto const & state_publsiher : resource_manager_->get_state_publishers()) - { - state_interface_names.push_back(state_publsiher->get_state_interface_name()); - topic_names.push_back(state_publsiher->get_topic_name()); - } - auto request = std::make_shared(); request->sub_controller_manager_namespace = get_namespace(); request->sub_controller_manager_name = get_name(); - request->state_interface_names = std::move(state_interface_names); - request->state_publisher_topics = std::move(topic_names); + + // 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_description_msg()); + } using namespace std::chrono_literals; while (!client->wait_for_service(1s)) diff --git a/controller_manager_msgs/CMakeLists.txt b/controller_manager_msgs/CMakeLists.txt index e5f92a8eae..99717550ba 100644 --- a/controller_manager_msgs/CMakeLists.txt +++ b/controller_manager_msgs/CMakeLists.txt @@ -13,8 +13,10 @@ find_package(rosidl_default_generators REQUIRED) set(msg_files msg/ControllerState.msg msg/ChainConnection.msg + msg/HandleName.msg msg/HardwareComponentState.msg msg/HardwareInterface.msg + msg/StatePublisherDescription.msg ) set(srv_files srv/ConfigureController.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/StatePublisherDescription.msg b/controller_manager_msgs/msg/StatePublisherDescription.msg new file mode 100644 index 0000000000..a9788f91b3 --- /dev/null +++ b/controller_manager_msgs/msg/StatePublisherDescription.msg @@ -0,0 +1,3 @@ +string ns # the namespace the StatePublisher is in +controller_manager_msgs/HandleName name # the full qualified name (prefix_name + interface_name) of the StateInterface +string state_publisher_topic # the topic via which the values are published. \ No newline at end of file diff --git a/controller_manager_msgs/srv/RegisterSubControllerManager.srv b/controller_manager_msgs/srv/RegisterSubControllerManager.srv index 1092226c5e..55d2af572b 100644 --- a/controller_manager_msgs/srv/RegisterSubControllerManager.srv +++ b/controller_manager_msgs/srv/RegisterSubControllerManager.srv @@ -5,7 +5,7 @@ # To register a controller the distributed sub controller manager has to # define: # * the full namespace it operates in -# * the StatePublisher it has +# * all statePublishers the sub controller provides # Res: # The response includes: @@ -13,7 +13,6 @@ string sub_controller_manager_namespace string sub_controller_manager_name -string[] state_interface_names -string[] state_publisher_topics +controller_manager_msgs/StatePublisherDescription[] state_publishers --- bool ok \ No newline at end of file diff --git a/hardware_interface/CMakeLists.txt b/hardware_interface/CMakeLists.txt index ed48a5e852..701ef4c104 100644 --- a/hardware_interface/CMakeLists.txt +++ b/hardware_interface/CMakeLists.txt @@ -7,6 +7,7 @@ endif() set(THIS_PACKAGE_INCLUDE_DEPENDS control_msgs + controller_manager_msgs lifecycle_msgs pluginlib rclcpp 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 index f6318e3cf9..3b939066e5 100644 --- a/hardware_interface/include/hardware_interface/distributed_control_interface/state_publisher.hpp +++ b/hardware_interface/include/hardware_interface/distributed_control_interface/state_publisher.hpp @@ -9,6 +9,8 @@ #include "hardware_interface/loaned_state_interface.hpp" +#include "controller_manager_msgs/msg/state_publisher_description.hpp" + #include "rclcpp/rclcpp.hpp" #include "rclcpp_lifecycle/lifecycle_node.hpp" #include "rclcpp_lifecycle/state.hpp" @@ -17,7 +19,7 @@ namespace distributed_control { -class StatePublisher final : public rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface +class StatePublisher final { public: explicit StatePublisher( @@ -30,10 +32,25 @@ class StatePublisher final : public rclcpp_lifecycle::node_interfaces::Lifecycle std::shared_ptr get_node() const; STATE_PUBLISHER_PUBLIC - std::string get_topic_name() const; + std::string get_namespace() const; + + STATE_PUBLISHER_PUBLIC + std::string topic_name() const; + + STATE_PUBLISHER_PUBLIC + std::string topic_name_relative_to_namespace() const; + + STATE_PUBLISHER_PUBLIC + std::string state_interface_name() const; + + STATE_PUBLISHER_PUBLIC + std::string state_interface_prefix_name() const; + + STATE_PUBLISHER_PUBLIC + std::string state_interface_interface_name() const; STATE_PUBLISHER_PUBLIC - std::string get_state_interface_name() const; + controller_manager_msgs::msg::StatePublisherDescription create_description_msg() const; private: void publish_value_on_timer(); diff --git a/hardware_interface/include/hardware_interface/distributed_control_interface/state_publisher_description.hpp b/hardware_interface/include/hardware_interface/distributed_control_interface/state_publisher_description.hpp new file mode 100644 index 0000000000..03910df928 --- /dev/null +++ b/hardware_interface/include/hardware_interface/distributed_control_interface/state_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/state_publisher_description.hpp" + +#include "rclcpp/rclcpp.hpp" + +namespace distributed_control +{ + +class StatePublisherDescription final +{ +public: + explicit StatePublisherDescription( + const controller_manager_msgs::msg::StatePublisherDescription & description) + : namespace_(description.ns), + prefix_name_(description.name.prefix_name), + interface_name_(description.name.interface_name), + topic_name_(description.state_publisher_topic) + { + } + + StatePublisherDescription() = delete; + + StatePublisherDescription(const StatePublisherDescription & other) = default; + + StatePublisherDescription(StatePublisherDescription && other) = default; + + ~StatePublisherDescription() {} + + 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/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..4ff833a376 --- /dev/null +++ b/hardware_interface/include/hardware_interface/distributed_control_interface/sub_controller_manager_wrapper.hpp @@ -0,0 +1,54 @@ +#ifndef DISTRIBUTED_CONTROL__SUB_CONTROLLER_MANAGER_WRAPPER_HPP_ +#define DISTRIBUTED_CONTROL__SUB_CONTROLLER_MANAGER_WRAPPER_HPP_ + +#include +#include +#include + +#include "controller_manager_msgs/msg/state_publisher_description.hpp" + +#include "hardware_interface/distributed_control_interface/state_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) + : NAMESPACE_(ns), + NAME_(name), + state_publisher_descriptions_({state_publishers.begin(), state_publishers.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_; + } + + size_t get_state_publisher_count() const { return state_publisher_descriptions_.size(); } + +private: + const std::string NAMESPACE_; + const std::string NAME_; + std::vector state_publisher_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 35ec5d64da..e3d9f148bb 100644 --- a/hardware_interface/include/hardware_interface/handle.hpp +++ b/hardware_interface/include/hardware_interface/handle.hpp @@ -18,8 +18,10 @@ #include #include +#include "hardware_interface/distributed_control_interface/state_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" @@ -71,9 +73,7 @@ class HandleInterface /// 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 @@ -82,17 +82,36 @@ class HandleInterface return get_name(); } + virtual std::string get_name() const { return prefix_name_ + "/" + interface_name_; } + + // TODO(Manuel): Maybe not the best place to put... But if put in DistributedHandles we + // violate Liskov std::shared_ptr get_node() const { THROW_ON_NULLPTR(node_); if (!node_.get()) { - throw std::runtime_error("DistributedReadOnlyHandle: Node not initialized!"); + throw std::runtime_error("Node not initialized!"); } return node_; } - const std::string & get_prefix_name() const { return prefix_name_; } + 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 + { + if (get_prefix_name().empty()) + { + return get_interface_name(); + } + return get_prefix_name() + "_" + get_interface_name(); + } protected: std::string prefix_name_; @@ -175,15 +194,21 @@ class DistributedReadOnlyHandle : public ReadOnlyHandle // 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 std::string & prefix_name, const std::string & interface_name, - const std::string & topic_name) - : ReadOnlyHandle(prefix_name, interface_name, &value_), topic_name_(topic_name) + const distributed_control::StatePublisherDescription & description, + const std::string & ns = "/") + : ReadOnlyHandle(description.prefix_name(), description.interface_name(), &value_), + topic_name_(description.topic_name()), + namespace_(ns), + interface_namespace_(description.get_namespace()) { rclcpp::NodeOptions node_options; + // create node for subscribing to StatePublisher described in StatePublisherDescription node_ = std::make_shared( - get_interface_name() + "_node", get_prefix_name(), node_options, false); + get_underscore_separated_name() + "_subscriber", namespace_, node_options, false); + + // subscribe to topic provided by StatePublisher state_value_subscription_ = node_->create_subscription( - topic_name, 10, + topic_name_, 10, std::bind(&DistributedReadOnlyHandle::set_value_cb, this, std::placeholders::_1)); } @@ -196,7 +221,9 @@ class DistributedReadOnlyHandle : public ReadOnlyHandle { } - DistributedReadOnlyHandle(const DistributedReadOnlyHandle & other) = default; + DistributedReadOnlyHandle() = delete; + + DistributedReadOnlyHandle(const DistributedReadOnlyHandle & other) = delete; DistributedReadOnlyHandle(DistributedReadOnlyHandle && other) = default; @@ -206,10 +233,71 @@ class DistributedReadOnlyHandle : public ReadOnlyHandle virtual ~DistributedReadOnlyHandle() = default; + virtual std::string get_name() const override + { + // append '/' to input string if not empty + auto append_slash = [](std::string str) + { + if (!str.empty()) + { + return str + "/"; + } + return str; + }; + // concatenate: interface_namespace/prefix_name/interface_name + return append_slash(interface_namespace_) + append_slash(get_prefix_name()) + + get_interface_name(); + } + + virtual std::string get_underscore_separated_name() const override + { + // append '_' to input string if not empty + auto append_underscore = [](std::string str) + { + if (!str.empty()) + { + return str + "_"; + } + return str; + }; + // remove first "/" from namespace and replace all follow occurrences with "_" + std::string ns = removeCharsFromString(erase_slash_at_begin(interface_namespace_), '/', '_'); + // concatenate: interface_namespace + _ + namespace_prefix + _ + name_interface_name + return append_underscore(ns) + append_underscore(get_prefix_name()) + get_interface_name(); + } + protected: - void set_value_cb(const std_msgs::msg::Float64 & msg) { value_ = msg.data; } + void set_value_cb(const std_msgs::msg::Float64 & msg) + { + value_ = msg.data; + RCLCPP_WARN_STREAM(node_->get_logger(), "Receiving:[" << value_ << "]."); + } + + std::string erase_slash_at_begin(std::string str) const + { + if (!str.empty()) + { + if (str.at(0) == '/') + { + return str.erase(0, 1); + } + } + return str; + } + + std::string removeCharsFromString( + 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 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_subscription_; double value_; }; @@ -226,6 +314,7 @@ class StateInterface : public ReadOnlyHandle class DistributedStateInterface : public DistributedReadOnlyHandle { +public: DistributedStateInterface(const DistributedStateInterface & other) = default; DistributedStateInterface(DistributedStateInterface && other) = default; diff --git a/hardware_interface/include/hardware_interface/loaned_command_interface.hpp b/hardware_interface/include/hardware_interface/loaned_command_interface.hpp index bb5807c398..1ae1a8d3f1 100644 --- a/hardware_interface/include/hardware_interface/loaned_command_interface.hpp +++ b/hardware_interface/include/hardware_interface/loaned_command_interface.hpp @@ -50,9 +50,9 @@ 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 @@ -61,7 +61,7 @@ class LoanedCommandInterface 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); } diff --git a/hardware_interface/include/hardware_interface/loaned_state_interface.hpp b/hardware_interface/include/hardware_interface/loaned_state_interface.hpp index 524da5c636..ed4d3bcbd5 100644 --- a/hardware_interface/include/hardware_interface/loaned_state_interface.hpp +++ b/hardware_interface/include/hardware_interface/loaned_state_interface.hpp @@ -49,9 +49,9 @@ 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 @@ -60,20 +60,13 @@ class LoanedStateInterface return state_interface_->get_name(); } - const std::string get_underscore_separated_name() const - { - std::string prefix = get_prefix_name(); - std::string interface = get_interface_name(); + std::string get_prefix_name() const { return state_interface_->get_prefix_name(); } - if (prefix.empty()) - { - return interface; - } - return prefix + "_" + interface; + std::string get_underscore_separated_name() const + { + return state_interface_->get_underscore_separated_name(); } - const std::string & get_prefix_name() const { return state_interface_->get_prefix_name(); } - double get_value() const { return state_interface_->get_value(); } protected: diff --git a/hardware_interface/include/hardware_interface/resource_manager.hpp b/hardware_interface/include/hardware_interface/resource_manager.hpp index 0be4fac031..2d0f3476b3 100644 --- a/hardware_interface/include/hardware_interface/resource_manager.hpp +++ b/hardware_interface/include/hardware_interface/resource_manager.hpp @@ -22,11 +22,14 @@ #include #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" @@ -115,10 +118,14 @@ class HARDWARE_INTERFACE_PUBLIC ResourceManager */ bool state_interface_is_available(const std::string & name) const; + std::vector> register_sub_controller_manager( + std::shared_ptr sub_controller_manager); + std::vector> create_hardware_state_publishers(const std::string & ns); std::vector> get_state_publishers() const; + /// Add controllers' reference interfaces to resource manager. /** * Interface for transferring management of reference interfaces to resource manager. diff --git a/hardware_interface/package.xml b/hardware_interface/package.xml index 2f5329ad9c..fc1779f601 100644 --- a/hardware_interface/package.xml +++ b/hardware_interface/package.xml @@ -10,6 +10,7 @@ ament_cmake control_msgs + controller_manager_msgs lifecycle_msgs pluginlib rclcpp 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 index 3f870f77ad..6c98691862 100644 --- a/hardware_interface/src/hardware_interface/distributed_control_interface/state_publisher.cpp +++ b/hardware_interface/src/hardware_interface/distributed_control_interface/state_publisher.cpp @@ -20,7 +20,7 @@ StatePublisher::StatePublisher( { rclcpp::NodeOptions node_options; node_ = std::make_shared( - loaned_state_interface_ptr_->get_underscore_separated_name() + "_node", namespace_, + loaned_state_interface_ptr_->get_underscore_separated_name() + "_publisher", namespace_, node_options, false); state_value_pub_ = node_->create_publisher(topic_name_, 10); timer_ = node_->create_wall_timer(50ms, std::bind(&StatePublisher::publish_value_on_timer, this)); @@ -32,19 +32,51 @@ std::shared_ptr StatePublisher::get_node() cons if (!node_.get()) { std::string msg( - "StatePublisher<" + get_state_interface_name() + ">: Node hasn't been configured yet!"); + "StatePublisher<" + state_interface_name() + ">: Node hasn't been configured yet!"); throw std::runtime_error(msg); } return node_; } -std::string StatePublisher::get_topic_name() const { return topic_name_; } +std::string StatePublisher::get_namespace() const { return namespace_; } -std::string StatePublisher::get_state_interface_name() const +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::StatePublisherDescription StatePublisher::create_description_msg() + const +{ + auto msg = controller_manager_msgs::msg::StatePublisherDescription(); + // we want a unique name for every StatePublisher. This gets relevant in the central ControllerManager + // where multiple sub ControllerManager are registering. Therefor we add the namespace to the prefix. + // However since that e.g. joint1/positions becomes /sub_namespace/joint1/position + msg.ns = get_namespace(); + msg.name.prefix_name = state_interface_prefix_name(); + msg.name.interface_name = state_interface_interface_name(); + msg.state_publisher_topic = topic_name_relative_to_namespace(); + + return msg; +} + void StatePublisher::publish_value_on_timer() { // Todo(Manuel) create custom msg and return success or failure not just nan. diff --git a/hardware_interface/src/resource_manager.cpp b/hardware_interface/src/resource_manager.cpp index 34fd7c0e5b..fa31a61b9f 100644 --- a/hardware_interface/src/resource_manager.cpp +++ b/hardware_interface/src/resource_manager.cpp @@ -409,18 +409,42 @@ class ResourceStorage return result; } + void add_state_interface(std::shared_ptr state_interface) + { + const auto [it, success] = + state_interface_map_.insert(std::make_pair(state_interface->get_name(), state_interface)); + if (!success) + { + std::string msg( + "ResourceStorage: Tried to insert StateInterface with already existing key. Insert[" + + state_interface->get_name() + "]"); + throw std::runtime_error(msg); + } + } + + void add_state_interface(const StateInterface & state_interface) + { + const auto [it, success] = state_interface_map_.emplace(std::make_pair( + state_interface.get_name(), std::make_shared(state_interface))); + if (!success) + { + std::string msg( + "ResourceStorage: Tried to insert StateInterface with already existing key. Insert[" + + state_interface.get_name() + "]"); + throw std::runtime_error(msg); + } + } + template void import_state_interfaces(HardwareT & hardware) { auto interfaces = hardware.export_state_interfaces(); std::vector interface_names; interface_names.reserve(interfaces.size()); - for (auto & interface : interfaces) + for (auto const & interface : interfaces) { - auto key = interface.get_name(); - state_interface_map_.emplace( - std::make_pair(key, std::make_shared(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( @@ -479,8 +503,15 @@ class ResourceStorage void add_state_publisher(std::shared_ptr state_publisher) { - state_interface_state_publisher_map_.insert( - std::pair{state_publisher->get_state_interface_name(), 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); + } } std::shared_ptr get_state_publisher( @@ -565,6 +596,40 @@ class ResourceStorage import_command_interfaces(systems_.back()); } + void add_sub_controller_manager( + std::shared_ptr sub_controller_manager) + { + const auto [it, success] = sub_controller_manager_map_.insert( + std::pair{sub_controller_manager->get_name(), sub_controller_manager}); + if (!success) + { + std::string msg( + "ResourceStorage: Tried to add sub ControllerManager with already existing key. Insert[" + + sub_controller_manager->get_name() + "]"); + throw std::runtime_error(msg); + } + } + + std::vector> + import_state_interfaces_of_sub_controller_manager( + std::shared_ptr sub_controller_manager) + { + std::vector> distributed_state_interfaces; + distributed_state_interfaces.reserve(sub_controller_manager->get_state_publisher_count()); + + for (const auto & state_publisher_description : + sub_controller_manager->get_state_publisher_descriptions()) + { + // create StateInterface from the Description and store in ResourceStorage. + auto state_interface = + std::make_shared(state_publisher_description); + add_state_interface(state_interface); + // add to return vector, node needs to added to executor. + distributed_state_interfaces.push_back(state_interface); + } + return distributed_state_interfaces; + } + // hardware plugins pluginlib::ClassLoader actuator_loader_; pluginlib::ClassLoader sensor_loader_; @@ -594,9 +659,12 @@ class ResourceStorage /// List of all claimed command interfaces std::unordered_map claimed_command_interface_map_; -protected: +private: std::map> state_interface_state_publisher_map_; + + std::map> + sub_controller_manager_map_; }; ResourceManager::ResourceManager() : resource_storage_(std::make_unique()) {} @@ -702,9 +770,20 @@ bool ResourceManager::state_interface_is_available(const std::string & name) con name) != resource_storage_->available_state_interfaces_.end(); } +std::vector> +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); + return resource_storage_->import_state_interfaces_of_sub_controller_manager( + sub_controller_manager); +} + std::vector> ResourceManager::create_hardware_state_publishers(const std::string & ns) { + std::lock_guard guard(resource_interfaces_lock_); std::vector> state_publishers_vec; state_publishers_vec.reserve(available_state_interfaces().size()); @@ -720,13 +799,14 @@ ResourceManager::create_hardware_state_publishers(const std::string & ns) resource_storage_->add_state_publisher(state_publisher); state_publishers_vec.push_back(state_publisher); } - + return state_publishers_vec; } std::vector> ResourceManager::get_state_publishers() const { + std::lock_guard guard(resource_interfaces_lock_); return resource_storage_->get_state_publishers(); } From 4f5d16e7125aad42feb5590714f30a040e4820f2 Mon Sep 17 00:00:00 2001 From: Manuel M Date: Wed, 30 Nov 2022 09:26:42 +0100 Subject: [PATCH 12/19] rename miss leading name of function --- hardware_interface/include/hardware_interface/handle.hpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/hardware_interface/include/hardware_interface/handle.hpp b/hardware_interface/include/hardware_interface/handle.hpp index e3d9f148bb..e7634b3cbf 100644 --- a/hardware_interface/include/hardware_interface/handle.hpp +++ b/hardware_interface/include/hardware_interface/handle.hpp @@ -261,7 +261,8 @@ class DistributedReadOnlyHandle : public ReadOnlyHandle return str; }; // remove first "/" from namespace and replace all follow occurrences with "_" - std::string ns = removeCharsFromString(erase_slash_at_begin(interface_namespace_), '/', '_'); + std::string ns = + replace_all_chars_from_string(erase_slash_at_begin(interface_namespace_), '/', '_'); // concatenate: interface_namespace + _ + namespace_prefix + _ + name_interface_name return append_underscore(ns) + append_underscore(get_prefix_name()) + get_interface_name(); } @@ -285,7 +286,7 @@ class DistributedReadOnlyHandle : public ReadOnlyHandle return str; } - std::string removeCharsFromString( + 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); From de73b6ac834c4e6cc393478eead9d89840832381 Mon Sep 17 00:00:00 2001 From: Manuel M Date: Wed, 30 Nov 2022 11:29:38 +0100 Subject: [PATCH 13/19] rename StatePublisherDescription to PublisherDescription --- controller_manager_msgs/CMakeLists.txt | 2 +- .../msg/PublisherDescription.msg | 3 + .../msg/StatePublisherDescription.msg | 3 - .../srv/RegisterSubControllerManager.srv | 4 +- ...cription.hpp => publisher_description.hpp} | 18 +-- .../state_publisher.hpp | 6 +- .../sub_controller_manager_wrapper.hpp | 10 +- .../include/hardware_interface/handle.hpp | 142 ++++++++++++------ .../state_publisher.cpp | 7 +- 9 files changed, 127 insertions(+), 68 deletions(-) create mode 100644 controller_manager_msgs/msg/PublisherDescription.msg delete mode 100644 controller_manager_msgs/msg/StatePublisherDescription.msg rename hardware_interface/include/hardware_interface/distributed_control_interface/{state_publisher_description.hpp => publisher_description.hpp} (61%) diff --git a/controller_manager_msgs/CMakeLists.txt b/controller_manager_msgs/CMakeLists.txt index 99717550ba..0225d91881 100644 --- a/controller_manager_msgs/CMakeLists.txt +++ b/controller_manager_msgs/CMakeLists.txt @@ -16,7 +16,7 @@ set(msg_files msg/HandleName.msg msg/HardwareComponentState.msg msg/HardwareInterface.msg - msg/StatePublisherDescription.msg + msg/PublisherDescription.msg ) set(srv_files srv/ConfigureController.srv 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/msg/StatePublisherDescription.msg b/controller_manager_msgs/msg/StatePublisherDescription.msg deleted file mode 100644 index a9788f91b3..0000000000 --- a/controller_manager_msgs/msg/StatePublisherDescription.msg +++ /dev/null @@ -1,3 +0,0 @@ -string ns # the namespace the StatePublisher is in -controller_manager_msgs/HandleName name # the full qualified name (prefix_name + interface_name) of the StateInterface -string state_publisher_topic # the topic via which the values are published. \ No newline at end of file diff --git a/controller_manager_msgs/srv/RegisterSubControllerManager.srv b/controller_manager_msgs/srv/RegisterSubControllerManager.srv index 55d2af572b..97aaf1f0cf 100644 --- a/controller_manager_msgs/srv/RegisterSubControllerManager.srv +++ b/controller_manager_msgs/srv/RegisterSubControllerManager.srv @@ -13,6 +13,8 @@ string sub_controller_manager_namespace string sub_controller_manager_name -controller_manager_msgs/StatePublisherDescription[] state_publishers +controller_manager_msgs/PublisherDescription[] state_publishers +controller_manager_msgs/PublisherDescription[] command_state_publishers --- +controller_manager_msgs/PublisherDescription[] command_state_publishers bool ok \ No newline at end of file diff --git a/hardware_interface/include/hardware_interface/distributed_control_interface/state_publisher_description.hpp b/hardware_interface/include/hardware_interface/distributed_control_interface/publisher_description.hpp similarity index 61% rename from hardware_interface/include/hardware_interface/distributed_control_interface/state_publisher_description.hpp rename to hardware_interface/include/hardware_interface/distributed_control_interface/publisher_description.hpp index 03910df928..2397d47a1b 100644 --- a/hardware_interface/include/hardware_interface/distributed_control_interface/state_publisher_description.hpp +++ b/hardware_interface/include/hardware_interface/distributed_control_interface/publisher_description.hpp @@ -1,32 +1,32 @@ #ifndef DISTRIBUTED_CONTROL__STATE_PUBLISHER_DESCRIPTION_HPP_ #define DISTRIBUTED_CONTROL__STATE_PUBLISHER_DESCRIPTION_HPP_ -#include "controller_manager_msgs/msg/state_publisher_description.hpp" +#include "controller_manager_msgs/msg/publisher_description.hpp" #include "rclcpp/rclcpp.hpp" namespace distributed_control { -class StatePublisherDescription final +class PublisherDescription final { public: - explicit StatePublisherDescription( - const controller_manager_msgs::msg::StatePublisherDescription & description) + 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.state_publisher_topic) + topic_name_(description.publisher_topic) { } - StatePublisherDescription() = delete; + PublisherDescription() = delete; - StatePublisherDescription(const StatePublisherDescription & other) = default; + PublisherDescription(const PublisherDescription & other) = default; - StatePublisherDescription(StatePublisherDescription && other) = default; + PublisherDescription(PublisherDescription && other) = default; - ~StatePublisherDescription() {} + ~PublisherDescription() {} std::string get_namespace() const { return namespace_; } 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 index 3b939066e5..0089860a66 100644 --- a/hardware_interface/include/hardware_interface/distributed_control_interface/state_publisher.hpp +++ b/hardware_interface/include/hardware_interface/distributed_control_interface/state_publisher.hpp @@ -9,7 +9,7 @@ #include "hardware_interface/loaned_state_interface.hpp" -#include "controller_manager_msgs/msg/state_publisher_description.hpp" +#include "controller_manager_msgs/msg/publisher_description.hpp" #include "rclcpp/rclcpp.hpp" #include "rclcpp_lifecycle/lifecycle_node.hpp" @@ -19,7 +19,7 @@ namespace distributed_control { -class StatePublisher final +class StatePublisher final { public: explicit StatePublisher( @@ -50,7 +50,7 @@ class StatePublisher final std::string state_interface_interface_name() const; STATE_PUBLISHER_PUBLIC - controller_manager_msgs::msg::StatePublisherDescription create_description_msg() const; + controller_manager_msgs::msg::PublisherDescription create_description_msg() const; private: void publish_value_on_timer(); 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 index 4ff833a376..6933bf7dd7 100644 --- 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 @@ -5,9 +5,9 @@ #include #include -#include "controller_manager_msgs/msg/state_publisher_description.hpp" +#include "controller_manager_msgs/msg/publisher_description.hpp" -#include "hardware_interface/distributed_control_interface/state_publisher_description.hpp" +#include "hardware_interface/distributed_control_interface/publisher_description.hpp" namespace distributed_control { @@ -17,7 +17,7 @@ class SubControllerManagerWrapper final public: explicit SubControllerManagerWrapper( const std::string & ns, const std::string & name, - const std::vector & state_publishers) + const std::vector & state_publishers) : NAMESPACE_(ns), NAME_(name), state_publisher_descriptions_({state_publishers.begin(), state_publishers.end()}) @@ -36,7 +36,7 @@ class SubControllerManagerWrapper final std::string get_name() const { return get_namespace() + "/" + get_controller_manager_name(); } - std::vector get_state_publisher_descriptions() const + std::vector get_state_publisher_descriptions() const { return state_publisher_descriptions_; } @@ -46,7 +46,7 @@ class SubControllerManagerWrapper final private: const std::string NAMESPACE_; const std::string NAME_; - std::vector state_publisher_descriptions_; + std::vector state_publisher_descriptions_; }; } // namespace distributed_control diff --git a/hardware_interface/include/hardware_interface/handle.hpp b/hardware_interface/include/hardware_interface/handle.hpp index e7634b3cbf..4163377faa 100644 --- a/hardware_interface/include/hardware_interface/handle.hpp +++ b/hardware_interface/include/hardware_interface/handle.hpp @@ -18,7 +18,7 @@ #include #include -#include "hardware_interface/distributed_control_interface/state_publisher_description.hpp" +#include "hardware_interface/distributed_control_interface/publisher_description.hpp" #include "hardware_interface/macros.hpp" #include "hardware_interface/visibility_control.h" @@ -151,41 +151,14 @@ class ReadOnlyHandle : public HandleInterface, ReadHandleInterface } }; -class ReadWriteHandle : public HandleInterface, ReadHandleInterface, WriteHandleInterface +class StateInterface : public ReadOnlyHandle { public: - ReadWriteHandle( - const std::string & prefix_name, const std::string & interface_name, - double * value_ptr = nullptr) - : HandleInterface(prefix_name, interface_name, value_ptr) - { - } - - explicit ReadWriteHandle(const std::string & interface_name) : HandleInterface(interface_name) {} - - explicit ReadWriteHandle(const char * interface_name) : HandleInterface(interface_name) {} - - ReadWriteHandle(const ReadWriteHandle & other) = default; - - ReadWriteHandle(ReadWriteHandle && other) = default; - - ReadWriteHandle & operator=(const ReadWriteHandle & other) = default; - - ReadWriteHandle & operator=(ReadWriteHandle && other) = default; - - virtual ~ReadWriteHandle() = default; + StateInterface(const StateInterface & other) = default; - double get_value() const override - { - THROW_ON_NULLPTR(value_ptr_); - return *value_ptr_; - } + StateInterface(StateInterface && other) = default; - void set_value(double value) override - { - THROW_ON_NULLPTR(this->value_ptr_); - *this->value_ptr_ = value; - } + using ReadOnlyHandle::ReadOnlyHandle; }; class DistributedReadOnlyHandle : public ReadOnlyHandle @@ -194,8 +167,7 @@ class DistributedReadOnlyHandle : public ReadOnlyHandle // 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::StatePublisherDescription & description, - const std::string & ns = "/") + const distributed_control::PublisherDescription & description, const std::string & ns = "/") : ReadOnlyHandle(description.prefix_name(), description.interface_name(), &value_), topic_name_(description.topic_name()), namespace_(ns), @@ -303,24 +275,51 @@ class DistributedReadOnlyHandle : public ReadOnlyHandle double value_; }; -class StateInterface : public ReadOnlyHandle +class DistributedStateInterface : public DistributedReadOnlyHandle { public: - StateInterface(const StateInterface & other) = default; + DistributedStateInterface(const DistributedStateInterface & other) = default; - StateInterface(StateInterface && other) = default; + DistributedStateInterface(DistributedStateInterface && other) = default; - using ReadOnlyHandle::ReadOnlyHandle; + using DistributedReadOnlyHandle::DistributedReadOnlyHandle; }; -class DistributedStateInterface : public DistributedReadOnlyHandle +class ReadWriteHandle : public HandleInterface, ReadHandleInterface, WriteHandleInterface { public: - DistributedStateInterface(const DistributedStateInterface & other) = default; + ReadWriteHandle( + const std::string & prefix_name, const std::string & interface_name, + double * value_ptr = nullptr) + : HandleInterface(prefix_name, interface_name, value_ptr) + { + } - DistributedStateInterface(DistributedStateInterface && other) = default; + explicit ReadWriteHandle(const std::string & interface_name) : HandleInterface(interface_name) {} - using DistributedReadOnlyHandle::DistributedReadOnlyHandle; + 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; + + double get_value() const override + { + THROW_ON_NULLPTR(value_ptr_); + return *value_ptr_; + } + + void set_value(double value) override + { + THROW_ON_NULLPTR(this->value_ptr_); + *this->value_ptr_ = value; + } }; class CommandInterface : public ReadWriteHandle @@ -338,6 +337,65 @@ class CommandInterface : public ReadWriteHandle using ReadWriteHandle::ReadWriteHandle; }; + +class DistributedReadWriteHandle : public ReadWriteHandle +{ +public: + DistributedReadWriteHandle( + const std::string & prefix_name, const std::string & interface_name, + double * value_ptr = nullptr) + : ReadWriteHandle(prefix_name, interface_name, value_ptr) + { + } + + 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; + + double get_value() const override + { + THROW_ON_NULLPTR(value_ptr_); + return *value_ptr_; + } + + void set_value(double value) override + { + THROW_ON_NULLPTR(this->value_ptr_); + *this->value_ptr_ = 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/src/hardware_interface/distributed_control_interface/state_publisher.cpp b/hardware_interface/src/hardware_interface/distributed_control_interface/state_publisher.cpp index 6c98691862..49a78efc75 100644 --- a/hardware_interface/src/hardware_interface/distributed_control_interface/state_publisher.cpp +++ b/hardware_interface/src/hardware_interface/distributed_control_interface/state_publisher.cpp @@ -62,17 +62,16 @@ std::string StatePublisher::state_interface_interface_name() const return loaned_state_interface_ptr_->get_interface_name(); } -controller_manager_msgs::msg::StatePublisherDescription StatePublisher::create_description_msg() - const +controller_manager_msgs::msg::PublisherDescription StatePublisher::create_description_msg() const { - auto msg = controller_manager_msgs::msg::StatePublisherDescription(); + auto msg = controller_manager_msgs::msg::PublisherDescription(); // we want a unique name for every StatePublisher. This gets relevant in the central ControllerManager // where multiple sub ControllerManager are registering. Therefor we add the namespace to the prefix. // However since that e.g. joint1/positions becomes /sub_namespace/joint1/position msg.ns = get_namespace(); msg.name.prefix_name = state_interface_prefix_name(); msg.name.interface_name = state_interface_interface_name(); - msg.state_publisher_topic = topic_name_relative_to_namespace(); + msg.publisher_topic = topic_name_relative_to_namespace(); return msg; } From e8931ced8e59f537fc51ccab419de88cbd5ab491 Mon Sep 17 00:00:00 2001 From: Manuel M Date: Thu, 1 Dec 2022 17:59:41 +0100 Subject: [PATCH 14/19] !Not working! Add registration of DistributedCommandInterfaces * Its not working, since the storage type of command interfaces needs to be changed to std::shared_ptr. This is done in follow_up since should change a lot. --- .../controller_manager/controller_manager.hpp | 4 +- controller_manager/src/controller_manager.cpp | 88 ++++++++- hardware_interface/CMakeLists.txt | 1 + .../command_forwarder.hpp | 66 +++++++ .../state_publisher.hpp | 20 +- .../sub_controller_manager_wrapper.hpp | 14 +- .../visibility_control.h | 49 ----- .../include/hardware_interface/handle.hpp | 174 +++++++++++------- .../loaned_command_interface.hpp | 5 + .../hardware_interface/resource_manager.hpp | 20 +- .../command_forwarder.cpp | 115 ++++++++++++ .../state_publisher.cpp | 23 +-- hardware_interface/src/resource_manager.cpp | 126 ++++++++++++- 13 files changed, 547 insertions(+), 158 deletions(-) create mode 100644 hardware_interface/include/hardware_interface/distributed_control_interface/command_forwarder.hpp delete mode 100644 hardware_interface/include/hardware_interface/distributed_control_interface/visibility_control.h create mode 100644 hardware_interface/src/hardware_interface/distributed_control_interface/command_forwarder.cpp diff --git a/controller_manager/include/controller_manager/controller_manager.hpp b/controller_manager/include/controller_manager/controller_manager.hpp index 8eeef6c713..fa67321421 100644 --- a/controller_manager/include/controller_manager/controller_manager.hpp +++ b/controller_manager/include/controller_manager/controller_manager.hpp @@ -42,7 +42,6 @@ #include "controller_manager_msgs/srv/unload_controller.hpp" #include "diagnostic_updater/diagnostic_updater.hpp" -#include "hardware_interface/distributed_control_interface/state_publisher.hpp" #include "hardware_interface/handle.hpp" #include "hardware_interface/resource_manager.hpp" @@ -206,6 +205,9 @@ class ControllerManager : public rclcpp::Node CONTROLLER_MANAGER_PUBLIC void add_hardware_state_publishers(); + CONTROLLER_MANAGER_PUBLIC + void add_hardware_command_forwarders(); + CONTROLLER_MANAGER_PUBLIC void register_sub_controller_manager(); diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 6a5a239a79..aa15ba96cd 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -24,6 +24,8 @@ #include "controller_interface/controller_interface_base.hpp" #include "controller_manager_msgs/msg/hardware_component_state.hpp" +#include "hardware_interface/distributed_control_interface/command_forwarder.hpp" +#include "hardware_interface/distributed_control_interface/state_publisher.hpp" #include "hardware_interface/handle.hpp" #include "hardware_interface/types/lifecycle_state_names.hpp" @@ -305,6 +307,7 @@ void ControllerManager::configure_controller_manager() if (distributed_sub_controller_manager) { add_hardware_state_publishers(); + add_hardware_command_forwarders(); register_sub_controller_manager(); } // This means we are the central controller manager @@ -347,13 +350,16 @@ void ControllerManager::register_sub_controller_manager_srv_cb( auto sub_ctrl_mng_wrapper = std::make_shared( request->sub_controller_manager_namespace, request->sub_controller_manager_name, - request->state_publishers); + request->state_publishers, request->command_state_publishers); - std::vector> state_interfaces; - state_interfaces.reserve(sub_ctrl_mng_wrapper->get_state_publisher_count()); - state_interfaces = resource_manager_->register_sub_controller_manager(sub_ctrl_mng_wrapper); + resource_manager_->register_sub_controller_manager(sub_ctrl_mng_wrapper); - for (const auto & state_interface : state_interfaces) + std::vector> + distributed_state_interfaces; + distributed_state_interfaces = + resource_manager_->import_state_interfaces_of_sub_controller_manager(sub_ctrl_mng_wrapper); + + for (const auto & state_interface : distributed_state_interfaces) { try { @@ -370,6 +376,36 @@ void ControllerManager::register_sub_controller_manager_srv_cb( } } + std::vector> + distributed_command_interfaces; + distributed_command_interfaces = + resource_manager_->import_command_interfaces_of_sub_controller_manager(sub_ctrl_mng_wrapper); + + for (const auto & command_interface : distributed_command_interfaces) + { + try + { + executor_->add_node(command_interface->get_node()->get_node_base_interface()); + 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(); + // want topic name relative to namespace + msg.publisher_topic = std::string(get_namespace()) + std::string("/") + + command_interface->forward_command_topic_name(); + response->command_state_publishers.push_back(msg); + } + catch (const std::runtime_error & e) + { + response->ok = false; + RCLCPP_WARN_STREAM( + get_logger(), + "ControllerManager: Caught exception while trying to register sub controller manager. " + "Exception:" + << e.what()); + } + } + response->ok = true; RCLCPP_INFO_STREAM( get_logger(), "ControllerManager: Registered sub controller manager <" @@ -397,6 +433,27 @@ void ControllerManager::add_hardware_state_publishers() } } +void ControllerManager::add_hardware_command_forwarders() +{ + std::vector> command_forwarder_vec; + command_forwarder_vec.reserve(resource_manager_->available_command_interfaces().size()); + command_forwarder_vec = resource_manager_->create_hardware_command_forwarders(get_namespace()); + + for (auto const & command_forwarder : command_forwarder_vec) + { + try + { + executor_->add_node(command_forwarder->get_node()->get_node_base_interface()); + } + catch (const std::runtime_error & e) + { + RCLCPP_WARN_STREAM( + get_logger(), "ControllerManager: Can't create StatePublishers<" + << command_forwarder->command_interface_name() << ">." << e.what()); + } + } +} + void ControllerManager::register_sub_controller_manager() { RCLCPP_INFO(get_logger(), "SubControllerManager:Trying to register StatePublishers."); @@ -415,7 +472,17 @@ void ControllerManager::register_sub_controller_manager() // 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_description_msg()); + 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; @@ -441,6 +508,15 @@ void ControllerManager::register_sub_controller_manager() rclcpp::FutureReturnCode::SUCCESS) && result.get()->ok) { + // TODO(Manuel) we should probably make the keys explicit (add key_generation function to handles) + // send keys with request + for (auto command_state_publisher : result.get()->command_state_publishers) + { + std::string key = command_state_publisher.name.prefix_name + "/" + + command_state_publisher.name.interface_name; + // auto command_forwarder = resource_manager_->find_command_forwarder(key); + // command_forwarder->subscribe_to_command(command_state_publisher); + } RCLCPP_INFO(get_logger(), "SubControllerManager: Successfully registered StatePublishers."); } else diff --git a/hardware_interface/CMakeLists.txt b/hardware_interface/CMakeLists.txt index 701ef4c104..cd86620afd 100644 --- a/hardware_interface/CMakeLists.txt +++ b/hardware_interface/CMakeLists.txt @@ -29,6 +29,7 @@ add_library( SHARED src/actuator.cpp src/component_parser.cpp + src/hardware_interface/distributed_control_interface/command_forwarder.cpp src/hardware_interface/distributed_control_interface/state_publisher.cpp src/resource_manager.cpp src/sensor.cpp 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..ba320dfcb4 --- /dev/null +++ b/hardware_interface/include/hardware_interface/distributed_control_interface/command_forwarder.hpp @@ -0,0 +1,66 @@ +#ifndef DISTRIBUTED_CONTROL__COMMAND_FORWARDER_HPP_ +#define DISTRIBUTED_CONTROL__COMMAND_FORWARDER_HPP_ + +#include +#include +#include + +#include "hardware_interface/distributed_control_interface/publisher_description.hpp" +#include "hardware_interface/loaned_command_interface.hpp" + +#include "controller_manager_msgs/msg/publisher_description.hpp" + +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_lifecycle/lifecycle_node.hpp" +#include "rclcpp_lifecycle/state.hpp" +#include "std_msgs/msg/float64.hpp" + +namespace distributed_control +{ + +class CommandForwarder final +{ +public: + explicit CommandForwarder( + std::unique_ptr loaned_command_interface_ptr_, + const std::string & ns = ""); + + CommandForwarder() = delete; + + ~CommandForwarder() {} + + std::shared_ptr get_node() const; + + std::string get_namespace() const; + + std::string topic_name() const; + + std::string topic_name_relative_to_namespace() const; + + std::string command_interface_name() const; + + std::string command_interface_prefix_name() const; + + std::string command_interface_interface_name() const; + + controller_manager_msgs::msg::PublisherDescription create_publisher_description_msg() const; + + void subscribe_to_command(const distributed_control::PublisherDescription & description); + +private: + void publish_value_on_timer(); + void forward_command(const std_msgs::msg::Float64 & msg); + + std::unique_ptr loaned_command_interface_ptr_; + const std::string namespace_; + const std::string topic_name_; + std::string subscription_topic_name_; + std::shared_ptr node_; + rclcpp::Publisher::SharedPtr state_value_pub_; + rclcpp::Subscription::SharedPtr command_subscription_; + rclcpp::TimerBase::SharedPtr timer_; +}; + +} // namespace distributed_control + +#endif // DISTRIBUTED_CONTROL__COMMAND_FORWARDER_HPP_ \ No newline at end of file diff --git a/hardware_interface/include/hardware_interface/distributed_control_interface/state_publisher.hpp b/hardware_interface/include/hardware_interface/distributed_control_interface/state_publisher.hpp index 0089860a66..6fe316c4b1 100644 --- a/hardware_interface/include/hardware_interface/distributed_control_interface/state_publisher.hpp +++ b/hardware_interface/include/hardware_interface/distributed_control_interface/state_publisher.hpp @@ -5,8 +5,6 @@ #include #include -#include "visibility_control.h" - #include "hardware_interface/loaned_state_interface.hpp" #include "controller_manager_msgs/msg/publisher_description.hpp" @@ -23,40 +21,34 @@ class StatePublisher final { public: explicit StatePublisher( - const std::string & ns = "", - std::unique_ptr loaned_state_interface_ptr = nullptr); + std::unique_ptr loaned_state_interface_ptr, + const std::string & ns = ""); + + StatePublisher() = delete; ~StatePublisher() {} - STATE_PUBLISHER_PUBLIC std::shared_ptr get_node() const; - STATE_PUBLISHER_PUBLIC std::string get_namespace() const; - STATE_PUBLISHER_PUBLIC std::string topic_name() const; - STATE_PUBLISHER_PUBLIC std::string topic_name_relative_to_namespace() const; - STATE_PUBLISHER_PUBLIC std::string state_interface_name() const; - STATE_PUBLISHER_PUBLIC std::string state_interface_prefix_name() const; - STATE_PUBLISHER_PUBLIC std::string state_interface_interface_name() const; - STATE_PUBLISHER_PUBLIC - controller_manager_msgs::msg::PublisherDescription create_description_msg() const; + controller_manager_msgs::msg::PublisherDescription create_publisher_description_msg() const; private: void publish_value_on_timer(); - const std::string namespace_; std::unique_ptr loaned_state_interface_ptr_; + const std::string namespace_; const std::string topic_name_; std::shared_ptr node_; rclcpp::Publisher::SharedPtr state_value_pub_; 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 index 6933bf7dd7..24fb64a5ff 100644 --- 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 @@ -17,10 +17,12 @@ class SubControllerManagerWrapper final public: explicit SubControllerManagerWrapper( const std::string & ns, const std::string & name, - const std::vector & state_publishers) + const std::vector & state_publishers, + std::vector & command_forwarders) : NAMESPACE_(ns), NAME_(name), - state_publisher_descriptions_({state_publishers.begin(), state_publishers.end()}) + state_publisher_descriptions_({state_publishers.begin(), state_publishers.end()}), + command_forwarder_descriptions_({command_forwarders.begin(), command_forwarders.end()}) { } @@ -41,12 +43,20 @@ class SubControllerManagerWrapper final return state_publisher_descriptions_; } + std::vector get_command_forwarder_descriptions() const + { + return command_forwarder_descriptions_; + } + size_t get_state_publisher_count() const { return state_publisher_descriptions_.size(); } + size_t get_command_forwarder_count() const { return command_forwarder_descriptions_.size(); } + private: const std::string NAMESPACE_; const std::string NAME_; std::vector state_publisher_descriptions_; + std::vector command_forwarder_descriptions_; }; } // namespace distributed_control diff --git a/hardware_interface/include/hardware_interface/distributed_control_interface/visibility_control.h b/hardware_interface/include/hardware_interface/distributed_control_interface/visibility_control.h deleted file mode 100644 index 86d509e6b2..0000000000 --- a/hardware_interface/include/hardware_interface/distributed_control_interface/visibility_control.h +++ /dev/null @@ -1,49 +0,0 @@ -// 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. - -#ifndef STATE_PUBLISHER__VISIBILITY_CONTROL_H_ -#define STATE_PUBLISHER__VISIBILITY_CONTROL_H_ - -// This logic was borrowed (then namespaced) from the examples on the gcc wiki: -// https://gcc.gnu.org/wiki/Visibility - -#if defined _WIN32 || defined __CYGWIN__ -#ifdef __GNUC__ -#define STATE_PUBLISHER_EXPORT __attribute__((dllexport)) -#define STATE_PUBLISHER_IMPORT __attribute__((dllimport)) -#else -#define STATE_PUBLISHER_EXPORT __declspec(dllexport) -#define STATE_PUBLISHER_IMPORT __declspec(dllimport) -#endif -#ifdef STATE_PUBLISHER_BUILDING_DLL -#define STATE_PUBLISHER_PUBLIC STATE_PUBLISHER_EXPORT -#else -#define STATE_PUBLISHER_PUBLIC STATE_PUBLISHER_IMPORT -#endif -#define STATE_PUBLISHER_PUBLIC_TYPE STATE_PUBLISHER_PUBLIC -#define STATE_PUBLISHER_LOCAL -#else -#define STATE_PUBLISHER_EXPORT __attribute__((visibility("default"))) -#define STATE_PUBLISHER_IMPORT -#if __GNUC__ >= 4 -#define STATE_PUBLISHER_PUBLIC __attribute__((visibility("default"))) -#define STATE_PUBLISHER_LOCAL __attribute__((visibility("hidden"))) -#else -#define STATE_PUBLISHER_PUBLIC -#define STATE_PUBLISHER_LOCAL -#endif -#define STATE_PUBLISHER_PUBLIC_TYPE -#endif - -#endif // STATE_PUBLISHER__VISIBILITY_CONTROL_H_ diff --git a/hardware_interface/include/hardware_interface/handle.hpp b/hardware_interface/include/hardware_interface/handle.hpp index 4163377faa..386aee7992 100644 --- a/hardware_interface/include/hardware_interface/handle.hpp +++ b/hardware_interface/include/hardware_interface/handle.hpp @@ -84,8 +84,7 @@ class HandleInterface virtual std::string get_name() const { return prefix_name_ + "/" + interface_name_; } - // TODO(Manuel): Maybe not the best place to put... But if put in DistributedHandles we - // violate Liskov + // TODO(Manuel): Maybe not the best place to put... std::shared_ptr get_node() const { THROW_ON_NULLPTR(node_); @@ -106,14 +105,38 @@ class HandleInterface */ virtual std::string get_underscore_separated_name() const { - if (get_prefix_name().empty()) + 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 get_interface_name(); + return str + char_to_append; } - return get_prefix_name() + "_" + get_interface_name(); + 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; } -protected: std::string prefix_name_; std::string interface_name_; double * value_ptr_; @@ -169,19 +192,20 @@ class DistributedReadOnlyHandle : public ReadOnlyHandle DistributedReadOnlyHandle( const distributed_control::PublisherDescription & description, const std::string & ns = "/") : ReadOnlyHandle(description.prefix_name(), description.interface_name(), &value_), - topic_name_(description.topic_name()), + get_value_topic_name_(description.topic_name()), namespace_(ns), interface_namespace_(description.get_namespace()) { rclcpp::NodeOptions node_options; // create node for subscribing to StatePublisher described in StatePublisherDescription node_ = std::make_shared( - get_underscore_separated_name() + "_subscriber", namespace_, node_options, false); + get_underscore_separated_name() + "_state_interface_subscriber", namespace_, node_options, + false); // subscribe to topic provided by StatePublisher - state_value_subscription_ = node_->create_subscription( - topic_name_, 10, - std::bind(&DistributedReadOnlyHandle::set_value_cb, this, std::placeholders::_1)); + state_value_sub_ = node_->create_subscription( + get_value_topic_name_, 10, + std::bind(&DistributedReadOnlyHandle::get_value_cb, this, std::placeholders::_1)); } explicit DistributedReadOnlyHandle(const std::string & interface_name) @@ -207,71 +231,35 @@ class DistributedReadOnlyHandle : public ReadOnlyHandle virtual std::string get_name() const override { - // append '/' to input string if not empty - auto append_slash = [](std::string str) - { - if (!str.empty()) - { - return str + "/"; - } - return str; - }; - // concatenate: interface_namespace/prefix_name/interface_name - return append_slash(interface_namespace_) + append_slash(get_prefix_name()) + + // 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 { - // append '_' to input string if not empty - auto append_underscore = [](std::string str) - { - if (!str.empty()) - { - return str + "_"; - } - return str; - }; - // remove first "/" from namespace and replace all follow occurrences with "_" + // remove first "/" from namespace and replace all follow occurrences of "/" with "_" std::string ns = - replace_all_chars_from_string(erase_slash_at_begin(interface_namespace_), '/', '_'); + replace_all_chars_from_string(erase_slash_at_start(interface_namespace_), '/', '_'); // concatenate: interface_namespace + _ + namespace_prefix + _ + name_interface_name - return append_underscore(ns) + append_underscore(get_prefix_name()) + get_interface_name(); + return append_char(ns, '_') + append_char(get_prefix_name(), '_') + get_interface_name(); } protected: - void set_value_cb(const std_msgs::msg::Float64 & msg) + void get_value_cb(const std_msgs::msg::Float64 & msg) { value_ = msg.data; RCLCPP_WARN_STREAM(node_->get_logger(), "Receiving:[" << value_ << "]."); } - std::string erase_slash_at_begin(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 topic_name_; + 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_subscription_; + rclcpp::Subscription::SharedPtr state_value_sub_; double value_; }; @@ -309,13 +297,13 @@ class ReadWriteHandle : public HandleInterface, ReadHandleInterface, WriteHandle virtual ~ReadWriteHandle() = default; - double get_value() const override + virtual double get_value() const override { THROW_ON_NULLPTR(value_ptr_); return *value_ptr_; } - void set_value(double value) override + virtual void set_value(double value) override { THROW_ON_NULLPTR(this->value_ptr_); *this->value_ptr_ = value; @@ -342,10 +330,27 @@ class DistributedReadWriteHandle : public ReadWriteHandle { public: DistributedReadWriteHandle( - const std::string & prefix_name, const std::string & interface_name, - double * value_ptr = nullptr) - : ReadWriteHandle(prefix_name, interface_name, value_ptr) + const distributed_control::PublisherDescription & description, const std::string & ns = "/") + : ReadWriteHandle(description.prefix_name(), description.interface_name(), &value_), + get_value_topic_name_(description.topic_name()), + namespace_(ns), + interface_namespace_(description.get_namespace()), + forward_command_topic_name_(get_underscore_separated_name() + "_command_forwarding") { + // create node for subscribing to StatePublisher described in StatePublisherDescription + rclcpp::NodeOptions node_options; + node_ = std::make_shared( + get_underscore_separated_name() + "_distributed_command_interface", namespace_, node_options, + false); + + // subscribe to topic provided by StatePublisher + command_value_sub_ = node_->create_subscription( + get_value_topic_name_, 10, + std::bind(&DistributedReadWriteHandle::get_value_cb, this, std::placeholders::_1)); + + // create publisher so that we can forward the commands + command_value_pub_ = + node_->create_publisher(forward_command_topic_name_, 10); } explicit DistributedReadWriteHandle(const std::string & interface_name) @@ -367,17 +372,54 @@ class DistributedReadWriteHandle : public ReadWriteHandle virtual ~DistributedReadWriteHandle() = default; - double get_value() const override + virtual std::string get_name() const override { - THROW_ON_NULLPTR(value_ptr_); - return *value_ptr_; + // 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 { - THROW_ON_NULLPTR(this->value_ptr_); - *this->value_ptr_ = value; + auto msg = std::make_unique(); + msg->data = value; + + RCLCPP_INFO(node_->get_logger(), "DistributedCommandInterface Publishing: '%.7lf'", msg->data); + std::flush(std::cout); + + command_value_pub_->publish(std::move(msg)); } + + std::string forward_command_topic_name() const { return forward_command_topic_name_; } + +protected: + void get_value_cb(const std_msgs::msg::Float64 & msg) + { + value_ = msg.data; + RCLCPP_WARN_STREAM( + node_->get_logger(), "DistributedCommandInterface Receiving:[" << value_ << "]."); + } + + std::string get_value_topic_name_; + // the current namespace we are in. Needed to create the node in the correct namespace + std::string namespace_; + // the namespace the actual CommandInterface we subscribe to is in. + // We need this to create unique names for the CommandInterface. + std::string interface_namespace_; + rclcpp::Subscription::SharedPtr command_value_sub_; + std::string forward_command_topic_name_; + rclcpp::Publisher::SharedPtr command_value_pub_; + double value_; }; class DistributedCommandInterface : public DistributedReadWriteHandle diff --git a/hardware_interface/include/hardware_interface/loaned_command_interface.hpp b/hardware_interface/include/hardware_interface/loaned_command_interface.hpp index 1ae1a8d3f1..8a931790e7 100644 --- a/hardware_interface/include/hardware_interface/loaned_command_interface.hpp +++ b/hardware_interface/include/hardware_interface/loaned_command_interface.hpp @@ -67,6 +67,11 @@ class LoanedCommandInterface 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_; Deleter deleter_; diff --git a/hardware_interface/include/hardware_interface/resource_manager.hpp b/hardware_interface/include/hardware_interface/resource_manager.hpp index 2d0f3476b3..e5591899ba 100644 --- a/hardware_interface/include/hardware_interface/resource_manager.hpp +++ b/hardware_interface/include/hardware_interface/resource_manager.hpp @@ -21,6 +21,7 @@ #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" @@ -118,14 +119,31 @@ class HARDWARE_INTERFACE_PUBLIC ResourceManager */ bool state_interface_is_available(const std::string & name) const; - std::vector> register_sub_controller_manager( + void register_sub_controller_manager( + std::shared_ptr sub_controller_manager); + + std::vector> + import_state_interfaces_of_sub_controller_manager( + std::shared_ptr sub_controller_manager); + + std::vector> + import_command_interfaces_of_sub_controller_manager( std::shared_ptr sub_controller_manager); std::vector> create_hardware_state_publishers(const std::string & ns); + std::vector> + create_hardware_command_forwarders(const std::string & ns); + + std::pair> find_command_forwarder( + const std::string & key); + std::vector> get_state_publishers() const; + std::vector> get_command_forwarders() + const; + /// Add controllers' reference interfaces to resource manager. /** * Interface for transferring management of reference interfaces to resource manager. diff --git a/hardware_interface/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..764d15731a --- /dev/null +++ b/hardware_interface/src/hardware_interface/distributed_control_interface/command_forwarder.cpp @@ -0,0 +1,115 @@ +#include "hardware_interface/distributed_control_interface/command_forwarder.hpp" + +#include +#include +#include +#include +#include + +using namespace std::chrono_literals; + +namespace distributed_control +{ + +CommandForwarder::CommandForwarder( + std::unique_ptr loaned_command_interface_ptr, + const std::string & ns) +: loaned_command_interface_ptr_(std::move(loaned_command_interface_ptr)), + namespace_(ns), + topic_name_(loaned_command_interface_ptr_->get_underscore_separated_name() + "_command_state") +{ + rclcpp::NodeOptions node_options; + node_ = std::make_shared( + loaned_command_interface_ptr_->get_underscore_separated_name() + "_command_forwarder", + namespace_, node_options, false); + + state_value_pub_ = node_->create_publisher(topic_name_, 10); + // TODO(Manuel): We should check if we cannot detect changes to LoanedStateInterface's value and only publish then + timer_ = + node_->create_wall_timer(50ms, std::bind(&CommandForwarder::publish_value_on_timer, this)); + RCLCPP_INFO(node_->get_logger(), "Creating CommandForwarder<%s>.", topic_name_.c_str()); +} + +std::shared_ptr CommandForwarder::get_node() const +{ + if (!node_.get()) + { + std::string msg( + "CommandForwarder<" + command_interface_name() + ">: Node hasn't been configured yet!"); + throw std::runtime_error(msg); + } + return node_; +} + +std::string CommandForwarder::get_namespace() const { return namespace_; } + +std::string CommandForwarder::topic_name() const { return topic_name_; } + +std::string CommandForwarder::topic_name_relative_to_namespace() const +{ + return get_namespace() + "/" + topic_name(); +} + +std::string CommandForwarder::command_interface_name() const +{ + return loaned_command_interface_ptr_->get_name(); +} + +std::string CommandForwarder::command_interface_prefix_name() const +{ + return loaned_command_interface_ptr_->get_prefix_name(); +} + +std::string CommandForwarder::command_interface_interface_name() const +{ + return loaned_command_interface_ptr_->get_interface_name(); +} + +controller_manager_msgs::msg::PublisherDescription +CommandForwarder::create_publisher_description_msg() const +{ + auto msg = controller_manager_msgs::msg::PublisherDescription(); + + msg.ns = get_namespace(); + msg.name.prefix_name = command_interface_prefix_name(); + msg.name.interface_name = command_interface_interface_name(); + msg.publisher_topic = topic_name_relative_to_namespace(); + + return msg; +} + +void CommandForwarder::subscribe_to_command( + const distributed_control::PublisherDescription & description) +{ + subscription_topic_name_ = description.topic_name(); + command_subscription_ = node_->create_subscription( + subscription_topic_name_, 10, + std::bind(&CommandForwarder::forward_command, this, std::placeholders::_1)); +} + +void CommandForwarder::publish_value_on_timer() +{ + // Todo(Manuel) create custom msg and return success or failure not just nan. + auto msg = std::make_unique(); + try + { + msg->data = loaned_command_interface_ptr_->get_value(); + } + catch (const std::runtime_error & e) + { + msg->data = std::numeric_limits::quiet_NaN(); + } + RCLCPP_INFO(node_->get_logger(), "Publishing: '%.7lf'", msg->data); + std::flush(std::cout); + + // Put the message into a queue to be processed by the middleware. + // This call is non-blocking. + state_value_pub_->publish(std::move(msg)); +} + +void CommandForwarder::forward_command(const std_msgs::msg::Float64 & msg) +{ + loaned_command_interface_ptr_->set_value(msg.data); +} + +} // namespace distributed_control \ No newline at end of file diff --git a/hardware_interface/src/hardware_interface/distributed_control_interface/state_publisher.cpp b/hardware_interface/src/hardware_interface/distributed_control_interface/state_publisher.cpp index 49a78efc75..3504a84098 100644 --- a/hardware_interface/src/hardware_interface/distributed_control_interface/state_publisher.cpp +++ b/hardware_interface/src/hardware_interface/distributed_control_interface/state_publisher.cpp @@ -12,17 +12,19 @@ namespace distributed_control { StatePublisher::StatePublisher( - const std::string & ns, - std::unique_ptr loaned_state_interface_ptr) -: namespace_(ns), - loaned_state_interface_ptr_(std::move(loaned_state_interface_ptr)), - topic_name_(loaned_state_interface_ptr_->get_underscore_separated_name()) + std::unique_ptr loaned_state_interface_ptr, + const std::string & ns) +: loaned_state_interface_ptr_(std::move(loaned_state_interface_ptr)), + namespace_(ns), + topic_name_(loaned_state_interface_ptr_->get_underscore_separated_name() + "_state") { rclcpp::NodeOptions node_options; node_ = std::make_shared( - loaned_state_interface_ptr_->get_underscore_separated_name() + "_publisher", namespace_, + loaned_state_interface_ptr_->get_underscore_separated_name() + "_state_publisher", namespace_, node_options, false); + state_value_pub_ = node_->create_publisher(topic_name_, 10); + // TODO(Manuel): We should check if we cannot detect changes to LoanedStateInterface's value and only publish then timer_ = node_->create_wall_timer(50ms, std::bind(&StatePublisher::publish_value_on_timer, this)); RCLCPP_INFO(node_->get_logger(), "Creating StatePublisher<%s>.", topic_name_.c_str()); } @@ -62,12 +64,10 @@ std::string StatePublisher::state_interface_interface_name() const return loaned_state_interface_ptr_->get_interface_name(); } -controller_manager_msgs::msg::PublisherDescription StatePublisher::create_description_msg() const +controller_manager_msgs::msg::PublisherDescription +StatePublisher::create_publisher_description_msg() const { auto msg = controller_manager_msgs::msg::PublisherDescription(); - // we want a unique name for every StatePublisher. This gets relevant in the central ControllerManager - // where multiple sub ControllerManager are registering. Therefor we add the namespace to the prefix. - // However since that e.g. joint1/positions becomes /sub_namespace/joint1/position msg.ns = get_namespace(); msg.name.prefix_name = state_interface_prefix_name(); msg.name.interface_name = state_interface_interface_name(); @@ -78,7 +78,6 @@ controller_manager_msgs::msg::PublisherDescription StatePublisher::create_descri void StatePublisher::publish_value_on_timer() { - // Todo(Manuel) create custom msg and return success or failure not just nan. auto msg = std::make_unique(); try { @@ -86,6 +85,8 @@ void StatePublisher::publish_value_on_timer() } 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_INFO(node_->get_logger(), "Publishing: '%.7lf'", msg->data); diff --git a/hardware_interface/src/resource_manager.cpp b/hardware_interface/src/resource_manager.cpp index fa31a61b9f..9233e287ec 100644 --- a/hardware_interface/src/resource_manager.cpp +++ b/hardware_interface/src/resource_manager.cpp @@ -514,6 +514,20 @@ class ResourceStorage } } + 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 { @@ -532,6 +546,30 @@ class ResourceStorage 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 = state_interface_state_publisher_map_.find(key); + // // we could not find a command forwarder for the provided key + // if (command_forwarder == state_interface_state_publisher_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 @@ -610,8 +648,7 @@ class ResourceStorage } } - std::vector> - import_state_interfaces_of_sub_controller_manager( + std::vector> import_distributed_state_interfaces( std::shared_ptr sub_controller_manager) { std::vector> distributed_state_interfaces; @@ -630,6 +667,25 @@ class ResourceStorage return distributed_state_interfaces; } + std::vector> import_distributed_command_interfaces( + std::shared_ptr sub_controller_manager) + { + std::vector> distributed_command_interfaces; + distributed_command_interfaces.reserve(sub_controller_manager->get_command_forwarder_count()); + + for (const auto & command_forwarder_description : + sub_controller_manager->get_command_forwarder_descriptions()) + { + // create StateInterface from the Description and store in ResourceStorage. + auto command_interface = + std::make_shared(command_forwarder_description); + //add_command_interface(command_interface); + // add to return vector, node needs to added to executor. + distributed_command_interfaces.push_back(command_interface); + } + return distributed_command_interfaces; + } + // hardware plugins pluginlib::ClassLoader actuator_loader_; pluginlib::ClassLoader sensor_loader_; @@ -663,6 +719,9 @@ class ResourceStorage std::map> state_interface_state_publisher_map_; + std::map> + command_interface_command_forwarder_map_; + std::map> sub_controller_manager_map_; }; @@ -770,14 +829,27 @@ bool ResourceManager::state_interface_is_available(const std::string & name) con name) != resource_storage_->available_state_interfaces_.end(); } -std::vector> -ResourceManager::register_sub_controller_manager( +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); - return resource_storage_->import_state_interfaces_of_sub_controller_manager( - sub_controller_manager); +} + +std::vector> +ResourceManager::import_state_interfaces_of_sub_controller_manager( + std::shared_ptr sub_controller_manager) +{ + std::lock_guard guard(resource_interfaces_lock_); + return resource_storage_->import_distributed_state_interfaces(sub_controller_manager); +} + +std::vector> +ResourceManager::import_command_interfaces_of_sub_controller_manager( + std::shared_ptr sub_controller_manager) +{ + std::lock_guard guard(resource_interfaces_lock_); + return resource_storage_->import_distributed_command_interfaces(sub_controller_manager); } std::vector> @@ -793,8 +865,9 @@ ResourceManager::create_hardware_state_publishers(const std::string & ns) rclcpp::get_logger("ResourceManager"), "Creating StatePublisher for interface:<%s>.", state_interface.c_str()); auto state_publisher = std::make_shared( - ns, std::move(std::make_unique( - claim_state_interface(state_interface)))); + std::move(std::make_unique( + claim_state_interface(state_interface))), + ns); resource_storage_->add_state_publisher(state_publisher); state_publishers_vec.push_back(state_publisher); @@ -803,6 +876,30 @@ ResourceManager::create_hardware_state_publishers(const std::string & ns) return state_publishers_vec; } +std::vector> +ResourceManager::create_hardware_command_forwarders(const std::string & ns) +{ + std::lock_guard guard(resource_interfaces_lock_); + std::vector> command_forwarders_vec; + command_forwarders_vec.reserve(available_command_interfaces().size()); + + for (const auto & command_interface : available_command_interfaces()) + { + RCLCPP_INFO( + rclcpp::get_logger("ResourceManager"), "Creating CommandForwarder for interface:<%s>.", + command_interface.c_str()); + auto command_forwarder = std::make_shared( + std::move(std::make_unique( + claim_command_interface(command_interface))), + ns); + + resource_storage_->add_command_forwarder(command_forwarder); + command_forwarders_vec.push_back(command_forwarder); + } + + return command_forwarders_vec; +} + std::vector> ResourceManager::get_state_publishers() const { @@ -810,6 +907,19 @@ ResourceManager::get_state_publishers() const return resource_storage_->get_state_publishers(); } +std::vector> +ResourceManager::get_command_forwarders() const +{ + std::lock_guard guard(resource_interfaces_lock_); + return resource_storage_->get_command_forwarders(); +} + +std::pair> +ResourceManager::find_command_forwarder(const std::string & key) +{ + // return resource_storage_->find_command_forwarder(key); +} + // CM API: Called in "callback/slow"-thread void ResourceManager::import_controller_reference_interfaces( const std::string & controller_name, std::vector & interfaces) From cad7070de7a24369b393778c64d51a5fae8535a8 Mon Sep 17 00:00:00 2001 From: Manuel M Date: Fri, 2 Dec 2022 10:37:46 +0100 Subject: [PATCH 15/19] change commandinterface from obj to shared_ptr --- controller_manager/src/controller_manager.cpp | 15 ++++-- .../command_forwarder.hpp | 2 +- .../loaned_command_interface.hpp | 20 ++++---- .../command_forwarder.cpp | 5 +- hardware_interface/src/resource_manager.cpp | 47 +++++++++++++++---- 5 files changed, 62 insertions(+), 27 deletions(-) diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index aa15ba96cd..3bff440ef8 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -510,12 +510,21 @@ void ControllerManager::register_sub_controller_manager() { // TODO(Manuel) we should probably make the keys explicit (add key_generation function to handles) // send keys with request - for (auto command_state_publisher : result.get()->command_state_publishers) + for (const auto & command_state_publisher : result.get()->command_state_publishers) { std::string key = command_state_publisher.name.prefix_name + "/" + command_state_publisher.name.interface_name; - // auto command_forwarder = resource_manager_->find_command_forwarder(key); - // command_forwarder->subscribe_to_command(command_state_publisher); + 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: Could not find a CommandForwarder for key[" + << key << "]. No subscription to command state possible."); + } } RCLCPP_INFO(get_logger(), "SubControllerManager: Successfully registered StatePublishers."); } 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 index ba320dfcb4..3597dbaf00 100644 --- a/hardware_interface/include/hardware_interface/distributed_control_interface/command_forwarder.hpp +++ b/hardware_interface/include/hardware_interface/distributed_control_interface/command_forwarder.hpp @@ -45,7 +45,7 @@ class CommandForwarder final controller_manager_msgs::msg::PublisherDescription create_publisher_description_msg() const; - void subscribe_to_command(const distributed_control::PublisherDescription & description); + void subscribe_to_command_publisher(const std::string & topic_name); private: void publish_value_on_timer(); diff --git a/hardware_interface/include/hardware_interface/loaned_command_interface.hpp b/hardware_interface/include/hardware_interface/loaned_command_interface.hpp index 8a931790e7..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,30 +50,30 @@ class LoanedCommandInterface } } - std::string get_name() const { return command_interface_.get_name(); } + std::string get_name() const { return command_interface_->get_name(); } - 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(); } - 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(); + return command_interface_->get_underscore_separated_name(); } protected: - CommandInterface & command_interface_; + std::shared_ptr command_interface_; Deleter deleter_; }; 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 index 764d15731a..fef4114bcc 100644 --- a/hardware_interface/src/hardware_interface/distributed_control_interface/command_forwarder.cpp +++ b/hardware_interface/src/hardware_interface/distributed_control_interface/command_forwarder.cpp @@ -78,10 +78,9 @@ CommandForwarder::create_publisher_description_msg() const return msg; } -void CommandForwarder::subscribe_to_command( - const distributed_control::PublisherDescription & description) +void CommandForwarder::subscribe_to_command_publisher(const std::string & topic_name) { - subscription_topic_name_ = description.topic_name(); + subscription_topic_name_ = topic_name; command_subscription_ = node_->create_subscription( subscription_topic_name_, 10, std::bind(&CommandForwarder::forward_command, this, std::placeholders::_1)); diff --git a/hardware_interface/src/resource_manager.cpp b/hardware_interface/src/resource_manager.cpp index 9233e287ec..554bd19ce5 100644 --- a/hardware_interface/src/resource_manager.cpp +++ b/hardware_interface/src/resource_manager.cpp @@ -458,6 +458,33 @@ class ResourceStorage hardware_info_map_[hardware.get_name()].command_interfaces = add_command_interfaces(interfaces); } + void add_command_interface(std::shared_ptr command_interface) + { + const auto [it, success] = command_interface_map_.insert( + std::make_pair(command_interface->get_name(), command_interface)); + if (!success) + { + std::string msg( + "ResourceStorage: Tried to insert CommandInterface with already existing key. Insert[" + + command_interface->get_name() + "]"); + throw std::runtime_error(msg); + } + } + + void add_command_interface(CommandInterface && command_interface) + { + const auto [it, success] = command_interface_map_.emplace(std::make_pair( + command_interface.get_name(), + std::make_shared(std::move(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); + } + } + /// Adds exported command interfaces into internal storage. /** * Add command interfaces to the internal storage. Command interfaces exported from hardware or @@ -476,7 +503,7 @@ class ResourceStorage for (auto & interface : interfaces) { auto key = interface.get_name(); - command_interface_map_.emplace(std::make_pair(key, std::move(interface))); + add_command_interface(std::move(interface)); claimed_command_interface_map_.emplace(std::make_pair(key, false)); interface_names.push_back(key); } @@ -561,13 +588,13 @@ class ResourceStorage std::pair> find_command_forwarder( const std::string & key) { - // auto command_forwarder = state_interface_state_publisher_map_.find(key); - // // we could not find a command forwarder for the provided key - // if (command_forwarder == state_interface_state_publisher_map_.end()) - // { - // return std::make_pair(false, nullptr); - // } - // return std::make_pair(true, command_forwarder->second); + 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) @@ -706,7 +733,7 @@ class ResourceStorage /// Storage of all available state interfaces 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_; @@ -917,7 +944,7 @@ ResourceManager::get_command_forwarders() const std::pair> ResourceManager::find_command_forwarder(const std::string & key) { - // return resource_storage_->find_command_forwarder(key); + return resource_storage_->find_command_forwarder(key); } // CM API: Called in "callback/slow"-thread From 92874f1aeb440b70b5ce93c70e94e254a456d85f Mon Sep 17 00:00:00 2001 From: Manuel M Date: Fri, 2 Dec 2022 15:46:30 +0100 Subject: [PATCH 16/19] Add own qualitiy of service and register command forwarders --- controller_manager/src/controller_manager.cpp | 78 ++++++++++++------- hardware_interface/src/resource_manager.cpp | 14 +--- 2 files changed, 52 insertions(+), 40 deletions(-) diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 3bff440ef8..fe03388f4a 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -47,6 +47,10 @@ rclcpp::QoS qos_services = .reliable() .durability_volatile(); +rclcpp::QoSInitialization qos_profile_services_keep_all_persist_init( + RMW_QOS_POLICY_HISTORY_KEEP_ALL, 1); +rclcpp::QoS qos_profile_services_keep_all(qos_profile_services_keep_all_persist_init); + inline bool is_controller_inactive(const controller_interface::ControllerInterfaceBase & controller) { return controller.get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE; @@ -338,7 +342,7 @@ void ControllerManager::init_distributed_main_controller_services() std::bind( &ControllerManager::register_sub_controller_manager_srv_cb, this, std::placeholders::_1, std::placeholders::_2), - rmw_qos_profile_services_hist_keep_all, distributed_system_srv_callback_group_); + qos_profile_services_keep_all, distributed_system_srv_callback_group_); } void ControllerManager::register_sub_controller_manager_srv_cb( @@ -356,6 +360,7 @@ void ControllerManager::register_sub_controller_manager_srv_cb( std::vector> distributed_state_interfaces; + distributed_state_interfaces.reserve(sub_ctrl_mng_wrapper->get_state_publisher_count()); distributed_state_interfaces = resource_manager_->import_state_interfaces_of_sub_controller_manager(sub_ctrl_mng_wrapper); @@ -378,6 +383,7 @@ void ControllerManager::register_sub_controller_manager_srv_cb( std::vector> distributed_command_interfaces; + distributed_command_interfaces.reserve(sub_ctrl_mng_wrapper->get_command_forwarder_count()); distributed_command_interfaces = resource_manager_->import_command_interfaces_of_sub_controller_manager(sub_ctrl_mng_wrapper); @@ -386,14 +392,6 @@ void ControllerManager::register_sub_controller_manager_srv_cb( try { executor_->add_node(command_interface->get_node()->get_node_base_interface()); - 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(); - // want topic name relative to namespace - msg.publisher_topic = std::string(get_namespace()) + std::string("/") + - command_interface->forward_command_topic_name(); - response->command_state_publishers.push_back(msg); } catch (const std::runtime_error & e) { @@ -404,6 +402,13 @@ void ControllerManager::register_sub_controller_manager_srv_cb( "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; @@ -456,7 +461,7 @@ void ControllerManager::add_hardware_command_forwarders() void ControllerManager::register_sub_controller_manager() { - RCLCPP_INFO(get_logger(), "SubControllerManager:Trying to register StatePublishers."); + RCLCPP_INFO(get_logger(), "SubControllerManager: Trying to register StatePublishers."); rclcpp::Client::SharedPtr client = create_client( "/register_sub_controller_manager"); @@ -504,29 +509,42 @@ void ControllerManager::register_sub_controller_manager() auto result = client->async_send_request(request); if ( - (rclcpp::spin_until_future_complete(this->get_node_base_interface(), result) == - rclcpp::FutureReturnCode::SUCCESS) && - result.get()->ok) - { - // 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 : result.get()->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::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) { - RCLCPP_WARN_STREAM( - get_logger(), "SubControllerManager: Could not find a CommandForwarder for key[" - << key << "]. No subscription to command state possible."); + 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: Could not find a CommandForwarder for key[" + << key << "]. No subscription to command state possible."); + } } + RCLCPP_INFO(get_logger(), "SubControllerManager: Successfully registered StatePublishers."); + } + else + { + RCLCPP_WARN( + get_logger(), + "SubControllerManager: Registration of StatePublishers failed. Central ControllerManager " + "returned error code."); } - RCLCPP_INFO(get_logger(), "SubControllerManager: Successfully registered StatePublishers."); } else { diff --git a/hardware_interface/src/resource_manager.cpp b/hardware_interface/src/resource_manager.cpp index 554bd19ce5..f12bb58a0a 100644 --- a/hardware_interface/src/resource_manager.cpp +++ b/hardware_interface/src/resource_manager.cpp @@ -473,14 +473,14 @@ class ResourceStorage void add_command_interface(CommandInterface && command_interface) { - const auto [it, success] = command_interface_map_.emplace(std::make_pair( - command_interface.get_name(), - std::make_shared(std::move(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[" + - command_interface.get_name() + "]"); + key + "]"); throw std::runtime_error(msg); } } @@ -888,9 +888,6 @@ ResourceManager::create_hardware_state_publishers(const std::string & ns) for (const auto & state_interface : available_state_interfaces()) { - RCLCPP_INFO( - rclcpp::get_logger("ResourceManager"), "Creating StatePublisher for interface:<%s>.", - state_interface.c_str()); auto state_publisher = std::make_shared( std::move(std::make_unique( claim_state_interface(state_interface))), @@ -912,9 +909,6 @@ ResourceManager::create_hardware_command_forwarders(const std::string & ns) for (const auto & command_interface : available_command_interfaces()) { - RCLCPP_INFO( - rclcpp::get_logger("ResourceManager"), "Creating CommandForwarder for interface:<%s>.", - command_interface.c_str()); auto command_forwarder = std::make_shared( std::move(std::make_unique( claim_command_interface(command_interface))), From 49347d16e91dfa0af2c160cacfccee6fff421880 Mon Sep 17 00:00:00 2001 From: Manuel M Date: Mon, 5 Dec 2022 16:30:44 +0100 Subject: [PATCH 17/19] public inheritance in handle --- hardware_interface/include/hardware_interface/handle.hpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/hardware_interface/include/hardware_interface/handle.hpp b/hardware_interface/include/hardware_interface/handle.hpp index 386aee7992..ac75c91edc 100644 --- a/hardware_interface/include/hardware_interface/handle.hpp +++ b/hardware_interface/include/hardware_interface/handle.hpp @@ -143,7 +143,7 @@ class HandleInterface std::shared_ptr node_; }; -class ReadOnlyHandle : public HandleInterface, ReadHandleInterface +class ReadOnlyHandle : public HandleInterface, public ReadHandleInterface { public: ReadOnlyHandle( @@ -273,7 +273,9 @@ class DistributedStateInterface : public DistributedReadOnlyHandle using DistributedReadOnlyHandle::DistributedReadOnlyHandle; }; -class ReadWriteHandle : public HandleInterface, ReadHandleInterface, WriteHandleInterface +class ReadWriteHandle : public HandleInterface, + public ReadHandleInterface, + public WriteHandleInterface { public: ReadWriteHandle( From 167e8dc9b9a8cc45bf574cf9d042c0df623baaf1 Mon Sep 17 00:00:00 2001 From: Manuel M Date: Wed, 14 Dec 2022 12:59:45 +0100 Subject: [PATCH 18/19] add distributed interfaces to available list --- hardware_interface/src/resource_manager.cpp | 72 ++++++++++++++++++++- 1 file changed, 71 insertions(+), 1 deletion(-) diff --git a/hardware_interface/src/resource_manager.cpp b/hardware_interface/src/resource_manager.cpp index f12bb58a0a..b423526a29 100644 --- a/hardware_interface/src/resource_manager.cpp +++ b/hardware_interface/src/resource_manager.cpp @@ -680,6 +680,8 @@ class ResourceStorage { 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()) @@ -687,9 +689,41 @@ class ResourceStorage // create StateInterface from the Description and store in ResourceStorage. auto state_interface = std::make_shared(state_publisher_description); + add_state_interface(state_interface); // add to return vector, node needs to added to executor. distributed_state_interfaces.push_back(state_interface); + interface_names.push_back(state_interface->get_name()); + } + // TODO(Manuel) this should be handled at one point DRY (adding, hardware_info_map, make available...), key should be made explicit + hardware_info_map_[sub_controller_manager->get_name()].state_interfaces = interface_names; + available_state_interfaces_.reserve( + available_state_interfaces_.capacity() + interface_names.size()); + + for (const auto & interface : + hardware_info_map_[sub_controller_manager->get_name()].state_interfaces) + { + // add all state interfaces to available list + auto found_it = std::find( + available_state_interfaces_.begin(), available_state_interfaces_.end(), interface); + + if (found_it == available_state_interfaces_.end()) + { + available_state_interfaces_.emplace_back(interface); + RCUTILS_LOG_DEBUG_NAMED( + "resource_manager", "(hardware '%s'): '%s' state interface added into available list", + sub_controller_manager->get_name().c_str(), interface.c_str()); + } + else + { + // TODO(destogl): do here error management if interfaces are only partially added into + // "available" list - this should never be the case! + RCUTILS_LOG_WARN_NAMED( + "resource_manager", + "(hardware '%s'): '%s' state interface already in available list." + " This can happen due to multiple calls to 'configure'", + sub_controller_manager->get_name().c_str(), interface.c_str()); + } } return distributed_state_interfaces; } @@ -699,6 +733,8 @@ class ResourceStorage { 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()) @@ -706,10 +742,44 @@ class ResourceStorage // create StateInterface from the Description and store in ResourceStorage. auto command_interface = std::make_shared(command_forwarder_description); - //add_command_interface(command_interface); + 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; } From 71f47570766697c19afeb0cce6d13eb496393fb4 Mon Sep 17 00:00:00 2001 From: Manuel M Date: Wed, 21 Dec 2022 13:40:41 +0100 Subject: [PATCH 19/19] add some output for knowing whats going on --- controller_manager/src/controller_manager.cpp | 47 ++++++++++++------- .../include/hardware_interface/handle.hpp | 2 +- .../command_forwarder.cpp | 2 +- .../state_publisher.cpp | 2 +- 4 files changed, 32 insertions(+), 21 deletions(-) diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index fe03388f4a..d2c071b497 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -453,7 +453,7 @@ void ControllerManager::add_hardware_command_forwarders() catch (const std::runtime_error & e) { RCLCPP_WARN_STREAM( - get_logger(), "ControllerManager: Can't create StatePublishers<" + get_logger(), "ControllerManager: Can't create CommandForwarder<" << command_forwarder->command_interface_name() << ">." << e.what()); } } @@ -461,7 +461,9 @@ void ControllerManager::add_hardware_command_forwarders() void ControllerManager::register_sub_controller_manager() { - RCLCPP_INFO(get_logger(), "SubControllerManager: Trying to register StatePublishers."); + RCLCPP_INFO_STREAM( + get_logger(), + "SubControllerManager:<" << get_namespace() << "/" << get_name() << "> trying to register."); rclcpp::Client::SharedPtr client = create_client( "/register_sub_controller_manager"); @@ -495,16 +497,18 @@ void ControllerManager::register_sub_controller_manager() { if (!rclcpp::ok()) { - RCLCPP_ERROR( - get_logger(), - "SubControllerManager: Interrupted while waiting for central controller managers " - "registration service. Exiting."); + RCLCPP_ERROR_STREAM( + get_logger(), "SubControllerManager:<" + << get_namespace() << "/" << get_name() + << ">. Interrupted while waiting for central controller managers " + "registration service. Exiting."); return; } - RCLCPP_INFO( - get_logger(), - "SubControllerManager:Central controller managers registration service not available, " - "waiting again..."); + 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); @@ -532,23 +536,30 @@ void ControllerManager::register_sub_controller_manager() else { RCLCPP_WARN_STREAM( - get_logger(), "SubControllerManager: Could not find a CommandForwarder for key[" - << key << "]. No subscription to command state possible."); + get_logger(), "SubControllerManager:<" + << get_namespace() << "/" << get_name() + << ">. Could not find a CommandForwarder for key[" << key + << "]. No subscription to command state possible."); } } - RCLCPP_INFO(get_logger(), "SubControllerManager: Successfully registered StatePublishers."); + RCLCPP_INFO_STREAM( + get_logger(), "SubControllerManager:<" << get_namespace() << "/" << get_name() + << ">. Successfully registered."); } else { - RCLCPP_WARN( - get_logger(), - "SubControllerManager: Registration of StatePublishers failed. Central ControllerManager " - "returned error code."); + RCLCPP_WARN_STREAM( + get_logger(), "SubControllerManager: <" + << get_namespace() << "/" << get_name() + << ">. Registration of StatePublishers failed. Central ControllerManager " + "returned error code."); } } else { - RCLCPP_WARN(get_logger(), "SubControllerManager: Registration of StatePublishers failed."); + RCLCPP_WARN_STREAM( + get_logger(), "SubControllerManager: <" << get_namespace() << "/" << get_name() + << ">. Registration of StatePublishers failed."); } } diff --git a/hardware_interface/include/hardware_interface/handle.hpp b/hardware_interface/include/hardware_interface/handle.hpp index ac75c91edc..1e506bf0b2 100644 --- a/hardware_interface/include/hardware_interface/handle.hpp +++ b/hardware_interface/include/hardware_interface/handle.hpp @@ -396,7 +396,7 @@ class DistributedReadWriteHandle : public ReadWriteHandle auto msg = std::make_unique(); msg->data = value; - RCLCPP_INFO(node_->get_logger(), "DistributedCommandInterface Publishing: '%.7lf'", msg->data); + RCLCPP_WARN(node_->get_logger(), "DistributedCommandInterface Publishing: '%.7lf'", msg->data); std::flush(std::cout); command_value_pub_->publish(std::move(msg)); 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 index fef4114bcc..a8a1eefa98 100644 --- a/hardware_interface/src/hardware_interface/distributed_control_interface/command_forwarder.cpp +++ b/hardware_interface/src/hardware_interface/distributed_control_interface/command_forwarder.cpp @@ -98,7 +98,7 @@ void CommandForwarder::publish_value_on_timer() { msg->data = std::numeric_limits::quiet_NaN(); } - RCLCPP_INFO(node_->get_logger(), "Publishing: '%.7lf'", msg->data); + RCLCPP_WARN(node_->get_logger(), "Publishing: '%.7lf'", msg->data); std::flush(std::cout); // Put the message into a queue to be processed by the middleware. 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 index 3504a84098..c84bfee1e5 100644 --- a/hardware_interface/src/hardware_interface/distributed_control_interface/state_publisher.cpp +++ b/hardware_interface/src/hardware_interface/distributed_control_interface/state_publisher.cpp @@ -89,7 +89,7 @@ void StatePublisher::publish_value_on_timer() // Make explicit note implicit!!! msg->data = std::numeric_limits::quiet_NaN(); } - RCLCPP_INFO(node_->get_logger(), "Publishing: '%.7lf'", msg->data); + RCLCPP_WARN(node_->get_logger(), "Publishing: '%.7lf'", msg->data); std::flush(std::cout); // Put the message into a queue to be processed by the middleware.