diff --git a/doc/index.rst b/doc/index.rst index f2ce2efdd3..fa74ef29b1 100644 --- a/doc/index.rst +++ b/doc/index.rst @@ -25,4 +25,5 @@ Concepts :titlesonly: Controller Manager <../controller_manager/doc/userdoc.rst> + Hardware Components <../hardware_interface/doc/hardware_components_userdoc.rst> Fake Components <../hardware_interface/doc/fake_components_userdoc.rst> diff --git a/hardware_interface/CMakeLists.txt b/hardware_interface/CMakeLists.txt index 3b654d3e70..fe28e1f070 100644 --- a/hardware_interface/CMakeLists.txt +++ b/hardware_interface/CMakeLists.txt @@ -8,6 +8,7 @@ endif() find_package(ament_cmake REQUIRED) find_package(control_msgs REQUIRED) find_package(pluginlib REQUIRED) +find_package(rclcpp_lifecycle REQUIRED) find_package(rcpputils REQUIRED) find_package(rcutils REQUIRED) find_package(tinyxml2_vendor REQUIRED) @@ -31,6 +32,7 @@ ament_target_dependencies( hardware_interface control_msgs pluginlib + rclcpp_lifecycle rcutils rcpputils ) diff --git a/hardware_interface/doc/hardware_components_userdoc.rst b/hardware_interface/doc/hardware_components_userdoc.rst new file mode 100644 index 0000000000..29721042d0 --- /dev/null +++ b/hardware_interface/doc/hardware_components_userdoc.rst @@ -0,0 +1,49 @@ +.. _hardware_components_userdoc: + +Hardware Components +------------------- +Hardware components represent abstraction of physical hardware in ros2_control framework. +There are three types of hardware Actuator, Sensor and System. +For details on each type check `Hardware Components description `_. + + +Migration from Foxy to Galactic +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +Between Foxy and Galactic we made substantial changes to the interface of hardware components to enable management of their lifecycle. +The following list shows a set of quick changes to port existing hardware components to Galactic: + +1. Rename ``configure`` to ``on_init`` and change return type to ``CallbackReturn`` + +2. If using BaseInterface as base class then you should remove it. Specifically, change: + +hardware_interface::BaseInterface to hardware_interface::[Actuator|Sensor|System]Interface + +3. Remove include of headers ``base_interface.hpp`` and ``hardware_interface_status_values.hpp`` + +4. Add include of header ``rclcpp_lifecycle/state.hpp`` although this may not be strictly necessary + +5. replace first three lines in ``on_init`` to: + +.. code-block:: c++ + + if (hardware_interface::[Actuator|Sensor|System]Interface::on_init(info) != CallbackReturn::SUCCESS) + { + return CallbackReturn::ERROR; + } + +6. Change last return of ``on_init`` to ``return CallbackReturn::SUCCESS;`` + +7. Remove all lines with ``status_ = ...`` or ``status::...`` + +8. Rename ``start()`` to ``on_activate(const rclcpp_lifecycle::State & previous_state)`` and + ``stop()`` to ``on_deactivate(const rclcpp_lifecycle::State & previous_state)`` + +9. Change return type of ``on_activate`` and ``on_deactivate`` to ``CallbackReturn`` + +10. Change last return of ``on_activate`` and ``on_deactivate`` to ``return CallbackReturn::SUCCESS;`` + +11. If you have any ``return_type::ERROR`` in ``on_init``, ``on_activate``, or ``in_deactivate`` change to ``CallbackReturn::ERROR`` + +12. If you get link errors with undefined refernences to symbols in ``rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface``, then add + ``rclcpp_lifecyle`` package dependency to ``CMakeLists.txt`` and ``package.xml`` diff --git a/hardware_interface/include/fake_components/generic_system.hpp b/hardware_interface/include/fake_components/generic_system.hpp index 2d09b21cdd..c416b91829 100644 --- a/hardware_interface/include/fake_components/generic_system.hpp +++ b/hardware_interface/include/fake_components/generic_system.hpp @@ -20,49 +20,34 @@ #include #include -#include "hardware_interface/base_interface.hpp" #include "hardware_interface/handle.hpp" #include "hardware_interface/hardware_info.hpp" #include "hardware_interface/system_interface.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 hardware_interface::return_type; namespace fake_components { -class HARDWARE_INTERFACE_PUBLIC GenericSystem -: public hardware_interface::BaseInterface +class HARDWARE_INTERFACE_PUBLIC GenericSystem : public hardware_interface::SystemInterface { public: - return_type configure(const hardware_interface::HardwareInfo & info) override; + CallbackReturn on_init(const hardware_interface::HardwareInfo & info) override; std::vector export_state_interfaces() override; std::vector export_command_interfaces() override; - return_type start() override - { - status_ = hardware_interface::status::STARTED; - return return_type::OK; - } - - return_type stop() override - { - status_ = hardware_interface::status::STOPPED; - return return_type::OK; - } - return_type read() override; return_type write() override { return return_type::OK; } protected: - /// Use standard interfaces for joints because they are relevant for dynamic behaviour + /// Use standard interfaces for joints because they are relevant for dynamic behavior /** * By splitting the standard interfaces from other type, the users are able to inherit this - * class and simply create small "simulation" with desired dynamic behaviour. + * class and simply create small "simulation" with desired dynamic behavior. * The advantage over using Gazebo is that enables "quick & dirty" tests of robot's URDF and * controllers. */ diff --git a/hardware_interface/include/hardware_interface/actuator.hpp b/hardware_interface/include/hardware_interface/actuator.hpp index 533056f993..5d98a78368 100644 --- a/hardware_interface/include/hardware_interface/actuator.hpp +++ b/hardware_interface/include/hardware_interface/actuator.hpp @@ -1,4 +1,4 @@ -// Copyright 2020 ros2_control Development Team +// Copyright 2020 - 2021 ros2_control Development Team // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -22,8 +22,8 @@ #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" +#include "rclcpp_lifecycle/state.hpp" namespace hardware_interface { @@ -42,7 +42,25 @@ class Actuator final ~Actuator() = default; HARDWARE_INTERFACE_PUBLIC - return_type configure(const HardwareInfo & actuator_info); + const rclcpp_lifecycle::State & initialize(const HardwareInfo & actuator_info); + + HARDWARE_INTERFACE_PUBLIC + const rclcpp_lifecycle::State & configure(); + + HARDWARE_INTERFACE_PUBLIC + const rclcpp_lifecycle::State & cleanup(); + + HARDWARE_INTERFACE_PUBLIC + const rclcpp_lifecycle::State & shutdown(); + + HARDWARE_INTERFACE_PUBLIC + const rclcpp_lifecycle::State & activate(); + + HARDWARE_INTERFACE_PUBLIC + const rclcpp_lifecycle::State & deactivate(); + + HARDWARE_INTERFACE_PUBLIC + const rclcpp_lifecycle::State & error(); HARDWARE_INTERFACE_PUBLIC std::vector export_state_interfaces(); @@ -60,17 +78,11 @@ class Actuator final const std::vector & start_interfaces, const std::vector & stop_interfaces); - HARDWARE_INTERFACE_PUBLIC - return_type start(); - - HARDWARE_INTERFACE_PUBLIC - return_type stop(); - HARDWARE_INTERFACE_PUBLIC std::string get_name() const; HARDWARE_INTERFACE_PUBLIC - status get_status() const; + const rclcpp_lifecycle::State & get_state() const; HARDWARE_INTERFACE_PUBLIC return_type read(); diff --git a/hardware_interface/include/hardware_interface/actuator_interface.hpp b/hardware_interface/include/hardware_interface/actuator_interface.hpp index 728d3d40f9..3ccd30f559 100644 --- a/hardware_interface/include/hardware_interface/actuator_interface.hpp +++ b/hardware_interface/include/hardware_interface/actuator_interface.hpp @@ -1,4 +1,4 @@ -// Copyright 2020 ros2_control Development Team +// Copyright 2020 - 2021 ros2_control Development Team // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -22,33 +22,83 @@ #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/types/lifecycle_state_names.hpp" +#include "lifecycle_msgs/msg/state.hpp" +#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" +#include "rclcpp_lifecycle/state.hpp" + +using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; namespace hardware_interface { /// Virtual Class to implement when integrating a 1 DoF actuator into ros2_control. /** - * The typical examples are conveyors or motors. - */ -class ActuatorInterface + * The typical examples are conveyors or motors. + * + * Methods return values have type rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn + * with the following meaning: + * + * \returns CallbackReturn::SUCCESS method execution was successful. + * \returns CallbackReturn::FAILURE method execution has failed and and can be called again. + * \returns CallbackReturn::ERROR critical error has happened that should be managed in + * "on_error" method. + * + * The hardware ends after each method in a state with the following meaning: + * + * UNCONFIGURED (on_init, on_cleanup): + * Hardware is initialized but communication is not started and therefore no interface is available. + * + * INACTIVE (on_configure, on_deactivate): + * Communication with the hardware is started and it is configured. + * States can be read and non-movement hardware interfaces commanded. + * Hardware interfaces for movement will NOT be available. + * Those interfaces are: HW_IF_POSITION, HW_IF_VELOCITY, HW_IF_ACCELERATION, and HW_IF_EFFORT. + * + * FINALIZED (on_shutdown): + * Hardware interface is ready for unloading/destruction. + * Allocated memory is cleaned up. + * + * ACTIVE (on_activate): + * Power circuits of hardware are active and hardware can be moved, e.g., brakes are disabled. + * Command interfaces for movement are available and have to be accepted. + * Those interfaces are: HW_IF_POSITION, HW_IF_VELOCITY, HW_IF_ACCELERATION, and HW_IF_EFFORT. + */ +class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface { public: - ActuatorInterface() = default; + ActuatorInterface() + : lifecycle_state_(rclcpp_lifecycle::State( + lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN, lifecycle_state_names::UNKNOWN)) + { + } + + /// SensorInterface copy constructor is actively deleted. + /** + * Hardware interfaces are having a unique ownership and thus can't be copied in order to avoid + * failed or simultaneous access to hardware. + */ + ActuatorInterface(const ActuatorInterface & other) = delete; + + ActuatorInterface(ActuatorInterface && other) = default; virtual ~ActuatorInterface() = default; - /// Configuration of the actuator from data parsed from the robot's URDF. + /// Initialization of the hardware interface from data parsed from the robot's URDF. /** - * \param[in] actuator_info structure with data from URDF. - * \return return_type::OK if required data are provided and can be parsed, - * return_type::ERROR otherwise. + * \param[in] hardware_info structure with data from URDF. + * \returns CallbackReturn::SUCCESS if required data are provided and can be parsed. + * \returns CallbackReturn::ERROR if any error happens or data are missing. */ - virtual return_type configure(const HardwareInfo & actuator_info) = 0; + virtual CallbackReturn on_init(const HardwareInfo & hardware_info) + { + info_ = hardware_info; + return CallbackReturn::SUCCESS; + }; - /// Exports all state interfaces for this actuator. + /// Exports all state interfaces for this hardware interface. /** * The state interfaces have to be created and transferred according - * to the actuator info passed in for the configuration. + * to the hardware info passed in for the configuration. * * Note the ownership over the state interfaces is transferred to the caller. * @@ -56,10 +106,10 @@ class ActuatorInterface */ virtual std::vector export_state_interfaces() = 0; - /// Exports all command interfaces for this actuator. + /// Exports all command interfaces for this hardware interface. /** * The command interfaces have to be created and transferred according - * to the actuator info passed in for the configuration. + * to the hardware info passed in for the configuration. * * Note the ownership over the state interfaces is transferred to the caller. * @@ -106,30 +156,6 @@ class ActuatorInterface return return_type::OK; } - /// Start exchange data with the hardware. - /** - * \return return_type:OK if everything worked as expected, return_type::ERROR otherwise. - */ - virtual return_type start() = 0; - - /// Stop exchange data with the hardware. - /** - * \return return_type:OK if everything worked as expected, return_type::ERROR otherwise. - */ - virtual return_type stop() = 0; - - /// Get name of the actuator hardware. - /** - * \return name. - */ - virtual std::string get_name() const = 0; - - /// Get current state of the actuator hardware. - /** - * \return current status. - */ - 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 @@ -148,6 +174,28 @@ class ActuatorInterface * \return return_type::OK if the read was successful, return_type::ERROR otherwise. */ virtual return_type write() = 0; + + /// Get name of the actuator hardware. + /** + * \return name. + */ + virtual std::string get_name() const { return info_.name; } + + /// Get life-cycle state of the actuator hardware. + /** + * \return state. + */ + const rclcpp_lifecycle::State & get_state() const { return lifecycle_state_; } + + /// Set life-cycle state of the actuator hardware. + /** + * \return state. + */ + void set_state(const rclcpp_lifecycle::State & new_state) { lifecycle_state_ = new_state; } + +protected: + HardwareInfo info_; + rclcpp_lifecycle::State lifecycle_state_; }; } // namespace hardware_interface diff --git a/hardware_interface/include/hardware_interface/base_interface.hpp b/hardware_interface/include/hardware_interface/base_interface.hpp deleted file mode 100644 index d93a269c4d..0000000000 --- a/hardware_interface/include/hardware_interface/base_interface.hpp +++ /dev/null @@ -1,53 +0,0 @@ -// Copyright 2020 ros2_control Development Team -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef HARDWARE_INTERFACE__BASE_INTERFACE_HPP_ -#define HARDWARE_INTERFACE__BASE_INTERFACE_HPP_ - -#include - -#include "hardware_interface/hardware_info.hpp" -#include "hardware_interface/types/hardware_interface_return_values.hpp" -#include "hardware_interface/types/hardware_interface_status_values.hpp" - -namespace hardware_interface -{ -template -class BaseInterface : public InterfaceType -{ -public: - return_type configure(const HardwareInfo & info) override - { - info_ = info; - status_ = status::CONFIGURED; - return return_type::OK; - } - - return_type configure_default(const HardwareInfo & info) - { - return BaseInterface::configure(info); - } - - std::string get_name() const final { return info_.name; } - - status get_status() const final { return status_; } - -protected: - HardwareInfo info_; - status status_; -}; - -} // namespace hardware_interface - -#endif // HARDWARE_INTERFACE__BASE_INTERFACE_HPP_ diff --git a/hardware_interface/include/hardware_interface/resource_manager.hpp b/hardware_interface/include/hardware_interface/resource_manager.hpp index e91bf4a97d..8206da880a 100644 --- a/hardware_interface/include/hardware_interface/resource_manager.hpp +++ b/hardware_interface/include/hardware_interface/resource_manager.hpp @@ -24,7 +24,7 @@ #include "hardware_interface/hardware_info.hpp" #include "hardware_interface/loaned_command_interface.hpp" #include "hardware_interface/loaned_state_interface.hpp" -#include "hardware_interface/types/hardware_interface_status_values.hpp" +#include "rclcpp_lifecycle/state.hpp" namespace hardware_interface { @@ -188,9 +188,9 @@ class HARDWARE_INTERFACE_PUBLIC ResourceManager /// Return status for all components. /** - * \return map of hardware names and their status + * \return map of hardware names and their states */ - std::unordered_map get_components_status(); + std::unordered_map get_components_states(); /// Prepare the hardware components for a new command interface mode /** diff --git a/hardware_interface/include/hardware_interface/sensor.hpp b/hardware_interface/include/hardware_interface/sensor.hpp index ad6f5d90d6..469f28b921 100644 --- a/hardware_interface/include/hardware_interface/sensor.hpp +++ b/hardware_interface/include/hardware_interface/sensor.hpp @@ -1,4 +1,4 @@ -// Copyright 2020 ros2_control Development Team +// Copyright 2020 - 2021 ros2_control Development Team // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -23,8 +23,8 @@ #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" +#include "rclcpp_lifecycle/state.hpp" namespace hardware_interface { @@ -43,22 +43,34 @@ class Sensor final ~Sensor() = default; HARDWARE_INTERFACE_PUBLIC - return_type configure(const HardwareInfo & sensor_info); + const rclcpp_lifecycle::State & initialize(const HardwareInfo & sensor_info); HARDWARE_INTERFACE_PUBLIC - std::vector export_state_interfaces(); + const rclcpp_lifecycle::State & configure(); + + HARDWARE_INTERFACE_PUBLIC + const rclcpp_lifecycle::State & cleanup(); + + HARDWARE_INTERFACE_PUBLIC + const rclcpp_lifecycle::State & shutdown(); HARDWARE_INTERFACE_PUBLIC - return_type start(); + const rclcpp_lifecycle::State & activate(); HARDWARE_INTERFACE_PUBLIC - return_type stop(); + const rclcpp_lifecycle::State & deactivate(); + + HARDWARE_INTERFACE_PUBLIC + const rclcpp_lifecycle::State & error(); + + HARDWARE_INTERFACE_PUBLIC + std::vector export_state_interfaces(); HARDWARE_INTERFACE_PUBLIC std::string get_name() const; HARDWARE_INTERFACE_PUBLIC - status get_status() const; + const rclcpp_lifecycle::State & get_state() const; HARDWARE_INTERFACE_PUBLIC return_type read(); diff --git a/hardware_interface/include/hardware_interface/sensor_interface.hpp b/hardware_interface/include/hardware_interface/sensor_interface.hpp index f6a42aa131..4db515f76a 100644 --- a/hardware_interface/include/hardware_interface/sensor_interface.hpp +++ b/hardware_interface/include/hardware_interface/sensor_interface.hpp @@ -1,4 +1,4 @@ -// Copyright 2020 ros2_control Development Team +// Copyright 2020 - 2021 ros2_control Development Team // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -22,74 +22,121 @@ #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" +#include "hardware_interface/types/lifecycle_state_names.hpp" +#include "lifecycle_msgs/msg/state.hpp" +#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" +#include "rclcpp_lifecycle/state.hpp" + +using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; namespace hardware_interface { +/// Virtual Class to implement when integrating a stand-alone sensor into ros2_control. /** - * Virtual Class to implement when integrating a stand-alone sensor into ros2_control. - * The typical examples are Force-Torque Sensor (FTS), Interial Measurement Unit (IMU). - */ -class SensorInterface + * The typical examples are Force-Torque Sensor (FTS), Interial Measurement Unit (IMU). + * + * Methods return values have type rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn + * with the following meaning: + * + * \returns CallbackReturn::SUCCESS method execution was successful. + * \returns CallbackReturn::FAILURE method execution has failed and and can be called again. + * \returns CallbackReturn::ERROR critical error has happened that should be managed in + * "on_error" method. + * + * The hardware ends after each method in a state with the following meaning: + * + * UNCONFIGURED (on_init, on_cleanup): + * Hardware is initialized but communication is not started and therefore no interface is available. + * + * INACTIVE (on_configure, on_deactivate): + * Communication with the hardware is started and it is configured. + * States can be read and non-movement hardware interfaces commanded. + * Hardware interfaces for movement will NOT be available. + * Those interfaces are: HW_IF_POSITION, HW_IF_VELOCITY, HW_IF_ACCELERATION, and HW_IF_EFFORT. + * + * FINALIZED (on_shutdown): + * Hardware interface is ready for unloading/destruction. + * Allocated memory is cleaned up. + * + * ACTIVE (on_activate): + * Power circuits of hardware are active and hardware can be moved, e.g., brakes are disabled. + * Command interfaces for movement are available and have to be accepted. + * Those interfaces are: HW_IF_POSITION, HW_IF_VELOCITY, HW_IF_ACCELERATION, and HW_IF_EFFORT. + */ +class SensorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface { public: - SensorInterface() = default; + SensorInterface() + : lifecycle_state_(rclcpp_lifecycle::State( + lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN, lifecycle_state_names::UNKNOWN)) + { + } + + /// SensorInterface copy constructor is actively deleted. + /** + * Hardware interfaces are having a unique ownership and thus can't be copied in order to avoid + * failed or simultaneous access to hardware. + */ + SensorInterface(const SensorInterface & other) = delete; + + SensorInterface(SensorInterface && other) = default; virtual ~SensorInterface() = default; - /// Configuration of the sensor from data parsed from the robot's URDF. + /// Initialization of the hardware interface from data parsed from the robot's URDF. /** - * \param[in] sensor_info structure with data from URDF. - * \return return_type::OK if required data are provided and can be parsed, - * return_type::ERROR otherwise. + * \param[in] hardware_info structure with data from URDF. + * \returns CallbackReturn::SUCCESS if required data are provided and can be parsed. + * \returns CallbackReturn::ERROR if any error happens or data are missing. */ - virtual return_type configure(const HardwareInfo & sensor_info) = 0; + virtual CallbackReturn on_init(const HardwareInfo & hardware_info) + { + info_ = hardware_info; + return CallbackReturn::SUCCESS; + }; - /// Exports all state interfaces for this sensor. + /// Exports all state interfaces for this hardware interface. /** * The state interfaces have to be created and transferred according - * to the sensor info passed in for the configuration. + * to the hardware info passed in for the configuration. * * Note the ownership over the state interfaces is transferred to the caller. * - * \return std::vector vector of state interfaces + * \return vector of state interfaces */ virtual std::vector export_state_interfaces() = 0; - /// Start exchange data with the hardware. + /// Read the current state values from the actuator. /** - * \return return_type:OK if everything worked as expected, return_type::ERROR otherwise. - */ - virtual return_type start() = 0; - - /// Stop exchange data with the hardware. - /** - * \return return_type:OK if everything worked as expected, return_type::ERROR otherwise. + * The data readings from the physical hardware has to be updated + * and reflected accordingly in the exported state interfaces. + * That is, the data pointed by the interfaces shall be updated. + * + * \return return_type::OK if the read was successful, return_type::ERROR otherwise. */ - virtual return_type stop() = 0; + virtual return_type read() = 0; - /// Get name of the sensor hardware. + /// Get name of the actuator hardware. /** * \return name. */ - virtual std::string get_name() const = 0; + virtual std::string get_name() const { return info_.name; } - /// Get current state of the sensor hardware. + /// Get life-cycle state of the actuator hardware. /** - * \return current status. + * \return state. */ - virtual status get_status() const = 0; + const rclcpp_lifecycle::State & get_state() const { return lifecycle_state_; } - /// Read the current state values from the sensor. + /// Set life-cycle state of the actuator hardware. /** - * The data readings from the physical hardware has to be updated - * and reflected accordingly in the exported state interfaces. - * That is, the data pointed by the interfaces shall be updated. - * - * \return return_type::OK if the read was successful, return_type::ERROR otherwise. + * \return state. */ - virtual return_type read() = 0; + void set_state(const rclcpp_lifecycle::State & new_state) { lifecycle_state_ = new_state; } + +protected: + HardwareInfo info_; + rclcpp_lifecycle::State lifecycle_state_; }; } // namespace hardware_interface diff --git a/hardware_interface/include/hardware_interface/system.hpp b/hardware_interface/include/hardware_interface/system.hpp index 9c238053da..b82113914b 100644 --- a/hardware_interface/include/hardware_interface/system.hpp +++ b/hardware_interface/include/hardware_interface/system.hpp @@ -1,4 +1,4 @@ -// Copyright 2020 ros2_control Development Team +// Copyright 2020 - 2021 ros2_control Development Team // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -23,8 +23,8 @@ #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" +#include "rclcpp_lifecycle/state.hpp" namespace hardware_interface { @@ -41,7 +41,25 @@ class System final ~System() = default; HARDWARE_INTERFACE_PUBLIC - return_type configure(const HardwareInfo & system_info); + const rclcpp_lifecycle::State & initialize(const HardwareInfo & system_info); + + HARDWARE_INTERFACE_PUBLIC + const rclcpp_lifecycle::State & configure(); + + HARDWARE_INTERFACE_PUBLIC + const rclcpp_lifecycle::State & cleanup(); + + HARDWARE_INTERFACE_PUBLIC + const rclcpp_lifecycle::State & shutdown(); + + HARDWARE_INTERFACE_PUBLIC + const rclcpp_lifecycle::State & activate(); + + HARDWARE_INTERFACE_PUBLIC + const rclcpp_lifecycle::State & deactivate(); + + HARDWARE_INTERFACE_PUBLIC + const rclcpp_lifecycle::State & error(); HARDWARE_INTERFACE_PUBLIC std::vector export_state_interfaces(); @@ -59,17 +77,11 @@ class System final const std::vector & start_interfaces, const std::vector & stop_interfaces); - HARDWARE_INTERFACE_PUBLIC - return_type start(); - - HARDWARE_INTERFACE_PUBLIC - return_type stop(); - HARDWARE_INTERFACE_PUBLIC std::string get_name() const; HARDWARE_INTERFACE_PUBLIC - status get_status() const; + const rclcpp_lifecycle::State & get_state() const; HARDWARE_INTERFACE_PUBLIC return_type read(); diff --git a/hardware_interface/include/hardware_interface/system_interface.hpp b/hardware_interface/include/hardware_interface/system_interface.hpp index 4d174cbca8..1315bacc44 100644 --- a/hardware_interface/include/hardware_interface/system_interface.hpp +++ b/hardware_interface/include/hardware_interface/system_interface.hpp @@ -1,4 +1,4 @@ -// Copyright 2020 ros2_control Development Team +// Copyright 2020 - 2021 ros2_control Development Team // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -22,47 +22,95 @@ #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" +#include "hardware_interface/types/lifecycle_state_names.hpp" +#include "lifecycle_msgs/msg/state.hpp" +#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" +#include "rclcpp_lifecycle/state.hpp" + +using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; namespace hardware_interface { /// Virtual Class to implement when integrating a complex system into ros2_control. /** -* The common examples for these types of hardware are multi-joint systems with or without sensors -* such as industrial or humanoid robots. -*/ -class SystemInterface + * The common examples for these types of hardware are multi-joint systems with or without sensors + * such as industrial or humanoid robots. + * + * Methods return values have type rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn + * with the following meaning: + * + * \returns CallbackReturn::SUCCESS method execution was successful. + * \returns CallbackReturn::FAILURE method execution has failed and and can be called again. + * \returns CallbackReturn::ERROR critical error has happened that should be managed in + * "on_error" method. + * + * The hardware ends after each method in a state with the following meaning: + * + * UNCONFIGURED (on_init, on_cleanup): + * Hardware is initialized but communication is not started and therefore no interface is available. + * + * INACTIVE (on_configure, on_deactivate): + * Communication with the hardware is started and it is configured. + * States can be read and non-movement hardware interfaces commanded. + * Hardware interfaces for movement will NOT be available. + * Those interfaces are: HW_IF_POSITION, HW_IF_VELOCITY, HW_IF_ACCELERATION, and HW_IF_EFFORT. + * + * FINALIZED (on_shutdown): + * Hardware interface is ready for unloading/destruction. + * Allocated memory is cleaned up. + * + * ACTIVE (on_activate): + * Power circuits of hardware are active and hardware can be moved, e.g., brakes are disabled. + * Command interfaces for movement are available and have to be accepted. + * Those interfaces are: HW_IF_POSITION, HW_IF_VELOCITY, HW_IF_ACCELERATION, and HW_IF_EFFORT. + */ +class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface { public: - SystemInterface() = default; + SystemInterface() + : lifecycle_state_(rclcpp_lifecycle::State( + lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN, lifecycle_state_names::UNKNOWN)) + { + } + + /// SensorInterface copy constructor is actively deleted. + /** + * Hardware interfaces are having a unique ownership and thus can't be copied in order to avoid + * failed or simultaneous access to hardware. + */ + SystemInterface(const SystemInterface & other) = delete; + + SystemInterface(SystemInterface && other) = default; virtual ~SystemInterface() = default; - /// Configuration of the system from data parsed from the robot's URDF. + /// Initialization of the hardware interface from data parsed from the robot's URDF. /** - * \param[in] system_info structure with data from URDF. - * \return return_type::OK if required data are provided and can be parsed, - * return_type::ERROR otherwise. + * \param[in] hardware_info structure with data from URDF. + * \returns CallbackReturn::SUCCESS if required data are provided and can be parsed. + * \returns CallbackReturn::ERROR if any error happens or data are missing. */ - virtual return_type configure(const HardwareInfo & system_info) = 0; + virtual CallbackReturn on_init(const HardwareInfo & hardware_info) + { + info_ = hardware_info; + return CallbackReturn::SUCCESS; + }; - /// Exports all state interfaces for this system. + /// Exports all state interfaces for this hardware interface. /** * The state interfaces have to be created and transferred according - * to the system info passed in for the configuration. + * to the hardware info passed in for the configuration. * * Note the ownership over the state interfaces is transferred to the caller. * - * * \return vector of state interfaces */ virtual std::vector export_state_interfaces() = 0; - /// Exports all command interfaces for this system. + /// Exports all command interfaces for this hardware interface. /** * The command interfaces have to be created and transferred according - * to the system info passed in for the configuration. + * to the hardware info passed in for the configuration. * * Note the ownership over the state interfaces is transferred to the caller. * @@ -109,31 +157,7 @@ class SystemInterface return return_type::OK; } - /// Start exchange data with the hardware. - /** - * \return return_type:OK if everything worked as expected, return_type::ERROR otherwise. - */ - virtual return_type start() = 0; - - /// Stop exchange data with the hardware. - /** - * \return return_type:OK if everything worked as expected, return_type::ERROR otherwise. - */ - virtual return_type stop() = 0; - - /// Get name of the system hardware. - /** - * \return name. - */ - virtual std::string get_name() const = 0; - - /// Get current state of the system hardware. - /** - * \return current status. - */ - virtual status get_status() const = 0; - - /// Read the current state values from the actuators and sensors within the system. + /// 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 interfaces. @@ -143,7 +167,7 @@ class SystemInterface */ virtual return_type read() = 0; - /// Write the current command values to the actuator within the system. + /// Write the current command values to the actuator. /** * The physical hardware shall be updated with the latest value from * the exported command interfaces. @@ -151,6 +175,28 @@ class SystemInterface * \return return_type::OK if the read was successful, return_type::ERROR otherwise. */ virtual return_type write() = 0; + + /// Get name of the actuator hardware. + /** + * \return name. + */ + virtual std::string get_name() const { return info_.name; } + + /// Get life-cycle state of the actuator hardware. + /** + * \return state. + */ + const rclcpp_lifecycle::State & get_state() const { return lifecycle_state_; } + + /// Set life-cycle state of the actuator hardware. + /** + * \return state. + */ + void set_state(const rclcpp_lifecycle::State & new_state) { lifecycle_state_ = new_state; } + +protected: + HardwareInfo info_; + rclcpp_lifecycle::State lifecycle_state_; }; } // namespace hardware_interface diff --git a/hardware_interface/include/hardware_interface/types/hardware_interface_status_values.hpp b/hardware_interface/include/hardware_interface/types/hardware_interface_status_values.hpp deleted file mode 100644 index fc7291baa3..0000000000 --- a/hardware_interface/include/hardware_interface/types/hardware_interface_status_values.hpp +++ /dev/null @@ -1,32 +0,0 @@ -// Copyright 2020 ros2_control Development Team -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef HARDWARE_INTERFACE__TYPES__HARDWARE_INTERFACE_STATUS_VALUES_HPP_ -#define HARDWARE_INTERFACE__TYPES__HARDWARE_INTERFACE_STATUS_VALUES_HPP_ - -#include - -namespace hardware_interface -{ -enum class status : std::uint8_t -{ - UNKNOWN = 0, - CONFIGURED = 1, - STARTED = 3, - STOPPED = 4, -}; - -} // namespace hardware_interface - -#endif // HARDWARE_INTERFACE__TYPES__HARDWARE_INTERFACE_STATUS_VALUES_HPP_ diff --git a/hardware_interface/include/hardware_interface/types/lifecycle_state_names.hpp b/hardware_interface/include/hardware_interface/types/lifecycle_state_names.hpp new file mode 100644 index 0000000000..f51f64f836 --- /dev/null +++ b/hardware_interface/include/hardware_interface/types/lifecycle_state_names.hpp @@ -0,0 +1,36 @@ +// Copyright (c) 2021, Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// +/// \author: Denis Stogl + +#ifndef HARDWARE_INTERFACE__TYPES__LIFECYCLE_STATE_NAMES_HPP_ +#define HARDWARE_INTERFACE__TYPES__LIFECYCLE_STATE_NAMES_HPP_ + +#include + +namespace hardware_interface +{ +namespace lifecycle_state_names +{ +/// Constants defining string labels corresponding to lifecycle states +constexpr char UNKNOWN[] = "unknown"; +constexpr char UNCONFIGURED[] = "unconfigured"; +constexpr char INACTIVE[] = "inactive"; +constexpr char ACTIVE[] = "active"; +constexpr char FINALIZED[] = "finalized"; +} // namespace lifecycle_state_names + +} // namespace hardware_interface + +#endif // HARDWARE_INTERFACE__TYPES__LIFECYCLE_STATE_NAMES_HPP_ diff --git a/hardware_interface/package.xml b/hardware_interface/package.xml index 1972dd1268..c06e535c72 100644 --- a/hardware_interface/package.xml +++ b/hardware_interface/package.xml @@ -11,6 +11,7 @@ control_msgs pluginlib + rclcpp_lifecycle rcpputils tinyxml2_vendor diff --git a/hardware_interface/src/actuator.cpp b/hardware_interface/src/actuator.cpp index f909927c97..774b769995 100644 --- a/hardware_interface/src/actuator.cpp +++ b/hardware_interface/src/actuator.cpp @@ -1,4 +1,4 @@ -// Copyright 2020 ros2_control Development Team +// Copyright 2020-2021 ros2_control Development Team // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -12,27 +12,178 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include "hardware_interface/actuator.hpp" + #include #include #include #include #include -#include "hardware_interface/actuator.hpp" #include "hardware_interface/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" +#include "hardware_interface/types/lifecycle_state_names.hpp" +#include "lifecycle_msgs/msg/state.hpp" +#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" +#include "rclcpp_lifecycle/state.hpp" +using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; + +// TODO(destogl): Add transition names also namespace hardware_interface { Actuator::Actuator(std::unique_ptr impl) : impl_(std::move(impl)) {} -return_type Actuator::configure(const HardwareInfo & actuator_info) +const rclcpp_lifecycle::State & Actuator::initialize(const HardwareInfo & actuator_info) +{ + if (impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN) + { + switch (impl_->on_init(actuator_info)) + { + case CallbackReturn::SUCCESS: + impl_->set_state(rclcpp_lifecycle::State( + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + lifecycle_state_names::UNCONFIGURED)); + break; + case CallbackReturn::FAILURE: + case CallbackReturn::ERROR: + impl_->set_state(rclcpp_lifecycle::State( + lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, lifecycle_state_names::FINALIZED)); + break; + } + } + return impl_->get_state(); +} + +const rclcpp_lifecycle::State & Actuator::configure() { - return impl_->configure(actuator_info); + if (impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED) + { + switch (impl_->on_configure(impl_->get_state())) + { + case CallbackReturn::SUCCESS: + impl_->set_state(rclcpp_lifecycle::State( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, lifecycle_state_names::INACTIVE)); + break; + case CallbackReturn::FAILURE: + impl_->set_state(rclcpp_lifecycle::State( + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + lifecycle_state_names::UNCONFIGURED)); + break; + case CallbackReturn::ERROR: + impl_->set_state(error()); + break; + } + } + return impl_->get_state(); +} + +const rclcpp_lifecycle::State & Actuator::cleanup() +{ + if (impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) + { + switch (impl_->on_cleanup(impl_->get_state())) + { + case CallbackReturn::SUCCESS: + impl_->set_state(rclcpp_lifecycle::State( + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + lifecycle_state_names::UNCONFIGURED)); + break; + case CallbackReturn::FAILURE: + case CallbackReturn::ERROR: + impl_->set_state(error()); + break; + } + } + return impl_->get_state(); +} + +const rclcpp_lifecycle::State & Actuator::shutdown() +{ + if ( + impl_->get_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN && + impl_->get_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED) + { + switch (impl_->on_shutdown(impl_->get_state())) + { + case CallbackReturn::SUCCESS: + impl_->set_state(rclcpp_lifecycle::State( + lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, lifecycle_state_names::FINALIZED)); + break; + case CallbackReturn::FAILURE: + case CallbackReturn::ERROR: + impl_->set_state(error()); + break; + } + } + return impl_->get_state(); +} + +const rclcpp_lifecycle::State & Actuator::activate() +{ + if (impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) + { + switch (impl_->on_activate(impl_->get_state())) + { + case CallbackReturn::SUCCESS: + impl_->set_state(rclcpp_lifecycle::State( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, lifecycle_state_names::ACTIVE)); + break; + case CallbackReturn::FAILURE: + impl_->set_state(rclcpp_lifecycle::State( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, lifecycle_state_names::INACTIVE)); + break; + case CallbackReturn::ERROR: + impl_->set_state(error()); + break; + } + } + return impl_->get_state(); +} + +const rclcpp_lifecycle::State & Actuator::deactivate() +{ + if (impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) + { + switch (impl_->on_deactivate(impl_->get_state())) + { + case CallbackReturn::SUCCESS: + impl_->set_state(rclcpp_lifecycle::State( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, lifecycle_state_names::INACTIVE)); + break; + case CallbackReturn::FAILURE: + impl_->set_state(rclcpp_lifecycle::State( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, lifecycle_state_names::ACTIVE)); + break; + case CallbackReturn::ERROR: + impl_->set_state(error()); + break; + } + } + return impl_->get_state(); +} + +const rclcpp_lifecycle::State & Actuator::error() +{ + if (impl_->get_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN) + { + switch (impl_->on_error(impl_->get_state())) + { + case CallbackReturn::SUCCESS: + impl_->set_state(rclcpp_lifecycle::State( + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + lifecycle_state_names::UNCONFIGURED)); + break; + case CallbackReturn::FAILURE: + case CallbackReturn::ERROR: + impl_->set_state(rclcpp_lifecycle::State( + lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, lifecycle_state_names::FINALIZED)); + break; + } + } + return impl_->get_state(); } std::vector Actuator::export_state_interfaces() @@ -61,16 +212,30 @@ return_type Actuator::perform_command_mode_switch( return impl_->perform_command_mode_switch(start_interfaces, stop_interfaces); } -return_type Actuator::start() { return impl_->start(); } - -return_type Actuator::stop() { return impl_->stop(); } - std::string Actuator::get_name() const { return impl_->get_name(); } -status Actuator::get_status() const { return impl_->get_status(); } +const rclcpp_lifecycle::State & Actuator::get_state() const { return impl_->get_state(); } -return_type Actuator::read() { return impl_->read(); } +return_type Actuator::read() +{ + if ( + impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE || + impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) + { + return impl_->read(); + } + return return_type::ERROR; +} -return_type Actuator::write() { return impl_->write(); } +return_type Actuator::write() +{ + if ( + impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE || + impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) + { + return impl_->write(); + } + return return_type::ERROR; +} } // namespace hardware_interface diff --git a/hardware_interface/src/fake_components/generic_system.cpp b/hardware_interface/src/fake_components/generic_system.cpp index 6b37186d03..b70132a5ca 100644 --- a/hardware_interface/src/fake_components/generic_system.cpp +++ b/hardware_interface/src/fake_components/generic_system.cpp @@ -28,11 +28,11 @@ namespace fake_components { -return_type GenericSystem::configure(const hardware_interface::HardwareInfo & info) +CallbackReturn GenericSystem::on_init(const hardware_interface::HardwareInfo & info) { - if (configure_default(info) != return_type::OK) + if (hardware_interface::SystemInterface::on_init(info) != CallbackReturn::SUCCESS) { - return return_type::ERROR; + return CallbackReturn::ERROR; } // check if to create fake command interface for sensor @@ -180,8 +180,7 @@ return_type GenericSystem::configure(const hardware_interface::HardwareInfo & in } initialize_storage_vectors(sensor_fake_commands_, sensor_states_, sensor_interfaces_); - status_ = hardware_interface::status::CONFIGURED; - return hardware_interface::return_type::OK; + return CallbackReturn::SUCCESS; } std::vector GenericSystem::export_state_interfaces() diff --git a/hardware_interface/src/resource_manager.cpp b/hardware_interface/src/resource_manager.cpp index 85c20a18a0..8ebefeaf5c 100644 --- a/hardware_interface/src/resource_manager.cpp +++ b/hardware_interface/src/resource_manager.cpp @@ -29,6 +29,7 @@ #include "hardware_interface/sensor_interface.hpp" #include "hardware_interface/system.hpp" #include "hardware_interface/system_interface.hpp" +#include "lifecycle_msgs/msg/state.hpp" #include "pluginlib/class_loader.hpp" #include "rcutils/logging_macros.h" @@ -63,7 +64,7 @@ class ResourceStorage HardwareT hardware(std::move(interface)); container.emplace_back(std::move(hardware)); hardware_status_map_.emplace( - std::make_pair(container.back().get_name(), container.back().get_status())); + std::make_pair(container.back().get_name(), container.back().get_state())); } template @@ -95,7 +96,13 @@ class ResourceStorage std::unordered_map & claimed_command_interface_map) { initialize_hardware(hardware_info, actuator_loader_, actuators_); - if (return_type::OK != actuators_.back().configure(hardware_info)) + if ( + actuators_.back().initialize(hardware_info).id() != + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED) + { + throw std::runtime_error(std::string("Failed to initialize '") + hardware_info.name + "'"); + } + if (actuators_.back().configure().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) { throw std::runtime_error(std::string("Failed to configure '") + hardware_info.name + "'"); } @@ -106,7 +113,13 @@ class ResourceStorage void initialize_sensor(const HardwareInfo & hardware_info) { initialize_hardware(hardware_info, sensor_loader_, sensors_); - if (return_type::OK != sensors_.back().configure(hardware_info)) + if ( + sensors_.back().initialize(hardware_info).id() != + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED) + { + throw std::runtime_error(std::string("Failed to initialize '") + hardware_info.name + "'"); + } + if (sensors_.back().configure().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) { throw std::runtime_error(std::string("Failed to configure '") + hardware_info.name + "'"); } @@ -118,7 +131,13 @@ class ResourceStorage std::unordered_map & claimed_command_interface_map) { initialize_hardware(hardware_info, system_loader_, systems_); - if (return_type::OK != systems_.back().configure(hardware_info)) + if ( + systems_.back().initialize(hardware_info).id() != + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED) + { + throw std::runtime_error(std::string("Failed to initialize '") + hardware_info.name + "'"); + } + if (systems_.back().configure().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) { throw std::runtime_error(std::string("Failed to configure '") + hardware_info.name + "'"); } @@ -135,7 +154,7 @@ class ResourceStorage std::vector sensors_; std::vector systems_; - std::unordered_map hardware_status_map_; + std::unordered_map hardware_status_map_; std::map state_interface_map_; std::map command_interface_map_; @@ -297,19 +316,19 @@ size_t ResourceManager::system_components_size() const return resource_storage_->systems_.size(); } -std::unordered_map ResourceManager::get_components_status() +std::unordered_map ResourceManager::get_components_states() { for (auto & component : resource_storage_->actuators_) { - resource_storage_->hardware_status_map_[component.get_name()] = component.get_status(); + resource_storage_->hardware_status_map_[component.get_name()] = component.get_state(); } for (auto & component : resource_storage_->sensors_) { - resource_storage_->hardware_status_map_[component.get_name()] = component.get_status(); + resource_storage_->hardware_status_map_[component.get_name()] = component.get_state(); } for (auto & component : resource_storage_->systems_) { - resource_storage_->hardware_status_map_[component.get_name()] = component.get_status(); + resource_storage_->hardware_status_map_[component.get_name()] = component.get_state(); } return resource_storage_->hardware_status_map_; @@ -395,15 +414,15 @@ void ResourceManager::start_components() { for (auto & component : resource_storage_->actuators_) { - component.start(); + component.activate(); } for (auto & component : resource_storage_->sensors_) { - component.start(); + component.activate(); } for (auto & component : resource_storage_->systems_) { - component.start(); + component.activate(); } } @@ -411,15 +430,15 @@ void ResourceManager::stop_components() { for (auto & component : resource_storage_->actuators_) { - component.stop(); + component.deactivate(); } for (auto & component : resource_storage_->sensors_) { - component.stop(); + component.deactivate(); } for (auto & component : resource_storage_->systems_) { - component.stop(); + component.deactivate(); } } diff --git a/hardware_interface/src/sensor.cpp b/hardware_interface/src/sensor.cpp index 490e8502b4..f581759463 100644 --- a/hardware_interface/src/sensor.cpp +++ b/hardware_interface/src/sensor.cpp @@ -1,4 +1,4 @@ -// Copyright 2020 ros2_control Development Team +// Copyright 2020 - 2021 ros2_control Development Team // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -12,41 +12,196 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include "hardware_interface/sensor.hpp" + #include #include #include #include -#include "hardware_interface/sensor.hpp" - #include "hardware_interface/handle.hpp" #include "hardware_interface/hardware_info.hpp" #include "hardware_interface/sensor_interface.hpp" #include "hardware_interface/types/hardware_interface_return_values.hpp" -#include "hardware_interface/types/hardware_interface_status_values.hpp" +#include "hardware_interface/types/lifecycle_state_names.hpp" +#include "lifecycle_msgs/msg/state.hpp" +#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" +#include "rclcpp_lifecycle/state.hpp" + +using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; namespace hardware_interface { Sensor::Sensor(std::unique_ptr impl) : impl_(std::move(impl)) {} -return_type Sensor::configure(const HardwareInfo & sensor_info) +const rclcpp_lifecycle::State & Sensor::initialize(const HardwareInfo & sensor_info) { - return impl_->configure(sensor_info); + if (impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN) + { + switch (impl_->on_init(sensor_info)) + { + case CallbackReturn::SUCCESS: + impl_->set_state(rclcpp_lifecycle::State( + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + lifecycle_state_names::UNCONFIGURED)); + break; + case CallbackReturn::FAILURE: + case CallbackReturn::ERROR: + impl_->set_state(rclcpp_lifecycle::State( + lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, lifecycle_state_names::FINALIZED)); + break; + } + } + return impl_->get_state(); } -std::vector Sensor::export_state_interfaces() +const rclcpp_lifecycle::State & Sensor::configure() { - return impl_->export_state_interfaces(); + if (impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED) + { + switch (impl_->on_configure(impl_->get_state())) + { + case CallbackReturn::SUCCESS: + impl_->set_state(rclcpp_lifecycle::State( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, lifecycle_state_names::INACTIVE)); + break; + case CallbackReturn::FAILURE: + impl_->set_state(rclcpp_lifecycle::State( + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + lifecycle_state_names::UNCONFIGURED)); + break; + case CallbackReturn::ERROR: + impl_->set_state(error()); + break; + } + } + return impl_->get_state(); } -return_type Sensor::start() { return impl_->start(); } +const rclcpp_lifecycle::State & Sensor::cleanup() +{ + if (impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) + { + switch (impl_->on_cleanup(impl_->get_state())) + { + case CallbackReturn::SUCCESS: + impl_->set_state(rclcpp_lifecycle::State( + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + lifecycle_state_names::UNCONFIGURED)); + break; + case CallbackReturn::FAILURE: + case CallbackReturn::ERROR: + impl_->set_state(error()); + break; + } + } + return impl_->get_state(); +} -return_type Sensor::stop() { return impl_->stop(); } +const rclcpp_lifecycle::State & Sensor::shutdown() +{ + if ( + impl_->get_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN && + impl_->get_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED) + { + switch (impl_->on_shutdown(impl_->get_state())) + { + case CallbackReturn::SUCCESS: + impl_->set_state(rclcpp_lifecycle::State( + lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, lifecycle_state_names::FINALIZED)); + break; + case CallbackReturn::FAILURE: + case CallbackReturn::ERROR: + impl_->set_state(error()); + break; + } + } + return impl_->get_state(); +} + +const rclcpp_lifecycle::State & Sensor::activate() +{ + if (impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) + { + switch (impl_->on_activate(impl_->get_state())) + { + case CallbackReturn::SUCCESS: + impl_->set_state(rclcpp_lifecycle::State( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, lifecycle_state_names::ACTIVE)); + break; + case CallbackReturn::FAILURE: + impl_->set_state(rclcpp_lifecycle::State( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, lifecycle_state_names::INACTIVE)); + break; + case CallbackReturn::ERROR: + impl_->set_state(error()); + break; + } + } + return impl_->get_state(); +} + +const rclcpp_lifecycle::State & Sensor::deactivate() +{ + if (impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) + { + switch (impl_->on_deactivate(impl_->get_state())) + { + case CallbackReturn::SUCCESS: + impl_->set_state(rclcpp_lifecycle::State( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, lifecycle_state_names::INACTIVE)); + break; + case CallbackReturn::FAILURE: + impl_->set_state(rclcpp_lifecycle::State( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, lifecycle_state_names::ACTIVE)); + break; + case CallbackReturn::ERROR: + impl_->set_state(error()); + break; + } + } + return impl_->get_state(); +} + +const rclcpp_lifecycle::State & Sensor::error() +{ + if (impl_->get_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN) + { + switch (impl_->on_error(impl_->get_state())) + { + case CallbackReturn::SUCCESS: + impl_->set_state(rclcpp_lifecycle::State( + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + lifecycle_state_names::UNCONFIGURED)); + break; + case CallbackReturn::FAILURE: + case CallbackReturn::ERROR: + impl_->set_state(rclcpp_lifecycle::State( + lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, lifecycle_state_names::FINALIZED)); + break; + } + } + return impl_->get_state(); +} + +std::vector Sensor::export_state_interfaces() +{ + return impl_->export_state_interfaces(); +} std::string Sensor::get_name() const { return impl_->get_name(); } -status Sensor::get_status() const { return impl_->get_status(); } +const rclcpp_lifecycle::State & Sensor::get_state() const { return impl_->get_state(); } -return_type Sensor::read() { return impl_->read(); } +return_type Sensor::read() +{ + if ( + impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE || + impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) + { + return impl_->read(); + } + return return_type::ERROR; +} } // namespace hardware_interface diff --git a/hardware_interface/src/system.cpp b/hardware_interface/src/system.cpp index 25b5570b2b..6462ef99fe 100644 --- a/hardware_interface/src/system.cpp +++ b/hardware_interface/src/system.cpp @@ -1,4 +1,4 @@ -// Copyright 2020 ros2_control Development Team +// Copyright 2020 - 2021 ros2_control Development Team // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -12,26 +12,176 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include "hardware_interface/system.hpp" + #include #include #include #include -#include "hardware_interface/system.hpp" - #include "hardware_interface/handle.hpp" #include "hardware_interface/hardware_info.hpp" #include "hardware_interface/system_interface.hpp" #include "hardware_interface/types/hardware_interface_return_values.hpp" -#include "hardware_interface/types/hardware_interface_status_values.hpp" +#include "hardware_interface/types/lifecycle_state_names.hpp" +#include "lifecycle_msgs/msg/state.hpp" +#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" +#include "rclcpp_lifecycle/state.hpp" + +using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; namespace hardware_interface { System::System(std::unique_ptr impl) : impl_(std::move(impl)) {} -return_type System::configure(const HardwareInfo & system_info) +const rclcpp_lifecycle::State & System::initialize(const HardwareInfo & system_info) +{ + if (impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN) + { + switch (impl_->on_init(system_info)) + { + case CallbackReturn::SUCCESS: + impl_->set_state(rclcpp_lifecycle::State( + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + lifecycle_state_names::UNCONFIGURED)); + break; + case CallbackReturn::FAILURE: + case CallbackReturn::ERROR: + impl_->set_state(rclcpp_lifecycle::State( + lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, lifecycle_state_names::FINALIZED)); + break; + } + } + return impl_->get_state(); +} + +const rclcpp_lifecycle::State & System::configure() { - return impl_->configure(system_info); + if (impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED) + { + switch (impl_->on_configure(impl_->get_state())) + { + case CallbackReturn::SUCCESS: + impl_->set_state(rclcpp_lifecycle::State( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, lifecycle_state_names::INACTIVE)); + break; + case CallbackReturn::FAILURE: + impl_->set_state(rclcpp_lifecycle::State( + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + lifecycle_state_names::UNCONFIGURED)); + break; + case CallbackReturn::ERROR: + impl_->set_state(error()); + break; + } + } + return impl_->get_state(); +} + +const rclcpp_lifecycle::State & System::cleanup() +{ + if (impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) + { + switch (impl_->on_cleanup(impl_->get_state())) + { + case CallbackReturn::SUCCESS: + impl_->set_state(rclcpp_lifecycle::State( + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + lifecycle_state_names::UNCONFIGURED)); + break; + case CallbackReturn::FAILURE: + case CallbackReturn::ERROR: + impl_->set_state(error()); + break; + } + } + return impl_->get_state(); +} + +const rclcpp_lifecycle::State & System::shutdown() +{ + if ( + impl_->get_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN && + impl_->get_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED) + { + switch (impl_->on_shutdown(impl_->get_state())) + { + case CallbackReturn::SUCCESS: + impl_->set_state(rclcpp_lifecycle::State( + lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, lifecycle_state_names::FINALIZED)); + break; + case CallbackReturn::FAILURE: + case CallbackReturn::ERROR: + impl_->set_state(error()); + break; + } + } + return impl_->get_state(); +} + +const rclcpp_lifecycle::State & System::activate() +{ + if (impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) + { + switch (impl_->on_activate(impl_->get_state())) + { + case CallbackReturn::SUCCESS: + impl_->set_state(rclcpp_lifecycle::State( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, lifecycle_state_names::ACTIVE)); + break; + case CallbackReturn::FAILURE: + impl_->set_state(rclcpp_lifecycle::State( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, lifecycle_state_names::INACTIVE)); + break; + case CallbackReturn::ERROR: + impl_->set_state(error()); + break; + } + } + return impl_->get_state(); +} + +const rclcpp_lifecycle::State & System::deactivate() +{ + if (impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) + { + switch (impl_->on_deactivate(impl_->get_state())) + { + case CallbackReturn::SUCCESS: + impl_->set_state(rclcpp_lifecycle::State( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, lifecycle_state_names::INACTIVE)); + break; + case CallbackReturn::FAILURE: + impl_->set_state(rclcpp_lifecycle::State( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, lifecycle_state_names::ACTIVE)); + break; + case CallbackReturn::ERROR: + impl_->set_state(error()); + break; + } + } + return impl_->get_state(); +} + +const rclcpp_lifecycle::State & System::error() +{ + if (impl_->get_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN) + { + switch (impl_->on_error(impl_->get_state())) + { + case CallbackReturn::SUCCESS: + impl_->set_state(rclcpp_lifecycle::State( + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + lifecycle_state_names::UNCONFIGURED)); + break; + case CallbackReturn::FAILURE: + case CallbackReturn::ERROR: + impl_->set_state(rclcpp_lifecycle::State( + lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, lifecycle_state_names::FINALIZED)); + break; + } + } + return impl_->get_state(); } std::vector System::export_state_interfaces() @@ -58,16 +208,30 @@ return_type System::perform_command_mode_switch( return impl_->perform_command_mode_switch(start_interfaces, stop_interfaces); } -return_type System::start() { return impl_->start(); } - -return_type System::stop() { return impl_->stop(); } - std::string System::get_name() const { return impl_->get_name(); } -status System::get_status() const { return impl_->get_status(); } +const rclcpp_lifecycle::State & System::get_state() const { return impl_->get_state(); } -return_type System::read() { return impl_->read(); } +return_type System::read() +{ + if ( + impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE || + impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) + { + return impl_->read(); + } + return return_type::ERROR; +} -return_type System::write() { return impl_->write(); } +return_type System::write() +{ + if ( + impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE || + impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) + { + return impl_->write(); + } + return return_type::ERROR; +} } // namespace hardware_interface diff --git a/hardware_interface/test/fake_components/test_generic_system.cpp b/hardware_interface/test/fake_components/test_generic_system.cpp index a085663521..2443fd3389 100644 --- a/hardware_interface/test/fake_components/test_generic_system.cpp +++ b/hardware_interface/test/fake_components/test_generic_system.cpp @@ -23,6 +23,7 @@ #include "hardware_interface/loaned_command_interface.hpp" #include "hardware_interface/loaned_state_interface.hpp" #include "hardware_interface/resource_manager.hpp" +#include "hardware_interface/types/lifecycle_state_names.hpp" #include "ros2_control_test_assets/components_urdfs.hpp" #include "ros2_control_test_assets/descriptions.hpp" @@ -412,9 +413,10 @@ void generic_system_functional_test(const std::string & urdf, const double offse hardware_interface::ResourceManager rm(urdf); // check is hardware is configured - std::unordered_map status_map; - status_map = rm.get_components_status(); - EXPECT_EQ(status_map["GenericSystem2dof"], hardware_interface::status::CONFIGURED); + std::unordered_map states_map; + states_map = rm.get_components_states(); + EXPECT_EQ( + states_map["GenericSystem2dof"].label(), hardware_interface::lifecycle_state_names::INACTIVE); // Check initial values hardware_interface::LoanedStateInterface j1p_s = rm.claim_state_interface("joint1/position"); @@ -491,12 +493,14 @@ void generic_system_functional_test(const std::string & urdf, const double offse ASSERT_EQ(0.88, j2v_c.get_value()); rm.start_components(); - status_map = rm.get_components_status(); - EXPECT_EQ(status_map["GenericSystem2dof"], hardware_interface::status::STARTED); + states_map = rm.get_components_states(); + EXPECT_EQ( + states_map["GenericSystem2dof"].label(), hardware_interface::lifecycle_state_names::ACTIVE); rm.stop_components(); - status_map = rm.get_components_status(); - EXPECT_EQ(status_map["GenericSystem2dof"], hardware_interface::status::STOPPED); + states_map = rm.get_components_states(); + EXPECT_EQ( + states_map["GenericSystem2dof"].label(), hardware_interface::lifecycle_state_names::INACTIVE); } TEST_F(TestGenericSystem, generic_system_2dof_functionality) @@ -925,9 +929,9 @@ TEST_F(TestGenericSystem, generic_system_2dof_functionality_with_offset_custom_i hardware_interface::ResourceManager rm(urdf); // check is hardware is configured - std::unordered_map status_map; - status_map = rm.get_components_status(); - EXPECT_EQ(status_map["GenericSystem2dof"], hardware_interface::status::CONFIGURED); + auto states_map = rm.get_components_states(); + EXPECT_EQ( + states_map["GenericSystem2dof"].label(), hardware_interface::lifecycle_state_names::INACTIVE); // Check initial values hardware_interface::LoanedStateInterface j1p_s = rm.claim_state_interface("joint1/position"); @@ -1014,10 +1018,12 @@ TEST_F(TestGenericSystem, generic_system_2dof_functionality_with_offset_custom_i ASSERT_EQ(0.88, j2v_c.get_value()); rm.start_components(); - status_map = rm.get_components_status(); - EXPECT_EQ(status_map["GenericSystem2dof"], hardware_interface::status::STARTED); + states_map = rm.get_components_states(); + EXPECT_EQ( + states_map["GenericSystem2dof"].label(), hardware_interface::lifecycle_state_names::ACTIVE); rm.stop_components(); - status_map = rm.get_components_status(); - EXPECT_EQ(status_map["GenericSystem2dof"], hardware_interface::status::STOPPED); + states_map = rm.get_components_states(); + EXPECT_EQ( + states_map["GenericSystem2dof"].label(), hardware_interface::lifecycle_state_names::INACTIVE); } diff --git a/hardware_interface/test/test_component_interfaces.cpp b/hardware_interface/test/test_component_interfaces.cpp index 6807337ec0..f0c1df1065 100644 --- a/hardware_interface/test/test_component_interfaces.cpp +++ b/hardware_interface/test/test_component_interfaces.cpp @@ -15,6 +15,7 @@ #include #include +#include #include #include #include @@ -28,20 +29,23 @@ #include "hardware_interface/system.hpp" #include "hardware_interface/system_interface.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" +#include "hardware_interface/types/lifecycle_state_names.hpp" +#include "lifecycle_msgs/msg/state.hpp" +#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" using namespace ::testing; // NOLINT namespace test_components { +using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; + class DummyActuator : public hardware_interface::ActuatorInterface { - hardware_interface::return_type configure( - const hardware_interface::HardwareInfo & /* info */) override + CallbackReturn on_init(const hardware_interface::HardwareInfo & /* info */) override { // We hardcode the info - return hardware_interface::return_type::OK; + return CallbackReturn::SUCCESS; } std::vector export_state_interfaces() override @@ -56,6 +60,14 @@ class DummyActuator : public hardware_interface::ActuatorInterface return state_interfaces; } + CallbackReturn on_configure(const rclcpp_lifecycle::State & /*previous_state*/) override + { + position_state_ = 0.0; + velocity_state_ = 0.0; + + return CallbackReturn::SUCCESS; + } + std::vector export_command_interfaces() override { // We can command in velocity @@ -66,17 +78,8 @@ class DummyActuator : public hardware_interface::ActuatorInterface return command_interfaces; } - hardware_interface::return_type start() override { return hardware_interface::return_type::OK; } - - hardware_interface::return_type stop() override { return hardware_interface::return_type::OK; } - std::string get_name() const override { return "DummyActuator"; } - hardware_interface::status get_status() const override - { - return hardware_interface::status::UNKNOWN; - } - hardware_interface::return_type read() override { // no-op, state is getting propagated within write. @@ -91,19 +94,30 @@ class DummyActuator : public hardware_interface::ActuatorInterface return hardware_interface::return_type::OK; } + CallbackReturn on_shutdown(const rclcpp_lifecycle::State & /*previous_state*/) override + { + velocity_state_ = 0; + return CallbackReturn::SUCCESS; + } + private: - double position_state_ = 0.0; - double velocity_state_ = 0.0; + double position_state_ = std::numeric_limits::quiet_NaN(); + double velocity_state_ = std::numeric_limits::quiet_NaN(); double velocity_command_ = 0.0; }; class DummySensor : public hardware_interface::SensorInterface { - hardware_interface::return_type configure( - const hardware_interface::HardwareInfo & /* info */) override + CallbackReturn on_init(const hardware_interface::HardwareInfo & /* info */) override { // We hardcode the info - return hardware_interface::return_type::OK; + return CallbackReturn::SUCCESS; + } + + CallbackReturn on_configure(const rclcpp_lifecycle::State & /*previous_state*/) override + { + voltage_level_ = 0.0; + return CallbackReturn::SUCCESS; } std::vector export_state_interfaces() override @@ -116,34 +130,36 @@ class DummySensor : public hardware_interface::SensorInterface return state_interfaces; } - hardware_interface::return_type start() override { return hardware_interface::return_type::OK; } - - hardware_interface::return_type stop() override { return hardware_interface::return_type::OK; } - std::string get_name() const override { return "DummySensor"; } - hardware_interface::status get_status() const override - { - return hardware_interface::status::UNKNOWN; - } - hardware_interface::return_type read() override { // no-op, static value + voltage_level_ = voltage_level_hw_value_; return hardware_interface::return_type::OK; } private: - double voltage_level_ = 0x666; + double voltage_level_ = std::numeric_limits::quiet_NaN(); + double voltage_level_hw_value_ = 0x666; }; class DummySystem : public hardware_interface::SystemInterface { - hardware_interface::return_type configure( - const hardware_interface::HardwareInfo & /* info */) override + CallbackReturn on_init(const hardware_interface::HardwareInfo & /* info */) override { // We hardcode the info - return hardware_interface::return_type::OK; + return CallbackReturn::SUCCESS; + } + + CallbackReturn on_configure(const rclcpp_lifecycle::State & /*previous_state*/) override + { + for (auto i = 0ul; i < 3; ++i) + { + position_state_[i] = 0.0; + velocity_state_[i] = 0.0; + } + return CallbackReturn::SUCCESS; } std::vector export_state_interfaces() override @@ -180,17 +196,8 @@ class DummySystem : public hardware_interface::SystemInterface return command_interfaces; } - hardware_interface::return_type start() override { return hardware_interface::return_type::OK; } - - hardware_interface::return_type stop() override { return hardware_interface::return_type::OK; } - std::string get_name() const override { return "DummySystem"; } - hardware_interface::status get_status() const override - { - return hardware_interface::status::UNKNOWN; - } - hardware_interface::return_type read() override { // no-op, state is getting propagated within write. @@ -207,20 +214,32 @@ class DummySystem : public hardware_interface::SystemInterface return hardware_interface::return_type::OK; } + CallbackReturn on_shutdown(const rclcpp_lifecycle::State & /*previous_state*/) override + { + for (auto i = 0ul; i < 3; ++i) + { + velocity_state_[i] = 0.0; + } + return CallbackReturn::SUCCESS; + } + private: - std::array position_state_ = {0.0, 0.0, 0.0}; - std::array velocity_state_ = {0.0, 0.0, 0.0}; + std::array position_state_ = { + std::numeric_limits::quiet_NaN(), std::numeric_limits::quiet_NaN(), + std::numeric_limits::quiet_NaN()}; + std::array velocity_state_ = { + std::numeric_limits::quiet_NaN(), std::numeric_limits::quiet_NaN(), + std::numeric_limits::quiet_NaN()}; std::array velocity_command_ = {0.0, 0.0, 0.0}; }; class DummySystemPreparePerform : public hardware_interface::SystemInterface { // Override the pure virtual functions with default behavior - hardware_interface::return_type configure( - const hardware_interface::HardwareInfo & /* info */) override + CallbackReturn on_init(const hardware_interface::HardwareInfo & /* info */) override { // We hardcode the info - return hardware_interface::return_type::OK; + return CallbackReturn::SUCCESS; } std::vector export_state_interfaces() override { return {}; } @@ -230,17 +249,8 @@ class DummySystemPreparePerform : public hardware_interface::SystemInterface return {}; } - hardware_interface::return_type start() override { return hardware_interface::return_type::OK; } - - hardware_interface::return_type stop() override { return hardware_interface::return_type::OK; } - std::string get_name() const override { return "DummySystemPreparePerform"; } - hardware_interface::status get_status() const override - { - return hardware_interface::status::UNKNOWN; - } - hardware_interface::return_type read() override { return hardware_interface::return_type::OK; } hardware_interface::return_type write() override { return hardware_interface::return_type::OK; } @@ -286,7 +296,9 @@ TEST(TestComponentInterfaces, dummy_actuator) hardware_interface::Actuator actuator_hw(std::make_unique()); hardware_interface::HardwareInfo mock_hw_info{}; - EXPECT_EQ(hardware_interface::return_type::OK, actuator_hw.configure(mock_hw_info)); + auto state = actuator_hw.initialize(mock_hw_info); + 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.export_state_interfaces(); ASSERT_EQ(2u, state_interfaces.size()); @@ -300,19 +312,66 @@ TEST(TestComponentInterfaces, dummy_actuator) EXPECT_EQ("joint1", command_interfaces[0].get_name()); EXPECT_EQ(hardware_interface::HW_IF_VELOCITY, command_interfaces[0].get_interface_name()); - command_interfaces[0].set_value(1.0); // velocity - ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.write()); + double velocity_value = 1.0; + command_interfaces[0].set_value(velocity_value); // velocity + ASSERT_EQ(hardware_interface::return_type::ERROR, actuator_hw.write()); + + // Noting should change because it is UNCONFIGURED + for (auto step = 0u; step < 10; ++step) + { + ASSERT_EQ(hardware_interface::return_type::ERROR, actuator_hw.read()); + + 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()); + } + + 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()); + + EXPECT_EQ(step * velocity_value, state_interfaces[0].get_value()); // position value + EXPECT_EQ(step ? velocity_value : 0, state_interfaces[1].get_value()); // velocity - for (auto step = 1u; step <= 10; ++step) + ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.write()); + } + + 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()); - ASSERT_EQ(step, state_interfaces[0].get_value()); // position value - ASSERT_EQ(1u, state_interfaces[1].get_value()); // velocity + 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()); } + 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()); + + 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()); + } + EXPECT_EQ( hardware_interface::return_type::OK, actuator_hw.prepare_command_mode_switch({""}, {""})); EXPECT_EQ( @@ -324,12 +383,28 @@ TEST(TestComponentInterfaces, dummy_sensor) hardware_interface::Sensor sensor_hw(std::make_unique()); hardware_interface::HardwareInfo mock_hw_info{}; - EXPECT_EQ(hardware_interface::return_type::OK, sensor_hw.configure(mock_hw_info)); + auto state = sensor_hw.initialize(mock_hw_info); + 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.export_state_interfaces(); ASSERT_EQ(1u, state_interfaces.size()); EXPECT_EQ("joint1", state_interfaces[0].get_name()); EXPECT_EQ("voltage", state_interfaces[0].get_interface_name()); + EXPECT_TRUE(std::isnan(state_interfaces[0].get_value())); + + // Not updated because is is UNCONFIGURED + sensor_hw.read(); + 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(); EXPECT_EQ(0x666, state_interfaces[0].get_value()); } @@ -338,7 +413,9 @@ TEST(TestComponentInterfaces, dummy_system) hardware_interface::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 = system_hw.initialize(mock_hw_info); + EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); + EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); auto state_interfaces = system_hw.export_state_interfaces(); ASSERT_EQ(6u, state_interfaces.size()); @@ -364,25 +441,84 @@ TEST(TestComponentInterfaces, dummy_system) EXPECT_EQ("joint3", command_interfaces[2].get_name()); EXPECT_EQ(hardware_interface::HW_IF_VELOCITY, command_interfaces[2].get_interface_name()); - command_interfaces[0].set_value(1.0); // velocity - command_interfaces[1].set_value(1.0); // velocity - command_interfaces[2].set_value(1.0); // velocity - ASSERT_EQ(hardware_interface::return_type::OK, system_hw.write()); + double velocity_value = 1.0; + command_interfaces[0].set_value(velocity_value); // velocity + command_interfaces[1].set_value(velocity_value); // velocity + command_interfaces[2].set_value(velocity_value); // velocity + ASSERT_EQ(hardware_interface::return_type::ERROR, system_hw.write()); + + // Noting should change because it is UNCONFIGURED + for (auto step = 0u; step < 10; ++step) + { + ASSERT_EQ(hardware_interface::return_type::ERROR, system_hw.read()); + + ASSERT_TRUE(std::isnan(state_interfaces[0].get_value())); // position value + ASSERT_TRUE(std::isnan(state_interfaces[1].get_value())); // velocity + ASSERT_TRUE(std::isnan(state_interfaces[2].get_value())); // position value + ASSERT_TRUE(std::isnan(state_interfaces[3].get_value())); // velocity + ASSERT_TRUE(std::isnan(state_interfaces[4].get_value())); // position value + ASSERT_TRUE(std::isnan(state_interfaces[5].get_value())); // velocity + + ASSERT_EQ(hardware_interface::return_type::ERROR, system_hw.write()); + } + + state = system_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, system_hw.read()); + + EXPECT_EQ(step * velocity_value, state_interfaces[0].get_value()); // position value + EXPECT_EQ(step ? velocity_value : 0, state_interfaces[1].get_value()); // velocity + EXPECT_EQ(step * velocity_value, state_interfaces[2].get_value()); // position value + EXPECT_EQ(step ? velocity_value : 0, state_interfaces[3].get_value()); // velocity + EXPECT_EQ(step * velocity_value, state_interfaces[4].get_value()); // position value + EXPECT_EQ(step ? velocity_value : 0, state_interfaces[5].get_value()); // velocity + + ASSERT_EQ(hardware_interface::return_type::OK, system_hw.write()); + } + + state = system_hw.activate(); + EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, state.id()); + EXPECT_EQ(hardware_interface::lifecycle_state_names::ACTIVE, state.label()); - for (auto step = 1u; step <= 10; ++step) + // Read and Write are working because it is ACTIVE + for (auto step = 0u; step < 10; ++step) { ASSERT_EQ(hardware_interface::return_type::OK, system_hw.read()); - ASSERT_EQ(step, state_interfaces[0].get_value()); // position value - ASSERT_EQ(1u, state_interfaces[1].get_value()); // velocity - ASSERT_EQ(step, state_interfaces[2].get_value()); // position value - ASSERT_EQ(1u, state_interfaces[3].get_value()); // velocity - ASSERT_EQ(step, state_interfaces[4].get_value()); // position value - ASSERT_EQ(1u, state_interfaces[5].get_value()); // velocity + EXPECT_EQ((10 + step) * velocity_value, state_interfaces[0].get_value()); // position value + EXPECT_EQ(velocity_value, state_interfaces[1].get_value()); // velocity + EXPECT_EQ((10 + step) * velocity_value, state_interfaces[2].get_value()); // position value + EXPECT_EQ(velocity_value, state_interfaces[3].get_value()); // velocity + EXPECT_EQ((10 + step) * velocity_value, state_interfaces[4].get_value()); // position value + EXPECT_EQ(velocity_value, state_interfaces[5].get_value()); // velocity ASSERT_EQ(hardware_interface::return_type::OK, system_hw.write()); } + state = system_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, system_hw.read()); + + EXPECT_EQ(20 * velocity_value, state_interfaces[0].get_value()); // position value + EXPECT_EQ(0.0, state_interfaces[1].get_value()); // velocity + EXPECT_EQ(20 * velocity_value, state_interfaces[2].get_value()); // position value + EXPECT_EQ(0.0, state_interfaces[3].get_value()); // velocity + EXPECT_EQ(20 * velocity_value, state_interfaces[4].get_value()); // position value + EXPECT_EQ(0.0, state_interfaces[5].get_value()); // velocity + + ASSERT_EQ(hardware_interface::return_type::ERROR, system_hw.write()); + } + EXPECT_EQ(hardware_interface::return_type::OK, system_hw.prepare_command_mode_switch({}, {})); EXPECT_EQ(hardware_interface::return_type::OK, system_hw.perform_command_mode_switch({}, {})); } @@ -392,7 +528,9 @@ TEST(TestComponentInterfaces, dummy_command_mode_system) hardware_interface::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 = system_hw.initialize(mock_hw_info); + EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); + EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); std::vector one_key = {"joint1/position"}; std::vector two_keys = {"joint1/position", "joint1/velocity"}; diff --git a/hardware_interface/test/test_components/test_actuator.cpp b/hardware_interface/test/test_components/test_actuator.cpp index 745555c7d9..b33c7123b7 100644 --- a/hardware_interface/test/test_components/test_actuator.cpp +++ b/hardware_interface/test/test_components/test_actuator.cpp @@ -16,36 +16,33 @@ #include #include "hardware_interface/actuator_interface.hpp" -#include "hardware_interface/base_interface.hpp" using hardware_interface::ActuatorInterface; -using hardware_interface::BaseInterface; using hardware_interface::CommandInterface; using hardware_interface::return_type; using hardware_interface::StateInterface; -using hardware_interface::status; -class TestActuator : public BaseInterface +class TestActuator : public ActuatorInterface { - return_type configure(const hardware_interface::HardwareInfo & info) override + CallbackReturn on_init(const hardware_interface::HardwareInfo & info) override { - if (configure_default(info) != return_type::OK) + if (ActuatorInterface::on_init(info) != CallbackReturn::SUCCESS) { - return return_type::ERROR; + return CallbackReturn::ERROR; } /* * a hardware can optional prove for incorrect info here. * * // can only control one joint - * if (info_.joints.size() != 1) {return return_type::ERROR;} + * if (info_.joints.size() != 1) {return CallbackReturn::ERROR;} * // can only control in position - * if (info_.joints[0].command_interfaces.size() != 1) {return return_type::ERROR;} + * if (info_.joints[0].command_interfaces.size() != 1) {return CallbackReturn::ERROR;} * // can only give feedback state for position and velocity - * if (info_.joints[0].state_interfaces.size() != 2) {return return_type::ERROR;} + * if (info_.joints[0].state_interfaces.size() != 2) {return CallbackReturn::ERROR;} */ - return return_type::OK; + return CallbackReturn::SUCCESS; } std::vector export_state_interfaces() override @@ -70,18 +67,6 @@ class TestActuator : public BaseInterface return command_interfaces; } - return_type start() override - { - status_ = status::STARTED; - return return_type::OK; - } - - return_type stop() override - { - status_ = status::STOPPED; - return return_type::OK; - } - return_type read() override { return return_type::OK; } return_type write() override { return return_type::OK; } diff --git a/hardware_interface/test/test_components/test_sensor.cpp b/hardware_interface/test/test_components/test_sensor.cpp index e904681525..600bef6474 100644 --- a/hardware_interface/test/test_components/test_sensor.cpp +++ b/hardware_interface/test/test_components/test_sensor.cpp @@ -15,29 +15,26 @@ #include #include -#include "hardware_interface/base_interface.hpp" #include "hardware_interface/sensor_interface.hpp" -using hardware_interface::BaseInterface; using hardware_interface::return_type; using hardware_interface::SensorInterface; using hardware_interface::StateInterface; -using hardware_interface::status; -class TestSensor : public BaseInterface +class TestSensor : public SensorInterface { - return_type configure(const hardware_interface::HardwareInfo & info) override + CallbackReturn on_init(const hardware_interface::HardwareInfo & info) override { - if (configure_default(info) != return_type::OK) + if (SensorInterface::on_init(info) != CallbackReturn::SUCCESS) { - return return_type::ERROR; + return CallbackReturn::ERROR; } // can only give feedback state for velocity if (info_.sensors[0].state_interfaces.size() != 1) { - return return_type::ERROR; + return CallbackReturn::ERROR; } - return return_type::OK; + return CallbackReturn::SUCCESS; } std::vector export_state_interfaces() override @@ -49,18 +46,6 @@ class TestSensor : public BaseInterface return state_interfaces; } - return_type start() override - { - status_ = status::STARTED; - return return_type::OK; - } - - return_type stop() override - { - status_ = status::STOPPED; - return return_type::OK; - } - return_type read() override { return return_type::OK; } private: diff --git a/hardware_interface/test/test_components/test_system.cpp b/hardware_interface/test/test_components/test_system.cpp index 9b25c16480..89f48a1a20 100644 --- a/hardware_interface/test/test_components/test_system.cpp +++ b/hardware_interface/test/test_components/test_system.cpp @@ -16,18 +16,15 @@ #include #include -#include "hardware_interface/base_interface.hpp" #include "hardware_interface/system_interface.hpp" #include "hardware_interface/types/hardware_interface_type_values.hpp" -using hardware_interface::BaseInterface; using hardware_interface::CommandInterface; using hardware_interface::return_type; using hardware_interface::StateInterface; -using hardware_interface::status; using hardware_interface::SystemInterface; -class TestSystem : public BaseInterface +class TestSystem : public SystemInterface { std::vector export_state_interfaces() override { @@ -57,18 +54,6 @@ class TestSystem : public BaseInterface return command_interfaces; } - return_type start() override - { - status_ = status::STARTED; - return return_type::OK; - } - - return_type stop() override - { - status_ = status::STOPPED; - return return_type::OK; - } - return_type read() override { return return_type::OK; } return_type write() override { return return_type::OK; } diff --git a/hardware_interface/test/test_hardware_components/test_force_torque_sensor.cpp b/hardware_interface/test/test_hardware_components/test_force_torque_sensor.cpp index 1f56614b73..63ecca8b5f 100644 --- a/hardware_interface/test/test_hardware_components/test_force_torque_sensor.cpp +++ b/hardware_interface/test/test_hardware_components/test_force_torque_sensor.cpp @@ -17,30 +17,27 @@ #include #include -#include "hardware_interface/base_interface.hpp" #include "hardware_interface/sensor_interface.hpp" -using hardware_interface::BaseInterface; using hardware_interface::return_type; using hardware_interface::SensorInterface; using hardware_interface::StateInterface; -using hardware_interface::status; namespace test_hardware_components { -class TestForceTorqueSensor : public BaseInterface +class TestForceTorqueSensor : public SensorInterface { - return_type configure(const hardware_interface::HardwareInfo & sensor_info) override + CallbackReturn on_init(const hardware_interface::HardwareInfo & sensor_info) override { - if (configure_default(sensor_info) != return_type::OK) + if (SensorInterface::on_init(sensor_info) != CallbackReturn::SUCCESS) { - return return_type::ERROR; + return CallbackReturn::ERROR; } const auto & state_interfaces = info_.sensors[0].state_interfaces; if (state_interfaces.size() != 6) { - return return_type::ERROR; + return CallbackReturn::ERROR; } for (const auto & ft_key : {"fx", "fy", "fz", "tx", "ty", "tz"}) { @@ -50,12 +47,12 @@ class TestForceTorqueSensor : public BaseInterface return interface_info.name == ft_key; }) == state_interfaces.end()) { - return return_type::ERROR; + return CallbackReturn::ERROR; } } fprintf(stderr, "TestForceTorqueSensor configured successfully.\n"); - return return_type::OK; + return CallbackReturn::SUCCESS; } std::vector export_state_interfaces() override @@ -79,10 +76,6 @@ class TestForceTorqueSensor : public BaseInterface return state_interfaces; } - return_type start() override { return return_type::OK; } - - return_type stop() override { return return_type::OK; } - return_type read() override { values_.fx = fmod((values_.fx + 1.0), 10); diff --git a/hardware_interface/test/test_hardware_components/test_single_joint_actuator.cpp b/hardware_interface/test/test_hardware_components/test_single_joint_actuator.cpp index 752a23e150..2212c6832b 100644 --- a/hardware_interface/test/test_hardware_components/test_single_joint_actuator.cpp +++ b/hardware_interface/test/test_hardware_components/test_single_joint_actuator.cpp @@ -16,47 +16,44 @@ #include #include "hardware_interface/actuator_interface.hpp" -#include "hardware_interface/base_interface.hpp" #include "hardware_interface/types/hardware_interface_type_values.hpp" using hardware_interface::ActuatorInterface; -using hardware_interface::BaseInterface; using hardware_interface::CommandInterface; using hardware_interface::return_type; using hardware_interface::StateInterface; -using hardware_interface::status; namespace test_hardware_components { -class TestSingleJointActuator : public BaseInterface +class TestSingleJointActuator : public ActuatorInterface { - return_type configure(const hardware_interface::HardwareInfo & actuator_info) override + CallbackReturn on_init(const hardware_interface::HardwareInfo & actuator_info) override { - if (configure_default(actuator_info) != return_type::OK) + if (ActuatorInterface::on_init(actuator_info) != CallbackReturn::SUCCESS) { - return return_type::ERROR; + return CallbackReturn::ERROR; } // can only control one joint if (info_.joints.size() != 1) { - return return_type::ERROR; + return CallbackReturn::ERROR; } // can only control in position const auto & command_interfaces = info_.joints[0].command_interfaces; if (command_interfaces.size() != 1) { - return return_type::ERROR; + return CallbackReturn::ERROR; } if (command_interfaces[0].name != hardware_interface::HW_IF_POSITION) { - return return_type::ERROR; + return CallbackReturn::ERROR; } // can only give feedback state for position and velocity const auto & state_interfaces = info_.joints[0].state_interfaces; if (state_interfaces.size() < 1) { - return return_type::ERROR; + return CallbackReturn::ERROR; } for (const auto & state_interface : state_interfaces) { @@ -64,11 +61,11 @@ class TestSingleJointActuator : public BaseInterface (state_interface.name != hardware_interface::HW_IF_POSITION) && (state_interface.name != hardware_interface::HW_IF_VELOCITY)) { - return return_type::ERROR; + return CallbackReturn::ERROR; } } fprintf(stderr, "TestSingleJointActuator configured successfully.\n"); - return return_type::OK; + return CallbackReturn::SUCCESS; } std::vector export_state_interfaces() override @@ -95,10 +92,6 @@ class TestSingleJointActuator : public BaseInterface return command_interfaces; } - return_type start() override { return return_type::OK; } - - return_type stop() override { return return_type::OK; } - return_type read() override { return return_type::OK; } return_type write() override diff --git a/hardware_interface/test/test_hardware_components/test_system_with_command_modes.cpp b/hardware_interface/test/test_hardware_components/test_system_with_command_modes.cpp index 881385503c..436f50b82c 100644 --- a/hardware_interface/test/test_hardware_components/test_system_with_command_modes.cpp +++ b/hardware_interface/test/test_hardware_components/test_system_with_command_modes.cpp @@ -16,28 +16,25 @@ #include #include -#include "hardware_interface/base_interface.hpp" #include "hardware_interface/system_interface.hpp" #include "hardware_interface/types/hardware_interface_type_values.hpp" namespace test_hardware_components { -class TestSystemCommandModes -: public hardware_interface::BaseInterface +class TestSystemCommandModes : public hardware_interface::SystemInterface { public: - hardware_interface::return_type configure( - const hardware_interface::HardwareInfo & system_info) override + CallbackReturn on_init(const hardware_interface::HardwareInfo & system_info) override { - if (configure_default(system_info) != hardware_interface::return_type::OK) + if (hardware_interface::SystemInterface::on_init(system_info) != CallbackReturn::SUCCESS) { - return hardware_interface::return_type::ERROR; + return CallbackReturn::ERROR; } // Can only control two joints if (info_.joints.size() != 2) { - return hardware_interface::return_type::ERROR; + return CallbackReturn::ERROR; } for (const hardware_interface::ComponentInfo & joint : info_.joints) { @@ -45,7 +42,7 @@ class TestSystemCommandModes const auto & command_interfaces = joint.command_interfaces; if (command_interfaces.size() != 2) { - return hardware_interface::return_type::ERROR; + return CallbackReturn::ERROR; } for (const auto & command_interface : command_interfaces) { @@ -53,14 +50,14 @@ class TestSystemCommandModes command_interface.name != hardware_interface::HW_IF_POSITION && command_interface.name != hardware_interface::HW_IF_VELOCITY) { - return hardware_interface::return_type::ERROR; + return CallbackReturn::ERROR; } } // Can give feedback state for position, velocity, and acceleration const auto & state_interfaces = joint.state_interfaces; if (state_interfaces.size() != 2) { - return hardware_interface::return_type::ERROR; + return CallbackReturn::ERROR; } for (const auto & state_interface : state_interfaces) { @@ -68,13 +65,13 @@ class TestSystemCommandModes state_interface.name != hardware_interface::HW_IF_POSITION && state_interface.name != hardware_interface::HW_IF_VELOCITY) { - return hardware_interface::return_type::ERROR; + return CallbackReturn::ERROR; } } } fprintf(stderr, "TestSystemCommandModes configured successfully.\n"); - return hardware_interface::return_type::OK; + return CallbackReturn::SUCCESS; } std::vector export_state_interfaces() override @@ -107,18 +104,6 @@ class TestSystemCommandModes return command_interfaces; } - hardware_interface::return_type start() override - { - status_ = hardware_interface::status::STARTED; - return hardware_interface::return_type::OK; - } - - hardware_interface::return_type stop() override - { - status_ = hardware_interface::status::STOPPED; - return hardware_interface::return_type::OK; - } - hardware_interface::return_type read() override { return hardware_interface::return_type::OK; } hardware_interface::return_type write() override { return hardware_interface::return_type::OK; } diff --git a/hardware_interface/test/test_hardware_components/test_two_joint_system.cpp b/hardware_interface/test/test_hardware_components/test_two_joint_system.cpp index 16f9c67b37..d8fc77509f 100644 --- a/hardware_interface/test/test_hardware_components/test_two_joint_system.cpp +++ b/hardware_interface/test/test_hardware_components/test_two_joint_system.cpp @@ -16,32 +16,29 @@ #include #include -#include "hardware_interface/base_interface.hpp" #include "hardware_interface/system_interface.hpp" #include "hardware_interface/types/hardware_interface_type_values.hpp" -using hardware_interface::BaseInterface; using hardware_interface::CommandInterface; using hardware_interface::return_type; using hardware_interface::StateInterface; -using hardware_interface::status; using hardware_interface::SystemInterface; namespace test_hardware_components { -class TestTwoJointSystem : public BaseInterface +class TestTwoJointSystem : public SystemInterface { - return_type configure(const hardware_interface::HardwareInfo & system_info) override + CallbackReturn on_init(const hardware_interface::HardwareInfo & system_info) override { - if (configure_default(system_info) != return_type::OK) + if (SystemInterface::on_init(system_info) != CallbackReturn::SUCCESS) { - return return_type::ERROR; + return CallbackReturn::ERROR; } // can only control two joint if (info_.joints.size() != 2) { - return return_type::ERROR; + return CallbackReturn::ERROR; } for (const auto & joint : info_.joints) { @@ -49,26 +46,26 @@ class TestTwoJointSystem : public BaseInterface const auto & command_interfaces = joint.command_interfaces; if (command_interfaces.size() != 1) { - return return_type::ERROR; + return CallbackReturn::ERROR; } if (command_interfaces[0].name != hardware_interface::HW_IF_POSITION) { - return return_type::ERROR; + return CallbackReturn::ERROR; } // can only give feedback state for position and velocity const auto & state_interfaces = joint.state_interfaces; if (state_interfaces.size() != 1) { - return return_type::ERROR; + return CallbackReturn::ERROR; } if (state_interfaces[0].name != hardware_interface::HW_IF_POSITION) { - return return_type::ERROR; + return CallbackReturn::ERROR; } } fprintf(stderr, "TestTwoJointSystem configured successfully.\n"); - return return_type::OK; + return CallbackReturn::SUCCESS; } std::vector export_state_interfaces() override @@ -95,10 +92,6 @@ class TestTwoJointSystem : public BaseInterface return command_interfaces; } - return_type start() override { return return_type::OK; } - - return_type stop() override { return return_type::OK; } - return_type read() override { return return_type::OK; } return_type write() override { return return_type::OK; } diff --git a/hardware_interface/test/test_resource_manager.cpp b/hardware_interface/test/test_resource_manager.cpp index bff8eb7c80..ab2ad293b3 100644 --- a/hardware_interface/test/test_resource_manager.cpp +++ b/hardware_interface/test/test_resource_manager.cpp @@ -22,6 +22,8 @@ #include "hardware_interface/actuator_interface.hpp" #include "hardware_interface/resource_manager.hpp" +#include "hardware_interface/types/lifecycle_state_names.hpp" +#include "lifecycle_msgs/msg/state.hpp" #include "ros2_control_test_assets/descriptions.hpp" class TestResourceManager : public ::testing::Test @@ -114,31 +116,60 @@ TEST_F(TestResourceManager, resource_status) { hardware_interface::ResourceManager rm(ros2_control_test_assets::minimal_robot_urdf); - std::unordered_map status_map; - - status_map = rm.get_components_status(); - EXPECT_EQ(status_map["TestActuatorHardware"], hardware_interface::status::CONFIGURED); - EXPECT_EQ(status_map["TestSensorHardware"], hardware_interface::status::CONFIGURED); - EXPECT_EQ(status_map["TestSystemHardware"], hardware_interface::status::CONFIGURED); + std::unordered_map status_map; + + status_map = rm.get_components_states(); + EXPECT_EQ( + status_map["TestActuatorHardware"].id(), lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); + EXPECT_EQ( + status_map["TestActuatorHardware"].label(), + hardware_interface::lifecycle_state_names::INACTIVE); + EXPECT_EQ( + status_map["TestSensorHardware"].id(), lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); + EXPECT_EQ( + status_map["TestSensorHardware"].label(), hardware_interface::lifecycle_state_names::INACTIVE); + EXPECT_EQ( + status_map["TestSystemHardware"].id(), lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); + EXPECT_EQ( + status_map["TestSystemHardware"].label(), hardware_interface::lifecycle_state_names::INACTIVE); } TEST_F(TestResourceManager, starting_and_stopping_resources) { hardware_interface::ResourceManager rm(ros2_control_test_assets::minimal_robot_urdf); - std::unordered_map status_map; + std::unordered_map status_map; rm.start_components(); - status_map = rm.get_components_status(); - EXPECT_EQ(status_map["TestActuatorHardware"], hardware_interface::status::STARTED); - EXPECT_EQ(status_map["TestSensorHardware"], hardware_interface::status::STARTED); - EXPECT_EQ(status_map["TestSystemHardware"], hardware_interface::status::STARTED); + status_map = rm.get_components_states(); + EXPECT_EQ( + status_map["TestActuatorHardware"].id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); + EXPECT_EQ( + status_map["TestActuatorHardware"].label(), hardware_interface::lifecycle_state_names::ACTIVE); + EXPECT_EQ( + status_map["TestSensorHardware"].id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); + EXPECT_EQ( + status_map["TestSensorHardware"].label(), hardware_interface::lifecycle_state_names::ACTIVE); + EXPECT_EQ( + status_map["TestSystemHardware"].id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); + EXPECT_EQ( + status_map["TestSystemHardware"].label(), hardware_interface::lifecycle_state_names::ACTIVE); rm.stop_components(); - status_map = rm.get_components_status(); - EXPECT_EQ(status_map["TestActuatorHardware"], hardware_interface::status::STOPPED); - EXPECT_EQ(status_map["TestSensorHardware"], hardware_interface::status::STOPPED); - EXPECT_EQ(status_map["TestSystemHardware"], hardware_interface::status::STOPPED); + status_map = rm.get_components_states(); + EXPECT_EQ( + status_map["TestActuatorHardware"].id(), lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); + EXPECT_EQ( + status_map["TestActuatorHardware"].label(), + hardware_interface::lifecycle_state_names::INACTIVE); + EXPECT_EQ( + status_map["TestSensorHardware"].id(), lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); + EXPECT_EQ( + status_map["TestSensorHardware"].label(), hardware_interface::lifecycle_state_names::INACTIVE); + EXPECT_EQ( + status_map["TestSystemHardware"].id(), lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); + EXPECT_EQ( + status_map["TestSystemHardware"].label(), hardware_interface::lifecycle_state_names::INACTIVE); } TEST_F(TestResourceManager, resource_claiming) @@ -204,11 +235,6 @@ TEST_F(TestResourceManager, resource_claiming) class ExternalComponent : public hardware_interface::ActuatorInterface { - hardware_interface::return_type configure(const hardware_interface::HardwareInfo &) override - { - return hardware_interface::return_type::OK; - } - std::vector export_state_interfaces() override { std::vector state_interfaces; @@ -227,17 +253,8 @@ class ExternalComponent : public hardware_interface::ActuatorInterface return command_interfaces; } - hardware_interface::return_type start() override { return hardware_interface::return_type::OK; } - - hardware_interface::return_type stop() override { return hardware_interface::return_type::OK; } - std::string get_name() const override { return "ExternalComponent"; } - hardware_interface::status get_status() const override - { - return hardware_interface::status::UNKNOWN; - } - hardware_interface::return_type read() override { return hardware_interface::return_type::OK; } hardware_interface::return_type write() override { return hardware_interface::return_type::OK; }