diff --git a/hardware_interface/CMakeLists.txt b/hardware_interface/CMakeLists.txt index 04763b5e96..ac7cd5a5f9 100644 --- a/hardware_interface/CMakeLists.txt +++ b/hardware_interface/CMakeLists.txt @@ -18,26 +18,44 @@ find_package(rcpputils REQUIRED) add_library( hardware_interface SHARED + src/actuator_hardware.cpp src/joint_command_handle.cpp src/joint_state_handle.cpp src/operation_mode_handle.cpp src/robot_hardware.cpp + src/sensor_hardware.cpp + src/system_hardware.cpp ) target_include_directories( hardware_interface PUBLIC - include) + include +) ament_target_dependencies( hardware_interface control_msgs rclcpp rcpputils ) - # Causes the visibility macros to use dllexport rather than dllimport, # which is appropriate when building the dll but not consuming it. target_compile_definitions(hardware_interface PRIVATE "HARDWARE_INTERFACE_BUILDING_DLL") +add_library( + components + SHARED + src/components/joint.cpp + src/components/sensor.cpp +) +target_include_directories( + components + PUBLIC + include +) +# Causes the visibility macros to use dllexport rather than dllimport, +# which is appropriate when building the dll but not consuming it. +target_compile_definitions(components PRIVATE "HARDWARE_INTERFACE_BUILDING_DLL") + install( DIRECTORY include/ DESTINATION include @@ -45,6 +63,7 @@ install( install( TARGETS + components hardware_interface RUNTIME DESTINATION bin ARCHIVE DESTINATION lib @@ -74,13 +93,23 @@ if(BUILD_TESTING) target_link_libraries(test_actuator_handle hardware_interface) ament_target_dependencies(test_actuator_handle rcpputils) + ament_add_gmock(test_component_interfaces test/test_component_interfaces.cpp) + target_include_directories(test_component_interfaces PRIVATE include) + target_link_libraries(test_component_interfaces components hardware_interface) endif() ament_export_include_directories( include ) ament_export_libraries( - hardware_interface) + components + hardware_interface +) ament_export_dependencies( - control_msgs) + control_msgs + rclcpp + rcpputils + tinyxml2_vendor + TinyXML2 +) ament_package() diff --git a/hardware_interface/include/hardware_interface/actuator_hardware.hpp b/hardware_interface/include/hardware_interface/actuator_hardware.hpp new file mode 100644 index 0000000000..1bcfbafe60 --- /dev/null +++ b/hardware_interface/include/hardware_interface/actuator_hardware.hpp @@ -0,0 +1,60 @@ +// 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__ACTUATOR_HARDWARE_HPP_ +#define HARDWARE_INTERFACE__ACTUATOR_HARDWARE_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" +#include "hardware_interface/visibility_control.h" + +namespace hardware_interface +{ + +namespace components +{ +class Joint; +} // namespace components +class ActuatorHardwareInterface; + +class ActuatorHardware final +{ +public: + ActuatorHardware() = default; + + explicit ActuatorHardware(std::unique_ptr impl); + + ~ActuatorHardware() = default; + + return_type configure(const HardwareInfo & actuator_info); + + return_type start(); + + return_type stop(); + + hardware_interface_status get_status() const; + + return_type read_joint(std::shared_ptr joint); + + return_type write_joint(const std::shared_ptr joint); + +private: + std::unique_ptr impl_; +}; + +} // namespace hardware_interface +#endif // HARDWARE_INTERFACE__ACTUATOR_HARDWARE_HPP_ diff --git a/hardware_interface/include/hardware_interface/actuator_hardware_interface.hpp b/hardware_interface/include/hardware_interface/actuator_hardware_interface.hpp new file mode 100644 index 0000000000..cf7ae22ee5 --- /dev/null +++ b/hardware_interface/include/hardware_interface/actuator_hardware_interface.hpp @@ -0,0 +1,105 @@ +// 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__ACTUATOR_HARDWARE_INTERFACE_HPP_ +#define HARDWARE_INTERFACE__ACTUATOR_HARDWARE_INTERFACE_HPP_ + +#include + +#include "hardware_interface/components/joint.hpp" +#include "hardware_interface/hardware_info.hpp" +#include "hardware_interface/types/hardware_interface_return_values.hpp" +#include "hardware_interface/types/hardware_interface_status_values.hpp" +#include "hardware_interface/visibility_control.h" + +namespace hardware_interface +{ + +/** + * \brief Virtual Class to implement when integrating a 1 DoF actuator into ros2_control. + * The typical examples are conveyors or motors. + */ +class ActuatorHardwareInterface +{ +public: + HARDWARE_INTERFACE_PUBLIC + ActuatorHardwareInterface() = default; + + HARDWARE_INTERFACE_PUBLIC + virtual + ~ActuatorHardwareInterface() = default; + + /** + * \brief Configuration of the actuator from data parsed from the robot's URDF. + * + * \param actuator_info structure with data from URDF. + * \return return_type::OK if required data are provided and can be parsed, + * return_type::ERROR otherwise. + */ + HARDWARE_INTERFACE_PUBLIC + virtual + return_type configure(const HardwareInfo & actuator_info) = 0; + + /** + * \brief Start exchange data with the hardware. + * + * \return return_type:OK if everything worked as expected, return_type::ERROR otherwise. + */ + HARDWARE_INTERFACE_PUBLIC + virtual + return_type start() = 0; + + /** + * \brief Stop exchange data with the hardware. + * + * \return return_type:OK if everything worked as expected, return_type::ERROR otherwise. + */ + HARDWARE_INTERFACE_PUBLIC + virtual + return_type stop() = 0; + + /** + * \brief Get current state of the system hardware. + * + * \return hardware_interface_status current status. + */ + HARDWARE_INTERFACE_PUBLIC + virtual + hardware_interface_status get_status() const = 0; + + /** + * \brief Read data fromt the hardware into the joint using "set_state" function of the Joint class. + * This function is always called by the resource manager. + * + * \param joint joint where data from the hardware are stored. + * \return return_type:OK if everything worked as expected, return_type::ERROR otherwise. + */ + HARDWARE_INTERFACE_PUBLIC + virtual + return_type read_joint(std::shared_ptr joint) const = 0; + + /** + * \brief Write data from the joint to the hardware using "get_command" function of the Joint class. + * This function is always called by the resource manager. + * + * \param joint the joint from which data are written to the hardware. + * \return return_type:OK if everything worked as expected, return_type::ERROR otherwise. + */ + HARDWARE_INTERFACE_PUBLIC + virtual + return_type write_joint(const std::shared_ptr joint) = 0; +}; + +} // namespace hardware_interface +#endif // HARDWARE_INTERFACE__ACTUATOR_HARDWARE_INTERFACE_HPP_ diff --git a/hardware_interface/include/hardware_interface/components/component_info.hpp b/hardware_interface/include/hardware_interface/components/component_info.hpp new file mode 100644 index 0000000000..0dc1b78dd2 --- /dev/null +++ b/hardware_interface/include/hardware_interface/components/component_info.hpp @@ -0,0 +1,63 @@ +// 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__COMPONENTS__COMPONENT_INFO_HPP_ +#define HARDWARE_INTERFACE__COMPONENTS__COMPONENT_INFO_HPP_ + +#include +#include +#include + +namespace hardware_interface +{ +namespace components +{ + +/** + * \brief This structure stores information about components defined for a specific hardware + * in robot's URDF. + */ +struct ComponentInfo +{ + /** + * \brief name of the component. + */ + std::string name; + /** + * \brief type of the component: sensor or actuator. + */ + std::string type; + /** + * \brief component's class, which will be dynamically loaded. + */ + std::string class_type; + /** + * \brief name of the command interfaces that can be set, e.g. "position", "velocity", etc. + * Used by joints. + */ + std::vector command_interfaces; + /** + * \brief name of the state interfaces that can be read, e.g. "position", "velocity", etc. + * Used by Joints and Sensors. + */ + std::vector state_interfaces; + /** + * \brief (optional) key-value pairs of components parameters. + */ + std::unordered_map parameters; +}; + +} // namespace components +} // namespace hardware_interface +#endif // HARDWARE_INTERFACE__COMPONENTS__COMPONENT_INFO_HPP_ diff --git a/hardware_interface/include/hardware_interface/components/joint.hpp b/hardware_interface/include/hardware_interface/components/joint.hpp new file mode 100644 index 0000000000..64ede0a2bc --- /dev/null +++ b/hardware_interface/include/hardware_interface/components/joint.hpp @@ -0,0 +1,198 @@ +// 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__COMPONENTS__JOINT_HPP_ +#define HARDWARE_INTERFACE__COMPONENTS__JOINT_HPP_ + +#include +#include + +#include "hardware_interface/components/component_info.hpp" +#include "hardware_interface/types/hardware_interface_return_values.hpp" +#include "hardware_interface/visibility_control.h" + +namespace hardware_interface +{ +namespace components +{ + +/** + * \brief Base Class for the "Joint" component used as a basic building block for a robot. + * A joint is always 1-DoF and can have one or more interfaces (e.g., position, velocity, etc.) + * A joint has to be able to receive command(s) and optionally can provide its state(s). + */ +class Joint +{ +public: + HARDWARE_INTERFACE_PUBLIC + Joint() = default; + + HARDWARE_INTERFACE_PUBLIC + virtual + ~Joint() = default; + + /** + * \brief Configure base joint class based on the description in the robot's URDF file. + * + * \param joint_info structure with data from URDF. + * \return return_type::OK if required data are provided and is successfully parsed, + * return_type::ERROR otherwise. + */ + HARDWARE_INTERFACE_PUBLIC + return_type configure(const ComponentInfo & joint_info); + + /** + * \brief Provide the list of command interfaces configured for the joint. + * + * \return string list with command interfaces. + */ + HARDWARE_INTERFACE_PUBLIC + std::vector get_command_interfaces() const; + + /** + * \brief Provide the list of state interfaces configured for the joint. + * + * \return string list with state interfaces. + */ + HARDWARE_INTERFACE_PUBLIC + std::vector get_state_interfaces() const; + + /** + * \brief Get command list from the joint. This function is used in the write function of the + * actuator or system hardware. The parameters command and interfaces have the same order, + * and number of elements. Using the interfaces list, the hardware can choose which values to + * provide. + * + * \param command list of doubles with commands for the hardware. + * \param interfaces list of interfaces on which commands have to set. + * \return return_type::INTERFACE_VALUE_SIZE_NOT_EQUAL if command and interfaces arguments do not + * have the same length; return_type::INTERFACE_NOT_FOUND if one of provided interfaces is not + * defined for the joint; return return_type::INTERFACE_NOT_PROVIDED if the list of interfaces + * is empty; return_type::OK otherwise. + */ + HARDWARE_INTERFACE_EXPORT + return_type get_command( + std::vector & command, + const std::vector & interfaces) const; + + /** + * \brief Get complete command list for the joint. This function is used by the hardware to get + * complete command for it. The hardware valus have the same order as interfaces which + * can be recived by get_hardware_interfaces() function. Return value is used for API consistency. + * + * \param command list of doubles with commands for the hardware. + * \return return_type::OK always. + */ + HARDWARE_INTERFACE_EXPORT + return_type get_command(std::vector & command) const; + + /** + * \brief Set command list for the joint. This function is used by the controller to set the goal + * values for the hardware. The parameters command, and interfaces have the same order and number + * of elements. Using the interfaces list, the controller can choose which values to set. + * + * \param command list of doubles with commands for the hardware. + * \param interfaces list of interfaces on which commands have to be provided. + * \return return_type::INTERFACE_VALUE_SIZE_NOT_EQUAL if command and interfaces arguments do not + * have the same length; return_type::COMMAND_OUT_OF_LIMITS if one of the command values is out + * of limits; return_type::INTERFACE_NOT_FOUND if one of provided interfaces is not + * defined for the joint; return_type::OK otherwise. + * + * \todo The error handling in this function could lead to incosistant command or state variables + * for different interfaces. This should be changed in the future. + * (see: https://github.com/ros-controls/ros2_control/issues/129) + */ + HARDWARE_INTERFACE_EXPORT + return_type set_command( + const std::vector & command, + const std::vector & interfaces); + + /** + * \brief Get complete state list from the joint. This function is used by the hardware to get + * complete command for it. The hardware valus have the same order as interfaces which + * can be recived by get_hardware_interfaces() function. + * + * \param command list of doubles with commands for the hardware. + * \return return_type::INTERFACE_VALUE_SIZE_NOT_EQUAL is command size is not equal to number of + * joint's command interfaces; return_type::COMMAND_OUT_OF_LIMITS if one of the command values is out + * of limits; return_type::OK otherwise. + */ + HARDWARE_INTERFACE_EXPORT + return_type set_command(const std::vector & command); + + /** + * \brief Get state list from the joint. This function is used by the controller to get the + * actual state of the hardware. The parameters state, and interfaces have the same order and + * number of elements. Using the interfaces list, the controller can choose which values to get. + * + * \param state list of doubles with states of the hardware. + * \param interfaces list of interfaces on which states have to be provided. + * \return return_type::INTERFACE_VALUE_SIZE_NOT_EQUAL if state and interfaces arguments do not + * have the same length; return_type::INTERFACE_NOT_FOUND if one of provided interfaces is not + * defined for the joint; return return_type::INTERFACE_NOT_PROVIDED if the list of interfaces + * is empty; return_type::OK otherwise. + */ + HARDWARE_INTERFACE_EXPORT + return_type get_state( + std::vector & state, + const std::vector & interfaces) const; + + /** + * \brief Get complete state list from the joint. This function is used by the controller to get + * complete actual state of the hardware. The state values have the same order as interfaces which + * can be recived by get_state_interfaces() function. Return value is used for API consistency. + * + * \param state list of doubles with states of the hardware. + * \return return_type::OK always. + */ + HARDWARE_INTERFACE_EXPORT + return_type get_state(std::vector & state) const; + + /** + * \brief Set state list for the joint. This function is used by the hardware to set its actual + * state. The parameters state, and interfaces have the same order and number of elements. Using + * the interfaces list, the hardware can choose which values to set. + * + * \param state list of doubles with states of the hardware. + * \param interfaces list of interfaces on which states have to be provided. + * \return return_type::INTERFACE_VALUE_SIZE_NOT_EQUAL if state and interfaces arguments do not + * have the same length; return_type::INTERFACE_NOT_FOUND if one of provided interfaces is not + * defined for the joint; return_type::OK otherwise. + */ + HARDWARE_INTERFACE_EXPORT + return_type set_state( + const std::vector & state, + const std::vector & interfaces); + + /** + * \brief Set complete state list from the joint.This function is used by the hardware to set its + * complete actual state. The state values have the same order as interfaces which can be recived + * by get_state_interfaces() function. + * + * \param state list of doubles with states from the hardware. + * \return return_type::INTERFACE_VALUE_SIZE_NOT_EQUAL is command size is not equal to number of + * joint's state interfaces, return_type::OK otherwise. + */ + HARDWARE_INTERFACE_EXPORT + return_type set_state(const std::vector & state); + +protected: + ComponentInfo info_; + std::vector commands_; + std::vector states_; +}; + +} // namespace components +} // namespace hardware_interface +#endif // HARDWARE_INTERFACE__COMPONENTS__JOINT_HPP_ diff --git a/hardware_interface/include/hardware_interface/components/sensor.hpp b/hardware_interface/include/hardware_interface/components/sensor.hpp new file mode 100644 index 0000000000..a99c85eec9 --- /dev/null +++ b/hardware_interface/include/hardware_interface/components/sensor.hpp @@ -0,0 +1,126 @@ +// 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__COMPONENTS__SENSOR_HPP_ +#define HARDWARE_INTERFACE__COMPONENTS__SENSOR_HPP_ + +#include +#include + +#include "hardware_interface/components/component_info.hpp" +#include "hardware_interface/types/hardware_interface_return_values.hpp" +#include "hardware_interface/visibility_control.h" + +namespace hardware_interface +{ +namespace components +{ + +/** + * \brief Base Class for "Sensor" component used as a basic build block for the devices which + * provide data. A sensor can have one or more interfaces (e.g., force, acceleration, etc.) to + * provide states for. The list of state interfaces defines this. + */ +class Sensor +{ +public: + HARDWARE_INTERFACE_PUBLIC + Sensor() = default; + + HARDWARE_INTERFACE_PUBLIC + virtual + ~Sensor() = default; + + /** + * \brief Configure base sensor class based on the description in the robot's URDF file. + * + * \param joint_info structure with data from URDF. + * \return return_type::OK if required data are provided and is successfully parsed, + * return_type::ERROR otherwise. + */ + HARDWARE_INTERFACE_PUBLIC + return_type configure(const ComponentInfo & joint_info); + + /** + * \brief Provide the list of state interfaces configured for the sensor. + * + * \return string list with state interfaces. + */ + HARDWARE_INTERFACE_PUBLIC + std::vector get_state_interfaces(); + + /** + * \brief Get state list from the sensor. This function is used by the controller to get the + * actual state of the hardware. The parameters state, and interfaces have the same order and + * number of elements. Using the interfaces list, the controller can choose which values to get. + * + * \param state list of doubles with states of the hardware. + * \param interfaces list of interfaces on which states have to be provided. + * \return return_type::INTERFACE_VALUE_SIZE_NOT_EQUAL if state and interfaces arguments do not + * have the same length; return_type::INTERFACE_NOT_FOUND if one of provided interfaces is not + * defined for the sensor; return return_type::INTERFACE_NOT_PROVIDED if the list of interfaces + * is empty; return_type::OK otherwise. + */ + HARDWARE_INTERFACE_EXPORT + return_type get_state( + std::vector & state, + const std::vector & interfaces) const; + + /** + * \brief Get complete state list from the sensor. This function is used by the controller to get + * complete actual state of the hardware. The state values have the same order as interfaces which + * can be recived by get_state_interfaces() function. Return value is used for API consistency. + * + * \param state list of doubles with states of the hardware. + * \return return_type::OK always. + */ + HARDWARE_INTERFACE_EXPORT + return_type get_state(std::vector & state) const; + + /** + * \brief Set state list for the sensor. This function is used by the hardware to set its actual + * state. The parameters state, and interfaces have the same order and number of elements. Using + * the interfaces list, the hardware can choose which values to set. + * + * \param state list of doubles with states of the hardware. + * \param interfaces list of interfaces on which states have to be provided. + * \return return_type::INTERFACE_VALUE_SIZE_NOT_EQUAL if state and interfaces arguments do not + * have the same length; return_type::INTERFACE_NOT_FOUND if one of provided interfaces is not + * defined for the sensor; return_type::OK otherwise. + */ + HARDWARE_INTERFACE_EXPORT + return_type set_state( + const std::vector & state, + const std::vector & interfaces); + + /** + * \brief Set complete state list from the sensor.This function is used by the hardware to set its + * complete actual state. The state values have the same order as interfaces which can be recived + * by get_state_interfaces() function. + * + * \param state list of doubles with states from the hardware. + * \return return_type::INTERFACE_VALUE_SIZE_NOT_EQUAL is state size is not equal to number of + * sensor's state interfaces, return_type::OK otherwise. + */ + HARDWARE_INTERFACE_EXPORT + return_type set_state(const std::vector & state); + +protected: + ComponentInfo info_; + std::vector states_; +}; + +} // namespace components +} // namespace hardware_interface +#endif // HARDWARE_INTERFACE__COMPONENTS__SENSOR_HPP_ diff --git a/hardware_interface/include/hardware_interface/hardware_info.hpp b/hardware_interface/include/hardware_interface/hardware_info.hpp new file mode 100644 index 0000000000..a8cf6901af --- /dev/null +++ b/hardware_interface/include/hardware_interface/hardware_info.hpp @@ -0,0 +1,67 @@ +// 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__HARDWARE_INFO_HPP_ +#define HARDWARE_INTERFACE__HARDWARE_INFO_HPP_ + +#include +#include +#include + +#include "hardware_interface/components/component_info.hpp" + +namespace hardware_interface +{ + +/** + * \brief This structure stores information about hardware defined in a robot's URDF. + */ +struct HardwareInfo +{ + /** + * \brief name of the hardware. + */ + std::string name; + /** + * \brief type of the hardware: actuator, sensor or system. + */ + std::string type; + /** + * \brief class of the hardware that will be dynamically loaded. + */ + std::string hardware_class_type; + /** + * \brief (optional) key-value pairs for hardware parameters. + */ + std::unordered_map hardware_parameters; + /** + * \brief map of joints provided by the hardware where the key is the joint name. + * Required for Actuator and System Hardware. + */ + std::unordered_map joints; + /** + * \brief map of joints provided by the hardware where the key is the joint name. + * Required for Sensor and optional for System Hardware. + */ + std::unordered_map sensors; + /** + * \brief map of transmissions to calcualte ration between joints and physical actuators. + * Optional for Actuator and System Hardware. + */ + std::unordered_map transmissions; +}; + +} // namespace hardware_interface +#endif // HARDWARE_INTERFACE__HARDWARE_INFO_HPP_ diff --git a/hardware_interface/include/hardware_interface/sensor_hardware.hpp b/hardware_interface/include/hardware_interface/sensor_hardware.hpp new file mode 100644 index 0000000000..fde489abc2 --- /dev/null +++ b/hardware_interface/include/hardware_interface/sensor_hardware.hpp @@ -0,0 +1,66 @@ +// 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__SENSOR_HARDWARE_HPP_ +#define HARDWARE_INTERFACE__SENSOR_HARDWARE_HPP_ + +#include +#include +#include +#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" +#include "hardware_interface/visibility_control.h" + +namespace hardware_interface +{ + +namespace components +{ +class Sensor; +} // namespace components +class SensorHardwareInterface; + +class SensorHardware final +{ +public: + SensorHardware() = default; + + explicit SensorHardware(std::unique_ptr impl); + + ~SensorHardware() = default; + + HARDWARE_INTERFACE_PUBLIC + return_type configure(const HardwareInfo & sensor_info); + + HARDWARE_INTERFACE_PUBLIC + return_type start(); + + HARDWARE_INTERFACE_PUBLIC + return_type stop(); + + HARDWARE_INTERFACE_PUBLIC + hardware_interface_status get_status() const; + + HARDWARE_INTERFACE_PUBLIC + return_type read_sensor(std::shared_ptr sensor); + +private: + std::unique_ptr impl_; +}; + +} // namespace hardware_interface +#endif // HARDWARE_INTERFACE__SENSOR_HARDWARE_HPP_ diff --git a/hardware_interface/include/hardware_interface/sensor_hardware_interface.hpp b/hardware_interface/include/hardware_interface/sensor_hardware_interface.hpp new file mode 100644 index 0000000000..7a4cd7abc8 --- /dev/null +++ b/hardware_interface/include/hardware_interface/sensor_hardware_interface.hpp @@ -0,0 +1,94 @@ +// 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__SENSOR_HARDWARE_INTERFACE_HPP_ +#define HARDWARE_INTERFACE__SENSOR_HARDWARE_INTERFACE_HPP_ + +#include + +#include "hardware_interface/components/sensor.hpp" +#include "hardware_interface/hardware_info.hpp" +#include "hardware_interface/types/hardware_interface_return_values.hpp" +#include "hardware_interface/types/hardware_interface_status_values.hpp" +#include "hardware_interface/visibility_control.h" + +namespace hardware_interface +{ + +/** + * \brief 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 SensorHardwareInterface +{ +public: + HARDWARE_INTERFACE_PUBLIC + SensorHardwareInterface() = default; + + HARDWARE_INTERFACE_PUBLIC + virtual + ~SensorHardwareInterface() = default; + + /** + * \brief Configuration of the sensor from data parsed from the robot's URDF. + * + * \param sensor_info structure with data from URDF. + * \return return_type::OK if required data are provided and can be parsed, + * return_type::ERROR otherwise. + */ + HARDWARE_INTERFACE_PUBLIC + virtual + return_type configure(const HardwareInfo & sensor_info) = 0; + + /** + * \brief Start exchange data with the hardware. + * + * \return return_type:OK if everything worked as expected, return_type::ERROR otherwise. + */ + HARDWARE_INTERFACE_PUBLIC + virtual + return_type start() = 0; + + /** + * \brief Stop exchange data with the hardware. + * + * \return return_type:OK if everything worked as expected, return_type::ERROR otherwise. + */ + HARDWARE_INTERFACE_PUBLIC + virtual + return_type stop() = 0; + + /** + * \brief Get current state of the system hardware. + * + * \return hardware_interface_status current status. + */ + HARDWARE_INTERFACE_PUBLIC + virtual + hardware_interface_status get_status() const = 0; + + /** + * \brief Read data from the hardware into sensors using "set_state" function in the Sensor class. + * This function is always called by the resource manager. + * + * \param sensors sensor where data from the hardware are stored. + * \return return_type:OK if everything worked as expected, return_type::ERROR otherwise. + */ + HARDWARE_INTERFACE_PUBLIC + virtual + return_type read_sensor(std::shared_ptr sensor) const = 0; +}; + +} // namespace hardware_interface +#endif // HARDWARE_INTERFACE__SENSOR_HARDWARE_INTERFACE_HPP_ diff --git a/hardware_interface/include/hardware_interface/system_hardware.hpp b/hardware_interface/include/hardware_interface/system_hardware.hpp new file mode 100644 index 0000000000..5a64148c29 --- /dev/null +++ b/hardware_interface/include/hardware_interface/system_hardware.hpp @@ -0,0 +1,72 @@ +// 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__SYSTEM_HARDWARE_HPP_ +#define HARDWARE_INTERFACE__SYSTEM_HARDWARE_HPP_ + +#include +#include +#include +#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" +#include "hardware_interface/visibility_control.h" + +namespace hardware_interface +{ + +namespace components +{ +class Joint; +class Sensor; +} // namespace components +class SystemHardwareInterface; + +class SystemHardware final +{ +public: + HARDWARE_INTERFACE_PUBLIC + explicit SystemHardware(std::unique_ptr impl); + + virtual ~SystemHardware() = default; + + HARDWARE_INTERFACE_PUBLIC + return_type configure(const HardwareInfo & system_info); + + HARDWARE_INTERFACE_PUBLIC + return_type start(); + + HARDWARE_INTERFACE_PUBLIC + return_type stop(); + + HARDWARE_INTERFACE_PUBLIC + hardware_interface_status get_status() const; + + HARDWARE_INTERFACE_PUBLIC + return_type read_sensors(std::vector> & sensors); + + HARDWARE_INTERFACE_PUBLIC + return_type read_joints(std::vector> & joints); + + HARDWARE_INTERFACE_PUBLIC + return_type write_joints(const std::vector> & joints); + +private: + std::unique_ptr impl_; +}; + +} // namespace hardware_interface +#endif // HARDWARE_INTERFACE__SYSTEM_HARDWARE_HPP_ diff --git a/hardware_interface/include/hardware_interface/system_hardware_interface.hpp b/hardware_interface/include/hardware_interface/system_hardware_interface.hpp new file mode 100644 index 0000000000..722c75c435 --- /dev/null +++ b/hardware_interface/include/hardware_interface/system_hardware_interface.hpp @@ -0,0 +1,121 @@ +// 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__SYSTEM_HARDWARE_INTERFACE_HPP_ +#define HARDWARE_INTERFACE__SYSTEM_HARDWARE_INTERFACE_HPP_ + +#include +#include + +#include "hardware_interface/components/joint.hpp" +#include "hardware_interface/components/sensor.hpp" +#include "hardware_interface/hardware_info.hpp" +#include "hardware_interface/types/hardware_interface_return_values.hpp" +#include "hardware_interface/types/hardware_interface_status_values.hpp" +#include "hardware_interface/visibility_control.h" + +namespace hardware_interface +{ + +/** +* \brief 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 SystemHardwareInterface +{ +public: + HARDWARE_INTERFACE_PUBLIC + SystemHardwareInterface() = default; + + HARDWARE_INTERFACE_PUBLIC + virtual + ~SystemHardwareInterface() = default; + + /** + * \brief Configuration of the system from data parsed from the robot's URDF. + * + * \param system_info structure with data from URDF. + * \return return_type::OK if required data are provided and can be parsed, + * return_type::ERROR otherwise. + */ + HARDWARE_INTERFACE_PUBLIC + virtual + return_type configure(const HardwareInfo & system_info) = 0; + + /** + * \brief Start exchange data with the hardware. + * + * \return return_type:OK if everything worked as expected, return_type::ERROR otherwise. + */ + HARDWARE_INTERFACE_PUBLIC + virtual + return_type start() = 0; + + /** + * \brief Stop exchange data with the hardware. + * + * \return return_type:OK if everything worked as expected, return_type::ERROR otherwise. + */ + HARDWARE_INTERFACE_PUBLIC + virtual + return_type stop() = 0; + + /** + * \brief Get current state of the system hardware. + * + * \return hardware_interface_status current status. + */ + HARDWARE_INTERFACE_PUBLIC + virtual + hardware_interface_status get_status() const = 0; + + /** + * \brief Read data from the hardware into sensors using "set_state" function in the Sensor class. + * This is used only if the system hardware has attached sensors. + * The function call from the resource manager is therefore optional. + * + * \param sensors list of sensors where data from the hardware are stored. + * \return return_type:OK if everything worked as expected, return_type::ERROR otherwise. + */ + HARDWARE_INTERFACE_PUBLIC + virtual + return_type read_sensors(std::vector> & sensors) const = 0; + + /** + * \brief Read data fromt the hardware into joints using "set_state" function of the Joint class. + * This function is always called by the resource manager. + * + * \param joints list of joints where data from the hardware are stored. + * \return return_type:OK if everything worked as expected, return_type::ERROR otherwise. + */ + HARDWARE_INTERFACE_PUBLIC + virtual + return_type read_joints(std::vector> & joints) const = 0; + + /** + * \brief Write data from the joints to the hardware using "get_command" function of the Joint class. + * This function is always called by the resource manager. + * + * \param joints list of joints from which data are written to the hardware. + * \return return_type:OK if everything worked as expected, return_type::ERROR otherwise. + */ + HARDWARE_INTERFACE_PUBLIC + return_type + virtual + write_joints(const std::vector> & joints) = 0; +}; + +} // namespace hardware_interface +#endif // HARDWARE_INTERFACE__SYSTEM_HARDWARE_INTERFACE_HPP_ diff --git a/hardware_interface/include/hardware_interface/types/hardware_interface_return_values.hpp b/hardware_interface/include/hardware_interface/types/hardware_interface_return_values.hpp index 9db359564e..d09fea6e1a 100644 --- a/hardware_interface/include/hardware_interface/types/hardware_interface_return_values.hpp +++ b/hardware_interface/include/hardware_interface/types/hardware_interface_return_values.hpp @@ -31,6 +31,12 @@ enum class return_type : std::uint8_t NON_CLAIMED_WRITE = 15, CAN_NOT_READ = 20, + + INTERFACE_NOT_FOUND = 30, + INTERFACE_VALUE_SIZE_NOT_EQUAL = 31, + INTERFACE_NOT_PROVIDED = 32, + + COMMAND_OUT_OF_LIMITS = 40, }; using hardware_interface_ret_t = return_type; 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 new file mode 100644 index 0000000000..7aff376a2f --- /dev/null +++ b/hardware_interface/include/hardware_interface/types/hardware_interface_status_values.hpp @@ -0,0 +1,32 @@ +// 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 hardware_interface_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/hardware_interface_type_values.hpp b/hardware_interface/include/hardware_interface/types/hardware_interface_type_values.hpp new file mode 100644 index 0000000000..7292f28229 --- /dev/null +++ b/hardware_interface/include/hardware_interface/types/hardware_interface_type_values.hpp @@ -0,0 +1,25 @@ +// 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_TYPE_VALUES_HPP_ +#define HARDWARE_INTERFACE__TYPES__HARDWARE_INTERFACE_TYPE_VALUES_HPP_ + +namespace hardware_interface +{ +constexpr const auto HW_IF_POSITION = "position"; +constexpr const auto HW_IF_VELOCITY = "velocity"; +constexpr const auto HW_IF_EFFORT = "effort"; +} // namespace hardware_interface + +#endif // HARDWARE_INTERFACE__TYPES__HARDWARE_INTERFACE_TYPE_VALUES_HPP_ diff --git a/hardware_interface/src/actuator_hardware.cpp b/hardware_interface/src/actuator_hardware.cpp new file mode 100644 index 0000000000..d6336f98c6 --- /dev/null +++ b/hardware_interface/src/actuator_hardware.cpp @@ -0,0 +1,67 @@ +// 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. + +#include +#include +#include +#include + +#include "hardware_interface/actuator_hardware.hpp" + +#include "hardware_interface/actuator_hardware_interface.hpp" +#include "hardware_interface/components/component_info.hpp" +#include "hardware_interface/components/joint.hpp" +#include "hardware_interface/hardware_info.hpp" +#include "hardware_interface/types/hardware_interface_return_values.hpp" +#include "hardware_interface/types/hardware_interface_status_values.hpp" +#include "hardware_interface/visibility_control.h" + +namespace hardware_interface +{ + +ActuatorHardware::ActuatorHardware(std::unique_ptr impl) +: impl_(std::move(impl)) +{} + +return_type ActuatorHardware::configure(const HardwareInfo & actuator_info) +{ + return impl_->configure(actuator_info); +} + +return_type ActuatorHardware::start() +{ + return impl_->start(); +} + +return_type ActuatorHardware::stop() +{ + return impl_->stop(); +} + +hardware_interface_status ActuatorHardware::get_status() const +{ + return impl_->get_status(); +} + +return_type ActuatorHardware::read_joint(std::shared_ptr joint) +{ + return impl_->read_joint(joint); +} + +return_type ActuatorHardware::write_joint(const std::shared_ptr joint) +{ + return impl_->write_joint(joint); +} + +} // namespace hardware_interface diff --git a/hardware_interface/src/components/component_lists_management.hpp b/hardware_interface/src/components/component_lists_management.hpp new file mode 100644 index 0000000000..deb949a980 --- /dev/null +++ b/hardware_interface/src/components/component_lists_management.hpp @@ -0,0 +1,142 @@ +// 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 COMPONENTS__COMPONENT_LISTS_MANAGEMENT_HPP_ +#define COMPONENTS__COMPONENT_LISTS_MANAGEMENT_HPP_ + +#include +#include +#include + +#include "hardware_interface/types/hardware_interface_return_values.hpp" +#include "hardware_interface/visibility_control.h" + +namespace hardware_interface +{ +namespace components +{ + +/** + * \brief Get values for queried_interfaces from the int_values. int_values data structure matches + * int_interfaces vector. + * + * \param values values to return. + * \param queried_interfaces interfaces for which values are queried. + * \param int_interfaces full list of interfaces of a component. + * \param int_values internal values of a component. + * \return return_type::INTERFACE_VALUE_SIZE_NOT_EQUAL if values and queried_interfaces arguments + * do not have the same length; return_type::INTERFACE_NOT_FOUND if one of queried_interfaces is + * not defined in int_interfaces; return return_type::INTERFACE_NOT_PROVIDED if queried_interfaces + * list is is empty; return_type::OK otherwise. + */ +inline return_type get_internal_values( + std::vector & values, const std::vector & queried_interfaces, + const std::vector & int_interfaces, const std::vector & int_values) +{ + if (queried_interfaces.size() == 0) { + return return_type::INTERFACE_NOT_PROVIDED; + } + + for (const auto & interface : queried_interfaces) { + auto it = std::find( + int_interfaces.begin(), int_interfaces.end(), interface); + if (it != int_interfaces.end()) { + values.push_back(int_values[std::distance(int_interfaces.begin(), it)]); + } else { + values.clear(); + return return_type::INTERFACE_NOT_FOUND; + } + } + return return_type::OK; +} + +/** + * \brief Set all internal values to to other vector. Return value is used for API consistency. + * + * \param values output list of values. + * \param int_values internal values of the component. + * \return return_type::OK always. + */ +inline return_type get_internal_values( + std::vector & values, const std::vector & int_values) +{ + values.clear(); + for (const auto & int_value : int_values) { + values.push_back(int_value); + } + return return_type::OK; +} + +/** + * \brief set values for queried_interfaces to the int_values. int_values data structure matches + * int_interfaces vector. + * + * \param values values to set. + * \param queried_interfaces interfaces for which values are queried. + * \param int_interfaces full list of interfaces of a component. + * \param int_values internal values of a component. + * \return return return_type::INTERFACE_NOT_PROVIDED if + * queried_interfaces list is is empty; return_type::INTERFACE_VALUE_SIZE_NOT_EQUAL if values and + * queried_interfaces arguments do not have the same length; return_type::INTERFACE_NOT_FOUND if + * one of queried_interfaces is not defined in int_interfaces; return_type::OK otherwise. + * + * \todo The error handling in this function could lead to incosistant command or state variables + * for different interfaces. This should be changed in the future. + * (see: https://github.com/ros-controls/ros2_control/issues/129) + */ +inline return_type set_internal_values( + const std::vector & values, const std::vector & queried_interfaces, + const std::vector & int_interfaces, std::vector & int_values) +{ + if (queried_interfaces.size() == 0) { + return return_type::INTERFACE_NOT_PROVIDED; + } + if (values.size() != queried_interfaces.size()) { + return return_type::INTERFACE_VALUE_SIZE_NOT_EQUAL; + } + + for (auto q_it = queried_interfaces.begin(); q_it != queried_interfaces.end(); ++q_it) { + auto it = std::find(int_interfaces.begin(), int_interfaces.end(), *q_it); + if (it != int_interfaces.end()) { + int_values[std::distance(int_interfaces.begin(), it)] = + values[std::distance(queried_interfaces.begin(), q_it)]; + } else { + return return_type::INTERFACE_NOT_FOUND; + } + } + return return_type::OK; +} + +/** + * \brief set all values to compoenents internal values. + * + * \param values values to set. + * \param int_values internal values of a component. + * \return return_type::INTERFACE_VALUE_SIZE_NOT_EQUAL if the size of the arguments is not equal, + * return_type::OK otherwise. + */ +inline return_type set_internal_values( + const std::vector & values, std::vector & int_values) +{ + if (values.size() == int_values.size()) { + int_values = values; + } else { + return return_type::INTERFACE_VALUE_SIZE_NOT_EQUAL; + } + return return_type::OK; +} + +} // namespace components +} // namespace hardware_interface +#endif // COMPONENTS__COMPONENT_LISTS_MANAGEMENT_HPP_ diff --git a/hardware_interface/src/components/joint.cpp b/hardware_interface/src/components/joint.cpp new file mode 100644 index 0000000000..badd4d3e9a --- /dev/null +++ b/hardware_interface/src/components/joint.cpp @@ -0,0 +1,98 @@ +// 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. + +#include +#include + +#include "hardware_interface/components/joint.hpp" + +#include "hardware_interface/components/component_info.hpp" +#include "hardware_interface/types/hardware_interface_return_values.hpp" + +#include "./component_lists_management.hpp" + +namespace hardware_interface +{ +namespace components +{ + +return_type Joint::configure(const ComponentInfo & joint_info) +{ + info_ = joint_info; + if (info_.command_interfaces.size() > 0) { + commands_.resize(info_.command_interfaces.size()); + } + if (info_.state_interfaces.size() > 0) { + states_.resize(info_.state_interfaces.size()); + } + return return_type::OK; +} + +std::vector Joint::get_command_interfaces() const +{ + return info_.command_interfaces; +} + +std::vector Joint::get_state_interfaces() const +{ + return info_.state_interfaces; +} + +return_type Joint::get_command( + std::vector & command, const std::vector & interfaces) const +{ + return get_internal_values(command, interfaces, info_.command_interfaces, commands_); +} + +return_type Joint::get_command(std::vector & command) const +{ + return get_internal_values(command, commands_); +} + +return_type Joint::set_command( + const std::vector & command, + const std::vector & interfaces) +{ + return set_internal_values(command, interfaces, info_.command_interfaces, commands_); +} + +return_type Joint::set_command(const std::vector & command) +{ + return set_internal_values(command, commands_); +} + +return_type Joint::get_state( + std::vector & state, const std::vector & interfaces) const +{ + return get_internal_values(state, interfaces, info_.state_interfaces, states_); +} + +return_type Joint::get_state(std::vector & state) const +{ + return get_internal_values(state, states_); +} + +return_type Joint::set_state( + const std::vector & state, const std::vector & interfaces) +{ + return set_internal_values(state, interfaces, info_.state_interfaces, states_); +} + +return_type Joint::set_state(const std::vector & state) +{ + return set_internal_values(state, states_); +} + +} // namespace components +} // namespace hardware_interface diff --git a/hardware_interface/src/components/sensor.cpp b/hardware_interface/src/components/sensor.cpp new file mode 100644 index 0000000000..0229e8b14f --- /dev/null +++ b/hardware_interface/src/components/sensor.cpp @@ -0,0 +1,67 @@ +// 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. + +#include +#include + +#include "hardware_interface/components/sensor.hpp" + +#include "hardware_interface/components/component_info.hpp" +#include "hardware_interface/types/hardware_interface_return_values.hpp" + +#include "./component_lists_management.hpp" + +namespace hardware_interface +{ +namespace components +{ + +return_type Sensor::configure(const ComponentInfo & joint_info) +{ + info_ = joint_info; + if (info_.state_interfaces.size() > 0) { + states_.resize(info_.state_interfaces.size()); + } + return return_type::OK; +} + +std::vector Sensor::get_state_interfaces() +{ + return info_.state_interfaces; +} + +return_type Sensor::get_state( + std::vector & state, const std::vector & interfaces) const +{ + return get_internal_values(state, interfaces, info_.state_interfaces, states_); +} + +return_type Sensor::get_state(std::vector & state) const +{ + return get_internal_values(state, states_); +} + +return_type Sensor::set_state( + const std::vector & state, const std::vector & interfaces) +{ + return set_internal_values(state, interfaces, info_.state_interfaces, states_); +} + +return_type Sensor::set_state(const std::vector & state) +{ + return set_internal_values(state, states_); +} + +} // namespace components +} // namespace hardware_interface diff --git a/hardware_interface/src/robot_hardware.cpp b/hardware_interface/src/robot_hardware.cpp index 8e99c34bba..749ce37953 100644 --- a/hardware_interface/src/robot_hardware.cpp +++ b/hardware_interface/src/robot_hardware.cpp @@ -217,7 +217,8 @@ RobotHardware::get_registered_operation_mode_handles() hardware_interface_ret_t RobotHardware::register_actuator( const std::string & actuator_name, - const std::string & interface_name, const double default_value) + const std::string & interface_name, + const double default_value) { if (actuator_name.empty() || interface_name.empty()) { RCLCPP_ERROR(rclcpp::get_logger(kActuatorLoggerName), "actuator name or interface is empty!"); @@ -269,7 +270,7 @@ hardware_interface_ret_t RobotHardware::get_actuator_handle(ActuatorHandle & act if (it == names_list.cend()) { RCLCPP_ERROR( rclcpp::get_logger( - kActuatorLoggerName), "actuator with name %s not found!", actuator_name); + kActuatorLoggerName), "actuator with name %s not found!", actuator_name.c_str()); return return_type::ERROR; } @@ -300,11 +301,11 @@ std::vector RobotHardware::get_registered_actuators() auto & interface_values = registered_actuators_.interface_values; assert(registered_actuators_.joint_names.size() == registered_actuators_.interface_values.size()); - for (size_t i = 0; i < actuator_names.size(); ++i) { + for (auto i = 0u; i < actuator_names.size(); ++i) { auto & actuator_interfaces = interface_values[i]; assert(actuator_interfaces.interface_names.size() == actuator_interfaces.values.size()); - for (size_t j = 0; j < actuator_interfaces.interface_names.size(); ++j) { + for (auto j = 0u; j < actuator_interfaces.interface_names.size(); ++j) { result.emplace_back( actuator_names[i], actuator_interfaces.interface_names[j], &actuator_interfaces.values[j]); diff --git a/hardware_interface/src/sensor_hardware.cpp b/hardware_interface/src/sensor_hardware.cpp new file mode 100644 index 0000000000..dc0543d720 --- /dev/null +++ b/hardware_interface/src/sensor_hardware.cpp @@ -0,0 +1,62 @@ +// 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. + +#include +#include +#include +#include + +#include "hardware_interface/sensor_hardware.hpp" + +#include "hardware_interface/components/component_info.hpp" +#include "hardware_interface/components/sensor.hpp" +#include "hardware_interface/hardware_info.hpp" +#include "hardware_interface/sensor_hardware_interface.hpp" +#include "hardware_interface/types/hardware_interface_return_values.hpp" +#include "hardware_interface/types/hardware_interface_status_values.hpp" +#include "hardware_interface/visibility_control.h" + +namespace hardware_interface +{ + +SensorHardware::SensorHardware(std::unique_ptr impl) +: impl_(std::move(impl)) +{} + +return_type SensorHardware::configure(const HardwareInfo & sensor_info) +{ + return impl_->configure(sensor_info); +} + +return_type SensorHardware::start() +{ + return impl_->start(); +} + +return_type SensorHardware::stop() +{ + return impl_->stop(); +} + +hardware_interface_status SensorHardware::get_status() const +{ + return impl_->get_status(); +} + +return_type SensorHardware::read_sensor(std::shared_ptr sensors) +{ + return impl_->read_sensor(sensors); +} + +} // namespace hardware_interface diff --git a/hardware_interface/src/system_hardware.cpp b/hardware_interface/src/system_hardware.cpp new file mode 100644 index 0000000000..9641aae82b --- /dev/null +++ b/hardware_interface/src/system_hardware.cpp @@ -0,0 +1,74 @@ +// 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. + +#include +#include +#include +#include + +#include "hardware_interface/system_hardware.hpp" + +#include "hardware_interface/components/component_info.hpp" +#include "hardware_interface/components/joint.hpp" +#include "hardware_interface/components/sensor.hpp" +#include "hardware_interface/hardware_info.hpp" +#include "hardware_interface/system_hardware_interface.hpp" +#include "hardware_interface/types/hardware_interface_return_values.hpp" +#include "hardware_interface/types/hardware_interface_status_values.hpp" +#include "hardware_interface/visibility_control.h" + +namespace hardware_interface +{ + +SystemHardware::SystemHardware(std::unique_ptr impl) +: impl_(std::move(impl)) +{} + +return_type SystemHardware::configure(const HardwareInfo & system_info) +{ + return impl_->configure(system_info); +} + +return_type SystemHardware::start() +{ + return impl_->start(); +} + +return_type SystemHardware::stop() +{ + return impl_->stop(); +} + +hardware_interface_status SystemHardware::get_status() const +{ + return impl_->get_status(); +} + +return_type SystemHardware::read_sensors(std::vector> & sensors) +{ + return impl_->read_sensors(sensors); +} + +return_type SystemHardware::read_joints(std::vector> & joints) +{ + return impl_->read_joints(joints); +} + +return_type SystemHardware::write_joints( + const std::vector> & joints) +{ + return impl_->write_joints(joints); +} + +} // namespace hardware_interface diff --git a/hardware_interface/test/test_component_interfaces.cpp b/hardware_interface/test/test_component_interfaces.cpp new file mode 100644 index 0000000000..a1e694a053 --- /dev/null +++ b/hardware_interface/test/test_component_interfaces.cpp @@ -0,0 +1,711 @@ +// 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. + +#include +#include +#include +#include +#include + +#include "hardware_interface/actuator_hardware.hpp" +#include "hardware_interface/actuator_hardware_interface.hpp" +#include "hardware_interface/components/component_info.hpp" +#include "hardware_interface/components/joint.hpp" +#include "hardware_interface/components/sensor.hpp" +#include "hardware_interface/hardware_info.hpp" +#include "hardware_interface/sensor_hardware_interface.hpp" +#include "hardware_interface/sensor_hardware.hpp" +#include "hardware_interface/system_hardware_interface.hpp" +#include "hardware_interface/system_hardware.hpp" +#include "hardware_interface/types/hardware_interface_return_values.hpp" +#include "hardware_interface/types/hardware_interface_status_values.hpp" +#include "hardware_interface/types/hardware_interface_type_values.hpp" + +using namespace ::testing; // NOLINT + +namespace hardware_interface +{ + +namespace hardware_interfaces_components_test +{ + +class DummyPositionJoint : public components::Joint +{ +public: + return_type configure(const components::ComponentInfo & joint_info) + { + if (Joint::configure(joint_info) != return_type::OK) { + return return_type::ERROR; + } + + if (info_.command_interfaces.size() > 1 || info_.state_interfaces.size() > 1) { + return return_type::ERROR; + } + if (info_.command_interfaces.size() == 0) { + info_.command_interfaces.push_back(HW_IF_POSITION); + commands_.resize(1); + } + if (info_.state_interfaces.size() == 0) { + info_.state_interfaces.push_back(HW_IF_POSITION); + states_.resize(1); + } + + max_position_ = stod(info_.parameters["max_position"]); + min_position_ = stod(info_.parameters["min_position"]); + return return_type::OK; + } + +private: + double max_position_, min_position_; +}; + +class DummyMultiJoint : public components::Joint +{ +public: + return_type configure(const components::ComponentInfo & joint_info) + { + if (Joint::configure(joint_info) != return_type::OK) { + return return_type::ERROR; + } + + if (info_.command_interfaces.size() < 2) { + return return_type::ERROR; + } + + max_position_ = stod(info_.parameters["max_position"]); + min_position_ = stod(info_.parameters["min_position"]); + max_velocity_ = stod(info_.parameters["max_velocity"]); + min_velocity_ = stod(info_.parameters["min_velocity"]); + return return_type::OK; + } + +private: + double max_position_, min_position_; + double max_velocity_, min_velocity_; +}; + +class DummyForceTorqueSensor : public components::Sensor +{ +public: + return_type configure(const components::ComponentInfo & sensor_info) + { + if (Sensor::configure(sensor_info) != return_type::OK) { + return return_type::ERROR; + } + + if (info_.parameters["frame_id"] == "") { + return return_type::ERROR; + } + if (info_.state_interfaces.size() == 0) { + info_.state_interfaces.push_back("force_x"); + info_.state_interfaces.push_back("force_y"); + info_.state_interfaces.push_back("force_z"); + info_.state_interfaces.push_back("torque_x"); + info_.state_interfaces.push_back("torque_y"); + info_.state_interfaces.push_back("torque_z"); + } + states_ = {1.34, 5.67, 8.21, 5.63, 5.99, 4.32}; + return return_type::OK; + } +}; + +class DummyActuatorHardware : public ActuatorHardwareInterface +{ + return_type configure(const HardwareInfo & actuator_info) override + { + info_ = actuator_info; + hw_read_time_ = stod(info_.hardware_parameters["example_param_read_for_sec"]); + hw_write_time_ = stod(info_.hardware_parameters["example_param_write_for_sec"]); + status_ = hardware_interface_status::CONFIGURED; + return return_type::OK; + } + + return_type start() override + { + if (status_ == hardware_interface_status::CONFIGURED || + status_ == hardware_interface_status::STOPPED) + { + status_ = hardware_interface_status::STARTED; + } else { + return return_type::ERROR; + } + return return_type::OK; + } + + return_type stop() override + { + if (status_ == hardware_interface_status::STARTED) { + status_ = hardware_interface_status::STOPPED; + } else { + return return_type::ERROR; + } + return return_type::OK; + } + + hardware_interface_status get_status() const override + { + return status_; + } + + return_type read_joint(std::shared_ptr joint) const override + { + std::vector interfaces = joint->get_state_interfaces(); + return joint->set_state(hw_values_, interfaces); + } + + return_type write_joint(const std::shared_ptr joint) override + { + std::vector interfaces = joint->get_command_interfaces(); + return joint->get_command(hw_values_, interfaces); + } + +private: + HardwareInfo info_; + hardware_interface_status status_ = hardware_interface_status::UNKNOWN; + std::vector hw_values_ = {1.2}; + double hw_read_time_, hw_write_time_; +}; + +class DummySensorHardware : public SensorHardwareInterface +{ + return_type configure(const HardwareInfo & sensor_info) override + { + info_ = sensor_info; + binary_to_voltage_factor_ = stod(info_.hardware_parameters["binary_to_voltage_factor"]); + status_ = hardware_interface_status::CONFIGURED; + return return_type::OK; + } + + return_type start() override + { + if (status_ == hardware_interface_status::CONFIGURED || + status_ == hardware_interface_status::STOPPED) + { + status_ = hardware_interface_status::STARTED; + } else { + return return_type::ERROR; + } + return return_type::OK; + } + + return_type stop() override + { + if (status_ == hardware_interface_status::STARTED) { + status_ = hardware_interface_status::STOPPED; + } else { + return return_type::ERROR; + } + return return_type::OK; + } + + hardware_interface_status get_status() const override + { + return status_; + } + + return_type read_sensor(std::shared_ptr sensor) const override + { + return sensor->set_state(ft_hw_values_); + } + +private: + HardwareInfo info_; + hardware_interface_status status_ = hardware_interface_status::UNKNOWN; + double binary_to_voltage_factor_; + std::vector ft_hw_values_ = {1, -1.0, 3.4, 7.9, 5.5, 4.4}; +}; + +class DummySystemHardware : public SystemHardwareInterface +{ + return_type configure(const HardwareInfo & system_info) override + { + info_ = system_info; + api_version_ = stod(info_.hardware_parameters["example_api_version"]); + hw_read_time_ = stod(info_.hardware_parameters["example_param_read_for_sec"]); + hw_write_time_ = stod(info_.hardware_parameters["example_param_write_for_sec"]); + status_ = hardware_interface_status::CONFIGURED; + return return_type::OK; + } + + return_type start() override + { + if (status_ == hardware_interface_status::CONFIGURED || + status_ == hardware_interface_status::STOPPED) + { + status_ = hardware_interface_status::STARTED; + } else { + return return_type::ERROR; + } + return return_type::OK; + } + + return_type stop() override + { + if (status_ == hardware_interface_status::STARTED) { + status_ = hardware_interface_status::STOPPED; + } else { + return return_type::ERROR; + } + return return_type::OK; + } + + hardware_interface_status get_status() const override + { + return status_; + } + + return_type read_sensors(std::vector> & sensors) const + override + { + return_type ret = return_type::OK; + for (const auto & sensor : sensors) { + ret = sensor->set_state(ft_hw_values_); + if (ret != return_type::OK) { + break; + } + } + return ret; + } + + return_type read_joints(std::vector> & joints) const override + { + return_type ret = return_type::OK; + std::vector interfaces; + std::vector joint_values; + for (uint i = 0; i < joints.size(); i++) { + joint_values.clear(); + joint_values.push_back(joints_hw_values_[i]); + interfaces = joints[i]->get_state_interfaces(); + ret = joints[i]->set_state(joint_values, interfaces); + if (ret != return_type::OK) { + break; + } + } + return ret; + } + + return_type write_joints(const std::vector> & joints) override + { + return_type ret = return_type::OK; + for (const auto & joint : joints) { + std::vector values; + std::vector interfaces = joint->get_command_interfaces(); + ret = joint->get_command(values, interfaces); + if (ret != return_type::OK) { + break; + } + } + return ret; + } + +private: + HardwareInfo info_; + hardware_interface_status status_; + double hw_write_time_, hw_read_time_, api_version_; + std::vector ft_hw_values_ = {-3.5, -2.1, -8.7, -5.4, -9.0, -11.2}; + std::vector joints_hw_values_ = {-1.575, -0.7543}; +}; + +} // namespace hardware_interfaces_components_test +} // namespace hardware_interface + +using hardware_interface::components::ComponentInfo; +using hardware_interface::components::Joint; +using hardware_interface::components::Sensor; +using hardware_interface::ActuatorHardware; +using hardware_interface::ActuatorHardwareInterface; +using hardware_interface::HardwareInfo; +using hardware_interface::hardware_interface_status; +using hardware_interface::return_type; +using hardware_interface::SensorHardware; +using hardware_interface::SensorHardwareInterface; +using hardware_interface::SystemHardware; +using hardware_interface::SystemHardwareInterface; + +using hardware_interface::hardware_interfaces_components_test::DummyForceTorqueSensor; +using hardware_interface::hardware_interfaces_components_test::DummyMultiJoint; +using hardware_interface::hardware_interfaces_components_test::DummyPositionJoint; + +using hardware_interface::hardware_interfaces_components_test::DummyActuatorHardware; +using hardware_interface::hardware_interfaces_components_test::DummySensorHardware; +using hardware_interface::hardware_interfaces_components_test::DummySystemHardware; + +class TestComponentInterfaces : public Test +{ +protected: + ComponentInfo joint_info; + ComponentInfo sensor_info; + + void SetUp() override + { + joint_info.name = "DummyPositionJoint"; + joint_info.parameters["max_position"] = "3.14"; + joint_info.parameters["min_position"] = "-3.14"; + + sensor_info.name = "DummyForceTorqueSensor"; + sensor_info.parameters["frame_id"] = "tcp_link"; + } +}; + +TEST_F(TestComponentInterfaces, joint_example_component_works) +{ + DummyPositionJoint joint; + + EXPECT_EQ(joint.configure(joint_info), return_type::OK); + ASSERT_THAT(joint.get_command_interfaces(), SizeIs(1)); + EXPECT_EQ(joint.get_command_interfaces()[0], hardware_interface::HW_IF_POSITION); + ASSERT_THAT(joint.get_state_interfaces(), SizeIs(1)); + EXPECT_EQ(joint.get_state_interfaces()[0], hardware_interface::HW_IF_POSITION); + + // Command getters and setters + std::vector interfaces; + std::vector input; + input.push_back(2.1); + EXPECT_EQ(joint.set_command(input, interfaces), return_type::INTERFACE_NOT_PROVIDED); + interfaces.push_back(hardware_interface::HW_IF_VELOCITY); + EXPECT_EQ(joint.set_command(input, interfaces), return_type::INTERFACE_NOT_FOUND); + interfaces.push_back(hardware_interface::HW_IF_POSITION); + EXPECT_EQ(joint.set_command(input, interfaces), return_type::INTERFACE_VALUE_SIZE_NOT_EQUAL); + interfaces.clear(); + interfaces.push_back(hardware_interface::HW_IF_POSITION); + input.clear(); + input.push_back(1.2); + EXPECT_EQ(joint.set_command(input, interfaces), return_type::OK); + + std::vector output; + interfaces.clear(); + EXPECT_EQ(joint.get_command(output, interfaces), return_type::INTERFACE_NOT_PROVIDED); + interfaces.push_back(hardware_interface::HW_IF_POSITION); + EXPECT_EQ(joint.get_command(output, interfaces), return_type::OK); + ASSERT_THAT(output, SizeIs(1)); + EXPECT_EQ(output[0], 1.2); + interfaces.clear(); + interfaces.push_back(hardware_interface::HW_IF_VELOCITY); + EXPECT_EQ(joint.get_command(output, interfaces), return_type::INTERFACE_NOT_FOUND); + + input.clear(); + EXPECT_EQ(joint.set_command(input), return_type::INTERFACE_VALUE_SIZE_NOT_EQUAL); + input.push_back(2.1); + EXPECT_EQ(joint.set_command(input), return_type::OK); + + EXPECT_EQ(joint.get_command(output), return_type::OK); + ASSERT_THAT(output, SizeIs(1)); + EXPECT_EQ(output[0], 2.1); + + // State getters and setters + interfaces.clear(); + input.clear(); + input.push_back(2.1); + EXPECT_EQ(joint.set_state(input, interfaces), return_type::INTERFACE_NOT_PROVIDED); + interfaces.push_back(hardware_interface::HW_IF_VELOCITY); + EXPECT_EQ(joint.set_state(input, interfaces), return_type::INTERFACE_NOT_FOUND); + interfaces.push_back(hardware_interface::HW_IF_POSITION); + EXPECT_EQ(joint.set_state(input, interfaces), return_type::INTERFACE_VALUE_SIZE_NOT_EQUAL); + interfaces.clear(); + interfaces.push_back(hardware_interface::HW_IF_POSITION); + input.clear(); + input.push_back(1.2); + EXPECT_EQ(joint.set_state(input, interfaces), return_type::OK); + + output.clear(); + interfaces.clear(); + EXPECT_EQ(joint.get_command(output, interfaces), return_type::INTERFACE_NOT_PROVIDED); + interfaces.push_back(hardware_interface::HW_IF_POSITION); + EXPECT_EQ(joint.get_state(output, interfaces), return_type::OK); + ASSERT_THAT(output, SizeIs(1)); + EXPECT_EQ(output[0], 1.2); + interfaces.clear(); + interfaces.push_back(hardware_interface::HW_IF_VELOCITY); + EXPECT_EQ(joint.get_state(output, interfaces), return_type::INTERFACE_NOT_FOUND); + + input.clear(); + EXPECT_EQ(joint.set_state(input), return_type::INTERFACE_VALUE_SIZE_NOT_EQUAL); + input.push_back(2.1); + EXPECT_EQ(joint.set_state(input), return_type::OK); + + EXPECT_EQ(joint.get_state(output), return_type::OK); + ASSERT_THAT(output, SizeIs(1)); + EXPECT_EQ(output[0], 2.1); + + // Test DummyPositionJoint + joint_info.command_interfaces.push_back(hardware_interface::HW_IF_POSITION); + joint_info.command_interfaces.push_back(hardware_interface::HW_IF_VELOCITY); + EXPECT_EQ(joint.configure(joint_info), return_type::ERROR); +} + +TEST_F(TestComponentInterfaces, multi_joint_example_component_works) +{ + DummyMultiJoint joint; + + joint_info.name = "DummyMultiJoint"; + joint_info.parameters["max_position"] = "3.14"; + joint_info.parameters["min_position"] = "-3.14"; + joint_info.parameters["max_velocity"] = "1.14"; + joint_info.parameters["min_velocity"] = "-1.14"; + + EXPECT_EQ(joint.configure(joint_info), return_type::ERROR); + + joint_info.command_interfaces.push_back(hardware_interface::HW_IF_POSITION); + joint_info.command_interfaces.push_back(hardware_interface::HW_IF_VELOCITY); + + EXPECT_EQ(joint.configure(joint_info), return_type::OK); + + ASSERT_THAT(joint.get_command_interfaces(), SizeIs(2)); + EXPECT_EQ(joint.get_command_interfaces()[0], hardware_interface::HW_IF_POSITION); + ASSERT_THAT(joint.get_state_interfaces(), SizeIs(0)); + + joint_info.state_interfaces.push_back(hardware_interface::HW_IF_POSITION); + joint_info.state_interfaces.push_back(hardware_interface::HW_IF_VELOCITY); + EXPECT_EQ(joint.configure(joint_info), return_type::OK); + ASSERT_THAT(joint.get_state_interfaces(), SizeIs(2)); + EXPECT_EQ(joint.get_command_interfaces()[1], hardware_interface::HW_IF_VELOCITY); + + // Command getters and setters + std::vector interfaces; + std::vector input; + input.push_back(2.1); + EXPECT_EQ(joint.set_command(input, interfaces), return_type::INTERFACE_NOT_PROVIDED); + interfaces.push_back(hardware_interface::HW_IF_EFFORT); + EXPECT_EQ(joint.set_command(input, interfaces), return_type::INTERFACE_NOT_FOUND); + interfaces.push_back(hardware_interface::HW_IF_VELOCITY); + EXPECT_EQ(joint.set_command(input, interfaces), return_type::INTERFACE_VALUE_SIZE_NOT_EQUAL); + interfaces.clear(); + interfaces.push_back(hardware_interface::HW_IF_VELOCITY); + input.clear(); + input.push_back(1.02); + EXPECT_EQ(joint.set_command(input, interfaces), return_type::OK); + + std::vector output; + EXPECT_EQ(joint.get_command(output, interfaces), return_type::OK); + ASSERT_THAT(output, SizeIs(1)); + EXPECT_EQ(output[0], 1.02); + interfaces.clear(); + interfaces.push_back(hardware_interface::HW_IF_EFFORT); + EXPECT_EQ(joint.get_command(output, interfaces), return_type::INTERFACE_NOT_FOUND); + + input.clear(); + EXPECT_EQ(joint.set_command(input), return_type::INTERFACE_VALUE_SIZE_NOT_EQUAL); + input.push_back(5.77); + EXPECT_EQ(joint.set_command(input), return_type::INTERFACE_VALUE_SIZE_NOT_EQUAL); + input.clear(); + input.push_back(1.2); + input.push_back(0.4); + EXPECT_EQ(joint.set_command(input), return_type::OK); + + EXPECT_EQ(joint.get_command(output), return_type::OK); + ASSERT_THAT(output, SizeIs(2)); + EXPECT_EQ(output[1], 0.4); + + // State getters and setters + interfaces.clear(); + input.clear(); + input.push_back(2.1); + EXPECT_EQ(joint.set_state(input, interfaces), return_type::INTERFACE_NOT_PROVIDED); + interfaces.push_back(hardware_interface::HW_IF_EFFORT); + EXPECT_EQ(joint.set_state(input, interfaces), return_type::INTERFACE_NOT_FOUND); + interfaces.push_back(hardware_interface::HW_IF_POSITION); + EXPECT_EQ(joint.set_state(input, interfaces), return_type::INTERFACE_VALUE_SIZE_NOT_EQUAL); + interfaces.clear(); + interfaces.push_back(hardware_interface::HW_IF_POSITION); + input.clear(); + input.push_back(1.2); + EXPECT_EQ(joint.set_state(input, interfaces), return_type::OK); + + output.clear(); + EXPECT_EQ(joint.get_state(output, interfaces), return_type::OK); + ASSERT_THAT(output, SizeIs(1)); + EXPECT_EQ(output[0], 1.2); + interfaces.clear(); + interfaces.push_back(hardware_interface::HW_IF_EFFORT); + EXPECT_EQ(joint.get_state(output, interfaces), return_type::INTERFACE_NOT_FOUND); + + input.clear(); + EXPECT_EQ(joint.set_state(input), return_type::INTERFACE_VALUE_SIZE_NOT_EQUAL); + input.push_back(2.1); + input.push_back(1.02); + EXPECT_EQ(joint.set_state(input), return_type::OK); + + EXPECT_EQ(joint.get_state(output), return_type::OK); + ASSERT_THAT(output, SizeIs(2)); + EXPECT_EQ(output[0], 2.1); +} + +TEST_F(TestComponentInterfaces, sensor_example_component_works) +{ + DummyForceTorqueSensor sensor; + + EXPECT_EQ(sensor.configure(sensor_info), return_type::OK); + ASSERT_THAT(sensor.get_state_interfaces(), SizeIs(6)); + EXPECT_EQ(sensor.get_state_interfaces()[0], "force_x"); + EXPECT_EQ(sensor.get_state_interfaces()[5], "torque_z"); + std::vector input = {5.23, 6.7, 2.5, 3.8, 8.9, 12.3}; + std::vector output; + std::vector interfaces; + EXPECT_EQ(sensor.get_state(output, interfaces), return_type::INTERFACE_NOT_PROVIDED); + interfaces.push_back("force_y"); + EXPECT_EQ(sensor.get_state(output, interfaces), return_type::OK); + ASSERT_THAT(output, SizeIs(1)); + EXPECT_EQ(output[0], 5.67); + + // State getters and setters + interfaces.clear(); + EXPECT_EQ(sensor.set_state(input, interfaces), return_type::INTERFACE_NOT_PROVIDED); + interfaces.push_back(hardware_interface::HW_IF_VELOCITY); + interfaces.push_back(hardware_interface::HW_IF_VELOCITY); + interfaces.push_back(hardware_interface::HW_IF_VELOCITY); + EXPECT_EQ(sensor.set_state(input, interfaces), return_type::INTERFACE_VALUE_SIZE_NOT_EQUAL); + interfaces.push_back(hardware_interface::HW_IF_VELOCITY); + interfaces.push_back(hardware_interface::HW_IF_VELOCITY); + interfaces.push_back(hardware_interface::HW_IF_VELOCITY); + EXPECT_EQ(sensor.set_state(input, interfaces), return_type::INTERFACE_NOT_FOUND); + interfaces.clear(); + interfaces = sensor.get_state_interfaces(); + EXPECT_EQ(sensor.set_state(input, interfaces), return_type::OK); + + output.clear(); + EXPECT_EQ(sensor.get_state(output, interfaces), return_type::OK); + ASSERT_THAT(output, SizeIs(6)); + EXPECT_EQ(output[0], 5.23); + interfaces.clear(); + interfaces.push_back(hardware_interface::HW_IF_VELOCITY); + EXPECT_EQ(sensor.get_state(output, interfaces), return_type::INTERFACE_NOT_FOUND); + + input.clear(); + EXPECT_EQ(sensor.set_state(input), return_type::INTERFACE_VALUE_SIZE_NOT_EQUAL); + input = {5.23, 6.7, 2.5, 3.8, 8.9, 12.3}; + EXPECT_EQ(sensor.set_state(input), return_type::OK); + + EXPECT_EQ(sensor.get_state(output), return_type::OK); + ASSERT_THAT(output, SizeIs(6)); + EXPECT_EQ(output[5], 12.3); + + sensor_info.parameters.clear(); + EXPECT_EQ(sensor.configure(sensor_info), return_type::ERROR); +} + +TEST_F(TestComponentInterfaces, actuator_hardware_interface_works) +{ + ActuatorHardware actuator_hw(std::make_unique()); + auto joint = std::make_shared(); + + HardwareInfo actuator_hw_info; + actuator_hw_info.name = "DummyActuatorHardware"; + actuator_hw_info.hardware_parameters["example_param_write_for_sec"] = "2"; + actuator_hw_info.hardware_parameters["example_param_read_for_sec"] = "3"; + + EXPECT_EQ(joint->configure(joint_info), return_type::OK); + + EXPECT_EQ(actuator_hw.configure(actuator_hw_info), return_type::OK); + EXPECT_EQ(actuator_hw.get_status(), hardware_interface_status::CONFIGURED); + EXPECT_EQ(actuator_hw.start(), return_type::OK); + EXPECT_EQ(actuator_hw.get_status(), hardware_interface_status::STARTED); + EXPECT_EQ(actuator_hw.read_joint(joint), return_type::OK); + std::vector interfaces = joint->get_state_interfaces(); + std::vector output; + EXPECT_EQ(joint->get_state(output, interfaces), return_type::OK); + ASSERT_THAT(output, SizeIs(1)); + EXPECT_EQ(output[0], 1.2); + EXPECT_EQ(interfaces[0], hardware_interface::HW_IF_POSITION); + EXPECT_EQ(actuator_hw.write_joint(joint), return_type::OK); + EXPECT_EQ(actuator_hw.stop(), return_type::OK); + EXPECT_EQ(actuator_hw.get_status(), hardware_interface_status::STOPPED); +} + +TEST_F(TestComponentInterfaces, sensor_interface_with_hardware_works) +{ + SensorHardware sensor_hw(std::make_unique()); + auto sensor = std::make_shared(); + + HardwareInfo sensor_hw_info; + sensor_hw_info.name = "DummySensor"; + sensor_hw_info.hardware_parameters["binary_to_voltage_factor"] = "0.0048828125"; + + EXPECT_EQ(sensor->configure(sensor_info), return_type::OK); + + EXPECT_EQ(sensor_hw.configure(sensor_hw_info), return_type::OK); + EXPECT_EQ(sensor_hw.get_status(), hardware_interface_status::CONFIGURED); + EXPECT_EQ(sensor_hw.start(), return_type::OK); + EXPECT_EQ(sensor_hw.get_status(), hardware_interface_status::STARTED); + EXPECT_EQ(sensor_hw.read_sensor(sensor), return_type::OK); + std::vector output; + std::vector interfaces = sensor->get_state_interfaces(); + EXPECT_EQ(sensor->get_state(output, interfaces), return_type::OK); + EXPECT_EQ(output[2], 3.4); + ASSERT_THAT(interfaces, SizeIs(6)); + EXPECT_EQ(interfaces[1], "force_y"); + EXPECT_EQ(sensor_hw.stop(), return_type::OK); + EXPECT_EQ(sensor_hw.get_status(), hardware_interface_status::STOPPED); + EXPECT_EQ(sensor_hw.start(), return_type::OK); +} + +TEST_F(TestComponentInterfaces, system_interface_with_hardware_works) +{ + SystemHardware system(std::make_unique()); + auto joint1 = std::make_shared(); + auto joint2 = std::make_shared(); + std::vector> joints; + joints.push_back(joint1); + joints.push_back(joint2); + + std::shared_ptr sensor(std::make_shared()); + std::vector> sensors; + sensors.push_back(sensor); + + EXPECT_EQ(joint1->configure(joint_info), return_type::OK); + EXPECT_EQ(joint2->configure(joint_info), return_type::OK); + EXPECT_EQ(sensor->configure(sensor_info), return_type::OK); + + HardwareInfo system_hw_info; + system_hw_info.name = "DummyActuatorHardware"; + system_hw_info.hardware_parameters["example_api_version"] = "1.1"; + system_hw_info.hardware_parameters["example_param_write_for_sec"] = "2"; + system_hw_info.hardware_parameters["example_param_read_for_sec"] = "3"; + + EXPECT_EQ(system.configure(system_hw_info), return_type::OK); + EXPECT_EQ(system.get_status(), hardware_interface_status::CONFIGURED); + EXPECT_EQ(system.start(), return_type::OK); + EXPECT_EQ(system.get_status(), hardware_interface_status::STARTED); + + EXPECT_EQ(system.read_sensors(sensors), return_type::OK); + std::vector output; + std::vector interfaces = sensor->get_state_interfaces(); + EXPECT_EQ(sensor->get_state(output, interfaces), return_type::OK); + ASSERT_THAT(output, SizeIs(6)); + EXPECT_EQ(output[2], -8.7); + ASSERT_THAT(interfaces, SizeIs(6)); + EXPECT_EQ(interfaces[4], "torque_y"); + output.clear(); + interfaces.clear(); + + EXPECT_EQ(system.read_joints(joints), return_type::OK); + interfaces = joint1->get_command_interfaces(); + EXPECT_EQ(joint1->get_state(output, interfaces), return_type::OK); + ASSERT_THAT(output, SizeIs(1)); + EXPECT_EQ(output[0], -1.575); + ASSERT_THAT(interfaces, SizeIs(1)); + EXPECT_EQ(interfaces[0], hardware_interface::HW_IF_POSITION); + output.clear(); + interfaces.clear(); + interfaces = joint2->get_state_interfaces(); + EXPECT_EQ(joint2->get_state(output, interfaces), return_type::OK); + ASSERT_THAT(output, SizeIs(1)); + EXPECT_EQ(output[0], -0.7543); + ASSERT_THAT(interfaces, SizeIs(1)); + EXPECT_EQ(interfaces[0], hardware_interface::HW_IF_POSITION); + + EXPECT_EQ(system.write_joints(joints), return_type::OK); + + EXPECT_EQ(system.stop(), return_type::OK); + EXPECT_EQ(system.get_status(), hardware_interface_status::STOPPED); +} diff --git a/hardware_interface/test/test_macros.cpp b/hardware_interface/test/test_macros.cpp index b18ad8ab0e..9997fa303e 100644 --- a/hardware_interface/test/test_macros.cpp +++ b/hardware_interface/test/test_macros.cpp @@ -32,6 +32,7 @@ class A TEST_F(TestMacros, throw_on_null) { int * i_ptr = nullptr; + // cppcheck-suppress unknownMacro EXPECT_ANY_THROW(THROW_ON_NULLPTR(i_ptr)); A * a_ptr = nullptr;