diff --git a/hardware_interface/CMakeLists.txt b/hardware_interface/CMakeLists.txt index 6d29d5e66c..09c96d9eea 100644 --- a/hardware_interface/CMakeLists.txt +++ b/hardware_interface/CMakeLists.txt @@ -103,6 +103,9 @@ if(BUILD_TESTING) target_link_libraries(test_joint_handle hardware_interface) ament_target_dependencies(test_joint_handle rcpputils) + ament_add_gmock(test_component_interfaces test/test_component_interfaces.cpp) + target_link_libraries(test_component_interfaces hardware_interface) + ament_add_gmock(test_component_parser test/test_component_parser.cpp) target_link_libraries(test_component_parser component_parser) ament_target_dependencies(test_component_parser TinyXML2) diff --git a/hardware_interface/include/hardware_interface/actuator_handle.hpp b/hardware_interface/include/hardware_interface/actuator_handle.hpp index 7f7ee90407..f4270c9478 100644 --- a/hardware_interface/include/hardware_interface/actuator_handle.hpp +++ b/hardware_interface/include/hardware_interface/actuator_handle.hpp @@ -18,22 +18,14 @@ #include #include "hardware_interface/handle.hpp" -#include "hardware_interface/macros.hpp" -#include "hardware_interface/visibility_control.h" namespace hardware_interface { /** A handle used to get and set a value on a given actuator interface. */ -class ActuatorHandle : public Handle +class ActuatorHandle : public ReadWriteHandle { public: - HARDWARE_INTERFACE_PUBLIC - ActuatorHandle( - const std::string & name, const std::string & interface_name, - double * value_ptr = nullptr) - : Handle(name, interface_name, value_ptr) - { - } + using ReadWriteHandle::ReadWriteHandle; }; } // namespace hardware_interface diff --git a/hardware_interface/include/hardware_interface/components/actuator.hpp b/hardware_interface/include/hardware_interface/components/actuator.hpp index c1cbc184f7..29dab6170f 100644 --- a/hardware_interface/include/hardware_interface/components/actuator.hpp +++ b/hardware_interface/include/hardware_interface/components/actuator.hpp @@ -16,7 +16,9 @@ #define HARDWARE_INTERFACE__COMPONENTS__ACTUATOR_HPP_ #include +#include +#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_status_values.hpp" @@ -34,18 +36,35 @@ class Actuator final public: Actuator() = default; + HARDWARE_INTERFACE_PUBLIC explicit Actuator(std::unique_ptr impl); ~Actuator() = default; + HARDWARE_INTERFACE_PUBLIC return_type configure(const HardwareInfo & actuator_info); + HARDWARE_INTERFACE_PUBLIC + std::vector export_state_handles(); + + HARDWARE_INTERFACE_PUBLIC + std::vector export_command_handles(); + + HARDWARE_INTERFACE_PUBLIC return_type start(); + HARDWARE_INTERFACE_PUBLIC return_type stop(); + HARDWARE_INTERFACE_PUBLIC status get_status() const; + HARDWARE_INTERFACE_PUBLIC + return_type read(); + + HARDWARE_INTERFACE_PUBLIC + return_type write(); + private: std::unique_ptr impl_; }; diff --git a/hardware_interface/include/hardware_interface/components/actuator_interface.hpp b/hardware_interface/include/hardware_interface/components/actuator_interface.hpp index 9324800bc9..299bf4727b 100644 --- a/hardware_interface/include/hardware_interface/components/actuator_interface.hpp +++ b/hardware_interface/include/hardware_interface/components/actuator_interface.hpp @@ -16,11 +16,12 @@ #define HARDWARE_INTERFACE__COMPONENTS__ACTUATOR_INTERFACE_HPP_ #include +#include +#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_status_values.hpp" -#include "hardware_interface/visibility_control.h" namespace hardware_interface { @@ -46,16 +47,38 @@ class ActuatorInterface * \return return_type::OK if required data are provided and can be parsed, * return_type::ERROR otherwise. */ - HARDWARE_INTERFACE_PUBLIC virtual return_type configure(const HardwareInfo & actuator_info) = 0; + /// Exports all state handles for this actuator. + /** + * The state handles have to be created and transfered according + * to the actuator info passed in for the configuration. + * + * Note the ownership over the state handles must be released. + * + * \return vector of state handles + */ + virtual + std::vector export_state_handles() = 0; + + /// Exports all command handles for this actuator. + /** + * The command handles have to be created and transfered according + * to the actuator info passed in for the configuration. + * + * Note the ownership over the command handles must be released. + * + * \return vector of command handles + */ + virtual + std::vector export_command_handles() = 0; + /** * \brief Start exchange data with the hardware. * * \return return_type:OK if everything worked as expected, return_type::ERROR otherwise. */ - HARDWARE_INTERFACE_PUBLIC virtual return_type start() = 0; @@ -64,7 +87,6 @@ class ActuatorInterface * * \return return_type:OK if everything worked as expected, return_type::ERROR otherwise. */ - HARDWARE_INTERFACE_PUBLIC virtual return_type stop() = 0; @@ -73,9 +95,29 @@ class ActuatorInterface * * \return status current status. */ - HARDWARE_INTERFACE_PUBLIC virtual status get_status() const = 0; + + /// Read the current state values from the actuator. + /** + * The data readings from the physical hardware has to be updated + * and reflected accordingly in the exported state handles. + * That is, the data pointed by the handles shall be updated. + * + * \return return_type::OK if the read was successful, return_type::ERROR otherwise. + */ + virtual + return_type read() = 0; + + /// Write the current command values to the actuator. + /** + * The physical hardware shall be updated with the latest value from + * the exported command handles. + * + * \return return_type::OK if the read was successful, return_type::ERROR otherwise. + */ + virtual + return_type write() = 0; }; } // namespace components diff --git a/hardware_interface/include/hardware_interface/components/sensor.hpp b/hardware_interface/include/hardware_interface/components/sensor.hpp index 6d824e6dd7..1ab9115b4e 100644 --- a/hardware_interface/include/hardware_interface/components/sensor.hpp +++ b/hardware_interface/include/hardware_interface/components/sensor.hpp @@ -20,6 +20,7 @@ #include #include +#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_status_values.hpp" @@ -37,6 +38,7 @@ class Sensor final public: Sensor() = default; + HARDWARE_INTERFACE_PUBLIC explicit Sensor(std::unique_ptr impl); ~Sensor() = default; @@ -44,6 +46,9 @@ class Sensor final HARDWARE_INTERFACE_PUBLIC return_type configure(const HardwareInfo & sensor_info); + HARDWARE_INTERFACE_PUBLIC + std::vector export_state_handles(); + HARDWARE_INTERFACE_PUBLIC return_type start(); @@ -53,6 +58,9 @@ class Sensor final HARDWARE_INTERFACE_PUBLIC status get_status() const; + HARDWARE_INTERFACE_PUBLIC + return_type read(); + private: std::unique_ptr impl_; }; diff --git a/hardware_interface/include/hardware_interface/components/sensor_interface.hpp b/hardware_interface/include/hardware_interface/components/sensor_interface.hpp index 503f6cee3a..53a76ff04a 100644 --- a/hardware_interface/include/hardware_interface/components/sensor_interface.hpp +++ b/hardware_interface/include/hardware_interface/components/sensor_interface.hpp @@ -47,16 +47,26 @@ class SensorInterface * \return return_type::OK if required data are provided and can be parsed, * return_type::ERROR otherwise. */ - HARDWARE_INTERFACE_PUBLIC virtual return_type configure(const HardwareInfo & sensor_info) = 0; + /// Exports all state handles for this sensor. + /** + * The state handles have to be created and transfered according + * to the sensor info passed in for the configuration. + * + * Note the ownership over the state handles must be released. + * + * \return vector of state handles + */ + virtual + std::vector export_state_handles() = 0; + /** * \brief Start exchange data with the hardware. * * \return return_type:OK if everything worked as expected, return_type::ERROR otherwise. */ - HARDWARE_INTERFACE_PUBLIC virtual return_type start() = 0; @@ -65,7 +75,6 @@ class SensorInterface * * \return return_type:OK if everything worked as expected, return_type::ERROR otherwise. */ - HARDWARE_INTERFACE_PUBLIC virtual return_type stop() = 0; @@ -74,9 +83,19 @@ class SensorInterface * * \return status current status. */ - HARDWARE_INTERFACE_PUBLIC virtual status get_status() const = 0; + + /// Read the current state values from the sensor. + /** + * The data readings from the physical hardware has to be updated + * and reflected accordingly in the exported state handles. + * That is, the data pointed by the handles shall be updated. + * + * \return return_type::OK if the read was successful, return_type::ERROR otherwise. + */ + virtual + return_type read() = 0; }; } // namespace components diff --git a/hardware_interface/include/hardware_interface/components/system.hpp b/hardware_interface/include/hardware_interface/components/system.hpp index fc57ec4e37..506e07544a 100644 --- a/hardware_interface/include/hardware_interface/components/system.hpp +++ b/hardware_interface/include/hardware_interface/components/system.hpp @@ -20,6 +20,7 @@ #include #include +#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_status_values.hpp" @@ -43,6 +44,12 @@ class System final HARDWARE_INTERFACE_PUBLIC return_type configure(const HardwareInfo & system_info); + HARDWARE_INTERFACE_PUBLIC + std::vector export_state_handles(); + + HARDWARE_INTERFACE_PUBLIC + std::vector export_command_handles(); + HARDWARE_INTERFACE_PUBLIC return_type start(); @@ -52,6 +59,12 @@ class System final HARDWARE_INTERFACE_PUBLIC status get_status() const; + HARDWARE_INTERFACE_PUBLIC + return_type read(); + + HARDWARE_INTERFACE_PUBLIC + return_type write(); + private: std::unique_ptr impl_; }; diff --git a/hardware_interface/include/hardware_interface/components/system_interface.hpp b/hardware_interface/include/hardware_interface/components/system_interface.hpp index 9896245d3d..4af654e781 100644 --- a/hardware_interface/include/hardware_interface/components/system_interface.hpp +++ b/hardware_interface/include/hardware_interface/components/system_interface.hpp @@ -48,16 +48,38 @@ class SystemInterface * \return return_type::OK if required data are provided and can be parsed, * return_type::ERROR otherwise. */ - HARDWARE_INTERFACE_PUBLIC virtual return_type configure(const HardwareInfo & system_info) = 0; + /// Exports all state handles for this system. + /** + * The state handles have to be created and transfered according + * to the system info passed in for the configuration. + * + * Note the ownership over the state handles must be released. + * + * \return vector of state handles + */ + virtual + std::vector export_state_handles() = 0; + + /// Exports all command handles for this system. + /** + * The command handles have to be created and transfered according + * to the system info passed in for the configuration. + * + * Note the ownership over the command handles must be released. + * + * \return vector of command handles + */ + virtual + std::vector export_command_handles() = 0; + /** * \brief Start exchange data with the hardware. * * \return return_type:OK if everything worked as expected, return_type::ERROR otherwise. */ - HARDWARE_INTERFACE_PUBLIC virtual return_type start() = 0; @@ -66,7 +88,6 @@ class SystemInterface * * \return return_type:OK if everything worked as expected, return_type::ERROR otherwise. */ - HARDWARE_INTERFACE_PUBLIC virtual return_type stop() = 0; @@ -75,9 +96,29 @@ class SystemInterface * * \return status current status. */ - HARDWARE_INTERFACE_PUBLIC virtual status get_status() const = 0; + + /// Read the current state values from the actuators and sensors within the system. + /** + * The data readings from the physical hardware has to be updated + * and reflected accordingly in the exported state handles. + * That is, the data pointed by the handles shall be updated. + * + * \return return_type::OK if the read was successful, return_type::ERROR otherwise. + */ + virtual + return_type read() = 0; + + /// Write the current command values to the actuator within the system. + /** + * The physical hardware shall be updated with the latest value from + * the exported command handles. + * + * \return return_type::OK if the read was successful, return_type::ERROR otherwise. + */ + virtual + return_type write() = 0; }; } // namespace components diff --git a/hardware_interface/include/hardware_interface/handle.hpp b/hardware_interface/include/hardware_interface/handle.hpp index 998fb0f89d..b32a760d47 100644 --- a/hardware_interface/include/hardware_interface/handle.hpp +++ b/hardware_interface/include/hardware_interface/handle.hpp @@ -16,6 +16,7 @@ #define HARDWARE_INTERFACE__HANDLE_HPP_ #include +#include #include "hardware_interface/macros.hpp" #include "hardware_interface/visibility_control.h" @@ -24,84 +25,135 @@ namespace hardware_interface { /** A handle used to get and set a value on a given interface. */ template -class Handle +class ReadOnlyHandle { public: - HARDWARE_INTERFACE_PUBLIC - Handle( - const std::string & name, const std::string & interface_name, + ReadOnlyHandle( + const std::string & name, + const std::string & interface_name, double * value_ptr = nullptr) : name_(name), interface_name_(interface_name), value_ptr_(value_ptr) { } - HARDWARE_INTERFACE_PUBLIC - explicit Handle(const std::string & interface_name) + explicit ReadOnlyHandle(const std::string & interface_name) : interface_name_(interface_name), value_ptr_(nullptr) { } - HARDWARE_INTERFACE_PUBLIC - explicit Handle(const char * interface_name) + explicit ReadOnlyHandle(const char * interface_name) : interface_name_(interface_name), value_ptr_(nullptr) { } + ReadOnlyHandle(const ReadOnlyHandle & other) = default; + + ReadOnlyHandle(ReadOnlyHandle && other) = default; + + ReadOnlyHandle & operator=(const ReadOnlyHandle & other) = default; + + ReadOnlyHandle & operator=(ReadOnlyHandle && other) = default; + + virtual ~ReadOnlyHandle() = default; + /// \brief returns true if handle references a value inline operator bool() const {return value_ptr_ != nullptr;} - HARDWARE_INTERFACE_PUBLIC HandleType with_value_ptr(double * value_ptr) { return HandleType(name_, interface_name_, value_ptr); } - HARDWARE_INTERFACE_PUBLIC const std::string & get_name() const { return name_; } - HARDWARE_INTERFACE_PUBLIC const std::string & get_interface_name() const { return interface_name_; } - HARDWARE_INTERFACE_PUBLIC double get_value() const { THROW_ON_NULLPTR(value_ptr_); return *value_ptr_; } - HARDWARE_INTERFACE_PUBLIC +protected: + std::string name_; + std::string interface_name_; + double * value_ptr_; +}; + +template +class ReadWriteHandle : public ReadOnlyHandle +{ +public: + ReadWriteHandle( + const std::string & name, + const std::string & interface_name, + double * value_ptr = nullptr) + : ReadOnlyHandle(name, interface_name, value_ptr) + {} + + 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(value_ptr_); - *value_ptr_ = value; + THROW_ON_NULLPTR(this->value_ptr_); + *this->value_ptr_ = value; } - HARDWARE_INTERFACE_PUBLIC void set_value(const std::string & name, double value) { - THROW_ON_NULLPTR(value_ptr_); - name_ = name; - *value_ptr_ = value; + THROW_ON_NULLPTR(this->value_ptr_); + this->name_ = name; + *this->value_ptr_ = value; } - HARDWARE_INTERFACE_PUBLIC void set_value(const char * name, double value) { - THROW_ON_NULLPTR(value_ptr_); - name_ = name; - *value_ptr_ = value; + THROW_ON_NULLPTR(this->value_ptr_); + this->name_ = name; + *this->value_ptr_ = value; } +}; -protected: - std::string name_; - std::string interface_name_; - double * value_ptr_; +class StateHandle : public ReadOnlyHandle +{ +public: + StateHandle(const StateHandle & other) = delete; + + StateHandle(StateHandle && other) = default; + + using ReadOnlyHandle::ReadOnlyHandle; +}; + +class CommandHandle : public ReadWriteHandle +{ +public: + CommandHandle(const CommandHandle & other) = delete; + + CommandHandle(CommandHandle && other) = default; + + using ReadWriteHandle::ReadWriteHandle; }; } // namespace hardware_interface diff --git a/hardware_interface/include/hardware_interface/hardware_info.hpp b/hardware_interface/include/hardware_interface/hardware_info.hpp index 8814ae138c..b858388600 100644 --- a/hardware_interface/include/hardware_interface/hardware_info.hpp +++ b/hardware_interface/include/hardware_interface/hardware_info.hpp @@ -52,7 +52,7 @@ struct HardwareInfo */ std::vector joints; /** - * \brief map of joints provided by the hardware where the key is the joint name. + * \brief map of sensors provided by the hardware where the key is the joint or link name. * Required for Sensor and optional for System Hardware. */ std::vector sensors; diff --git a/hardware_interface/include/hardware_interface/joint_handle.hpp b/hardware_interface/include/hardware_interface/joint_handle.hpp index e1ad5e7c04..ac61db4a35 100644 --- a/hardware_interface/include/hardware_interface/joint_handle.hpp +++ b/hardware_interface/include/hardware_interface/joint_handle.hpp @@ -15,21 +15,16 @@ #ifndef HARDWARE_INTERFACE__JOINT_HANDLE_HPP_ #define HARDWARE_INTERFACE__JOINT_HANDLE_HPP_ -#include - #include "hardware_interface/handle.hpp" -#include "hardware_interface/macros.hpp" -#include "hardware_interface/visibility_control.h" namespace hardware_interface { /** A handle used to get and set a value on a given joint interface. */ -class JointHandle : public Handle +class JointHandle : public ReadWriteHandle { public: - using Handle::Handle; + using ReadWriteHandle::ReadWriteHandle; }; - } // namespace hardware_interface #endif // HARDWARE_INTERFACE__JOINT_HANDLE_HPP_ diff --git a/hardware_interface/src/components/actuator.cpp b/hardware_interface/src/components/actuator.cpp index 7f0f41bdfc..911d81322f 100644 --- a/hardware_interface/src/components/actuator.cpp +++ b/hardware_interface/src/components/actuator.cpp @@ -16,10 +16,12 @@ #include #include #include +#include #include "hardware_interface/components/actuator.hpp" - #include "hardware_interface/components/actuator_interface.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_status_values.hpp" @@ -38,6 +40,18 @@ return_type Actuator::configure(const HardwareInfo & actuator_info) return impl_->configure(actuator_info); } +std::vector Actuator::export_state_handles() +{ + // TODO(karsten1987): Might be worth to do some brief sanity check here + return impl_->export_state_handles(); +} + +std::vector Actuator::export_command_handles() +{ + // TODO(karsten1987): Might be worth to do some brief sanity check here + return impl_->export_command_handles(); +} + return_type Actuator::start() { return impl_->start(); @@ -53,5 +67,15 @@ status Actuator::get_status() const return impl_->get_status(); } +return_type Actuator::read() +{ + return impl_->read(); +} + +return_type Actuator::write() +{ + return impl_->write(); +} + } // namespace components } // namespace hardware_interface diff --git a/hardware_interface/src/components/sensor.cpp b/hardware_interface/src/components/sensor.cpp index fb2b1d15b8..f533538a9b 100644 --- a/hardware_interface/src/components/sensor.cpp +++ b/hardware_interface/src/components/sensor.cpp @@ -19,6 +19,7 @@ #include "hardware_interface/components/sensor.hpp" +#include "hardware_interface/handle.hpp" #include "hardware_interface/hardware_info.hpp" #include "hardware_interface/components/sensor_interface.hpp" #include "hardware_interface/types/hardware_interface_return_values.hpp" @@ -38,6 +39,11 @@ return_type Sensor::configure(const HardwareInfo & sensor_info) return impl_->configure(sensor_info); } +std::vector Sensor::export_state_handles() +{ + return impl_->export_state_handles(); +} + return_type Sensor::start() { return impl_->start(); @@ -53,5 +59,10 @@ status Sensor::get_status() const return impl_->get_status(); } +return_type Sensor::read() +{ + return impl_->read(); +} + } // namespace components } // namespace hardware_interface diff --git a/hardware_interface/src/components/system.cpp b/hardware_interface/src/components/system.cpp index 5f28ab480f..798757d2b4 100644 --- a/hardware_interface/src/components/system.cpp +++ b/hardware_interface/src/components/system.cpp @@ -19,6 +19,7 @@ #include "hardware_interface/components/system.hpp" +#include "hardware_interface/handle.hpp" #include "hardware_interface/hardware_info.hpp" #include "hardware_interface/components/system_interface.hpp" #include "hardware_interface/types/hardware_interface_return_values.hpp" @@ -38,6 +39,16 @@ return_type System::configure(const HardwareInfo & system_info) return impl_->configure(system_info); } +std::vector System::export_state_handles() +{ + return impl_->export_state_handles(); +} + +std::vector System::export_command_handles() +{ + return impl_->export_command_handles(); +} + return_type System::start() { return impl_->start(); @@ -53,5 +64,15 @@ status System::get_status() const return impl_->get_status(); } +return_type System::read() +{ + return impl_->read(); +} + +return_type System::write() +{ + return impl_->write(); +} + } // namespace components } // namespace hardware_interface diff --git a/hardware_interface/test/test_component_interfaces.cpp b/hardware_interface/test/test_component_interfaces.cpp index 1458c77293..5ebf2fa084 100644 --- a/hardware_interface/test/test_component_interfaces.cpp +++ b/hardware_interface/test/test_component_interfaces.cpp @@ -13,730 +13,315 @@ // limitations under the License. #include + +#include #include #include #include #include -#include "hardware_interface/actuator_hardware.hpp" -#include "hardware_interface/actuator_hardware_interface.hpp" -#include "hardware_interface/components/component_info.hpp" -#include "hardware_interface/components/joint.hpp" +#include "hardware_interface/components/actuator.hpp" +#include "hardware_interface/components/actuator_interface.hpp" +#include "hardware_interface/components/sensor_interface.hpp" #include "hardware_interface/components/sensor.hpp" +#include "hardware_interface/components/system_interface.hpp" +#include "hardware_interface/components/system.hpp" #include "hardware_interface/hardware_info.hpp" -#include "hardware_interface/sensor_hardware_interface.hpp" -#include "hardware_interface/sensor_hardware.hpp" -#include "hardware_interface/system_hardware_interface.hpp" -#include "hardware_interface/system_hardware.hpp" #include "hardware_interface/types/hardware_interface_return_values.hpp" #include "hardware_interface/types/hardware_interface_status_values.hpp" #include "hardware_interface/types/hardware_interface_type_values.hpp" using namespace ::testing; // NOLINT -namespace hardware_interface -{ - -namespace hardware_interfaces_components_test +namespace test_components { -class DummyPositionJoint : public components::Joint +class DummyActuator : public hardware_interface::components::ActuatorInterface { -public: - return_type configure(const components::ComponentInfo & joint_info) + hardware_interface::return_type configure( + const hardware_interface::HardwareInfo & /* info */) override { - if (Joint::configure(joint_info) != return_type::OK) { - return return_type::ERROR; - } - - if (info_.command_interfaces.size() > 1 || info_.state_interfaces.size() > 1) { - return return_type::ERROR; - } - - hardware_interface::components::InterfaceInfo dummy_position_interface; - dummy_position_interface.name = HW_IF_POSITION; - dummy_position_interface.max = "1"; - dummy_position_interface.min = "-1"; - - if (info_.command_interfaces.size() == 0) { - info_.command_interfaces.push_back(dummy_position_interface); - commands_.resize(1); - } - if (info_.state_interfaces.size() == 0) { - info_.state_interfaces.push_back(dummy_position_interface); - states_.resize(1); - } - - return return_type::OK; + // We hardcode the info + return hardware_interface::return_type::OK; } -}; -class DummyMultiJoint : public components::Joint -{ -public: - return_type configure(const components::ComponentInfo & joint_info) + std::vector export_state_handles() override { - if (Joint::configure(joint_info) != return_type::OK) { - return return_type::ERROR; - } - - if (info_.command_interfaces.size() < 2) { - return return_type::ERROR; - } - - info_.command_interfaces = joint_info.command_interfaces; - info_.state_interfaces = joint_info.state_interfaces; - - return return_type::OK; + // We can read a position and a velocity + std::vector state_handles; + state_handles.emplace_back( + hardware_interface::StateHandle("joint1", "position", &position_state_)); + state_handles.emplace_back( + hardware_interface::StateHandle("joint1", "velocity", &velocity_state_)); + + return state_handles; } -}; -class DummyForceTorqueSensor : public components::Sensor -{ -public: - return_type configure(const components::ComponentInfo & sensor_info) + std::vector export_command_handles() override { - if (Sensor::configure(sensor_info) != return_type::OK) { - return return_type::ERROR; - } - - if (info_.parameters["frame_id"] == "") { - return return_type::ERROR; - } - if (info_.state_interfaces.size() == 0) { - hardware_interface::components::InterfaceInfo dummy_ft_interface; - - dummy_ft_interface.name = "force_x"; - info_.state_interfaces.push_back(dummy_ft_interface); - - dummy_ft_interface.name = "force_y"; - info_.state_interfaces.push_back(dummy_ft_interface); - - dummy_ft_interface.name = "force_z"; - info_.state_interfaces.push_back(dummy_ft_interface); - - dummy_ft_interface.name = "torque_x"; - info_.state_interfaces.push_back(dummy_ft_interface); + // We can command in velocity + std::vector command_handles; + command_handles.emplace_back( + hardware_interface::CommandHandle("joint1", "velocity", &velocity_command_)); - dummy_ft_interface.name = "torque_y"; - info_.state_interfaces.push_back(dummy_ft_interface); - - dummy_ft_interface.name = "torque_z"; - info_.state_interfaces.push_back(dummy_ft_interface); - } - states_ = {1.34, 5.67, 8.21, 5.63, 5.99, 4.32}; - return return_type::OK; + return command_handles; } -}; -class DummyActuatorHardware : public ActuatorHardwareInterface -{ - return_type configure(const HardwareInfo & actuator_info) override + hardware_interface::return_type start() override { - info_ = actuator_info; - hw_read_time_ = stod(info_.hardware_parameters["example_param_read_for_sec"]); - hw_write_time_ = stod(info_.hardware_parameters["example_param_write_for_sec"]); - status_ = status::CONFIGURED; - return return_type::OK; + return hardware_interface::return_type::OK; } - return_type start() override + hardware_interface::return_type stop() override { - if (status_ == status::CONFIGURED || - status_ == status::STOPPED) - { - status_ = status::STARTED; - } else { - return return_type::ERROR; - } - return return_type::OK; + return hardware_interface::return_type::OK; } - return_type stop() override + hardware_interface::status get_status() const override { - if (status_ == status::STARTED) { - status_ = status::STOPPED; - } else { - return return_type::ERROR; - } - return return_type::OK; + return hardware_interface::status::UNKNOWN; } - status get_status() const override + hardware_interface::return_type read() override { - return status_; + // no-op, state is getting propagated within write. + return hardware_interface::return_type::OK; } - return_type read_joint(std::shared_ptr joint) const override + hardware_interface::return_type write() override { - std::vector interfaces = joint->get_state_interface_names(); - return joint->set_state(hw_values_, interfaces); - } + position_state_ += velocity_command_; + velocity_state_ = velocity_command_; - return_type write_joint(const std::shared_ptr joint) override - { - std::vector interfaces = joint->get_command_interface_names(); - return joint->get_command(hw_values_, interfaces); + return hardware_interface::return_type::OK; } private: - HardwareInfo info_; - status status_ = status::UNKNOWN; - std::vector hw_values_ = {1.2}; - double hw_read_time_, hw_write_time_; + double position_state_ = 0.0; + double velocity_state_ = 0.0; + double velocity_command_ = 0.0; }; -class DummySensorHardware : public SensorHardwareInterface +class DummySensor : public hardware_interface::components::SensorInterface { - return_type configure(const HardwareInfo & sensor_info) override + hardware_interface::return_type configure( + const hardware_interface::HardwareInfo & /* info */) override { - info_ = sensor_info; - binary_to_voltage_factor_ = stod(info_.hardware_parameters["binary_to_voltage_factor"]); - status_ = status::CONFIGURED; - return return_type::OK; + // We hardcode the info + return hardware_interface::return_type::OK; } - return_type start() override + std::vector export_state_handles() override { - if (status_ == status::CONFIGURED || - status_ == status::STOPPED) - { - status_ = status::STARTED; - } else { - return return_type::ERROR; - } - return return_type::OK; + // We can read some voltage level + std::vector state_handles; + state_handles.emplace_back( + hardware_interface::StateHandle("joint1", "voltage", &voltage_level_)); + + return state_handles; } - return_type stop() override + hardware_interface::return_type start() override { - if (status_ == status::STARTED) { - status_ = status::STOPPED; - } else { - return return_type::ERROR; - } - return return_type::OK; + return hardware_interface::return_type::OK; } - status get_status() const override + hardware_interface::return_type stop() override { - return status_; + return hardware_interface::return_type::OK; } - return_type read_sensors(const std::vector> & sensors) const - override + hardware_interface::status get_status() const override { - return_type ret = return_type::OK; - for (const auto & sensor : sensors) { - ret = sensor->set_state(ft_hw_values_); - if (ret != return_type::OK) { - break; - } - } - return ret; + return hardware_interface::status::UNKNOWN; + } + + hardware_interface::return_type read() override + { + // no-op, static value + return hardware_interface::return_type::OK; } private: - HardwareInfo info_; - status status_ = status::UNKNOWN; - double binary_to_voltage_factor_; - std::vector ft_hw_values_ = {1, -1.0, 3.4, 7.9, 5.5, 4.4}; + double voltage_level_ = 0x666; }; -class DummySystemHardware : public SystemHardwareInterface +class DummySystem : public hardware_interface::components::SystemInterface { - return_type configure(const HardwareInfo & system_info) override + hardware_interface::return_type configure( + const hardware_interface::HardwareInfo & /* info */) override { - info_ = system_info; - api_version_ = stod(info_.hardware_parameters["example_api_version"]); - hw_read_time_ = stod(info_.hardware_parameters["example_param_read_for_sec"]); - hw_write_time_ = stod(info_.hardware_parameters["example_param_write_for_sec"]); - status_ = status::CONFIGURED; - return return_type::OK; + // We hardcode the info + return hardware_interface::return_type::OK; } - return_type start() override + std::vector export_state_handles() override { - if (status_ == status::CONFIGURED || - status_ == status::STOPPED) - { - status_ = status::STARTED; - } else { - return return_type::ERROR; - } - return return_type::OK; + // We can read a position and a velocity + std::vector state_handles; + state_handles.emplace_back( + hardware_interface::StateHandle("joint1", "position", &position_state_[0])); + state_handles.emplace_back( + hardware_interface::StateHandle("joint1", "velocity", &velocity_state_[0])); + state_handles.emplace_back( + hardware_interface::StateHandle("joint2", "position", &position_state_[1])); + state_handles.emplace_back( + hardware_interface::StateHandle("joint2", "velocity", &velocity_state_[1])); + state_handles.emplace_back( + hardware_interface::StateHandle("joint3", "position", &position_state_[2])); + state_handles.emplace_back( + hardware_interface::StateHandle("joint3", "velocity", &velocity_state_[2])); + + return state_handles; } - return_type stop() override + std::vector export_command_handles() override { - if (status_ == status::STARTED) { - status_ = status::STOPPED; - } else { - return return_type::ERROR; - } - return return_type::OK; + // We can command in velocity + std::vector command_handles; + command_handles.emplace_back( + hardware_interface::CommandHandle("joint1", "velocity", &velocity_command_[0])); + command_handles.emplace_back( + hardware_interface::CommandHandle("joint2", "velocity", &velocity_command_[1])); + command_handles.emplace_back( + hardware_interface::CommandHandle("joint3", "velocity", &velocity_command_[2])); + + return command_handles; } - status get_status() const override + hardware_interface::return_type start() override { - return status_; + return hardware_interface::return_type::OK; } - return_type read_sensors(std::vector> & sensors) const - override + hardware_interface::return_type stop() override { - return_type ret = return_type::OK; - for (const auto & sensor : sensors) { - ret = sensor->set_state(ft_hw_values_); - if (ret != return_type::OK) { - break; - } - } - return ret; + return hardware_interface::return_type::OK; } - return_type read_joints(std::vector> & joints) const override + hardware_interface::status get_status() const override { - return_type ret = return_type::OK; - std::vector interfaces; - std::vector joint_values; - for (uint i = 0; i < joints.size(); i++) { - joint_values.clear(); - joint_values.push_back(joints_hw_values_[i]); - interfaces = joints[i]->get_state_interface_names(); - ret = joints[i]->set_state(joint_values, interfaces); - if (ret != return_type::OK) { - break; - } - } - return ret; + return hardware_interface::status::UNKNOWN; } - return_type write_joints(const std::vector> & joints) override + hardware_interface::return_type read() override { - return_type ret = return_type::OK; - for (const auto & joint : joints) { - std::vector values; - std::vector interfaces = joint->get_command_interface_names(); - ret = joint->get_command(values, interfaces); - if (ret != return_type::OK) { - break; - } + // no-op, state is getting propagated within write. + return hardware_interface::return_type::OK; + } + + hardware_interface::return_type write() override + { + for (auto i = 0; i < 3; ++i) { + position_state_[i] += velocity_command_[0]; + velocity_state_[i] = velocity_command_[0]; } - return ret; + return hardware_interface::return_type::OK; } private: - HardwareInfo info_; - status status_; - double hw_write_time_, hw_read_time_, api_version_; - std::vector ft_hw_values_ = {-3.5, -2.1, -8.7, -5.4, -9.0, -11.2}; - std::vector joints_hw_values_ = {-1.575, -0.7543}; + std::array position_state_ = {0.0, 0.0, 0.0}; + std::array velocity_state_ = {0.0, 0.0, 0.0}; + std::array velocity_command_ = {0.0, 0.0, 0.0}; }; -} // namespace hardware_interfaces_components_test -} // namespace hardware_interface - -using hardware_interface::components::ComponentInfo; -using hardware_interface::components::Joint; -using hardware_interface::components::Sensor; -using hardware_interface::ActuatorHardware; -using hardware_interface::ActuatorHardwareInterface; -using hardware_interface::HardwareInfo; -using hardware_interface::status; -using hardware_interface::return_type; -using hardware_interface::SensorHardware; -using hardware_interface::SensorHardwareInterface; -using hardware_interface::SystemHardware; -using hardware_interface::SystemHardwareInterface; -using hardware_interface::hardware_interfaces_components_test::DummyForceTorqueSensor; -using hardware_interface::hardware_interfaces_components_test::DummyMultiJoint; -using hardware_interface::hardware_interfaces_components_test::DummyPositionJoint; - -using hardware_interface::hardware_interfaces_components_test::DummyActuatorHardware; -using hardware_interface::hardware_interfaces_components_test::DummySensorHardware; -using hardware_interface::hardware_interfaces_components_test::DummySystemHardware; - -class TestComponentInterfaces : public Test +} // namespace test_components + +TEST(TestComponentInterfaces, dummy_actuator) { -protected: - ComponentInfo joint_info_; - ComponentInfo sensor_info_; + hardware_interface::components::Actuator actuator_hw( + std::make_unique()); - void SetUp() override - { - joint_info_.name = "DummyPositionJoint"; - joint_info_.parameters["max_position"] = "3.14"; - joint_info_.parameters["min_position"] = "-3.14"; + hardware_interface::HardwareInfo mock_hw_info{}; + EXPECT_EQ(hardware_interface::return_type::OK, actuator_hw.configure(mock_hw_info)); - sensor_info_.name = "DummyForceTorqueSensor"; - sensor_info_.parameters["frame_id"] = "tcp_link"; - } -}; + auto state_handles = actuator_hw.export_state_handles(); + ASSERT_EQ(2u, state_handles.size()); + EXPECT_EQ("joint1", state_handles[0].get_name()); + EXPECT_EQ("position", state_handles[0].get_interface_name()); + EXPECT_EQ("joint1", state_handles[1].get_name()); + EXPECT_EQ("velocity", state_handles[1].get_interface_name()); -TEST_F(TestComponentInterfaces, joint_example_component_works) -{ - DummyPositionJoint joint; - - EXPECT_EQ(joint.configure(joint_info_), return_type::OK); - ASSERT_THAT(joint.get_command_interfaces(), SizeIs(1)); - EXPECT_EQ(joint.get_command_interfaces()[0].name, hardware_interface::HW_IF_POSITION); - ASSERT_THAT(joint.get_state_interfaces(), SizeIs(1)); - EXPECT_EQ(joint.get_state_interface_names()[0], hardware_interface::HW_IF_POSITION); - - // Command getters and setters - std::vector interfaces; - std::vector input; - input.push_back(2.1); - EXPECT_EQ(joint.set_command(input, interfaces), return_type::INTERFACE_NOT_PROVIDED); - interfaces.push_back(hardware_interface::HW_IF_VELOCITY); - EXPECT_EQ(joint.set_command(input, interfaces), return_type::INTERFACE_NOT_FOUND); - interfaces.push_back(hardware_interface::HW_IF_POSITION); - EXPECT_EQ(joint.set_command(input, interfaces), return_type::INTERFACE_VALUE_SIZE_NOT_EQUAL); - interfaces.clear(); - interfaces.push_back(joint.get_command_interface_names()[0]); - input.clear(); - input.push_back(1.2); - EXPECT_EQ(joint.set_command(input, interfaces), return_type::OK); - - std::vector output; - interfaces.clear(); - EXPECT_EQ(joint.get_command(output, interfaces), return_type::INTERFACE_NOT_PROVIDED); - interfaces.push_back(joint.get_command_interface_names()[0]); - EXPECT_EQ(joint.get_command(output, interfaces), return_type::OK); - ASSERT_THAT(output, SizeIs(1)); - EXPECT_EQ(output[0], 1.2); - interfaces.clear(); - interfaces.push_back(hardware_interface::HW_IF_VELOCITY); - EXPECT_EQ(joint.get_command(output, interfaces), return_type::INTERFACE_NOT_FOUND); - - input.clear(); - EXPECT_EQ(joint.set_command(input), return_type::INTERFACE_VALUE_SIZE_NOT_EQUAL); - input.push_back(2.1); - EXPECT_EQ(joint.set_command(input), return_type::OK); - - EXPECT_EQ(joint.get_command(output), return_type::OK); - ASSERT_THAT(output, SizeIs(1)); - EXPECT_EQ(output[0], 2.1); - - // State getters and setters - interfaces.clear(); - input.clear(); - input.push_back(2.1); - EXPECT_EQ(joint.set_state(input, interfaces), return_type::INTERFACE_NOT_PROVIDED); - interfaces.push_back(hardware_interface::HW_IF_VELOCITY); - EXPECT_EQ(joint.set_state(input, interfaces), return_type::INTERFACE_NOT_FOUND); - interfaces.push_back(hardware_interface::HW_IF_POSITION); - EXPECT_EQ(joint.set_state(input, interfaces), return_type::INTERFACE_VALUE_SIZE_NOT_EQUAL); - interfaces.clear(); - interfaces.push_back(hardware_interface::HW_IF_POSITION); - input.clear(); - input.push_back(1.2); - EXPECT_EQ(joint.set_state(input, interfaces), return_type::OK); - - output.clear(); - interfaces.clear(); - EXPECT_EQ(joint.get_command(output, interfaces), return_type::INTERFACE_NOT_PROVIDED); - interfaces.push_back(joint.get_command_interface_names()[0]); - EXPECT_EQ(joint.get_state(output, interfaces), return_type::OK); - ASSERT_THAT(output, SizeIs(1)); - EXPECT_EQ(output[0], 1.2); - interfaces.clear(); - interfaces.push_back(hardware_interface::HW_IF_VELOCITY); - EXPECT_EQ(joint.get_state(output, interfaces), return_type::INTERFACE_NOT_FOUND); - - input.clear(); - EXPECT_EQ(joint.set_state(input), return_type::INTERFACE_VALUE_SIZE_NOT_EQUAL); - input.push_back(2.1); - EXPECT_EQ(joint.set_state(input), return_type::OK); - - EXPECT_EQ(joint.get_state(output), return_type::OK); - ASSERT_THAT(output, SizeIs(1)); - EXPECT_EQ(output[0], 2.1); - - // Test DummyPositionJoint - // Cannot push a velocity interface - joint_info_.command_interfaces.push_back(joint.get_command_interfaces()[0]); - hardware_interface::components::InterfaceInfo velocity_interface; - velocity_interface.name = hardware_interface::HW_IF_VELOCITY; - joint_info_.command_interfaces.push_back(velocity_interface); - EXPECT_EQ(joint.configure(joint_info_), return_type::ERROR); -} + auto command_handles = actuator_hw.export_command_handles(); + ASSERT_EQ(1u, command_handles.size()); + EXPECT_EQ("joint1", command_handles[0].get_name()); + EXPECT_EQ("velocity", command_handles[0].get_interface_name()); -TEST_F(TestComponentInterfaces, multi_joint_example_component_works) -{ - DummyMultiJoint joint; - - joint_info_.name = "DummyMultiJoint"; - // Error if fewer than 2 interfaces for a MultiJoint - EXPECT_EQ(joint.configure(joint_info_), return_type::ERROR); - - // Define position and velocity interfaces - hardware_interface::components::InterfaceInfo position_interface; - position_interface.name = hardware_interface::HW_IF_POSITION; - position_interface.min = -1; - position_interface.max = 1; - joint_info_.command_interfaces.push_back(position_interface); - hardware_interface::components::InterfaceInfo velocity_interface; - velocity_interface.name = hardware_interface::HW_IF_VELOCITY; - joint_info_.command_interfaces.push_back(velocity_interface); - velocity_interface.min = -1; - velocity_interface.max = 1; - - EXPECT_EQ(joint.configure(joint_info_), return_type::OK); - - ASSERT_THAT(joint.get_command_interfaces(), SizeIs(2)); - EXPECT_EQ(joint.get_command_interfaces()[0].name, hardware_interface::HW_IF_POSITION); - ASSERT_THAT(joint.get_state_interfaces(), SizeIs(0)); - - joint_info_.state_interfaces.push_back(velocity_interface); - joint_info_.state_interfaces.push_back(velocity_interface); - EXPECT_EQ(joint.configure(joint_info_), return_type::OK); - ASSERT_THAT(joint.get_state_interfaces(), SizeIs(2)); - EXPECT_EQ(joint.get_command_interfaces()[1].name, hardware_interface::HW_IF_VELOCITY); - - // Command getters and setters - std::vector interfaces; - std::vector input; - input.push_back(2.1); - EXPECT_EQ(joint.set_command(input, interfaces), return_type::INTERFACE_NOT_PROVIDED); - interfaces.push_back(hardware_interface::HW_IF_EFFORT); - EXPECT_EQ(joint.set_command(input, interfaces), return_type::INTERFACE_NOT_FOUND); - interfaces.push_back(hardware_interface::HW_IF_VELOCITY); - EXPECT_EQ(joint.set_command(input, interfaces), return_type::INTERFACE_VALUE_SIZE_NOT_EQUAL); - interfaces.clear(); - interfaces.push_back(hardware_interface::HW_IF_VELOCITY); - input.clear(); - input.push_back(1.02); - EXPECT_EQ(joint.set_command(input, interfaces), return_type::OK); - - std::vector output; - EXPECT_EQ(joint.get_command(output, interfaces), return_type::OK); - ASSERT_THAT(output, SizeIs(1)); - EXPECT_EQ(output[0], 1.02); - interfaces.clear(); - interfaces.push_back(hardware_interface::HW_IF_EFFORT); - EXPECT_EQ(joint.get_command(output, interfaces), return_type::INTERFACE_NOT_FOUND); - - input.clear(); - EXPECT_EQ(joint.set_command(input), return_type::INTERFACE_VALUE_SIZE_NOT_EQUAL); - input.push_back(5.77); - EXPECT_EQ(joint.set_command(input), return_type::INTERFACE_VALUE_SIZE_NOT_EQUAL); - input.clear(); - input.push_back(1.2); - input.push_back(0.4); - EXPECT_EQ(joint.set_command(input), return_type::OK); - - EXPECT_EQ(joint.get_command(output), return_type::OK); - ASSERT_THAT(output, SizeIs(2)); - EXPECT_EQ(output[1], 0.4); - - // State getters and setters - interfaces.clear(); - input.clear(); - input.push_back(2.1); - EXPECT_EQ(joint.set_state(input, interfaces), return_type::INTERFACE_NOT_PROVIDED); - interfaces.push_back(hardware_interface::HW_IF_EFFORT); - EXPECT_EQ(joint.set_state(input, interfaces), return_type::INTERFACE_NOT_FOUND); - interfaces.push_back(hardware_interface::HW_IF_POSITION); - EXPECT_EQ(joint.set_state(input, interfaces), return_type::INTERFACE_VALUE_SIZE_NOT_EQUAL); - - - interfaces.clear(); - interfaces.push_back(hardware_interface::HW_IF_VELOCITY); - input.clear(); - input.push_back(1.2); - EXPECT_EQ(joint.set_state(input, interfaces), return_type::OK); - - output.clear(); - EXPECT_EQ(joint.get_state(output, interfaces), return_type::OK); - ASSERT_THAT(output, SizeIs(1)); - EXPECT_EQ(output[0], 1.2); - interfaces.clear(); - interfaces.push_back(hardware_interface::HW_IF_EFFORT); - EXPECT_EQ(joint.get_state(output, interfaces), return_type::INTERFACE_NOT_FOUND); - - input.clear(); - EXPECT_EQ(joint.set_state(input), return_type::INTERFACE_VALUE_SIZE_NOT_EQUAL); - input.push_back(2.1); - input.push_back(1.02); - EXPECT_EQ(joint.set_state(input), return_type::OK); - - EXPECT_EQ(joint.get_state(output), return_type::OK); - ASSERT_THAT(output, SizeIs(2)); - EXPECT_EQ(output[0], 2.1); -} + command_handles[0].set_value(1.0); // velocity + ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.write()); -TEST_F(TestComponentInterfaces, sensor_example_component_works) -{ - DummyForceTorqueSensor sensor; - - EXPECT_EQ(sensor.configure(sensor_info_), return_type::OK); - ASSERT_THAT(sensor.get_state_interfaces(), SizeIs(6)); - EXPECT_EQ(sensor.get_state_interface_names()[0], "force_x"); - EXPECT_EQ(sensor.get_state_interface_names()[5], "torque_z"); - std::vector input = {5.23, 6.7, 2.5, 3.8, 8.9, 12.3}; - std::vector output; - std::vector interfaces; - EXPECT_EQ(sensor.get_state(output, interfaces), return_type::INTERFACE_NOT_PROVIDED); - interfaces.push_back("force_y"); - EXPECT_EQ(sensor.get_state(output, interfaces), return_type::OK); - ASSERT_THAT(output, SizeIs(1)); - EXPECT_EQ(output[0], 5.67); - - // State getters and setters - interfaces.clear(); - EXPECT_EQ(sensor.set_state(input, interfaces), return_type::INTERFACE_NOT_PROVIDED); - interfaces.push_back(hardware_interface::HW_IF_VELOCITY); - interfaces.push_back(hardware_interface::HW_IF_VELOCITY); - interfaces.push_back(hardware_interface::HW_IF_VELOCITY); - EXPECT_EQ(sensor.set_state(input, interfaces), return_type::INTERFACE_VALUE_SIZE_NOT_EQUAL); - interfaces.push_back(hardware_interface::HW_IF_VELOCITY); - interfaces.push_back(hardware_interface::HW_IF_VELOCITY); - interfaces.push_back(hardware_interface::HW_IF_VELOCITY); - EXPECT_EQ(sensor.set_state(input, interfaces), return_type::INTERFACE_NOT_FOUND); - interfaces.clear(); - interfaces = sensor.get_state_interface_names(); - EXPECT_EQ(sensor.set_state(input, interfaces), return_type::OK); - - output.clear(); - EXPECT_EQ(sensor.get_state(output, interfaces), return_type::OK); - ASSERT_THAT(output, SizeIs(6)); - EXPECT_EQ(output[0], 5.23); - interfaces.clear(); - interfaces.push_back(hardware_interface::HW_IF_VELOCITY); - EXPECT_EQ(sensor.get_state(output, interfaces), return_type::INTERFACE_NOT_FOUND); - - input.clear(); - EXPECT_EQ(sensor.set_state(input), return_type::INTERFACE_VALUE_SIZE_NOT_EQUAL); - input = {5.23, 6.7, 2.5, 3.8, 8.9, 12.3}; - EXPECT_EQ(sensor.set_state(input), return_type::OK); - - EXPECT_EQ(sensor.get_state(output), return_type::OK); - ASSERT_THAT(output, SizeIs(6)); - EXPECT_EQ(output[5], 12.3); - - sensor_info_.parameters.clear(); - EXPECT_EQ(sensor.configure(sensor_info_), return_type::ERROR); -} + for (auto step = 1u; step <= 10; ++step) { + ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.read()); -TEST_F(TestComponentInterfaces, actuator_hardware_interface_works) -{ - ActuatorHardware actuator_hw(std::make_unique()); - auto joint = std::make_shared(); - - HardwareInfo actuator_hw_info; - actuator_hw_info.name = "DummyActuatorHardware"; - actuator_hw_info.hardware_parameters["example_param_write_for_sec"] = "2"; - actuator_hw_info.hardware_parameters["example_param_read_for_sec"] = "3"; - - EXPECT_EQ(joint->configure(joint_info_), return_type::OK); - - EXPECT_EQ(actuator_hw.configure(actuator_hw_info), return_type::OK); - EXPECT_EQ(actuator_hw.get_status(), status::CONFIGURED); - EXPECT_EQ(actuator_hw.start(), return_type::OK); - EXPECT_EQ(actuator_hw.get_status(), status::STARTED); - EXPECT_EQ(actuator_hw.read_joint(joint), return_type::OK); - std::vector interfaces = joint->get_state_interface_names(); - std::vector output; - EXPECT_EQ(joint->get_state(output, interfaces), return_type::OK); - ASSERT_THAT(output, SizeIs(1)); - EXPECT_EQ(output[0], 1.2); - EXPECT_EQ(interfaces[0], hardware_interface::HW_IF_POSITION); - EXPECT_EQ(actuator_hw.write_joint(joint), return_type::OK); - EXPECT_EQ(actuator_hw.stop(), return_type::OK); - EXPECT_EQ(actuator_hw.get_status(), status::STOPPED); -} + ASSERT_EQ(step, state_handles[0].get_value()); // position value + ASSERT_EQ(1u, state_handles[1].get_value()); // velocity -TEST_F(TestComponentInterfaces, sensor_interface_with_hardware_works) -{ - SensorHardware sensor_hw(std::make_unique()); - auto sensor = std::make_shared(); - - HardwareInfo sensor_hw_info; - sensor_hw_info.name = "DummySensor"; - sensor_hw_info.hardware_parameters["binary_to_voltage_factor"] = "0.0048828125"; - - EXPECT_EQ(sensor->configure(sensor_info_), return_type::OK); - - EXPECT_EQ(sensor_hw.configure(sensor_hw_info), return_type::OK); - EXPECT_EQ(sensor_hw.get_status(), status::CONFIGURED); - EXPECT_EQ(sensor_hw.start(), return_type::OK); - EXPECT_EQ(sensor_hw.get_status(), status::STARTED); - std::vector> sensors; - sensors.push_back(sensor); - EXPECT_EQ(sensor_hw.read_sensors(sensors), return_type::OK); - std::vector output; - std::vector interfaces = sensor->get_state_interface_names(); - EXPECT_EQ(sensor->get_state(output, interfaces), return_type::OK); - EXPECT_EQ(output[2], 3.4); - ASSERT_THAT(interfaces, SizeIs(6)); - EXPECT_EQ(interfaces[1], "force_y"); - EXPECT_EQ(sensor_hw.stop(), return_type::OK); - EXPECT_EQ(sensor_hw.get_status(), status::STOPPED); - EXPECT_EQ(sensor_hw.start(), return_type::OK); + ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.write()); + } } -TEST_F(TestComponentInterfaces, system_interface_with_hardware_works) +TEST(TestComponentInterfaces, dummy_sensor) { - SystemHardware system(std::make_unique()); - auto joint1 = std::make_shared(); - auto joint2 = std::make_shared(); - std::vector> joints; - joints.push_back(joint1); - joints.push_back(joint2); - - std::shared_ptr sensor(std::make_shared()); - std::vector> sensors; - sensors.push_back(sensor); - - EXPECT_EQ(joint1->configure(joint_info_), return_type::OK); - EXPECT_EQ(joint2->configure(joint_info_), return_type::OK); - EXPECT_EQ(sensor->configure(sensor_info_), return_type::OK); - - HardwareInfo system_hw_info; - system_hw_info.name = "DummyActuatorHardware"; - system_hw_info.hardware_parameters["example_api_version"] = "1.1"; - system_hw_info.hardware_parameters["example_param_write_for_sec"] = "2"; - system_hw_info.hardware_parameters["example_param_read_for_sec"] = "3"; - - EXPECT_EQ(system.configure(system_hw_info), return_type::OK); - EXPECT_EQ(system.get_status(), status::CONFIGURED); - EXPECT_EQ(system.start(), return_type::OK); - EXPECT_EQ(system.get_status(), status::STARTED); - - EXPECT_EQ(system.read_sensors(sensors), return_type::OK); - std::vector output; - { - std::vector interfaces = sensor->get_state_interface_names(); - EXPECT_EQ(sensor->get_state(output, interfaces), return_type::OK); - ASSERT_THAT(output, SizeIs(6)); - EXPECT_EQ(output[2], -8.7); - ASSERT_THAT(interfaces, SizeIs(6)); - EXPECT_EQ(interfaces[4], "torque_y"); - } - output.clear(); - - EXPECT_EQ(system.read_joints(joints), return_type::OK); - { - std::vector interfaces = joint1->get_command_interface_names(); - EXPECT_EQ(joint1->get_state(output, interfaces), return_type::OK); - ASSERT_THAT(output, SizeIs(1)); - EXPECT_EQ(output[0], -1.575); - ASSERT_THAT(interfaces, SizeIs(1)); - EXPECT_EQ(interfaces[0], hardware_interface::HW_IF_POSITION); - } - output.clear(); + hardware_interface::components::Sensor sensor_hw( + std::make_unique()); - std::vector interfaces = joint2->get_state_interface_names(); - EXPECT_EQ(joint2->get_state(output, interfaces), return_type::OK); - ASSERT_THAT(output, SizeIs(1)); - EXPECT_EQ(output[0], -0.7543); - ASSERT_THAT(interfaces, SizeIs(1)); - EXPECT_EQ(interfaces[0], hardware_interface::HW_IF_POSITION); + hardware_interface::HardwareInfo mock_hw_info{}; + EXPECT_EQ(hardware_interface::return_type::OK, sensor_hw.configure(mock_hw_info)); - EXPECT_EQ(system.write_joints(joints), return_type::OK); + auto state_handles = sensor_hw.export_state_handles(); + ASSERT_EQ(1u, state_handles.size()); + EXPECT_EQ("joint1", state_handles[0].get_name()); + EXPECT_EQ("voltage", state_handles[0].get_interface_name()); + EXPECT_EQ(0x666, state_handles[0].get_value()); +} - EXPECT_EQ(system.stop(), return_type::OK); - EXPECT_EQ(system.get_status(), status::STOPPED); +TEST(TestComponentInterfaces, dummy_system) +{ + hardware_interface::components::System system_hw( + std::make_unique()); + + hardware_interface::HardwareInfo mock_hw_info{}; + EXPECT_EQ(hardware_interface::return_type::OK, system_hw.configure(mock_hw_info)); + + auto state_handles = system_hw.export_state_handles(); + ASSERT_EQ(6u, state_handles.size()); + EXPECT_EQ("joint1", state_handles[0].get_name()); + EXPECT_EQ("position", state_handles[0].get_interface_name()); + EXPECT_EQ("joint1", state_handles[1].get_name()); + EXPECT_EQ("velocity", state_handles[1].get_interface_name()); + EXPECT_EQ("joint2", state_handles[2].get_name()); + EXPECT_EQ("position", state_handles[2].get_interface_name()); + EXPECT_EQ("joint2", state_handles[3].get_name()); + EXPECT_EQ("velocity", state_handles[3].get_interface_name()); + EXPECT_EQ("joint3", state_handles[4].get_name()); + EXPECT_EQ("position", state_handles[4].get_interface_name()); + EXPECT_EQ("joint3", state_handles[5].get_name()); + EXPECT_EQ("velocity", state_handles[5].get_interface_name()); + + auto command_handles = system_hw.export_command_handles(); + ASSERT_EQ(3u, command_handles.size()); + EXPECT_EQ("joint1", command_handles[0].get_name()); + EXPECT_EQ("velocity", command_handles[0].get_interface_name()); + EXPECT_EQ("joint2", command_handles[1].get_name()); + EXPECT_EQ("velocity", command_handles[1].get_interface_name()); + EXPECT_EQ("joint3", command_handles[2].get_name()); + EXPECT_EQ("velocity", command_handles[2].get_interface_name()); + + command_handles[0].set_value(1.0); // velocity + command_handles[1].set_value(1.0); // velocity + command_handles[2].set_value(1.0); // velocity + ASSERT_EQ(hardware_interface::return_type::OK, system_hw.write()); + + for (auto step = 1u; step <= 10; ++step) { + ASSERT_EQ(hardware_interface::return_type::OK, system_hw.read()); + + ASSERT_EQ(step, state_handles[0].get_value()); // position value + ASSERT_EQ(1u, state_handles[1].get_value()); // velocity + ASSERT_EQ(step, state_handles[2].get_value()); // position value + ASSERT_EQ(1u, state_handles[3].get_value()); // velocity + ASSERT_EQ(step, state_handles[4].get_value()); // position value + ASSERT_EQ(1u, state_handles[5].get_value()); // velocity + + ASSERT_EQ(hardware_interface::return_type::OK, system_hw.write()); + } } diff --git a/hardware_interface/test/test_joint_handle.cpp b/hardware_interface/test/test_joint_handle.cpp index d42c13fc19..9a4512f13b 100644 --- a/hardware_interface/test/test_joint_handle.cpp +++ b/hardware_interface/test/test_joint_handle.cpp @@ -16,6 +16,8 @@ #include "hardware_interface/joint_handle.hpp" using hardware_interface::JointHandle; +using hardware_interface::CommandHandle; +using hardware_interface::StateHandle; namespace { @@ -23,6 +25,23 @@ constexpr auto JOINT_NAME = "joint_1"; constexpr auto FOO_INTERFACE = "FooInterface"; } // namespace +TEST(TestJointHandle, command_handle) +{ + double value = 1.337; + CommandHandle handle{JOINT_NAME, FOO_INTERFACE, &value}; + EXPECT_DOUBLE_EQ(handle.get_value(), value); + EXPECT_NO_THROW(handle.set_value(0.0)); + EXPECT_DOUBLE_EQ(handle.get_value(), 0.0); +} + +TEST(TestJointHandle, state_handle) +{ + double value = 1.337; + StateHandle handle{JOINT_NAME, FOO_INTERFACE, &value}; + EXPECT_DOUBLE_EQ(handle.get_value(), value); + // handle.set_value(5); compiler error, no set_value function +} + TEST(TestJointHandle, name_getters_work) { JointHandle handle{JOINT_NAME, FOO_INTERFACE};