diff --git a/controller_interface/CMakeLists.txt b/controller_interface/CMakeLists.txt index 034556d19f..cf0919ccdc 100644 --- a/controller_interface/CMakeLists.txt +++ b/controller_interface/CMakeLists.txt @@ -30,6 +30,7 @@ ament_target_dependencies(controller_interface PUBLIC ${THIS_PACKAGE_INCLUDE_DEP # which is appropriate when building the dll but not consuming it. target_compile_definitions(controller_interface PRIVATE "CONTROLLER_INTERFACE_BUILDING_DLL") +set(BUILD_TESTING 0) if(BUILD_TESTING) find_package(ament_cmake_gmock REQUIRED) find_package(sensor_msgs REQUIRED) diff --git a/controller_manager/CMakeLists.txt b/controller_manager/CMakeLists.txt index b1a6e336ee..e50b8f40e2 100644 --- a/controller_manager/CMakeLists.txt +++ b/controller_manager/CMakeLists.txt @@ -43,6 +43,7 @@ target_link_libraries(ros2_control_node PRIVATE controller_manager ) +set(BUILD_TESTING 0) if(BUILD_TESTING) find_package(ament_cmake_gmock REQUIRED) find_package(ros2_control_test_assets REQUIRED) diff --git a/controller_manager/include/controller_manager/controller_manager.hpp b/controller_manager/include/controller_manager/controller_manager.hpp index ae1c6e2175..779eb1285c 100644 --- a/controller_manager/include/controller_manager/controller_manager.hpp +++ b/controller_manager/include/controller_manager/controller_manager.hpp @@ -201,6 +201,9 @@ class ControllerManager : public rclcpp::Node CONTROLLER_MANAGER_PUBLIC unsigned int get_update_rate() const; + CONTROLLER_MANAGER_PUBLIC + rmw_qos_profile_t determine_qos_profile(const std::string & key) const; + CONTROLLER_MANAGER_PUBLIC bool is_central_controller_manager() const; @@ -210,9 +213,6 @@ class ControllerManager : public rclcpp::Node CONTROLLER_MANAGER_PUBLIC bool use_multiple_nodes() const; - CONTROLLER_MANAGER_PUBLIC - std::chrono::milliseconds distributed_interfaces_publish_period() const; - protected: CONTROLLER_MANAGER_PUBLIC void init_services(); @@ -478,6 +478,10 @@ class ControllerManager : public rclcpp::Node bool sub_controller_manager_ = false; bool central_controller_manager_ = false; bool use_multiple_nodes_ = false; + std::shared_ptr qos_helper_; + std::string handles_qos_key_ = "system_default"; + bool publish_evaluation_msg_ = true; + std::string evaluation_qos_key_ = "system_default"; std::vector command_interfaces_to_export_ = std::vector({}); std::vector state_interfaces_to_export_ = std::vector({}); @@ -486,7 +490,6 @@ class ControllerManager : public rclcpp::Node // associated with it ... (create on distributed Handles and StatePublisher/CommandForwarder) // needs to be checked if is nullptr before usage std::shared_ptr distributed_pub_sub_node_ = nullptr; - std::chrono::milliseconds distributed_interfaces_publish_period_ = std::chrono::milliseconds(12); rclcpp::CallbackGroup::SharedPtr distributed_system_srv_callback_group_; /** diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 96625c2f2b..4e28965ebb 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -26,6 +26,7 @@ #include "controller_manager_msgs/msg/publisher_description.hpp" #include "hardware_interface/distributed_control_interface/command_forwarder.hpp" +#include "hardware_interface/distributed_control_interface/evaluation_helper.hpp" #include "hardware_interface/distributed_control_interface/state_publisher.hpp" #include "hardware_interface/handle.hpp" #include "hardware_interface/types/lifecycle_state_names.hpp" @@ -48,10 +49,6 @@ 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; @@ -366,19 +363,6 @@ void ControllerManager::get_and_initialize_distributed_parameters() central_controller_manager_ ? "true" : "false"); } - int64_t distributed_interfaces_publish_period; - if (get_parameter("distributed_interfaces_publish_period", distributed_interfaces_publish_period)) - { - distributed_interfaces_publish_period_ = - std::chrono::milliseconds(distributed_interfaces_publish_period); - } - else - { - RCLCPP_WARN( - get_logger(), - "'distributed_interfaces_publish_period' parameter not set, using default value."); - } - if (!get_parameter("export_command_interfaces", command_interfaces_to_export_)) { RCLCPP_WARN( @@ -408,6 +392,24 @@ void ControllerManager::get_and_initialize_distributed_parameters() get_logger(), "'use_multiple_nodes' parameter not set, using default value:%s", use_multiple_nodes_ ? "true" : "false"); } + if (!get_parameter("handles_qos_key", handles_qos_key_)) + { + RCLCPP_WARN( + get_logger(), "'handles_qos_key' parameter not set, using default value:%s", + handles_qos_key_.c_str()); + } + if (!get_parameter("publish_evaluation_msg", publish_evaluation_msg_)) + { + RCLCPP_WARN( + get_logger(), "'publish_evaluation_msg' parameter not set, using default value:%s", + publish_evaluation_msg_ ? "true" : "false"); + } + if (!get_parameter("evaluation_qos_key", evaluation_qos_key_)) + { + RCLCPP_WARN( + get_logger(), "'evaluation_qos_key' parameter not set, using default value:%s", + evaluation_qos_key_.c_str()); + } } void ControllerManager::configure_controller_manager() @@ -462,13 +464,48 @@ ControllerManager::controller_manager_type ControllerManager::determine_controll return controller_manager_type::unkown_type; } +rmw_qos_profile_t ControllerManager::determine_qos_profile(const std::string & key) const +{ + if (key == "sensor_data") + { + return evaluation_helper::rmw_qos_profile_sensor_data; + } + else if (key == "sensor_data_1") + { + return evaluation_helper::rmw_qos_profile_sensor_data_1; + } + else if (key == "sensor_data_100") + { + return evaluation_helper::rmw_qos_profile_sensor_data_100; + } + else if (key == "reliable") + { + return evaluation_helper::rmw_qos_profile_reliable; + } + else if (key == "reliable_100") + { + return evaluation_helper::rmw_qos_profile_reliable_100; + } + else if (key == "system_default") + { + return evaluation_helper::rmw_qos_profile_system_default; + } + throw std::runtime_error("Given qos profile not know"); +} + void ControllerManager::init_distributed_sub_controller_manager() { + // just for evaluation of concept + auto handle_qos_profile = determine_qos_profile(handles_qos_key_); + auto evaluation_qos_profile = determine_qos_profile(evaluation_qos_key_); + qos_helper_ = evaluation_helper::Evaluation_Helper::create_instance( + handle_qos_profile, publish_evaluation_msg_, evaluation_qos_profile); // if only one node per sub controller manager is used if (!use_multiple_nodes()) { // create node for publishing/subscribing rclcpp::NodeOptions node_options; + node_options.clock_type(rcl_clock_type_t::RCL_STEADY_TIME); distributed_pub_sub_node_ = std::make_shared( std::string(get_name()) + "_pub_sub_node", get_namespace(), node_options, false); //try to add to executor @@ -510,9 +547,15 @@ void ControllerManager::init_distributed_sub_controller_manager() void ControllerManager::init_distributed_central_controller_manager() { + // just for evaluation of concept + auto handle_qos_profile = determine_qos_profile(handles_qos_key_); + auto evaluation_qos_profile = determine_qos_profile(evaluation_qos_key_); + qos_helper_ = evaluation_helper::Evaluation_Helper::create_instance( + handle_qos_profile, publish_evaluation_msg_, evaluation_qos_profile); if (!use_multiple_nodes()) { rclcpp::NodeOptions node_options; + node_options.clock_type(rcl_clock_type_t::RCL_STEADY_TIME); distributed_pub_sub_node_ = std::make_shared( std::string(get_name()) + "_pub_sub_node", get_namespace(), node_options, false); //try to add to executor @@ -533,6 +576,11 @@ void ControllerManager::init_distributed_central_controller_manager() void ControllerManager::init_distributed_central_controller_manager_services() { + rclcpp::QoS qos_distributed_services_keep_10 = + rclcpp::QoS(rclcpp::QoSInitialization(RMW_QOS_POLICY_HISTORY_KEEP_ALL, 10)) + .reliable() + .durability_volatile(); + distributed_system_srv_callback_group_ = create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); @@ -542,7 +590,7 @@ void ControllerManager::init_distributed_central_controller_manager_services() std::bind( &ControllerManager::register_sub_controller_manager_srv_cb, this, std::placeholders::_1, std::placeholders::_2), - qos_profile_services_keep_all, distributed_system_srv_callback_group_); + qos_distributed_services_keep_10, distributed_system_srv_callback_group_); register_sub_controller_manager_references_srv_ = create_service( @@ -550,7 +598,7 @@ void ControllerManager::init_distributed_central_controller_manager_services() std::bind( &ControllerManager::register_sub_controller_manager_references_srv_cb, this, std::placeholders::_1, std::placeholders::_2), - qos_profile_services_keep_all, distributed_system_srv_callback_group_); + qos_distributed_services_keep_10, distributed_system_srv_callback_group_); } void ControllerManager::register_sub_controller_manager_srv_cb( @@ -708,7 +756,7 @@ void ControllerManager::create_hardware_state_publishers( state_publisher = std::make_shared( std::move(std::make_unique( resource_manager_->claim_state_interface(state_interface))), - get_namespace(), distributed_interfaces_publish_period(), distributed_pub_sub_node_); + get_namespace(), distributed_pub_sub_node_); } catch (const std::exception & e) { @@ -747,7 +795,7 @@ void ControllerManager::create_hardware_command_forwarders( command_forwarder = std::make_shared( std::move(std::make_unique( resource_manager_->claim_command_interface(command_interface))), - get_namespace(), distributed_interfaces_publish_period(), distributed_pub_sub_node_); + get_namespace(), distributed_pub_sub_node_); } catch (const std::exception & e) { @@ -2622,11 +2670,6 @@ bool ControllerManager::is_sub_controller_manager() const { return sub_controlle bool ControllerManager::use_multiple_nodes() const { return use_multiple_nodes_; } -std::chrono::milliseconds ControllerManager::distributed_interfaces_publish_period() const -{ - return distributed_interfaces_publish_period_; -} - void ControllerManager::propagate_deactivation_of_chained_mode( const std::vector & controllers) { diff --git a/controller_manager_msgs/CMakeLists.txt b/controller_manager_msgs/CMakeLists.txt index cd51c6ab22..3630ae0776 100644 --- a/controller_manager_msgs/CMakeLists.txt +++ b/controller_manager_msgs/CMakeLists.txt @@ -9,9 +9,12 @@ find_package(rosidl_default_generators REQUIRED) set(msg_files msg/ControllerState.msg msg/ChainConnection.msg + msg/Evaluation.msg msg/HandleName.msg msg/HardwareComponentState.msg msg/HardwareInterface.msg + msg/Header.msg + msg/InterfaceData.msg msg/PublisherDescription.msg ) set(srv_files diff --git a/controller_manager_msgs/msg/Evaluation.msg b/controller_manager_msgs/msg/Evaluation.msg new file mode 100644 index 0000000000..9147146a08 --- /dev/null +++ b/controller_manager_msgs/msg/Evaluation.msg @@ -0,0 +1,5 @@ +string identifier +string type +uint32 seq +uint64 receive_time +builtin_interfaces/Time receive_stamp \ No newline at end of file diff --git a/controller_manager_msgs/msg/Header.msg b/controller_manager_msgs/msg/Header.msg new file mode 100644 index 0000000000..71b6028182 --- /dev/null +++ b/controller_manager_msgs/msg/Header.msg @@ -0,0 +1 @@ +uint32 seq diff --git a/controller_manager_msgs/msg/InterfaceData.msg b/controller_manager_msgs/msg/InterfaceData.msg new file mode 100644 index 0000000000..54129ad221 --- /dev/null +++ b/controller_manager_msgs/msg/InterfaceData.msg @@ -0,0 +1,2 @@ +controller_manager_msgs/Header header +float64 data \ No newline at end of file diff --git a/hardware_interface/CMakeLists.txt b/hardware_interface/CMakeLists.txt index 761b95f0bb..3144e10682 100644 --- a/hardware_interface/CMakeLists.txt +++ b/hardware_interface/CMakeLists.txt @@ -29,6 +29,7 @@ add_library(hardware_interface SHARED src/component_parser.cpp src/hardware_interface/distributed_control_interface/command_forwarder.cpp src/hardware_interface/distributed_control_interface/state_publisher.cpp + src/hardware_interface/distributed_control_interface/evaluation_helper.cpp src/resource_manager.cpp src/sensor.cpp src/system.cpp @@ -76,6 +77,7 @@ target_compile_definitions(fake_components PRIVATE "HARDWARE_INTERFACE_BUILDING_ pluginlib_export_plugin_description_file( hardware_interface fake_components_plugin_description.xml) +set(BUILD_TESTING 0) if(BUILD_TESTING) find_package(ament_cmake_gmock REQUIRED) 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 cfcc6ce9e0..c3a0ae9b1f 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 @@ -6,15 +6,17 @@ #include #include +#include "hardware_interface/distributed_control_interface/evaluation_helper.hpp" #include "hardware_interface/distributed_control_interface/publisher_description.hpp" #include "hardware_interface/loaned_command_interface.hpp" #include "controller_manager_msgs/msg/publisher_description.hpp" +#include "controller_manager_msgs/msg/evaluation.hpp" +#include "controller_manager_msgs/msg/interface_data.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 { @@ -24,8 +26,7 @@ class CommandForwarder final public: explicit CommandForwarder( std::unique_ptr loaned_command_interface_ptr, - const std::string & ns, std::chrono::milliseconds period_in_ms, - std::shared_ptr node); + const std::string & ns, std::shared_ptr node); CommandForwarder() = delete; @@ -50,19 +51,24 @@ class CommandForwarder final void subscribe_to_command_publisher(const std::string & topic_name); private: - void publish_value_on_timer(); - void forward_command(const std_msgs::msg::Float64 & msg); + void forward_command(const controller_manager_msgs::msg::InterfaceData & msg); std::unique_ptr loaned_command_interface_ptr_; const std::string namespace_; - const std::chrono::milliseconds period_in_ms_; - + std::shared_ptr node_; + const std::string node_name_; const std::string topic_name_; + const std::string evaluation_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_; + rclcpp::Publisher::SharedPtr state_value_pub_; + rclcpp::Subscription::SharedPtr + command_subscription_; + std::shared_ptr evaluation_node_; + rclcpp::Publisher::SharedPtr evaluation_pub_; + const std::string evaluation_type_ = "commandInterface"; + std::string evaluation_identifier_; + bool publish_evaluation_msg_; + rclcpp::Time receive_time_; }; } // namespace distributed_control diff --git a/hardware_interface/include/hardware_interface/distributed_control_interface/evaluation_helper.hpp b/hardware_interface/include/hardware_interface/distributed_control_interface/evaluation_helper.hpp new file mode 100644 index 0000000000..84905fd9f6 --- /dev/null +++ b/hardware_interface/include/hardware_interface/distributed_control_interface/evaluation_helper.hpp @@ -0,0 +1,156 @@ +// Copyright 2020 - 2021 ros2_control Development Team +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef DISTRIBUTED_CONTROL__QOS_PROFILES_HELPER_HPP_ +#define DISTRIBUTED_CONTROL__QOS_PROFILES_HELPER_HPP_ + +#include +#include + +#include "rmw/qos_profiles.h" +#include "rmw/types.h" + +namespace evaluation_helper +{ + +// All profiles used for evaluation, copy them to make sure that exactly those params are used. +static const rmw_qos_profile_t rmw_qos_profile_sensor_data = { + RMW_QOS_POLICY_HISTORY_KEEP_LAST, + 5, + RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT, + RMW_QOS_POLICY_DURABILITY_VOLATILE, + RMW_QOS_DEADLINE_DEFAULT, + RMW_QOS_LIFESPAN_DEFAULT, + RMW_QOS_POLICY_LIVELINESS_SYSTEM_DEFAULT, + RMW_QOS_LIVELINESS_LEASE_DURATION_DEFAULT, + false}; + +static const rmw_qos_profile_t rmw_qos_profile_sensor_data_1 = { + RMW_QOS_POLICY_HISTORY_KEEP_LAST, + 1, + RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT, + RMW_QOS_POLICY_DURABILITY_VOLATILE, + RMW_QOS_DEADLINE_DEFAULT, + RMW_QOS_LIFESPAN_DEFAULT, + RMW_QOS_POLICY_LIVELINESS_SYSTEM_DEFAULT, + RMW_QOS_LIVELINESS_LEASE_DURATION_DEFAULT, + false}; + +static const rmw_qos_profile_t rmw_qos_profile_sensor_data_100 = { + RMW_QOS_POLICY_HISTORY_KEEP_LAST, + 100, + RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT, + RMW_QOS_POLICY_DURABILITY_VOLATILE, + RMW_QOS_DEADLINE_DEFAULT, + RMW_QOS_LIFESPAN_DEFAULT, + RMW_QOS_POLICY_LIVELINESS_SYSTEM_DEFAULT, + RMW_QOS_LIVELINESS_LEASE_DURATION_DEFAULT, + false}; + +static const rmw_qos_profile_t rmw_qos_profile_reliable = { + RMW_QOS_POLICY_HISTORY_KEEP_LAST, + 1, + RMW_QOS_POLICY_RELIABILITY_RELIABLE, + RMW_QOS_POLICY_DURABILITY_VOLATILE, + RMW_QOS_DEADLINE_DEFAULT, + RMW_QOS_LIFESPAN_DEFAULT, + RMW_QOS_POLICY_LIVELINESS_SYSTEM_DEFAULT, + RMW_QOS_LIVELINESS_LEASE_DURATION_DEFAULT, + false}; + +static const rmw_qos_profile_t rmw_qos_profile_reliable_100 = { + RMW_QOS_POLICY_HISTORY_KEEP_LAST, + 100, + RMW_QOS_POLICY_RELIABILITY_RELIABLE, + RMW_QOS_POLICY_DURABILITY_VOLATILE, + RMW_QOS_DEADLINE_DEFAULT, + RMW_QOS_LIFESPAN_DEFAULT, + RMW_QOS_POLICY_LIVELINESS_SYSTEM_DEFAULT, + RMW_QOS_LIVELINESS_LEASE_DURATION_DEFAULT, + false}; + +static const rmw_qos_profile_t rmw_qos_profile_system_default = { + RMW_QOS_POLICY_HISTORY_SYSTEM_DEFAULT, + RMW_QOS_POLICY_DEPTH_SYSTEM_DEFAULT, + RMW_QOS_POLICY_RELIABILITY_SYSTEM_DEFAULT, + RMW_QOS_POLICY_DURABILITY_SYSTEM_DEFAULT, + RMW_QOS_DEADLINE_DEFAULT, + RMW_QOS_LIFESPAN_DEFAULT, + RMW_QOS_POLICY_LIVELINESS_SYSTEM_DEFAULT, + RMW_QOS_LIVELINESS_LEASE_DURATION_DEFAULT, + false}; + +class Evaluation_Helper +{ +protected: + Evaluation_Helper( + const rmw_qos_profile_t & qos_profile, const bool & publish_evaluation_msg, + const rmw_qos_profile_t & evaluation_qos_profile) + : qos_profile_(qos_profile), + publish_evaluation_msg_(publish_evaluation_msg), + evaluation_qos_profile_(evaluation_qos_profile) + { + } + +public: + /** + * Evaluation_Helpers should not be cloneable. + */ + Evaluation_Helper(Evaluation_Helper & other) = delete; + /** + * Evaluation_Helpers should not be assignable. + */ + void operator=(const Evaluation_Helper &) = delete; + + static std::shared_ptr create_instance( + const rmw_qos_profile_t & qos_profile, const bool & publish_evaluation_msg, + const rmw_qos_profile_t & evaluation_qos_profile) + { + std::lock_guard lock(mutex_); + if (qos_profile_instance_ == nullptr) + { + qos_profile_instance_ = std::shared_ptr( + new Evaluation_Helper(qos_profile, publish_evaluation_msg, evaluation_qos_profile)); + } + return qos_profile_instance_; + } + + static std::shared_ptr get_instance() + { + std::lock_guard lock(mutex_); + if (qos_profile_instance_ == nullptr) + { + throw std::runtime_error("Evaluation_Helper not initialized!"); + } + return qos_profile_instance_; + } + + rmw_qos_profile_t get_qos_profile() const { return qos_profile_; } + + rmw_qos_profile_t get_evaluation_qos_profile() const { return evaluation_qos_profile_; } + + bool publish_evaluation_msg() const { return publish_evaluation_msg_; } + +protected: + static std::shared_ptr qos_profile_instance_; + static std::mutex mutex_; + + const rmw_qos_profile_t qos_profile_; + const bool publish_evaluation_msg_; + const rmw_qos_profile_t evaluation_qos_profile_; +}; + +} // namespace evaluation_helper + +#endif // DISTRIBUTED_CONTROL__EVALUATION_HELPER_HPP_ \ No newline at end of file diff --git a/hardware_interface/include/hardware_interface/distributed_control_interface/state_publisher.hpp b/hardware_interface/include/hardware_interface/distributed_control_interface/state_publisher.hpp index 8d260beda8..012977235c 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,14 +5,15 @@ #include #include +#include "hardware_interface/distributed_control_interface/evaluation_helper.hpp" #include "hardware_interface/loaned_state_interface.hpp" #include "controller_manager_msgs/msg/publisher_description.hpp" +#include "controller_manager_msgs/msg/interface_data.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,8 +23,7 @@ class StatePublisher final public: explicit StatePublisher( std::unique_ptr loaned_state_interface_ptr, - const std::string & ns, std::chrono::milliseconds period_in_ms, - std::shared_ptr node); + const std::string & ns, std::shared_ptr node); StatePublisher() = delete; ~StatePublisher() {} @@ -45,16 +45,11 @@ class StatePublisher final controller_manager_msgs::msg::PublisherDescription create_publisher_description_msg() const; private: - void publish_value_on_timer(); - std::unique_ptr loaned_state_interface_ptr_; const std::string namespace_; - const std::chrono::milliseconds period_in_ms_; - + const std::string node_name_; const std::string topic_name_; std::shared_ptr node_; - rclcpp::Publisher::SharedPtr state_value_pub_; - rclcpp::TimerBase::SharedPtr timer_; }; } // namespace distributed_control diff --git a/hardware_interface/include/hardware_interface/handle.hpp b/hardware_interface/include/hardware_interface/handle.hpp index 4900b429f4..b21f47b528 100644 --- a/hardware_interface/include/hardware_interface/handle.hpp +++ b/hardware_interface/include/hardware_interface/handle.hpp @@ -1,3 +1,4 @@ + // Copyright 2020 PAL Robotics S.L. // // Licensed under the Apache License, Version 2.0 (the "License"); @@ -15,29 +16,67 @@ #ifndef HARDWARE_INTERFACE__HANDLE_HPP_ #define HARDWARE_INTERFACE__HANDLE_HPP_ +#include #include #include +#include "hardware_interface/distributed_control_interface/evaluation_helper.hpp" #include "hardware_interface/distributed_control_interface/publisher_description.hpp" +#include "hardware_interface/hardware_info.hpp" #include "hardware_interface/macros.hpp" #include "hardware_interface/visibility_control.h" +#include "controller_manager_msgs/msg/interface_data.hpp" #include "rclcpp/rclcpp.hpp" #include "rclcpp_lifecycle/lifecycle_node.hpp" #include "std_msgs/msg/float64.hpp" namespace hardware_interface { /// A handle used to get and set a value on a given interface. -class ReadHandleInterface + +class SetValueBehavior { public: - virtual double get_value() const = 0; + virtual double set_value(double value) = 0; }; -class WriteHandleInterface +class Identity : public SetValueBehavior { public: - virtual void set_value(double value) = 0; + double set_value(double value) override { return value; } +}; + +class PublishBehavior : public SetValueBehavior +{ +public: + explicit PublishBehavior( + const std::string & ns, std::shared_ptr node, + const std::string & topic_name, rclcpp::QoS qos_profile) + : namespace_(ns), node_(node), topic_name_(topic_name) + { + state_value_pub_ = node_->create_publisher( + topic_name_, qos_profile); + } + + double set_value(double value) override + { + auto msg = std::make_unique(); + msg->data = value; + RCLCPP_DEBUG(node_->get_logger(), "Publishing: '%.7lf'", msg->data); + // Put the message into a queue to be processed by the middleware. + // This call is non-blocking. + msg->header.seq = seq_number_; + ++seq_number_; + state_value_pub_->publish(std::move(msg)); + return value; + } + +protected: + const std::string namespace_; + std::shared_ptr node_; + const std::string topic_name_; + rclcpp::Publisher::SharedPtr state_value_pub_; + uint32_t seq_number_ = 0; }; class HandleInterface @@ -45,19 +84,35 @@ class HandleInterface public: HandleInterface( const std::string & prefix_name, const std::string & interface_name, - double * value_ptr = nullptr, std::shared_ptr node = nullptr) - : prefix_name_(prefix_name), interface_name_(interface_name), value_ptr_(value_ptr), node_(node) + std::shared_ptr node = nullptr) + : prefix_name_(prefix_name), + interface_name_(interface_name), + has_new_value_(false), + is_valid_(false), + value_(std::numeric_limits::quiet_NaN()), + node_(node) { + behavior_ = std::make_shared(); } explicit HandleInterface(const std::string & interface_name) - : interface_name_(interface_name), value_ptr_(nullptr), node_(nullptr) + : interface_name_(interface_name), + has_new_value_(false), + is_valid_(false), + value_(std::numeric_limits::quiet_NaN()), + node_(nullptr) { + behavior_ = std::make_shared(); } explicit HandleInterface(const char * interface_name) - : interface_name_(interface_name), value_ptr_(nullptr), node_(nullptr) + : interface_name_(interface_name), + has_new_value_(false), + is_valid_(false), + value_(std::numeric_limits::quiet_NaN()), + node_(nullptr) { + behavior_ = std::make_shared(); } HandleInterface(const HandleInterface & other) = default; @@ -70,8 +125,8 @@ class HandleInterface virtual ~HandleInterface() = default; - /// Returns true if handle references a value. - inline operator bool() const { return value_ptr_ != nullptr; } + // /// Returns true if handle references a value. + // inline operator bool() const { return value_ptr_ != nullptr; } std::string get_interface_name() const { return interface_name_; } @@ -82,6 +137,8 @@ class HandleInterface return get_name(); } + void set_behavior(std::shared_ptr behavior) { behavior_ = behavior; } + virtual std::string get_name() const { return prefix_name_ + "/" + interface_name_; } // TODO(Manuel): Maybe not the best place to put... @@ -99,8 +156,8 @@ class HandleInterface /** * @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. - * + * 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 @@ -108,6 +165,58 @@ class HandleInterface return append_char(get_prefix_name(), '_') + get_interface_name(); } + /** + * @brief Get the value of the handle an mark the handle as "has_new_value_ = false" + * since the value has been read and not be changed since last read access. + * + * @return HandleValue is the current stored value of the handle. + */ + virtual double get_value() + { + has_new_value_ = false; + RCLCPP_ERROR_STREAM(rclcpp::get_logger("Handle"), "get_value value[" << value_ << "]"); + return value_; + } + + /** + * @brief Set the new value of the handle and mark the Handle as "has_new_value_ = true". + * This indicates that new data has been set since last read access. + * + * @param value current stored value in the handle. + */ + virtual void set_value(double value) + { + value_ = behavior_->set_value(value); + RCLCPP_ERROR_STREAM( + rclcpp::get_logger("Handle"), + "set_value with value[" << value << "] and after set: value[" << value_ << "]"); + has_new_value_ = true; + } + + /** + * @brief Indicates if new value has been stored in the handle since the last + * read access. + * + * @return true => new value has been stored since last read access to the handle. + * @return false => no new value has been stored since last read access to the handle. + */ + virtual bool has_new_value() const { return has_new_value_; } + + /** + * @brief Indicates if the value stored inside the handle is valid + * + * @return true => stored value is valid and can be used. + * @return false => false stored value is not valid and should not be used. + */ + virtual bool value_is_valid() const + { + if (value_ == std::numeric_limits::quiet_NaN()) + { + return false; + } + return true; + } + protected: std::string append_char(std::string str, const char & char_to_append) const { @@ -139,17 +248,23 @@ class HandleInterface std::string prefix_name_; std::string interface_name_; - double * value_ptr_; + // marks if value is new or has already been read + bool has_new_value_; + // stores if the stored value is valid + bool is_valid_; + // the current stored value of the handle + double value_; std::shared_ptr node_; + std::shared_ptr behavior_; }; -class ReadOnlyHandle : public HandleInterface, public ReadHandleInterface +class ReadOnlyHandle : public HandleInterface { public: ReadOnlyHandle( const std::string & prefix_name, const std::string & interface_name, - double * value_ptr = nullptr, std::shared_ptr node = nullptr) - : HandleInterface(prefix_name, interface_name, value_ptr, node) + std::shared_ptr node = nullptr) + : HandleInterface(prefix_name, interface_name, node) { } @@ -166,17 +281,15 @@ class ReadOnlyHandle : public HandleInterface, public ReadHandleInterface ReadOnlyHandle & operator=(ReadOnlyHandle && other) = default; virtual ~ReadOnlyHandle() = default; - - double get_value() const override - { - THROW_ON_NULLPTR(value_ptr_); - return *value_ptr_; - } }; class StateInterface : public ReadOnlyHandle { public: + explicit StateInterface(const InterfaceDescription & interface_description) + : ReadOnlyHandle(interface_description.prefix_name, interface_description.interface_info.name) + { + } StateInterface(const StateInterface & other) = default; StateInterface(StateInterface && other) = default; @@ -187,12 +300,10 @@ class StateInterface : 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 distributed_control::PublisherDescription & description, const std::string & ns, std::shared_ptr node) - : ReadOnlyHandle(description.prefix_name(), description.interface_name(), &value_, node), + : ReadOnlyHandle(description.prefix_name(), description.interface_name(), node), get_value_topic_name_(description.topic_name()), namespace_(ns), interface_namespace_(description.get_namespace()) @@ -202,14 +313,19 @@ class DistributedReadOnlyHandle : public ReadOnlyHandle if (!node_.get()) { rclcpp::NodeOptions node_options; + node_options.clock_type(rcl_clock_type_t::RCL_STEADY_TIME); node_ = std::make_shared( get_underscore_separated_name() + "_state_interface_subscriber", namespace_, node_options, false); } + + auto evaluation_helper = evaluation_helper::Evaluation_Helper::get_instance(); + rclcpp::QoS qos_profile( + rclcpp::QoSInitialization::from_rmw(evaluation_helper->get_qos_profile())); // subscribe to topic provided by StatePublisher - state_value_sub_ = node_->create_subscription( - get_value_topic_name_, 10, - std::bind(&DistributedReadOnlyHandle::get_value_cb, this, std::placeholders::_1)); + state_value_sub_ = node_->create_subscription( + get_value_topic_name_, qos_profile, + std::bind(&DistributedReadOnlyHandle::receive_value_cb, this, std::placeholders::_1)); } explicit DistributedReadOnlyHandle(const std::string & interface_name) @@ -251,9 +367,9 @@ class DistributedReadOnlyHandle : public ReadOnlyHandle } protected: - void get_value_cb(const std_msgs::msg::Float64 & msg) + void receive_value_cb(const controller_manager_msgs::msg::InterfaceData & msg) { - value_ = msg.data; + set_value(msg.data); RCLCPP_DEBUG_STREAM(node_->get_logger(), "Receiving:[" << value_ << "]."); } @@ -263,8 +379,7 @@ class DistributedReadOnlyHandle : public ReadOnlyHandle // the namespace the actual StateInterface we subscribe to is in. // We need this to create unique names for the StateInterface. std::string interface_namespace_; - rclcpp::Subscription::SharedPtr state_value_sub_; - double value_; + rclcpp::Subscription::SharedPtr state_value_sub_; }; class DistributedStateInterface : public DistributedReadOnlyHandle @@ -277,15 +392,13 @@ class DistributedStateInterface : public DistributedReadOnlyHandle using DistributedReadOnlyHandle::DistributedReadOnlyHandle; }; -class ReadWriteHandle : public HandleInterface, - public ReadHandleInterface, - public WriteHandleInterface +class ReadWriteHandle : public HandleInterface { public: ReadWriteHandle( const std::string & prefix_name, const std::string & interface_name, - double * value_ptr = nullptr, std::shared_ptr node = nullptr) - : HandleInterface(prefix_name, interface_name, value_ptr, node) + std::shared_ptr node = nullptr) + : HandleInterface(prefix_name, interface_name, node) { } @@ -301,24 +414,22 @@ class ReadWriteHandle : public HandleInterface, ReadWriteHandle & operator=(ReadWriteHandle && other) = default; - virtual ~ReadWriteHandle() = default; - - virtual double get_value() const override + virtual void set_value_on_receive(double value) { - THROW_ON_NULLPTR(value_ptr_); - return *value_ptr_; + has_new_value_ = true; + value_ = value; } - virtual void set_value(double value) override - { - THROW_ON_NULLPTR(this->value_ptr_); - *this->value_ptr_ = value; - } + virtual ~ReadWriteHandle() = default; }; class CommandInterface : public ReadWriteHandle { public: + explicit CommandInterface(const InterfaceDescription & interface_description) + : ReadWriteHandle(interface_description.prefix_name, interface_description.interface_info.name) + { + } /// CommandInterface copy constructor is actively deleted. /** * Command interfaces are having a unique ownership and thus @@ -338,7 +449,7 @@ class DistributedReadWriteHandle : public ReadWriteHandle DistributedReadWriteHandle( const distributed_control::PublisherDescription & description, const std::string & ns, std::shared_ptr node) - : ReadWriteHandle(description.prefix_name(), description.interface_name(), &value_, node), + : ReadWriteHandle(description.prefix_name(), description.interface_name(), node), get_value_topic_name_(description.topic_name()), namespace_(ns), interface_namespace_(description.get_namespace()), @@ -349,19 +460,23 @@ class DistributedReadWriteHandle : public ReadWriteHandle if (!node_.get()) { rclcpp::NodeOptions node_options; + node_options.clock_type(rcl_clock_type_t::RCL_STEADY_TIME); node_ = std::make_shared( get_underscore_separated_name() + "_distributed_command_interface", namespace_, node_options, false); } + auto evaluation_helper = evaluation_helper::Evaluation_Helper::get_instance(); + rclcpp::QoS qos_profile( + rclcpp::QoSInitialization::from_rmw(evaluation_helper->get_qos_profile())); // subscribe to topic provided by CommandForwarder - command_value_sub_ = node_->create_subscription( - get_value_topic_name_, 10, - std::bind(&DistributedReadWriteHandle::get_value_cb, this, std::placeholders::_1)); + command_value_sub_ = node_->create_subscription( + get_value_topic_name_, qos_profile, + std::bind(&DistributedReadWriteHandle::receive_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); + command_value_pub_ = node_->create_publisher( + forward_command_topic_name_, qos_profile); } explicit DistributedReadWriteHandle(const std::string & interface_name) @@ -402,21 +517,25 @@ class DistributedReadWriteHandle : public ReadWriteHandle void set_value(double value) override { - auto msg = std::make_unique(); - msg->data = value; + value_ = value; + has_new_value_ = true; - RCLCPP_DEBUG(node_->get_logger(), "DistributedCommandInterface Publishing: '%.7lf'", msg->data); - std::flush(std::cout); + auto msg = std::make_unique(); + msg->data = get_value(); + msg->header.seq = seq_number_; + ++seq_number_; + RCLCPP_DEBUG(node_->get_logger(), "DistributedCommandInterface Publishing: '%.7lf'", msg->data); 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) + void receive_value_cb(const controller_manager_msgs::msg::InterfaceData & msg) { value_ = msg.data; + has_new_value_ = true; RCLCPP_DEBUG_STREAM( node_->get_logger(), "DistributedCommandInterface Receiving:[" << value_ << "]."); } @@ -427,10 +546,11 @@ class DistributedReadWriteHandle : public ReadWriteHandle // 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_; + rclcpp::Subscription::SharedPtr command_value_sub_; std::string forward_command_topic_name_; - rclcpp::Publisher::SharedPtr command_value_pub_; + rclcpp::Publisher::SharedPtr command_value_pub_; double value_; + uint_fast32_t seq_number_ = 0; }; class DistributedCommandInterface : public DistributedReadWriteHandle diff --git a/hardware_interface/include/hardware_interface/hardware_info.hpp b/hardware_interface/include/hardware_interface/hardware_info.hpp index eb6b63cfc3..f117a15127 100644 --- a/hardware_interface/include/hardware_interface/hardware_info.hpp +++ b/hardware_interface/include/hardware_interface/hardware_info.hpp @@ -15,12 +15,38 @@ #ifndef HARDWARE_INTERFACE__HARDWARE_INFO_HPP_ #define HARDWARE_INTERFACE__HARDWARE_INFO_HPP_ +#include #include #include #include namespace hardware_interface { + +template +std::ostream & operator<<(std::ostream & os, const std::vector & input) +{ + os << "["; + for (auto const & i : input) + { + os << i << ", "; + } + os << "]"; + return os; +} + +template +std::ostream & operator<<(std::ostream & os, const std::unordered_map & input) +{ + os << "["; + for (auto const & [key, value] : input) + { + os << "{" << key << ", " << value << "}, "; + } + os << "]"; + return os; +} + /** * This structure stores information about components defined for a specific hardware * in robot's URDF. @@ -42,6 +68,41 @@ struct InterfaceInfo std::string data_type; /// (Optional) If the handle is an array, the size of the array. Used by GPIOs. int size; + + friend std::ostream & operator<<(std::ostream & os, const InterfaceInfo & interface_info) + { + os << ""; + return os; + } +}; + +/** + * This structure stores information about an interface for a specific hardware which should be instantiated internally. + */ +struct InterfaceDescription +{ + InterfaceDescription(const std::string & prefix_name, const InterfaceInfo & interface_info) + : prefix_name(prefix_name), interface_info(interface_info) + { + } + + /** + * Name of the interface defined by the user. + */ + std::string prefix_name; + + InterfaceInfo interface_info; + + friend std::ostream & operator<<(std::ostream & os, const InterfaceDescription & interface_descr) + { + os << ""; + return os; + } + + // InterfaceInfo interface_info; }; /** @@ -67,6 +128,15 @@ struct ComponentInfo std::vector state_interfaces; /// (Optional) Key-value pairs of component parameters, e.g. min/max values or serial number. std::unordered_map parameters; + + friend std::ostream & operator<<(std::ostream & os, const ComponentInfo & component_info) + { + os << ""; + return os; + } }; /// Contains semantic info about a given joint loaded from URDF for a transmission @@ -78,6 +148,17 @@ struct JointInfo std::string role; double mechanical_reduction = 1.0; double offset = 0.0; + + friend std::ostream & operator<<(std::ostream & os, const JointInfo & joint_info) + { + os << ""; + return os; + } }; /// Contains semantic info about a given actuator loaded from URDF for a transmission @@ -89,6 +170,17 @@ struct ActuatorInfo std::string role; double mechanical_reduction = 1.0; double offset = 0.0; + + friend std::ostream & operator<<(std::ostream & os, const ActuatorInfo & actuator_info) + { + os << ""; + return os; + } }; /// Contains semantic info about a given transmission loaded from URDF @@ -100,6 +192,15 @@ struct TransmissionInfo std::vector actuators; /// (Optional) Key-value pairs of custom parameters std::unordered_map parameters; + + friend std::ostream & operator<<(std::ostream & os, const TransmissionInfo & transmission_info) + { + os << ""; + return os; + } }; /// This structure stores information about hardware defined in a robot's URDF. @@ -139,6 +240,19 @@ struct HardwareInfo * The XML contents prior to parsing */ std::string original_xml; + + friend std::ostream & operator<<(std::ostream & os, const HardwareInfo & hardware_info) + { + os << " + +#include "hardware_interface/hardware_info.hpp" +#include "hardware_interface/loaned_hw_command_interface.hpp" +#include "hardware_interface/loaned_hw_state_interface.hpp" +#include "hardware_interface/visibility_control.h" + +namespace hardware_interface +{ + +class HardwareInterface +{ +public: + virtual ~HardwareInterface() {} + // Could be possible to provide default implementation and parse the hardware_info here. Only if + // user wants to export subset or special cases he needs to override. + + /** + * @brief Only export information describing the interfaces. Handle construction + * and management internally. No need for the user to initialize and manage shared memory. + * + * @return std::vector A vector containing all the information + * needed to create the interfaces exported by the hardware. + */ + HARDWARE_INTERFACE_PUBLIC + virtual std::vector export_state_interfaces_descriptions() = 0; + + /** + * @brief Only export information describing the interfaces. Handle construction + * and management internally. No need for the user to initialize and manage shared memory. + * + * @return std::vector A vector containing all the information + * needed to create the interfaces exported by the hardware. + */ + HARDWARE_INTERFACE_PUBLIC + virtual std::vector export_command_interfaces_descriptions() = 0; + + // Could be possible to provide default implementation and store the loans in the interface + // itself. User could then set/get states via function calls. Ordering could be made explicit. + + /** + * @brief Import the LoanedHwStateInterface to the before exported StateInterface InterfaceDescription. + * + */ + HARDWARE_INTERFACE_PUBLIC + virtual void import_loaned_hw_state_interfaces( + std::vector loaned_hw_state_interfaces) = 0; + + /** + * @brief Import the LoanedHwCommandInterface to the before exported CommandInterface InterfaceDescription. + * + */ + HARDWARE_INTERFACE_PUBLIC + virtual void import_loaned_hw_command_interfaces( + std::vector loaned_hw_command_interfaces) = 0; +}; + +} // namespace hardware_interface +#endif // HARDWARE_INTERFACE__HARDWARE_INTERFACE_HPP_ diff --git a/hardware_interface/include/hardware_interface/loaned_command_interface.hpp b/hardware_interface/include/hardware_interface/loaned_command_interface.hpp index 1658261541..52a5d5b8f3 100644 --- a/hardware_interface/include/hardware_interface/loaned_command_interface.hpp +++ b/hardware_interface/include/hardware_interface/loaned_command_interface.hpp @@ -20,6 +20,7 @@ #include #include "hardware_interface/handle.hpp" +#include "hardware_interface/types/hardware_interface_return_values.hpp" namespace hardware_interface { @@ -63,8 +64,6 @@ class LoanedCommandInterface std::string get_prefix_name() const { return command_interface_->get_prefix_name(); } - void set_value(double val) { command_interface_->set_value(val); } - double get_value() const { return command_interface_->get_value(); } std::string get_underscore_separated_name() const @@ -72,6 +71,19 @@ class LoanedCommandInterface return command_interface_->get_underscore_separated_name(); } + bool has_new_value() const { return command_interface_->has_new_value(); } + + void set_behavior(std::shared_ptr behavior) + { + command_interface_->set_behavior(behavior); + } + + void set_value(double value) { command_interface_->set_value(value); } + + void set_value_on_receive(double value) { command_interface_->set_value_on_receive(value); } + + bool value_is_valid() const { return command_interface_->value_is_valid(); } + protected: std::shared_ptr command_interface_; Deleter deleter_; diff --git a/hardware_interface/include/hardware_interface/loaned_hw_command_interface.hpp b/hardware_interface/include/hardware_interface/loaned_hw_command_interface.hpp new file mode 100644 index 0000000000..c126471f6a --- /dev/null +++ b/hardware_interface/include/hardware_interface/loaned_hw_command_interface.hpp @@ -0,0 +1,84 @@ +// 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 HARDWARE_INTERFACE__LOANED_HW_COMMAND_INTERFACE_HPP_ +#define HARDWARE_INTERFACE__LOANED_HW_COMMAND_INTERFACE_HPP_ + +#include +#include +#include +#include + +#include "hardware_interface/handle.hpp" +#include "hardware_interface/types/hardware_interface_return_values.hpp" + +namespace hardware_interface +{ +class LoanedHwCommandInterface +{ +public: + using Deleter = std::function; + + explicit LoanedHwCommandInterface(std::shared_ptr & command_interface) + : LoanedHwCommandInterface(command_interface, nullptr) + { + } + + LoanedHwCommandInterface(std::shared_ptr & command_interface, Deleter && deleter) + : command_interface_(command_interface), deleter_(std::forward(deleter)) + { + } + + LoanedHwCommandInterface(const LoanedHwCommandInterface & other) = delete; + + LoanedHwCommandInterface(LoanedHwCommandInterface && other) = default; + + virtual ~LoanedHwCommandInterface() + { + if (deleter_) + { + deleter_(); + } + } + + const std::string get_name() const { return command_interface_->get_name(); } + + const 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(); + } + + const std::string get_prefix_name() const { return command_interface_->get_prefix_name(); } + + double get_value() const { return command_interface_->get_value(); } + + bool has_new_value() const { return command_interface_->has_new_value(); } + + void set_value(double value) { command_interface_->set_value(value); } + + void reset_command() { command_interface_->set_value(std::numeric_limits::quiet_NaN()); } + + bool value_is_valid() const { return command_interface_->value_is_valid(); } + +protected: + std::shared_ptr command_interface_; + Deleter deleter_; +}; + +} // namespace hardware_interface +#endif // HARDWARE_INTERFACE__LOANED_HW_COMMAND_INTERFACE_HPP_ diff --git a/hardware_interface/include/hardware_interface/loaned_hw_state_interface.hpp b/hardware_interface/include/hardware_interface/loaned_hw_state_interface.hpp new file mode 100644 index 0000000000..1c196f1d97 --- /dev/null +++ b/hardware_interface/include/hardware_interface/loaned_hw_state_interface.hpp @@ -0,0 +1,81 @@ +// 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 HARDWARE_INTERFACE__LOANED_HW_STATE_INTERFACE_HPP_ +#define HARDWARE_INTERFACE__LOANED_HW_STATE_INTERFACE_HPP_ + +#include +#include +#include + +#include "hardware_interface/handle.hpp" +#include "hardware_interface/types/hardware_interface_return_values.hpp" + +namespace hardware_interface +{ +class LoanedHwStateInterface +{ +public: + using Deleter = std::function; + + explicit LoanedHwStateInterface(std::shared_ptr & state_interface) + : LoanedHwStateInterface(state_interface, nullptr) + { + } + + LoanedHwStateInterface(std::shared_ptr & state_interface, Deleter && deleter) + : state_interface_(state_interface), deleter_(std::forward(deleter)) + { + } + + LoanedHwStateInterface(const LoanedHwStateInterface & other) = delete; + + LoanedHwStateInterface(LoanedHwStateInterface && other) = default; + + virtual ~LoanedHwStateInterface() + { + if (deleter_) + { + deleter_(); + } + } + + const std::string get_name() const { return state_interface_->get_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(); + } + + const std::string get_prefix_name() const { return state_interface_->get_prefix_name(); } + + double get_value() const { return state_interface_->get_value(); } + + bool has_new_value() const { return state_interface_->has_new_value(); } + + void set_value(double value) const { return state_interface_->set_value(value); } + + bool value_is_valid() const { return state_interface_->value_is_valid(); } + +protected: + std::shared_ptr state_interface_; + Deleter deleter_; +}; + +} // namespace hardware_interface +#endif // HARDWARE_INTERFACE__LOANED_HW_STATE_INTERFACE_HPP_ diff --git a/hardware_interface/include/hardware_interface/loaned_state_interface.hpp b/hardware_interface/include/hardware_interface/loaned_state_interface.hpp index ed4d3bcbd5..656cf357a9 100644 --- a/hardware_interface/include/hardware_interface/loaned_state_interface.hpp +++ b/hardware_interface/include/hardware_interface/loaned_state_interface.hpp @@ -20,6 +20,8 @@ #include #include "hardware_interface/handle.hpp" +#include "hardware_interface/types/hardware_interface_return_values.hpp" + namespace hardware_interface { class LoanedStateInterface @@ -67,8 +69,17 @@ class LoanedStateInterface return state_interface_->get_underscore_separated_name(); } + void set_behavior(std::shared_ptr behavior) + { + state_interface_->set_behavior(behavior); + } + double get_value() const { return state_interface_->get_value(); } + bool has_new_value() const { return state_interface_->has_new_value(); } + + bool value_is_valid() const { return state_interface_->value_is_valid(); } + protected: std::shared_ptr state_interface_; Deleter deleter_; diff --git a/hardware_interface/include/hardware_interface/system.hpp b/hardware_interface/include/hardware_interface/system.hpp index 4c9ae67ae2..92e71b946b 100644 --- a/hardware_interface/include/hardware_interface/system.hpp +++ b/hardware_interface/include/hardware_interface/system.hpp @@ -15,6 +15,7 @@ #ifndef HARDWARE_INTERFACE__SYSTEM_HPP_ #define HARDWARE_INTERFACE__SYSTEM_HPP_ +#include #include #include #include @@ -22,6 +23,8 @@ #include "hardware_interface/handle.hpp" #include "hardware_interface/hardware_info.hpp" +#include "hardware_interface/loaned_hw_command_interface.hpp" +#include "hardware_interface/loaned_hw_state_interface.hpp" #include "hardware_interface/types/hardware_interface_return_values.hpp" #include "hardware_interface/visibility_control.h" #include "rclcpp/duration.hpp" @@ -66,9 +69,17 @@ class System final HARDWARE_INTERFACE_PUBLIC std::vector export_state_interfaces(); + HARDWARE_INTERFACE_PUBLIC + void assign_state_interface_loans_to_hw( + std::vector && loaned_hw_state_interfaces); + HARDWARE_INTERFACE_PUBLIC std::vector export_command_interfaces(); + HARDWARE_INTERFACE_PUBLIC + void assign_command_interface_loans_to_hw( + std::vector && loaned_hw_command_interfaces); + HARDWARE_INTERFACE_PUBLIC return_type prepare_command_mode_switch( const std::vector & start_interfaces, diff --git a/hardware_interface/include/hardware_interface/system_interface.hpp b/hardware_interface/include/hardware_interface/system_interface.hpp index 75e1a1bc29..028d858755 100644 --- a/hardware_interface/include/hardware_interface/system_interface.hpp +++ b/hardware_interface/include/hardware_interface/system_interface.hpp @@ -15,16 +15,24 @@ #ifndef HARDWARE_INTERFACE__SYSTEM_INTERFACE_HPP_ #define HARDWARE_INTERFACE__SYSTEM_INTERFACE_HPP_ +#include #include #include +#include #include #include "hardware_interface/handle.hpp" #include "hardware_interface/hardware_info.hpp" +#include "hardware_interface/hardware_interface.hpp" +#include "hardware_interface/loaned_hw_command_interface.hpp" +#include "hardware_interface/loaned_hw_state_interface.hpp" #include "hardware_interface/types/hardware_interface_return_values.hpp" #include "hardware_interface/types/lifecycle_state_names.hpp" + #include "lifecycle_msgs/msg/state.hpp" + #include "rclcpp/duration.hpp" +#include "rclcpp/rclcpp.hpp" #include "rclcpp/time.hpp" #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" #include "rclcpp_lifecycle/state.hpp" @@ -108,7 +116,7 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI * * \return vector of state interfaces */ - virtual std::vector export_state_interfaces() = 0; + virtual std::vector export_state_interfaces() {} /// Exports all command interfaces for this hardware interface. /** @@ -119,7 +127,81 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI * * \return vector of command interfaces */ - virtual std::vector export_command_interfaces() = 0; + virtual std::vector export_command_interfaces() {} + + /** + * @brief Only export information describing the interfaces. Handle construction + * and management internally. No need for the user to initialize and manage shared memory. + * + * @return std::vector A vector containing all the information + * needed to create the interfaces exported by the hardware. + */ + virtual std::vector export_state_interfaces_descriptions() + { + RCLCPP_INFO_STREAM( + rclcpp::get_logger(info_.name), "Exporting following StateInterface description:"); + std::vector state_interface_descriptions; + for (const auto & joint : info_.joints) + { + for (const auto & state_interface : joint.state_interfaces) + { + InterfaceDescription description(joint.name, state_interface); + RCLCPP_INFO_STREAM(rclcpp::get_logger(info_.name), description); + state_interface_descriptions.push_back(description); + } + } + + return state_interface_descriptions; + } + + /** + * @brief Import the loans for the internally stored and managed StateInterfaces. + * + * @param loaned_hw_state_interfaces The StateInterfaces are stored in a map where the + * key is the loaned interfaces's name and the value is the loaned interface. + */ + virtual void import_loaned_hw_state_interfaces( + std::vector && loaned_hw_state_interfaces) + { + hw_states_ = std::move(loaned_hw_state_interfaces); + } + + /** + * @brief Only export information describing the interfaces. Handle construction + * and management internally. No need for the user to initialize and manage shared memory. + * + * @return std::vector A vector containing all the information + * needed to create the interfaces exported by the hardware. + */ + virtual std::vector export_command_interfaces_descriptions() + { + RCLCPP_INFO_STREAM( + rclcpp::get_logger(info_.name), "Exporting following CommandInterface description:"); + std::vector command_interface_descriptions; + for (const auto & joint : info_.joints) + { + for (const auto & command_interface : joint.command_interfaces) + { + InterfaceDescription description(joint.name, command_interface); + RCLCPP_INFO_STREAM(rclcpp::get_logger(info_.name), description); + command_interface_descriptions.push_back(description); + } + } + + return command_interface_descriptions; + } + + /** + * @brief Import the loans for the internally stored and managed CommandInterfaces. + * + * @param loaned_hw_command_interfaces The CommandInterfaces are stored in a map where the + * key is the loaned interfaces's name and the value is the loaned interface. + */ + virtual void import_loaned_hw_command_interfaces( + std::vector && loaned_hw_command_interfaces) + { + hw_commands_ = std::move(loaned_hw_command_interfaces); + } /// Prepare for a new command interface switch. /** @@ -202,8 +284,28 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI void set_state(const rclcpp_lifecycle::State & new_state) { lifecycle_state_ = new_state; } protected: + virtual double hw_states(const size_t & index) { return hw_states_.at(index).get_value(); } + + virtual void hw_states(const size_t & index, const double & value) + { + hw_states_.at(index).set_value(value); + } + + virtual double hw_commands(const size_t & index) { return hw_commands_.at(index).get_value(); } + + virtual void hw_commands(const size_t & index, const double & value) + { + hw_commands_.at(index).set_value(value); + } + + virtual void hw_command_reset(const size_t & index) { hw_commands_.at(index).reset_command(); } + HardwareInfo info_; rclcpp_lifecycle::State lifecycle_state_; + +private: + std::vector hw_states_; + std::vector hw_commands_; }; } // namespace hardware_interface diff --git a/hardware_interface/include/mock_components/generic_system.hpp b/hardware_interface/include/mock_components/generic_system.hpp index 78ae1488a6..4f2087c3ea 100644 --- a/hardware_interface/include/mock_components/generic_system.hpp +++ b/hardware_interface/include/mock_components/generic_system.hpp @@ -22,6 +22,8 @@ #include "hardware_interface/handle.hpp" #include "hardware_interface/hardware_info.hpp" +#include "hardware_interface/loaned_hw_command_interface.hpp" +#include "hardware_interface/loaned_hw_state_interface.hpp" #include "hardware_interface/system_interface.hpp" #include "hardware_interface/types/hardware_interface_return_values.hpp" #include "hardware_interface/types/hardware_interface_type_values.hpp" @@ -41,6 +43,19 @@ class HARDWARE_INTERFACE_PUBLIC GenericSystem : public hardware_interface::Syste std::vector export_command_interfaces() override; + // virtual std::vector + // export_state_interfaces_descriptions() override; + + // virtual std::vector + // export_command_interfaces_descriptions() override; + + // virtual void import_loaned_hw_state_interfaces( + // std::vector && loaned_hw_state_interfaces) override; + + // virtual void import_loaned_hw_command_interfaces( + // std::vector && loaned_hw_command_interfaces) + // override; + return_type read(const rclcpp::Time & time, const rclcpp::Duration & period) override; return_type write(const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) override 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 c9db5cbfee..0a48f3a0b4 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 @@ -15,28 +15,45 @@ namespace distributed_control CommandForwarder::CommandForwarder( std::unique_ptr loaned_command_interface_ptr, - const std::string & ns, std::chrono::milliseconds period_in_ms, - std::shared_ptr node) + const std::string & ns, std::shared_ptr node) : loaned_command_interface_ptr_(std::move(loaned_command_interface_ptr)), namespace_(ns), - period_in_ms_(period_in_ms), node_(node), - topic_name_(loaned_command_interface_ptr_->get_underscore_separated_name() + "_command_state") + node_name_(loaned_command_interface_ptr_->get_underscore_separated_name() + "_command_forwarder"), + topic_name_(loaned_command_interface_ptr_->get_underscore_separated_name() + "_command_state"), + evaluation_topic_name_( + loaned_command_interface_ptr_->get_underscore_separated_name() + "_EVALUATION") { // if we did not get a node passed, we create one ourselves if (!node_.get()) { rclcpp::NodeOptions node_options; + node_options.clock_type(rcl_clock_type_t::RCL_STEADY_TIME); node_ = std::make_shared( - loaned_command_interface_ptr_->get_underscore_separated_name() + "_command_forwarder", - namespace_, node_options, false); + node_name_, 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( - period_in_ms_, std::bind(&CommandForwarder::publish_value_on_timer, this)); - RCLCPP_INFO(node_->get_logger(), "Creating CommandForwarder<%s>.", topic_name_.c_str()); + auto evaluation_helper = evaluation_helper::Evaluation_Helper::get_instance(); + publish_evaluation_msg_ = evaluation_helper->publish_evaluation_msg(); + rclcpp::QoS qos_profile( + rclcpp::QoSInitialization::from_rmw(evaluation_helper->get_qos_profile())); + std::shared_ptr distributed_behavior = + std::make_shared( + namespace_, node_, topic_name_, qos_profile); + loaned_command_interface_ptr_->set_behavior(distributed_behavior); + if (publish_evaluation_msg_) + { + rclcpp::NodeOptions node_options; + node_options.clock_type(rcl_clock_type_t::RCL_STEADY_TIME); + evaluation_node_ = std::make_shared( + loaned_command_interface_ptr_->get_underscore_separated_name() + "evaluation_node", + namespace_, node_options, false); + rclcpp::QoS evaluation_qos_profile( + rclcpp::QoSInitialization::from_rmw(evaluation_helper->get_evaluation_qos_profile())); + evaluation_pub_ = evaluation_node_->create_publisher( + evaluation_topic_name_, evaluation_qos_profile); + evaluation_identifier_ = loaned_command_interface_ptr_->get_underscore_separated_name(); + } } std::shared_ptr CommandForwarder::get_node() const @@ -89,35 +106,38 @@ CommandForwarder::create_publisher_description_msg() const void CommandForwarder::subscribe_to_command_publisher(const std::string & topic_name) { + auto evaluation_helper = evaluation_helper::Evaluation_Helper::get_instance(); + rclcpp::QoS qos_profile( + rclcpp::QoSInitialization::from_rmw(evaluation_helper->get_qos_profile())); subscription_topic_name_ = topic_name; - command_subscription_ = node_->create_subscription( - subscription_topic_name_, 10, + command_subscription_ = node_->create_subscription( + subscription_topic_name_, qos_profile, std::bind(&CommandForwarder::forward_command, this, std::placeholders::_1)); } -void CommandForwarder::publish_value_on_timer() +void CommandForwarder::forward_command(const controller_manager_msgs::msg::InterfaceData & msg) { - // Todo(Manuel) create custom msg and return success or failure not just nan. - auto msg = std::make_unique(); - try + // first get timestamp to be as precise as possible + if (publish_evaluation_msg_) { - msg->data = loaned_command_interface_ptr_->get_value(); + receive_time_ = evaluation_node_->now(); } - catch (const std::runtime_error & e) + //set value before publishing + loaned_command_interface_ptr_->set_value_on_receive(msg.data); + + if (publish_evaluation_msg_) { - msg->data = std::numeric_limits::quiet_NaN(); + auto evaluation_msg = std::make_unique(); + evaluation_msg->receive_stamp = receive_time_; + evaluation_msg->receive_time = + static_cast(evaluation_msg->receive_stamp.sec) * 1'000'000'000ULL + + evaluation_msg->receive_stamp.nanosec; + evaluation_msg->type = evaluation_type_; + evaluation_msg->identifier = evaluation_identifier_; + evaluation_msg->seq = msg.header.seq; + // todo check for QoS to publish immediately and never block to be fast as possible + evaluation_pub_->publish(std::move(evaluation_msg)); } - RCLCPP_DEBUG(node_->get_logger(), "Publishing: '%.7lf'", msg->data); - std::flush(std::cout); - - // Put the message into a queue to be processed by the middleware. - // This call is non-blocking. - state_value_pub_->publish(std::move(msg)); -} - -void CommandForwarder::forward_command(const std_msgs::msg::Float64 & msg) -{ - loaned_command_interface_ptr_->set_value(msg.data); } } // namespace distributed_control \ No newline at end of file diff --git a/hardware_interface/src/hardware_interface/distributed_control_interface/evaluation_helper.cpp b/hardware_interface/src/hardware_interface/distributed_control_interface/evaluation_helper.cpp new file mode 100644 index 0000000000..35c7f213bd --- /dev/null +++ b/hardware_interface/src/hardware_interface/distributed_control_interface/evaluation_helper.cpp @@ -0,0 +1,5 @@ +#include "hardware_interface/distributed_control_interface/evaluation_helper.hpp" + +std::shared_ptr + evaluation_helper::Evaluation_Helper::qos_profile_instance_{nullptr}; +std::mutex evaluation_helper::Evaluation_Helper::mutex_; \ No newline at end of file diff --git a/hardware_interface/src/hardware_interface/distributed_control_interface/state_publisher.cpp b/hardware_interface/src/hardware_interface/distributed_control_interface/state_publisher.cpp index b353ef63bf..da764a9e99 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 @@ -15,28 +15,30 @@ namespace distributed_control StatePublisher::StatePublisher( std::unique_ptr loaned_state_interface_ptr, - const std::string & ns, std::chrono::milliseconds period_in_ms, - std::shared_ptr node) + const std::string & ns, std::shared_ptr node) : loaned_state_interface_ptr_(std::move(loaned_state_interface_ptr)), namespace_(ns), - period_in_ms_(period_in_ms), node_(node), + node_name_(loaned_state_interface_ptr_->get_underscore_separated_name() + "_state_publisher"), topic_name_(loaned_state_interface_ptr_->get_underscore_separated_name() + "_state") { // if we did not get a node passed, we create one ourselves if (!node_.get()) { rclcpp::NodeOptions node_options; + node_options.clock_type(rcl_clock_type_t::RCL_STEADY_TIME); node_ = std::make_shared( - loaned_state_interface_ptr_->get_underscore_separated_name() + "_state_publisher", namespace_, - node_options, false); + node_name_, 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( - period_in_ms_, std::bind(&StatePublisher::publish_value_on_timer, this)); - RCLCPP_INFO(node_->get_logger(), "Creating StatePublisher<%s>.", topic_name_.c_str()); + auto evaluation_helper = evaluation_helper::Evaluation_Helper::get_instance(); + rclcpp::QoS qos_profile( + rclcpp::QoSInitialization::from_rmw(evaluation_helper->get_qos_profile())); + std::shared_ptr distributed_behavior = + std::make_shared( + namespace_, node_, topic_name_, qos_profile); + + loaned_state_interface_ptr->set_behavior(distributed_behavior); } std::shared_ptr StatePublisher::get_node() const @@ -86,25 +88,4 @@ StatePublisher::create_publisher_description_msg() const return msg; } -void StatePublisher::publish_value_on_timer() -{ - auto msg = std::make_unique(); - try - { - msg->data = loaned_state_interface_ptr_->get_value(); - } - catch (const std::runtime_error & e) - { - // Todo(Manuel) create custom msg and return success or failure not just nan. - // Make explicit note implicit!!! - msg->data = std::numeric_limits::quiet_NaN(); - } - RCLCPP_DEBUG(node_->get_logger(), "Publishing: '%.7lf'", msg->data); - std::flush(std::cout); - - // Put the message into a queue to be processed by the middleware. - // This call is non-blocking. - state_value_pub_->publish(std::move(msg)); -} - } // namespace distributed_control \ No newline at end of file diff --git a/hardware_interface/src/mock_components/generic_system.cpp b/hardware_interface/src/mock_components/generic_system.cpp index d2c2e56510..b5b0022a27 100644 --- a/hardware_interface/src/mock_components/generic_system.cpp +++ b/hardware_interface/src/mock_components/generic_system.cpp @@ -344,6 +344,30 @@ std::vector GenericSystem::export_command_ return command_interfaces; } +// std::vector +// GenericSystem::export_state_interfaces_descriptions() +// { +// throw std::runtime_error( +// "Generic system export_state_interfaces_descriptions() not implemented yet."); +// } + +// std::vector +// GenericSystem::export_command_interfaces_descriptions() +// { +// throw std::runtime_error( +// "Generic system export_command_interfaces_descriptions() not implemented yet."); +// } + +// void GenericSystem::import_loaned_hw_state_interfaces( +// std::vector loaned_hw_state_interfaces) +// { +// } + +// void GenericSystem::import_loaned_hw_command_interfaces( +// std::vector loaned_hw_command_interfaces) +// { +// } + return_type GenericSystem::read(const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) { auto mirror_command_to_state = [](auto & states_, auto commands_, size_t start_index = 0) @@ -430,7 +454,7 @@ bool GenericSystem::get_interface( if (it != interface_list.end()) { auto j = std::distance(interface_list.begin(), it); - interfaces.emplace_back(name, *it, &values[j][vector_index]); + // interfaces.emplace_back(name, *it, &values[j][vector_index]); return true; } return false; diff --git a/hardware_interface/src/resource_manager.cpp b/hardware_interface/src/resource_manager.cpp index e2cad29e83..4430afaae2 100644 --- a/hardware_interface/src/resource_manager.cpp +++ b/hardware_interface/src/resource_manager.cpp @@ -26,6 +26,8 @@ #include "hardware_interface/actuator_interface.hpp" #include "hardware_interface/component_parser.hpp" #include "hardware_interface/hardware_component_info.hpp" +#include "hardware_interface/loaned_hw_command_interface.hpp" +#include "hardware_interface/loaned_hw_state_interface.hpp" #include "hardware_interface/sensor.hpp" #include "hardware_interface/sensor_interface.hpp" #include "hardware_interface/system.hpp" @@ -436,6 +438,12 @@ class ResourceStorage } } + template + std::vector get_state_interface_names(const HardwareT & hardware) + { + return hardware_info_map_[hardware.get_name()].state_interfaces; + } + template void import_state_interfaces(HardwareT & hardware) { @@ -452,6 +460,20 @@ class ResourceStorage available_state_interfaces_.capacity() + interface_names.size()); } + template + void assign_state_interface_loans_to_hw(HardwareT & hardware) + { + std::vector hw_state_interface_loans; + for (const auto & state_interface_name : get_state_interface_names(hardware)) + { + // TODO(Manuel): should we mark as hw claimed??? + LoanedHwStateInterface loaned_hw_state_interface( + state_interface_map_.at(state_interface_name)); + hw_state_interface_loans.push_back(std::move(loaned_hw_state_interface)); + } + hardware.assign_state_interface_loans_to_hw(std::move(hw_state_interface_loans)); + } + template void import_command_interfaces(HardwareT & hardware) { @@ -514,6 +536,26 @@ class ResourceStorage return interface_names; } + template + std::vector get_command_interface_names(const HardwareT & hardware) + { + return hardware_info_map_[hardware.get_name()].command_interfaces; + } + + template + void assign_command_interface_loans_to_hw(HardwareT & hardware) + { + std::vector hw_command_interface_loans; + for (const auto & command_interface_name : get_command_interface_names(hardware)) + { + // TODO(Manuel): should we mark as hw claimed??? + LoanedHwCommandInterface loaned_hw_command_interface( + command_interface_map_.at(command_interface_name)); + hw_command_interface_loans.push_back(std::move(loaned_hw_command_interface)); + } + hardware.assign_command_interface_loans_to_hw(std::move(hw_command_interface_loans)); + } + /// Removes command interfaces from internal storage. /** * Command interface are removed from the maps with theirs storage and their claimed status. @@ -633,7 +675,9 @@ class ResourceStorage load_hardware(hardware_info, system_loader_, systems_); initialize_hardware(hardware_info, systems_.back()); import_state_interfaces(systems_.back()); + assign_state_interface_loans_to_hw(systems_.back()); import_command_interfaces(systems_.back()); + assign_command_interface_loans_to_hw(systems_.back()); } void initialize_actuator( @@ -659,7 +703,9 @@ class ResourceStorage this->systems_.emplace_back(System(std::move(system))); initialize_hardware(hardware_info, systems_.back()); import_state_interfaces(systems_.back()); + assign_state_interface_loans_to_hw(systems_.back()); import_command_interfaces(systems_.back()); + assign_command_interface_loans_to_hw(systems_.back()); } void add_sub_controller_manager( diff --git a/hardware_interface/src/system.cpp b/hardware_interface/src/system.cpp index f8703a47bc..43231676ae 100644 --- a/hardware_interface/src/system.cpp +++ b/hardware_interface/src/system.cpp @@ -14,6 +14,7 @@ #include "hardware_interface/system.hpp" +#include #include #include #include @@ -24,7 +25,10 @@ #include "hardware_interface/system_interface.hpp" #include "hardware_interface/types/hardware_interface_return_values.hpp" #include "hardware_interface/types/lifecycle_state_names.hpp" + #include "lifecycle_msgs/msg/state.hpp" + +#include "rclcpp/rclcpp.hpp" #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" #include "rclcpp_lifecycle/state.hpp" @@ -186,12 +190,38 @@ const rclcpp_lifecycle::State & System::error() std::vector System::export_state_interfaces() { - return impl_->export_state_interfaces(); + std::vector state_interfaces; + + for (const auto & state_interface_descr : impl_->export_state_interfaces_descriptions()) + { + state_interfaces.emplace_back(StateInterface(state_interface_descr)); + } + + return state_interfaces; +} + +void System::assign_state_interface_loans_to_hw( + std::vector && state_intefaces_for_hw) +{ + impl_->import_loaned_hw_state_interfaces(std::move(state_intefaces_for_hw)); } std::vector System::export_command_interfaces() { - return impl_->export_command_interfaces(); + std::vector command_interfaces; + + for (const auto & command_interface_descr : impl_->export_command_interfaces_descriptions()) + { + command_interfaces.emplace_back(CommandInterface(command_interface_descr)); + } + + return command_interfaces; +} + +void System::assign_command_interface_loans_to_hw( + std::vector && command_intefaces_for_hw) +{ + impl_->import_loaned_hw_command_interfaces(std::move(command_intefaces_for_hw)); } return_type System::prepare_command_mode_switch( diff --git a/transmission_interface/COLCON_IGNORE b/transmission_interface/COLCON_IGNORE new file mode 100644 index 0000000000..e69de29bb2