diff --git a/hardware_interface/CMakeLists.txt b/hardware_interface/CMakeLists.txt index bcd03a0a16..820937389c 100644 --- a/hardware_interface/CMakeLists.txt +++ b/hardware_interface/CMakeLists.txt @@ -73,6 +73,7 @@ if(BUILD_TESTING) ament_add_gmock(test_component_interfaces test/test_component_interfaces.cpp) target_link_libraries(test_component_interfaces hardware_interface) + ament_target_dependencies(test_component_interfaces ros2_control_test_assets) ament_add_gmock(test_component_parser test/test_component_parser.cpp) target_link_libraries(test_component_parser hardware_interface) diff --git a/hardware_interface/include/hardware_interface/actuator.hpp b/hardware_interface/include/hardware_interface/actuator.hpp index 4082863370..5252bafcab 100644 --- a/hardware_interface/include/hardware_interface/actuator.hpp +++ b/hardware_interface/include/hardware_interface/actuator.hpp @@ -64,11 +64,23 @@ class Actuator final HARDWARE_INTERFACE_PUBLIC const rclcpp_lifecycle::State & error(); + [[deprecated( + "Replaced by vector> on_export_state_interfaces() method. " + "Exporting is handled by the Framework.")]] HARDWARE_INTERFACE_PUBLIC + std::vector + export_state_interfaces(); + HARDWARE_INTERFACE_PUBLIC - std::vector export_state_interfaces(); + std::vector> on_export_state_interfaces(); + + [[deprecated( + "Replaced by vector> on_export_state_interfaces() method. " + "Exporting is handled by the Framework.")]] HARDWARE_INTERFACE_PUBLIC + std::vector + export_command_interfaces(); HARDWARE_INTERFACE_PUBLIC - std::vector export_command_interfaces(); + std::vector> on_export_command_interfaces(); HARDWARE_INTERFACE_PUBLIC return_type prepare_command_mode_switch( diff --git a/hardware_interface/include/hardware_interface/actuator_interface.hpp b/hardware_interface/include/hardware_interface/actuator_interface.hpp index abfd8eb45a..111cd05f02 100644 --- a/hardware_interface/include/hardware_interface/actuator_interface.hpp +++ b/hardware_interface/include/hardware_interface/actuator_interface.hpp @@ -15,10 +15,14 @@ #ifndef HARDWARE_INTERFACE__ACTUATOR_INTERFACE_HPP_ #define HARDWARE_INTERFACE__ACTUATOR_INTERFACE_HPP_ +#include +#include #include #include +#include #include +#include "hardware_interface/component_parser.hpp" #include "hardware_interface/handle.hpp" #include "hardware_interface/hardware_info.hpp" #include "hardware_interface/types/hardware_interface_return_values.hpp" @@ -97,11 +101,46 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod virtual CallbackReturn on_init(const HardwareInfo & hardware_info) { info_ = hardware_info; + import_state_interface_descriptions(info_); + import_command_interface_descriptions(info_); return CallbackReturn::SUCCESS; }; + /** + * Import the InterfaceDescription for the StateInterfaces from the HardwareInfo. + * Separate them into the possible types: Joint and store them. + */ + virtual void import_state_interface_descriptions(const HardwareInfo & hardware_info) + { + auto joint_state_interface_descriptions = + parse_joint_state_interface_descriptions_from_hardware_info(hardware_info); + for (const auto & description : joint_state_interface_descriptions) + { + joint_state_interfaces_.insert(std::make_pair(description.get_name(), description)); + } + } + + /** + * Import the InterfaceDescription for the CommandInterfaces from the HardwareInfo. + * Separate them into the possible types: Joint and store them. + */ + virtual void import_command_interface_descriptions(const HardwareInfo & hardware_info) + { + auto joint_command_interface_descriptions = + parse_joint_command_interface_descriptions_from_hardware_info(hardware_info); + for (const auto & description : joint_command_interface_descriptions) + { + joint_command_interfaces_.insert(std::make_pair(description.get_name(), description)); + } + } + /// Exports all state interfaces for this hardware interface. /** + * Default implementation for exporting the StateInterfaces. The StateInterfaces are created + * according to the InterfaceDescription. The memory accessed by the controllers and hardware is + * assigned here and resides in the system_interface. + * + * If overwritten: * The state interfaces have to be created and transferred according * to the hardware info passed in for the configuration. * @@ -109,10 +148,37 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod * * \return vector of state interfaces */ - virtual std::vector export_state_interfaces() = 0; + [[deprecated( + "Replaced by vector> on_export_state_interfaces() method. " + "Exporting is " + "handled " + "by the Framework.")]] virtual std::vector + export_state_interfaces() + { + // return empty vector by default. For backward compatibility we check if all vectors is empty + // and if so call on_export_state_interfaces() + std::vector state_interfaces; + return state_interfaces; + } + + std::vector> on_export_state_interfaces() + { + std::vector> state_interfaces; + state_interfaces.reserve(joint_state_interfaces_.size()); + for (const auto & [name, descr] : joint_state_interfaces_) + { + actuator_states_.insert(std::make_pair(name, std::make_shared(descr))); + state_interfaces.push_back(actuator_states_.at(name)); + } + return state_interfaces; + } /// Exports all command interfaces for this hardware interface. /** + * Default implementation for exporting the CommandInterfaces. The CommandInterfaces are created + * according to the InterfaceDescription. The memory accessed by the controllers and hardware is + * assigned here and resides in the system_interface. + * * The command interfaces have to be created and transferred according * to the hardware info passed in for the configuration. * @@ -120,8 +186,32 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod * * \return vector of command interfaces */ - virtual std::vector export_command_interfaces() = 0; + [[deprecated( + "Replaced by vector> on_export_command_interfaces() method. " + "Exporting is " + "handled " + "by the Framework.")]] virtual std::vector + export_command_interfaces() + { + // return empty vector by default. For backward compatibility we check if all vectors is empty + // and if so call on_export_command_interfaces() + std::vector command_interfaces; + return command_interfaces; + } + std::vector> on_export_command_interfaces() + { + std::vector> command_interfaces; + command_interfaces.reserve(joint_command_interfaces_.size()); + + for (const auto & [name, descr] : joint_command_interfaces_) + { + actuator_commands_.insert(std::make_pair(name, std::make_shared(descr))); + command_interfaces.push_back(actuator_commands_.at(name)); + } + + return command_interfaces; + } /// Prepare for a new command interface switch. /** * Prepare for any mode-switching required by the new command interface combination. @@ -202,8 +292,35 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod */ void set_state(const rclcpp_lifecycle::State & new_state) { lifecycle_state_ = new_state; } + void set_state(const std::string & interface_name, const double & value) + { + actuator_states_.at(interface_name)->set_value(value); + } + + double get_state(const std::string & interface_name) const + { + return actuator_states_.at(interface_name)->get_value(); + } + + void set_command(const std::string & interface_name, const double & value) + { + actuator_commands_.at(interface_name)->set_value(value); + } + + double get_command(const std::string & interface_name) const + { + return actuator_commands_.at(interface_name)->get_value(); + } + protected: HardwareInfo info_; + std::map joint_state_interfaces_; + std::map joint_command_interfaces_; + +private: + std::map> actuator_states_; + std::map> actuator_commands_; + rclcpp_lifecycle::State lifecycle_state_; }; diff --git a/hardware_interface/include/hardware_interface/component_parser.hpp b/hardware_interface/include/hardware_interface/component_parser.hpp index 5112f7927e..db11c6f0a6 100644 --- a/hardware_interface/include/hardware_interface/component_parser.hpp +++ b/hardware_interface/include/hardware_interface/component_parser.hpp @@ -33,6 +33,51 @@ namespace hardware_interface HARDWARE_INTERFACE_PUBLIC std::vector parse_control_resources_from_urdf(const std::string & urdf); +/** + * \param[in] hw_info the hardware description + * \return vector filled with information about robot's SommandInterfaces for the joints + * which are exported + */ +HARDWARE_INTERFACE_PUBLIC +std::vector parse_joint_state_interface_descriptions_from_hardware_info( + const HardwareInfo & hw_info); + +/** + * \param[in] hw_info the hardware description + * \return vector filled with information about robot's SommandInterfaces for the sensors + * which are exported + */ +HARDWARE_INTERFACE_PUBLIC +std::vector parse_sensor_state_interface_descriptions_from_hardware_info( + const HardwareInfo & hw_info); + +/** + * \param[in] hw_info the hardware description + * \return vector filled with information about robot's SommandInterfaces for the gpios + * which are exported + */ +HARDWARE_INTERFACE_PUBLIC +std::vector parse_gpio_state_interface_descriptions_from_hardware_info( + const HardwareInfo & hw_info); + +/** + * \param[in] hw_info the hardware description + * \return vector filled with information about robot's CommandInterfaces for the joints + * which are exported + */ +HARDWARE_INTERFACE_PUBLIC +std::vector parse_joint_command_interface_descriptions_from_hardware_info( + const HardwareInfo & hw_info); + +/** + * \param[in] hw_info the hardware description + * \return vector filled with information about robot's CommandInterfaces for the gpios + * which are exported + */ +HARDWARE_INTERFACE_PUBLIC +std::vector parse_gpio_command_interface_descriptions_from_hardware_info( + const HardwareInfo & hw_info); + HARDWARE_INTERFACE_PUBLIC bool parse_bool(const std::string & bool_string); diff --git a/hardware_interface/include/hardware_interface/handle.hpp b/hardware_interface/include/hardware_interface/handle.hpp index 1aea017754..e8103df5d1 100644 --- a/hardware_interface/include/hardware_interface/handle.hpp +++ b/hardware_interface/include/hardware_interface/handle.hpp @@ -15,44 +15,66 @@ #ifndef HARDWARE_INTERFACE__HANDLE_HPP_ #define HARDWARE_INTERFACE__HANDLE_HPP_ +#include #include #include +#include +#include "hardware_interface/hardware_info.hpp" #include "hardware_interface/macros.hpp" #include "hardware_interface/visibility_control.h" namespace hardware_interface { + +typedef std::variant HANDLE_DATATYPE; + /// A handle used to get and set a value on a given interface. -class ReadOnlyHandle +class Handle { public: - ReadOnlyHandle( + [[deprecated("Use InterfaceDescription for initializing the Interface")]] + + Handle( 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) { } - explicit ReadOnlyHandle(const std::string & interface_name) + explicit Handle(const InterfaceDescription & interface_description) + : prefix_name_(interface_description.prefix_name), + interface_name_(interface_description.interface_info.name) + { + // As soon as multiple datatypes are used in HANDLE_DATATYPE + // we need to initialize according the type passed in interface description + value_ = std::numeric_limits::quiet_NaN(); + value_ptr_ = std::get_if(&value_); + } + + [[deprecated("Use InterfaceDescription for initializing the Interface")]] + + explicit Handle(const std::string & interface_name) : interface_name_(interface_name), value_ptr_(nullptr) { } - explicit ReadOnlyHandle(const char * interface_name) + [[deprecated("Use InterfaceDescription for initializing the Interface")]] + + explicit Handle(const char * interface_name) : interface_name_(interface_name), value_ptr_(nullptr) { } - ReadOnlyHandle(const ReadOnlyHandle & other) = default; + Handle(const Handle & other) = default; - ReadOnlyHandle(ReadOnlyHandle && other) = default; + Handle(Handle && other) = default; - ReadOnlyHandle & operator=(const ReadOnlyHandle & other) = default; + Handle & operator=(const Handle & other) = default; - ReadOnlyHandle & operator=(ReadOnlyHandle && other) = default; + Handle & operator=(Handle && other) = default; - virtual ~ReadOnlyHandle() = default; + virtual ~Handle() = default; /// Returns true if handle references a value. inline operator bool() const { return value_ptr_ != nullptr; } @@ -71,61 +93,54 @@ class ReadOnlyHandle const std::string & get_prefix_name() const { return prefix_name_; } double get_value() const - { + { // BEGIN (Handle export change): for backward compatibility + // TODO(Manuel) return value_ if old functionality is removed THROW_ON_NULLPTR(value_ptr_); return *value_ptr_; + // END + } + + void set_value(double value) + { + // BEGIN (Handle export change): for backward compatibility + // TODO(Manuel) set value_ directly if old functionality is removed + THROW_ON_NULLPTR(this->value_ptr_); + *this->value_ptr_ = value; + // END } protected: std::string prefix_name_; std::string interface_name_; + HANDLE_DATATYPE value_; + // BEGIN (Handle export change): for backward compatibility + // TODO(Manuel) redeclare as HANDLE_DATATYPE * value_ptr_ if old functionality is removed double * value_ptr_; + // END }; -class ReadWriteHandle : public ReadOnlyHandle +class StateInterface : public Handle { public: - ReadWriteHandle( - const std::string & prefix_name, const std::string & interface_name, - double * value_ptr = nullptr) - : ReadOnlyHandle(prefix_name, interface_name, value_ptr) + explicit StateInterface(const InterfaceDescription & interface_description) + : Handle(interface_description) { } - explicit ReadWriteHandle(const std::string & interface_name) : ReadOnlyHandle(interface_name) {} - - explicit ReadWriteHandle(const char * interface_name) : ReadOnlyHandle(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; - - void set_value(double value) - { - THROW_ON_NULLPTR(this->value_ptr_); - *this->value_ptr_ = value; - } -}; - -class StateInterface : public ReadOnlyHandle -{ -public: StateInterface(const StateInterface & other) = default; StateInterface(StateInterface && other) = default; - using ReadOnlyHandle::ReadOnlyHandle; + using Handle::Handle; }; -class CommandInterface : public ReadWriteHandle +class CommandInterface : public Handle { public: + explicit CommandInterface(const InterfaceDescription & interface_description) + : Handle(interface_description) + { + } /// CommandInterface copy constructor is actively deleted. /** * Command interfaces are having a unique ownership and thus @@ -136,7 +151,7 @@ class CommandInterface : public ReadWriteHandle CommandInterface(CommandInterface && other) = default; - using ReadWriteHandle::ReadWriteHandle; + using Handle::Handle; }; } // namespace hardware_interface diff --git a/hardware_interface/include/hardware_interface/hardware_info.hpp b/hardware_interface/include/hardware_interface/hardware_info.hpp index eb6b63cfc3..be5ba036f1 100644 --- a/hardware_interface/include/hardware_interface/hardware_info.hpp +++ b/hardware_interface/include/hardware_interface/hardware_info.hpp @@ -102,6 +102,37 @@ struct TransmissionInfo std::unordered_map parameters; }; +/** + * This structure stores information about an interface for a specific hardware which should be + * instantiated internally. + */ +struct InterfaceDescription +{ + InterfaceDescription(const std::string & prefix_name_in, const InterfaceInfo & interface_info_in) + : prefix_name(prefix_name_in), interface_info(interface_info_in) + { + } + + /** + * Name of the interface defined by the user. + */ + std::string prefix_name; + + /** + * What type of component is exported: joint, sensor or gpio + */ + std::string component_type; + + /** + * Information about the Interface type (position, velocity,...) as well as limits and so on. + */ + InterfaceInfo interface_info; + + std::string get_name() const { return prefix_name + "/" + interface_info.name; } + + std::string get_interface_type() const { return interface_info.name; } +}; + /// This structure stores information about hardware defined in a robot's URDF. struct HardwareInfo { diff --git a/hardware_interface/include/hardware_interface/sensor.hpp b/hardware_interface/include/hardware_interface/sensor.hpp index 5d0677c587..8aaa2cf923 100644 --- a/hardware_interface/include/hardware_interface/sensor.hpp +++ b/hardware_interface/include/hardware_interface/sensor.hpp @@ -65,8 +65,14 @@ class Sensor final HARDWARE_INTERFACE_PUBLIC const rclcpp_lifecycle::State & error(); + [[deprecated( + "Replaced by vector> on_export_state_interfaces() method. " + "Exporting is handled by the Framework.")]] HARDWARE_INTERFACE_PUBLIC + std::vector + export_state_interfaces(); + HARDWARE_INTERFACE_PUBLIC - std::vector export_state_interfaces(); + std::vector> on_export_state_interfaces(); HARDWARE_INTERFACE_PUBLIC std::string get_name() const; diff --git a/hardware_interface/include/hardware_interface/sensor_interface.hpp b/hardware_interface/include/hardware_interface/sensor_interface.hpp index 14a59e4588..239e1b06c1 100644 --- a/hardware_interface/include/hardware_interface/sensor_interface.hpp +++ b/hardware_interface/include/hardware_interface/sensor_interface.hpp @@ -15,10 +15,14 @@ #ifndef HARDWARE_INTERFACE__SENSOR_INTERFACE_HPP_ #define HARDWARE_INTERFACE__SENSOR_INTERFACE_HPP_ +#include +#include #include #include +#include #include +#include "hardware_interface/component_parser.hpp" #include "hardware_interface/handle.hpp" #include "hardware_interface/hardware_info.hpp" #include "hardware_interface/types/hardware_interface_return_values.hpp" @@ -97,11 +101,31 @@ class SensorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI virtual CallbackReturn on_init(const HardwareInfo & hardware_info) { info_ = hardware_info; + import_state_interface_descriptions(info_); return CallbackReturn::SUCCESS; }; + /** + * Import the InterfaceDescription for the StateInterfaces from the HardwareInfo. + * Separate them into the possible types: Sensor and store them. + */ + virtual void import_state_interface_descriptions(const HardwareInfo & hardware_info) + { + auto sensor_state_interface_descriptions = + parse_sensor_state_interface_descriptions_from_hardware_info(hardware_info); + for (const auto & description : sensor_state_interface_descriptions) + { + sensor_state_interfaces_.insert(std::make_pair(description.get_name(), description)); + } + } + /// Exports all state interfaces for this hardware interface. /** + * Default implementation for exporting the StateInterfaces. The StateInterfaces are created + * according to the InterfaceDescription. The memory accessed by the controllers and hardware is + * assigned here and resides in the system_interface. + * + * If overwritten: * The state interfaces have to be created and transferred according * to the hardware info passed in for the configuration. * @@ -109,7 +133,33 @@ class SensorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI * * \return vector of state interfaces */ - virtual std::vector export_state_interfaces() = 0; + [[deprecated( + "Replaced by vector> on_export_state_interfaces() method. " + "Exporting is handled " + "by the Framework.")]] virtual std::vector + export_state_interfaces() + { + // return empty vector by default. For backward compatibility we check if all vectors is empty + // and if so call on_export_state_interfaces() + std::vector state_interfaces; + return state_interfaces; + } + + std::vector> on_export_state_interfaces() + { + std::vector> state_interfaces; + state_interfaces.reserve(sensor_state_interfaces_.size()); + + for (const auto & [name, descr] : sensor_state_interfaces_) + { + // TODO(Manuel) maybe check for duplicates otherwise only the first appearance of "name" is + // inserted + sensor_states_.insert(std::make_pair(name, std::make_shared(descr))); + state_interfaces.push_back(sensor_states_.at(name)); + } + + return state_interfaces; + } /// Read the current state values from the actuator. /** @@ -141,8 +191,24 @@ class SensorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI */ void set_state(const rclcpp_lifecycle::State & new_state) { lifecycle_state_ = new_state; } + void set_state(const std::string & interface_name, const double & value) + { + sensor_states_.at(interface_name)->set_value(value); + } + + double get_state(const std::string & interface_name) const + { + return sensor_states_.at(interface_name)->get_value(); + } + protected: HardwareInfo info_; + + std::map sensor_state_interfaces_; + +private: + std::map> sensor_states_; + rclcpp_lifecycle::State lifecycle_state_; }; diff --git a/hardware_interface/include/hardware_interface/system.hpp b/hardware_interface/include/hardware_interface/system.hpp index ece14f814d..5267eaad59 100644 --- a/hardware_interface/include/hardware_interface/system.hpp +++ b/hardware_interface/include/hardware_interface/system.hpp @@ -65,11 +65,23 @@ class System final HARDWARE_INTERFACE_PUBLIC const rclcpp_lifecycle::State & error(); + [[deprecated( + "Replaced by vector> on_export_state_interfaces() method. " + "Exporting is handled by the Framework.")]] HARDWARE_INTERFACE_PUBLIC + std::vector + export_state_interfaces(); + HARDWARE_INTERFACE_PUBLIC - std::vector export_state_interfaces(); + std::vector> on_export_state_interfaces(); + + [[deprecated( + "Replaced by vector> on_export_state_interfaces() method. " + "Exporting is handled by the Framework.")]] HARDWARE_INTERFACE_PUBLIC + std::vector + export_command_interfaces(); HARDWARE_INTERFACE_PUBLIC - std::vector export_command_interfaces(); + std::vector> on_export_command_interfaces(); HARDWARE_INTERFACE_PUBLIC return_type prepare_command_mode_switch( diff --git a/hardware_interface/include/hardware_interface/system_interface.hpp b/hardware_interface/include/hardware_interface/system_interface.hpp index e5c6f2f542..601b20a893 100644 --- a/hardware_interface/include/hardware_interface/system_interface.hpp +++ b/hardware_interface/include/hardware_interface/system_interface.hpp @@ -15,13 +15,18 @@ #ifndef HARDWARE_INTERFACE__SYSTEM_INTERFACE_HPP_ #define HARDWARE_INTERFACE__SYSTEM_INTERFACE_HPP_ +#include +#include #include #include +#include #include +#include "hardware_interface/component_parser.hpp" #include "hardware_interface/handle.hpp" #include "hardware_interface/hardware_info.hpp" #include "hardware_interface/types/hardware_interface_return_values.hpp" +#include "hardware_interface/types/hardware_interface_type_values.hpp" #include "hardware_interface/types/lifecycle_state_names.hpp" #include "lifecycle_msgs/msg/state.hpp" #include "rclcpp/duration.hpp" @@ -98,11 +103,64 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI virtual CallbackReturn on_init(const HardwareInfo & hardware_info) { info_ = hardware_info; + import_state_interface_descriptions(info_); + import_command_interface_descriptions(info_); return CallbackReturn::SUCCESS; }; + /** + * Import the InterfaceDescription for the StateInterfaces from the HardwareInfo. + * Separate them into the possible types: Joint, GPIO, Sensor and store them. + */ + void import_state_interface_descriptions(const HardwareInfo & hardware_info) + { + auto joint_state_interface_descriptions = + parse_joint_state_interface_descriptions_from_hardware_info(hardware_info); + for (const auto & description : joint_state_interface_descriptions) + { + joint_state_interfaces_.insert(std::make_pair(description.get_name(), description)); + } + auto sensor_state_interface_descriptions = + parse_sensor_state_interface_descriptions_from_hardware_info(hardware_info); + for (const auto & description : sensor_state_interface_descriptions) + { + sensor_state_interfaces_.insert(std::make_pair(description.get_name(), description)); + } + auto gpio_state_interface_descriptions = + parse_gpio_state_interface_descriptions_from_hardware_info(hardware_info); + for (const auto & description : gpio_state_interface_descriptions) + { + gpio_state_interfaces_.insert(std::make_pair(description.get_name(), description)); + } + } + + /** + * Import the InterfaceDescription for the CommandInterfaces from the HardwareInfo. + * Separate them into the possible types: Joint and GPIO and store them. + */ + void import_command_interface_descriptions(const HardwareInfo & hardware_info) + { + auto joint_command_interface_descriptions = + parse_joint_command_interface_descriptions_from_hardware_info(hardware_info); + for (const auto & description : joint_command_interface_descriptions) + { + joint_command_interfaces_.insert(std::make_pair(description.get_name(), description)); + } + auto gpio_command_interface_descriptions = + parse_gpio_command_interface_descriptions_from_hardware_info(hardware_info); + for (const auto & description : gpio_command_interface_descriptions) + { + gpio_command_interfaces_.insert(std::make_pair(description.get_name(), description)); + } + } + /// Exports all state interfaces for this hardware interface. /** + * Default implementation for exporting the StateInterfaces. The StateInterfaces are created + * according to the InterfaceDescription. The memory accessed by the controllers and hardware is + * assigned here and resides in the system_interface. + * + * If overwritten: * The state interfaces have to be created and transferred according * to the hardware info passed in for the configuration. * @@ -110,10 +168,49 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI * * \return vector of state interfaces */ - virtual std::vector export_state_interfaces() = 0; + [[deprecated( + "Replaced by vector> on_export_state_interfaces() method. " + "Exporting is handled " + "by the Framework.")]] virtual std::vector + export_state_interfaces() + { + // return empty vector by default. For backward compatibility we check if all vectors is empty + // and if so call on_export_state_interfaces() + std::vector state_interfaces; + return state_interfaces; + } + + std::vector> on_export_state_interfaces() + { + std::vector> state_interfaces; + state_interfaces.reserve( + joint_state_interfaces_.size() + sensor_state_interfaces_.size() + + gpio_state_interfaces_.size()); + + for (const auto & [name, descr] : joint_state_interfaces_) + { + system_states_.insert(std::make_pair(name, std::make_shared(descr))); + state_interfaces.push_back(system_states_.at(name)); + } + for (const auto & [name, descr] : sensor_state_interfaces_) + { + system_states_.insert(std::make_pair(name, std::make_shared(descr))); + state_interfaces.push_back(system_states_.at(name)); + } + for (const auto & [name, descr] : gpio_state_interfaces_) + { + system_states_.insert(std::make_pair(name, std::make_shared(descr))); + state_interfaces.push_back(system_states_.at(name)); + } + return state_interfaces; + } /// Exports all command interfaces for this hardware interface. /** + * Default implementation for exporting the CommandInterfaces. The CommandInterfaces are created + * according to the InterfaceDescription. The memory accessed by the controllers and hardware is + * assigned here and resides in the system_interface. + * * The command interfaces have to be created and transferred according * to the hardware info passed in for the configuration. * @@ -121,7 +218,37 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI * * \return vector of command interfaces */ - virtual std::vector export_command_interfaces() = 0; + [[deprecated( + "Replaced by vector> on_export_command_interfaces() method. " + "Exporting is " + "handled " + "by the Framework.")]] virtual std::vector + export_command_interfaces() + { + // return empty vector by default. For backward compatibility we check if all vectors is empty + // and if so call on_export_command_interfaces() + std::vector command_interfaces; + return command_interfaces; + } + + std::vector> on_export_command_interfaces() + { + std::vector> command_interfaces; + command_interfaces.reserve(joint_command_interfaces_.size() + gpio_command_interfaces_.size()); + + for (const auto & [name, descr] : joint_command_interfaces_) + { + system_commands_.insert(std::make_pair(name, std::make_shared(descr))); + command_interfaces.push_back(system_commands_.at(name)); + } + + for (const auto & [name, descr] : gpio_command_interfaces_) + { + system_commands_.insert(std::make_pair(name, std::make_shared(descr))); + command_interfaces.push_back(system_commands_.at(name)); + } + return command_interfaces; + } /// Prepare for a new command interface switch. /** @@ -203,8 +330,40 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI */ void set_state(const rclcpp_lifecycle::State & new_state) { lifecycle_state_ = new_state; } + void set_state(const std::string & interface_name, const double & value) + { + system_states_.at(interface_name)->set_value(value); + } + + double get_state(const std::string & interface_name) const + { + return system_states_.at(interface_name)->get_value(); + } + + void set_command(const std::string & interface_name, const double & value) + { + system_commands_.at(interface_name)->set_value(value); + } + + double get_command(const std::string & interface_name) const + { + return system_commands_.at(interface_name)->get_value(); + } + protected: HardwareInfo info_; + std::map joint_state_interfaces_; + std::map joint_command_interfaces_; + + std::map sensor_state_interfaces_; + + std::map gpio_state_interfaces_; + std::map gpio_command_interfaces_; + +private: + std::map> system_states_; + std::map> system_commands_; + rclcpp_lifecycle::State lifecycle_state_; }; diff --git a/hardware_interface/src/actuator.cpp b/hardware_interface/src/actuator.cpp index 694e92355e..837449873e 100644 --- a/hardware_interface/src/actuator.cpp +++ b/hardware_interface/src/actuator.cpp @@ -192,12 +192,22 @@ std::vector Actuator::export_state_interfaces() return impl_->export_state_interfaces(); } +std::vector> Actuator::on_export_state_interfaces() +{ + return impl_->on_export_state_interfaces(); +} + std::vector Actuator::export_command_interfaces() { // TODO(karsten1987): Might be worth to do some brief sanity check here return impl_->export_command_interfaces(); } +std::vector> Actuator::on_export_command_interfaces() +{ + return impl_->on_export_command_interfaces(); +} + return_type Actuator::prepare_command_mode_switch( const std::vector & start_interfaces, const std::vector & stop_interfaces) diff --git a/hardware_interface/src/component_parser.cpp b/hardware_interface/src/component_parser.cpp index 8e9b6bf16b..2f90e9e26d 100644 --- a/hardware_interface/src/component_parser.cpp +++ b/hardware_interface/src/component_parser.cpp @@ -621,4 +621,91 @@ bool parse_bool(const std::string & bool_string) return bool_string == "true" || bool_string == "True"; } +std::vector parse_joint_state_interface_descriptions_from_hardware_info( + const HardwareInfo & hw_info) +{ + std::vector joint_state_interface_descriptions; + joint_state_interface_descriptions.reserve(hw_info.joints.size()); + + for (const auto & joint : hw_info.joints) + { + for (const auto & state_interface : joint.state_interfaces) + { + joint_state_interface_descriptions.emplace_back( + InterfaceDescription(joint.name, state_interface)); + } + } + return joint_state_interface_descriptions; +} + +std::vector parse_sensor_state_interface_descriptions_from_hardware_info( + const HardwareInfo & hw_info) +{ + std::vector sensor_state_interface_descriptions; + sensor_state_interface_descriptions.reserve(hw_info.sensors.size()); + + for (const auto & sensor : hw_info.sensors) + { + for (const auto & state_interface : sensor.state_interfaces) + { + sensor_state_interface_descriptions.emplace_back( + InterfaceDescription(sensor.name, state_interface)); + } + } + return sensor_state_interface_descriptions; +} + +std::vector parse_gpio_state_interface_descriptions_from_hardware_info( + const HardwareInfo & hw_info) +{ + std::vector gpio_state_interface_descriptions; + gpio_state_interface_descriptions.reserve(hw_info.gpios.size()); + + for (const auto & gpio : hw_info.gpios) + { + for (const auto & state_interface : gpio.state_interfaces) + { + gpio_state_interface_descriptions.emplace_back( + InterfaceDescription(gpio.name, state_interface)); + } + } + return gpio_state_interface_descriptions; +} + +std::vector parse_joint_command_interface_descriptions_from_hardware_info( + const HardwareInfo & hw_info) +{ + std::vector + gpio_state_intejoint_command_interface_descriptionsrface_descriptions; + gpio_state_intejoint_command_interface_descriptionsrface_descriptions.reserve( + hw_info.joints.size()); + + for (const auto & joint : hw_info.joints) + { + for (const auto & command_interface : joint.command_interfaces) + { + gpio_state_intejoint_command_interface_descriptionsrface_descriptions.emplace_back( + InterfaceDescription(joint.name, command_interface)); + } + } + return gpio_state_intejoint_command_interface_descriptionsrface_descriptions; +} + +std::vector parse_gpio_command_interface_descriptions_from_hardware_info( + const HardwareInfo & hw_info) +{ + std::vector gpio_command_interface_descriptions; + gpio_command_interface_descriptions.reserve(hw_info.gpios.size()); + + for (const auto & gpio : hw_info.gpios) + { + for (const auto & command_interface : gpio.command_interfaces) + { + gpio_command_interface_descriptions.emplace_back( + InterfaceDescription(gpio.name, command_interface)); + } + } + return gpio_command_interface_descriptions; +} + } // namespace hardware_interface diff --git a/hardware_interface/src/resource_manager.cpp b/hardware_interface/src/resource_manager.cpp index 5c5d5af3e6..0e1dafd57b 100644 --- a/hardware_interface/src/resource_manager.cpp +++ b/hardware_interface/src/resource_manager.cpp @@ -429,28 +429,126 @@ class ResourceStorage return result; } + void insert_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); + } + } + + // BEGIN (Handle export change): for backward compatibility, can be removed if + // export_state_interfaces() method is removed + void insert_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); + } + } + // END: for backward compatibility + 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) + std::vector interfaces = hardware.export_state_interfaces(); + // If no StateInterfaces has been exported, this could mean: + // a) there is nothing to export -> on_export_state_interfaces() does return nothing as well + // b) default implementation for export_state_interfaces() is used -> new functionality -> + // Framework exports and creates everything + if (interfaces.empty()) { - auto key = interface.get_name(); - state_interface_map_.emplace(std::make_pair(key, std::move(interface))); - interface_names.push_back(key); + std::vector> interface_ptrs = + hardware.on_export_state_interfaces(); + interface_names.reserve(interface_ptrs.size()); + for (auto const & interface : interface_ptrs) + { + insert_state_interface(interface); + interface_names.push_back(interface->get_name()); + } } + // BEGIN (Handle export change): for backward compatibility, can be removed if + // export_state_interfaces() method is removed + else + { + interface_names.reserve(interfaces.size()); + for (auto const & interface : interfaces) + { + insert_state_interface(interface); + interface_names.push_back(interface.get_name()); + } + } + // END: for backward compatibility + hardware_info_map_[hardware.get_name()].state_interfaces = interface_names; available_state_interfaces_.reserve( available_state_interfaces_.capacity() + interface_names.size()); } + void insert_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); + } + } + + // BEGIN (Handle export change): for backward compatibility, can be removed if + // export_command_interfaces() method is removed + void insert_command_interface(CommandInterface && command_interface) + { + std::string key = command_interface.get_name(); + const auto [it, success] = command_interface_map_.emplace( + std::make_pair(key, std::make_shared(std::move(command_interface)))); + if (!success) + { + std::string msg( + "ResourceStorage: Tried to insert CommandInterface with already existing key. Insert[" + + key + "]"); + throw std::runtime_error(msg); + } + } + // END: for backward compatibility + template void import_command_interfaces(HardwareT & hardware) { auto interfaces = hardware.export_command_interfaces(); - hardware_info_map_[hardware.get_name()].command_interfaces = add_command_interfaces(interfaces); + // If no CommandInterface has been exported, this could mean: + // a) there is nothing to export -> on_export_command_interfaces() does return nothing as well + // b) default implementation for export_command_interfaces() is used -> new functionality -> + // Framework exports and creates everything + if (interfaces.empty()) + { + std::vector> interface_ptrs = + hardware.on_export_command_interfaces(); + hardware_info_map_[hardware.get_name()].command_interfaces = + add_command_interfaces(interface_ptrs); + } + // BEGIN (Handle export change): for backward compatibility, can be removed if + // export_command_interfaces() method is removed + else + { + hardware_info_map_[hardware.get_name()].command_interfaces = + add_command_interfaces(interfaces); + } + // END: for backward compatibility } /// Adds exported command interfaces into internal storage. @@ -464,6 +562,8 @@ class ResourceStorage * \returns list of interface names that are added into internal storage. The output is used to * avoid additional iterations to cache interface names, e.g., for initializing info structures. */ + // BEGIN (Handle export change): for backward compatibility, can be removed if + // export_command_interfaces() method is removed std::vector add_command_interfaces(std::vector & interfaces) { std::vector interface_names; @@ -471,7 +571,26 @@ class ResourceStorage for (auto & interface : interfaces) { auto key = interface.get_name(); - command_interface_map_.emplace(std::make_pair(key, std::move(interface))); + insert_command_interface(std::move(interface)); + claimed_command_interface_map_.emplace(std::make_pair(key, false)); + interface_names.push_back(key); + } + available_command_interfaces_.reserve( + available_command_interfaces_.capacity() + interface_names.size()); + + return interface_names; + } + // END: for backward compatibility + + std::vector add_command_interfaces( + std::vector> & interfaces) + { + std::vector interface_names; + interface_names.reserve(interfaces.size()); + for (auto & interface : interfaces) + { + auto key = interface->get_name(); + insert_command_interface(interface); claimed_command_interface_map_.emplace(std::make_pair(key, false)); interface_names.push_back(key); } @@ -708,9 +827,9 @@ class ResourceStorage std::unordered_map> controllers_reference_interfaces_map_; /// Storage of all available state interfaces - std::map state_interface_map_; + std::map> state_interface_map_; /// Storage of all available command interfaces - std::map command_interface_map_; + std::map> command_interface_map_; /// Vectors with interfaces available to controllers (depending on hardware component state) std::vector available_state_interfaces_; @@ -805,7 +924,7 @@ LoanedStateInterface ResourceManager::claim_state_interface(const std::string & } std::lock_guard guard(resource_interfaces_lock_); - return LoanedStateInterface(resource_storage_->state_interface_map_.at(key)); + return LoanedStateInterface(*(resource_storage_->state_interface_map_.at(key))); } // CM API: Called in "callback/slow"-thread @@ -976,7 +1095,7 @@ LoanedCommandInterface ResourceManager::claim_command_interface(const std::strin resource_storage_->claimed_command_interface_map_[key] = true; std::lock_guard guard(resource_interfaces_lock_); return LoanedCommandInterface( - resource_storage_->command_interface_map_.at(key), + *(resource_storage_->command_interface_map_.at(key)), std::bind(&ResourceManager::release_command_interface, this, key)); } diff --git a/hardware_interface/src/sensor.cpp b/hardware_interface/src/sensor.cpp index ade9b8781a..41e29957fd 100644 --- a/hardware_interface/src/sensor.cpp +++ b/hardware_interface/src/sensor.cpp @@ -189,6 +189,11 @@ std::vector Sensor::export_state_interfaces() return impl_->export_state_interfaces(); } +std::vector> Sensor::on_export_state_interfaces() +{ + return impl_->on_export_state_interfaces(); +} + std::string Sensor::get_name() const { return impl_->get_name(); } const rclcpp_lifecycle::State & Sensor::get_state() const { return impl_->get_state(); } diff --git a/hardware_interface/src/system.cpp b/hardware_interface/src/system.cpp index f8703a47bc..1ef70a8782 100644 --- a/hardware_interface/src/system.cpp +++ b/hardware_interface/src/system.cpp @@ -189,11 +189,21 @@ std::vector System::export_state_interfaces() return impl_->export_state_interfaces(); } +std::vector> System::on_export_state_interfaces() +{ + return impl_->on_export_state_interfaces(); +} + std::vector System::export_command_interfaces() { return impl_->export_command_interfaces(); } +std::vector> System::on_export_command_interfaces() +{ + return impl_->on_export_command_interfaces(); +} + return_type System::prepare_command_mode_switch( const std::vector & start_interfaces, const std::vector & stop_interfaces) diff --git a/hardware_interface/test/mock_components/test_generic_system.cpp b/hardware_interface/test/mock_components/test_generic_system.cpp index 57b5796d0d..ad19353f0f 100644 --- a/hardware_interface/test/mock_components/test_generic_system.cpp +++ b/hardware_interface/test/mock_components/test_generic_system.cpp @@ -515,7 +515,6 @@ class TestGenericSystem : public ::testing::Test 2.78 - @@ -546,7 +545,6 @@ class TestGenericSystem : public ::testing::Test 2.78 - @@ -555,7 +553,6 @@ class TestGenericSystem : public ::testing::Test 2.78 - diff --git a/hardware_interface/test/test_component_interfaces.cpp b/hardware_interface/test/test_component_interfaces.cpp index 128771058b..27f13cd202 100644 --- a/hardware_interface/test/test_component_interfaces.cpp +++ b/hardware_interface/test/test_component_interfaces.cpp @@ -33,6 +33,8 @@ #include "hardware_interface/types/lifecycle_state_names.hpp" #include "lifecycle_msgs/msg/state.hpp" #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" +#include "ros2_control_test_assets/components_urdfs.hpp" +#include "ros2_control_test_assets/descriptions.hpp" // Values to send over command interface to trigger error in write and read methods @@ -993,3 +995,353 @@ TEST(TestComponentInterfaces, dummy_system_write_error_behavior) EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, state.id()); EXPECT_EQ(hardware_interface::lifecycle_state_names::FINALIZED, state.label()); } + +namespace test_components +{ +class DummySensorDefault : public hardware_interface::SensorInterface +{ + CallbackReturn on_init(const hardware_interface::HardwareInfo & info) override + { + if ( + hardware_interface::SensorInterface::on_init(info) != + hardware_interface::CallbackReturn::SUCCESS) + { + return hardware_interface::CallbackReturn::ERROR; + } + + // We hardcode the info + return CallbackReturn::SUCCESS; + } + + CallbackReturn on_configure(const rclcpp_lifecycle::State & /*previous_state*/) override + { + for (const auto & [name, descr] : sensor_state_interfaces_) + { + set_state(name, 0.0); + } + read_calls_ = 0; + return CallbackReturn::SUCCESS; + } + + std::string get_name() const override { return "DummySensorDefault"; } + + hardware_interface::return_type read( + const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) override + { + ++read_calls_; + if (read_calls_ == TRIGGER_READ_WRITE_ERROR_CALLS) + { + return hardware_interface::return_type::ERROR; + } + + // no-op, static value + set_state("joint1/voltage", voltage_level_hw_value_); + return hardware_interface::return_type::OK; + } + + CallbackReturn on_error(const rclcpp_lifecycle::State & /*previous_state*/) override + { + if (!recoverable_error_happened_) + { + recoverable_error_happened_ = true; + return CallbackReturn::SUCCESS; + } + else + { + return CallbackReturn::ERROR; + } + return CallbackReturn::FAILURE; + } + +private: + double voltage_level_hw_value_ = 0x666; + + // Helper variables to initiate error on read + int read_calls_ = 0; + bool recoverable_error_happened_ = false; +}; +} // namespace test_components + +TEST(TestComponentInterfaces, dummy_sensor_default_interface_export) +{ + hardware_interface::Sensor sensor_hw(std::make_unique()); + + const std::string urdf_to_test = + std::string(ros2_control_test_assets::urdf_head) + + ros2_control_test_assets::valid_urdf_ros2_control_voltage_sensor_only + + ros2_control_test_assets::urdf_tail; + const std::vector control_resources = + hardware_interface::parse_control_resources_from_urdf(urdf_to_test); + const hardware_interface::HardwareInfo voltage_sensor_res = control_resources[0]; + auto state = sensor_hw.initialize(voltage_sensor_res); + EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); + EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); + + auto state_interfaces = sensor_hw.on_export_state_interfaces(); + ASSERT_EQ(1u, state_interfaces.size()); + EXPECT_EQ("joint1/voltage", state_interfaces[0]->get_name()); + EXPECT_EQ("voltage", state_interfaces[0]->get_interface_name()); + EXPECT_EQ("joint1", state_interfaces[0]->get_prefix_name()); + EXPECT_TRUE(std::isnan(state_interfaces[0]->get_value())); + + // Not updated because is is UNCONFIGURED + sensor_hw.read(TIME, PERIOD); + EXPECT_TRUE(std::isnan(state_interfaces[0]->get_value())); + + // Updated because is is INACTIVE + state = sensor_hw.configure(); + EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, state.id()); + EXPECT_EQ(hardware_interface::lifecycle_state_names::INACTIVE, state.label()); + EXPECT_EQ(0.0, state_interfaces[0]->get_value()); + + // It can read now + sensor_hw.read(TIME, PERIOD); + EXPECT_EQ(0x666, state_interfaces[0]->get_value()); +} + +TEST(TestComponentInterfaces, dummy_sensor_default_read_error_behavior) +{ + hardware_interface::Sensor sensor_hw(std::make_unique()); + + const std::string urdf_to_test = + std::string(ros2_control_test_assets::urdf_head) + + ros2_control_test_assets::valid_urdf_ros2_control_voltage_sensor_only + + ros2_control_test_assets::urdf_tail; + const std::vector control_resources = + hardware_interface::parse_control_resources_from_urdf(urdf_to_test); + const hardware_interface::HardwareInfo voltage_sensor_res = control_resources[0]; + auto state = sensor_hw.initialize(voltage_sensor_res); + + auto state_interfaces = sensor_hw.on_export_state_interfaces(); + // Updated because is is INACTIVE + state = sensor_hw.configure(); + state = sensor_hw.activate(); + EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, state.id()); + EXPECT_EQ(hardware_interface::lifecycle_state_names::ACTIVE, state.label()); + + ASSERT_EQ(hardware_interface::return_type::OK, sensor_hw.read(TIME, PERIOD)); + + // Initiate recoverable error - call read 99 times OK and on 100-time will return error + for (auto i = 2ul; i < TRIGGER_READ_WRITE_ERROR_CALLS; ++i) + { + ASSERT_EQ(hardware_interface::return_type::OK, sensor_hw.read(TIME, PERIOD)); + } + ASSERT_EQ(hardware_interface::return_type::ERROR, sensor_hw.read(TIME, PERIOD)); + + state = sensor_hw.get_state(); + EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); + EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); + + // activate again and expect reset values + state = sensor_hw.configure(); + EXPECT_EQ(state_interfaces[0]->get_value(), 0.0); + + state = sensor_hw.activate(); + EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, state.id()); + EXPECT_EQ(hardware_interface::lifecycle_state_names::ACTIVE, state.label()); + + // Initiate unrecoverable error - call read 99 times OK and on 100-time will return error + for (auto i = 1ul; i < TRIGGER_READ_WRITE_ERROR_CALLS; ++i) + { + ASSERT_EQ(hardware_interface::return_type::OK, sensor_hw.read(TIME, PERIOD)); + } + ASSERT_EQ(hardware_interface::return_type::ERROR, sensor_hw.read(TIME, PERIOD)); + + state = sensor_hw.get_state(); + EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, state.id()); + EXPECT_EQ(hardware_interface::lifecycle_state_names::FINALIZED, state.label()); + + // can not change state anymore + state = sensor_hw.configure(); + EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, state.id()); + EXPECT_EQ(hardware_interface::lifecycle_state_names::FINALIZED, state.label()); +} + +namespace test_components +{ + +class DummyActuatorDefault : public hardware_interface::ActuatorInterface +{ + CallbackReturn on_init(const hardware_interface::HardwareInfo & info) override + { + // We hardcode the info + if ( + hardware_interface::ActuatorInterface::on_init(info) != + hardware_interface::CallbackReturn::SUCCESS) + { + return hardware_interface::CallbackReturn::ERROR; + } + return CallbackReturn::SUCCESS; + } + + CallbackReturn on_configure(const rclcpp_lifecycle::State & /*previous_state*/) override + { + set_state("joint1/position", 0.0); + set_state("joint1/velocity", 0.0); + + if (recoverable_error_happened_) + { + set_command("joint1/velocity", 0.0); + } + + read_calls_ = 0; + write_calls_ = 0; + + return CallbackReturn::SUCCESS; + } + + std::string get_name() const override { return "DummyActuatorDefault"; } + + hardware_interface::return_type read( + const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) override + { + ++read_calls_; + if (read_calls_ == TRIGGER_READ_WRITE_ERROR_CALLS) + { + return hardware_interface::return_type::ERROR; + } + + // no-op, state is getting propagated within write. + return hardware_interface::return_type::OK; + } + + hardware_interface::return_type write( + const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) override + { + ++write_calls_; + if (write_calls_ == TRIGGER_READ_WRITE_ERROR_CALLS) + { + return hardware_interface::return_type::ERROR; + } + auto position_state = get_state("joint1/position"); + set_state("joint1/position", position_state + get_command("joint1/velocity")); + set_state("joint1/velocity", get_command("joint1/velocity")); + + return hardware_interface::return_type::OK; + } + + CallbackReturn on_shutdown(const rclcpp_lifecycle::State & /*previous_state*/) override + { + set_state("joint1/velocity", 0.0); + return CallbackReturn::SUCCESS; + } + + CallbackReturn on_error(const rclcpp_lifecycle::State & /*previous_state*/) override + { + if (!recoverable_error_happened_) + { + recoverable_error_happened_ = true; + return CallbackReturn::SUCCESS; + } + else + { + return CallbackReturn::ERROR; + } + return CallbackReturn::FAILURE; + } + +private: + // Helper variables to initiate error on read + unsigned int read_calls_ = 0; + unsigned int write_calls_ = 0; + bool recoverable_error_happened_ = false; +}; + +} // namespace test_components + +TEST(TestComponentInterfaces, dummy_actuator_default) +{ + hardware_interface::Actuator actuator_hw( + std::make_unique()); + const std::string urdf_to_test = + std::string(ros2_control_test_assets::urdf_head) + + ros2_control_test_assets::valid_urdf_ros2_control_dummy_actuator_only + + ros2_control_test_assets::urdf_tail; + const std::vector control_resources = + hardware_interface::parse_control_resources_from_urdf(urdf_to_test); + const hardware_interface::HardwareInfo dummy_actuator = control_resources[0]; + auto state = actuator_hw.initialize(dummy_actuator); + + EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); + EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); + + auto state_interfaces = actuator_hw.on_export_state_interfaces(); + ASSERT_EQ(2u, state_interfaces.size()); + EXPECT_EQ("joint1/position", state_interfaces[0]->get_name()); + EXPECT_EQ(hardware_interface::HW_IF_POSITION, state_interfaces[0]->get_interface_name()); + EXPECT_EQ("joint1", state_interfaces[0]->get_prefix_name()); + EXPECT_EQ("joint1/velocity", state_interfaces[1]->get_name()); + EXPECT_EQ(hardware_interface::HW_IF_VELOCITY, state_interfaces[1]->get_interface_name()); + EXPECT_EQ("joint1", state_interfaces[1]->get_prefix_name()); + + auto command_interfaces = actuator_hw.on_export_command_interfaces(); + ASSERT_EQ(1u, command_interfaces.size()); + EXPECT_EQ("joint1/velocity", command_interfaces[0]->get_name()); + EXPECT_EQ(hardware_interface::HW_IF_VELOCITY, command_interfaces[0]->get_interface_name()); + EXPECT_EQ("joint1", command_interfaces[0]->get_prefix_name()); + + double velocity_value = 1.0; + command_interfaces[0]->set_value(velocity_value); // velocity + ASSERT_EQ(hardware_interface::return_type::ERROR, actuator_hw.write(TIME, PERIOD)); + + // Noting should change because it is UNCONFIGURED + for (auto step = 0u; step < 10; ++step) + { + ASSERT_EQ(hardware_interface::return_type::ERROR, actuator_hw.read(TIME, PERIOD)); + + ASSERT_TRUE(std::isnan(state_interfaces[0]->get_value())); // position value + ASSERT_TRUE(std::isnan(state_interfaces[1]->get_value())); // velocity + + ASSERT_EQ(hardware_interface::return_type::ERROR, actuator_hw.write(TIME, PERIOD)); + } + + state = actuator_hw.configure(); + EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, state.id()); + EXPECT_EQ(hardware_interface::lifecycle_state_names::INACTIVE, state.label()); + + // Read and Write are working because it is INACTIVE + for (auto step = 0u; step < 10; ++step) + { + ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.read(TIME, PERIOD)); + + EXPECT_EQ(step * velocity_value, state_interfaces[0]->get_value()); // position value + EXPECT_EQ(step ? velocity_value : 0, state_interfaces[1]->get_value()); // velocity + + ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.write(TIME, PERIOD)); + } + + state = actuator_hw.activate(); + EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, state.id()); + EXPECT_EQ(hardware_interface::lifecycle_state_names::ACTIVE, state.label()); + + // Read and Write are working because it is ACTIVE + for (auto step = 0u; step < 10; ++step) + { + ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.read(TIME, PERIOD)); + + EXPECT_EQ((10 + step) * velocity_value, state_interfaces[0]->get_value()); // position value + EXPECT_EQ(velocity_value, state_interfaces[1]->get_value()); // velocity + + ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.write(TIME, PERIOD)); + } + + state = actuator_hw.shutdown(); + EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, state.id()); + EXPECT_EQ(hardware_interface::lifecycle_state_names::FINALIZED, state.label()); + + // Noting should change because it is FINALIZED + for (auto step = 0u; step < 10; ++step) + { + ASSERT_EQ(hardware_interface::return_type::ERROR, actuator_hw.read(TIME, PERIOD)); + + EXPECT_EQ(20 * velocity_value, state_interfaces[0]->get_value()); // position value + EXPECT_EQ(0, state_interfaces[1]->get_value()); // velocity + + ASSERT_EQ(hardware_interface::return_type::ERROR, actuator_hw.write(TIME, PERIOD)); + } + + EXPECT_EQ( + hardware_interface::return_type::OK, actuator_hw.prepare_command_mode_switch({""}, {""})); + EXPECT_EQ( + hardware_interface::return_type::OK, actuator_hw.perform_command_mode_switch({""}, {""})); +} diff --git a/hardware_interface/test/test_component_parser.cpp b/hardware_interface/test/test_component_parser.cpp index b0c7c5a16d..bf50ef0d1e 100644 --- a/hardware_interface/test/test_component_parser.cpp +++ b/hardware_interface/test/test_component_parser.cpp @@ -674,3 +674,113 @@ TEST_F(TestComponentParser, transmission_given_too_many_joints_throws_error) ros2_control_test_assets::urdf_tail; ASSERT_THROW(parse_control_resources_from_urdf(urdf_to_test), std::runtime_error); } + +TEST_F(TestComponentParser, parse_joint_state_interface_descriptions_from_hardware_info) +{ + const std::string urdf_to_test = + std::string(ros2_control_test_assets::urdf_head) + + ros2_control_test_assets::valid_urdf_ros2_control_system_multi_joints_transmission + + ros2_control_test_assets::urdf_tail; + const auto control_hardware = parse_control_resources_from_urdf(urdf_to_test); + + const auto joint_state_descriptions = + parse_joint_state_interface_descriptions_from_hardware_info(control_hardware[0]); + EXPECT_EQ(joint_state_descriptions[0].prefix_name, "joint1"); + EXPECT_EQ(joint_state_descriptions[0].get_interface_type(), "position"); + EXPECT_EQ(joint_state_descriptions[0].get_name(), "joint1/position"); + + EXPECT_EQ(joint_state_descriptions[1].prefix_name, "joint2"); + EXPECT_EQ(joint_state_descriptions[1].get_interface_type(), "position"); + EXPECT_EQ(joint_state_descriptions[1].get_name(), "joint2/position"); +} + +TEST_F(TestComponentParser, parse_joint_command_interface_descriptions_from_hardware_info) +{ + const std::string urdf_to_test = + std::string(ros2_control_test_assets::urdf_head) + + ros2_control_test_assets::valid_urdf_ros2_control_system_multi_joints_transmission + + ros2_control_test_assets::urdf_tail; + const auto control_hardware = parse_control_resources_from_urdf(urdf_to_test); + + const auto joint_command_descriptions = + parse_joint_command_interface_descriptions_from_hardware_info(control_hardware[0]); + EXPECT_EQ(joint_command_descriptions[0].prefix_name, "joint1"); + EXPECT_EQ(joint_command_descriptions[0].get_interface_type(), "position"); + EXPECT_EQ(joint_command_descriptions[0].get_name(), "joint1/position"); + EXPECT_EQ(joint_command_descriptions[0].interface_info.min, "-1"); + EXPECT_EQ(joint_command_descriptions[0].interface_info.max, "1"); + + EXPECT_EQ(joint_command_descriptions[1].prefix_name, "joint2"); + EXPECT_EQ(joint_command_descriptions[1].get_interface_type(), "position"); + EXPECT_EQ(joint_command_descriptions[1].get_name(), "joint2/position"); + EXPECT_EQ(joint_command_descriptions[1].interface_info.min, "-1"); + EXPECT_EQ(joint_command_descriptions[1].interface_info.max, "1"); +} + +TEST_F(TestComponentParser, parse_sensor_state_interface_descriptions_from_hardware_info) +{ + const std::string urdf_to_test = std::string(ros2_control_test_assets::urdf_head) + + ros2_control_test_assets::valid_urdf_ros2_control_sensor_only + + ros2_control_test_assets::urdf_tail; + const auto control_hardware = parse_control_resources_from_urdf(urdf_to_test); + + const auto sensor_state_descriptions = + parse_sensor_state_interface_descriptions_from_hardware_info(control_hardware[0]); + EXPECT_EQ(sensor_state_descriptions[0].prefix_name, "sensor1"); + EXPECT_EQ(sensor_state_descriptions[0].get_interface_type(), "roll"); + EXPECT_EQ(sensor_state_descriptions[0].get_name(), "sensor1/roll"); + EXPECT_EQ(sensor_state_descriptions[1].prefix_name, "sensor1"); + EXPECT_EQ(sensor_state_descriptions[1].get_interface_type(), "pitch"); + EXPECT_EQ(sensor_state_descriptions[1].get_name(), "sensor1/pitch"); + EXPECT_EQ(sensor_state_descriptions[2].prefix_name, "sensor1"); + EXPECT_EQ(sensor_state_descriptions[2].get_interface_type(), "yaw"); + EXPECT_EQ(sensor_state_descriptions[2].get_name(), "sensor1/yaw"); + + EXPECT_EQ(sensor_state_descriptions[3].prefix_name, "sensor2"); + EXPECT_EQ(sensor_state_descriptions[3].get_interface_type(), "image"); + EXPECT_EQ(sensor_state_descriptions[3].get_name(), "sensor2/image"); +} + +TEST_F(TestComponentParser, parse_gpio_state_interface_descriptions_from_hardware_info) +{ + const std::string urdf_to_test = + std::string(ros2_control_test_assets::urdf_head) + + ros2_control_test_assets::valid_urdf_ros2_control_system_robot_with_gpio + + ros2_control_test_assets::urdf_tail; + const auto control_hardware = parse_control_resources_from_urdf(urdf_to_test); + + const auto gpio_state_descriptions = + parse_gpio_state_interface_descriptions_from_hardware_info(control_hardware[0]); + EXPECT_EQ(gpio_state_descriptions[0].prefix_name, "flange_analog_IOs"); + EXPECT_EQ(gpio_state_descriptions[0].get_interface_type(), "analog_output1"); + EXPECT_EQ(gpio_state_descriptions[0].get_name(), "flange_analog_IOs/analog_output1"); + EXPECT_EQ(gpio_state_descriptions[1].prefix_name, "flange_analog_IOs"); + EXPECT_EQ(gpio_state_descriptions[1].get_interface_type(), "analog_input1"); + EXPECT_EQ(gpio_state_descriptions[1].get_name(), "flange_analog_IOs/analog_input1"); + EXPECT_EQ(gpio_state_descriptions[2].prefix_name, "flange_analog_IOs"); + EXPECT_EQ(gpio_state_descriptions[2].get_interface_type(), "analog_input2"); + EXPECT_EQ(gpio_state_descriptions[2].get_name(), "flange_analog_IOs/analog_input2"); + + EXPECT_EQ(gpio_state_descriptions[3].prefix_name, "flange_vacuum"); + EXPECT_EQ(gpio_state_descriptions[3].get_interface_type(), "vacuum"); + EXPECT_EQ(gpio_state_descriptions[3].get_name(), "flange_vacuum/vacuum"); +} + +TEST_F(TestComponentParser, parse_gpio_command_interface_descriptions_from_hardware_info) +{ + const std::string urdf_to_test = + std::string(ros2_control_test_assets::urdf_head) + + ros2_control_test_assets::valid_urdf_ros2_control_system_robot_with_gpio + + ros2_control_test_assets::urdf_tail; + const auto control_hardware = parse_control_resources_from_urdf(urdf_to_test); + + const auto gpio_state_descriptions = + parse_gpio_command_interface_descriptions_from_hardware_info(control_hardware[0]); + EXPECT_EQ(gpio_state_descriptions[0].prefix_name, "flange_analog_IOs"); + EXPECT_EQ(gpio_state_descriptions[0].get_interface_type(), "analog_output1"); + EXPECT_EQ(gpio_state_descriptions[0].get_name(), "flange_analog_IOs/analog_output1"); + + EXPECT_EQ(gpio_state_descriptions[1].prefix_name, "flange_vacuum"); + EXPECT_EQ(gpio_state_descriptions[1].get_interface_type(), "vacuum"); + EXPECT_EQ(gpio_state_descriptions[1].get_name(), "flange_vacuum/vacuum"); +} diff --git a/hardware_interface/test/test_handle.cpp b/hardware_interface/test/test_handle.cpp index 16ca710e9d..844adeaecb 100644 --- a/hardware_interface/test/test_handle.cpp +++ b/hardware_interface/test/test_handle.cpp @@ -14,8 +14,11 @@ #include #include "hardware_interface/handle.hpp" +#include "hardware_interface/hardware_info.hpp" using hardware_interface::CommandInterface; +using hardware_interface::InterfaceDescription; +using hardware_interface::InterfaceInfo; using hardware_interface::StateInterface; namespace @@ -68,3 +71,31 @@ TEST(TestHandle, value_methods_work_on_non_nullptr) EXPECT_NO_THROW(handle.set_value(0.0)); EXPECT_DOUBLE_EQ(handle.get_value(), 0.0); } + +TEST(TestHandle, interface_description_state_interface_name_getters_work) +{ + const std::string POSITION_INTERFACE = "position"; + const std::string JOINT_NAME_1 = "joint1"; + InterfaceInfo info; + info.name = POSITION_INTERFACE; + InterfaceDescription interface_descr(JOINT_NAME_1, info); + StateInterface handle{interface_descr}; + + EXPECT_EQ(handle.get_name(), JOINT_NAME_1 + "/" + POSITION_INTERFACE); + EXPECT_EQ(handle.get_interface_name(), POSITION_INTERFACE); + EXPECT_EQ(handle.get_prefix_name(), JOINT_NAME_1); +} + +TEST(TestHandle, interface_description_command_interface_name_getters_work) +{ + const std::string POSITION_INTERFACE = "position"; + const std::string JOINT_NAME_1 = "joint1"; + InterfaceInfo info; + info.name = POSITION_INTERFACE; + InterfaceDescription interface_descr(JOINT_NAME_1, info); + CommandInterface handle{interface_descr}; + + EXPECT_EQ(handle.get_name(), JOINT_NAME_1 + "/" + POSITION_INTERFACE); + EXPECT_EQ(handle.get_interface_name(), POSITION_INTERFACE); + EXPECT_EQ(handle.get_prefix_name(), JOINT_NAME_1); +} diff --git a/ros2_control_test_assets/include/ros2_control_test_assets/components_urdfs.hpp b/ros2_control_test_assets/include/ros2_control_test_assets/components_urdfs.hpp index a42d39a241..9e183a24ca 100644 --- a/ros2_control_test_assets/include/ros2_control_test_assets/components_urdfs.hpp +++ b/ros2_control_test_assets/include/ros2_control_test_assets/components_urdfs.hpp @@ -398,6 +398,46 @@ const auto valid_urdf_ros2_control_system_robot_with_size_and_data_type = )"; +// Voltage Sensor only +const auto valid_urdf_ros2_control_voltage_sensor_only = + R"( + + + ros2_control_demo_hardware/CameraWithIMUSensor + 2 + + + + + +)"; + +const auto valid_urdf_ros2_control_dummy_actuator_only = + R"( + + + ros2_control_demo_hardware/VelocityActuatorHardware + 1.13 + 3 + + + + -1 + 1 + + + + + + transmission_interface/RotationToLinerTansmission + + 325.949 + + 1337 + + +)"; + const auto valid_urdf_ros2_control_parameter_empty = R"( diff --git a/transmission_interface/include/transmission_interface/handle.hpp b/transmission_interface/include/transmission_interface/handle.hpp index bc1c0a78d8..7696db03d9 100644 --- a/transmission_interface/include/transmission_interface/handle.hpp +++ b/transmission_interface/include/transmission_interface/handle.hpp @@ -22,17 +22,17 @@ namespace transmission_interface { /** A handle used to get and set a value on a given actuator interface. */ -class ActuatorHandle : public hardware_interface::ReadWriteHandle +class ActuatorHandle : public hardware_interface::Handle { public: - using hardware_interface::ReadWriteHandle::ReadWriteHandle; + using hardware_interface::Handle::Handle; }; /** A handle used to get and set a value on a given joint interface. */ -class JointHandle : public hardware_interface::ReadWriteHandle +class JointHandle : public hardware_interface::Handle { public: - using hardware_interface::ReadWriteHandle::ReadWriteHandle; + using hardware_interface::Handle::Handle; }; } // namespace transmission_interface