From ceb4d67601338356bef1748c049ba5a0a42bfdac Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Fri, 12 Jun 2020 11:13:20 +0200 Subject: [PATCH 01/29] Initial version of new package implementing components for controling the robot. --- robot_control_components/CMakeLists.txt | 75 +++++++++ .../robot_control_components/actuator.hpp | 46 ++++++ .../robot_control_components/component.hpp | 142 ++++++++++++++++++ .../robot_control_components/robot.hpp | 47 ++++++ .../ros2_control_types.h | 62 ++++++++ .../ros2_control_utils.hpp | 69 +++++++++ .../robot_control_components/sensor.hpp | 44 ++++++ .../simple_component.hpp | 70 +++++++++ .../visibility_control.h | 64 ++++++++ robot_control_components/package.xml | 21 +++ robot_control_components/src/actuator.cpp | 22 +++ robot_control_components/src/robot.cpp | 22 +++ robot_control_components/src/sensor.cpp | 22 +++ robot_control_components/test/test_dummy.cpp | 23 +++ 14 files changed, 729 insertions(+) create mode 100644 robot_control_components/CMakeLists.txt create mode 100644 robot_control_components/include/robot_control_components/actuator.hpp create mode 100644 robot_control_components/include/robot_control_components/component.hpp create mode 100644 robot_control_components/include/robot_control_components/robot.hpp create mode 100644 robot_control_components/include/robot_control_components/ros2_control_types.h create mode 100644 robot_control_components/include/robot_control_components/ros2_control_utils.hpp create mode 100644 robot_control_components/include/robot_control_components/sensor.hpp create mode 100644 robot_control_components/include/robot_control_components/simple_component.hpp create mode 100644 robot_control_components/include/robot_control_components/visibility_control.h create mode 100644 robot_control_components/package.xml create mode 100644 robot_control_components/src/actuator.cpp create mode 100644 robot_control_components/src/robot.cpp create mode 100644 robot_control_components/src/sensor.cpp create mode 100644 robot_control_components/test/test_dummy.cpp diff --git a/robot_control_components/CMakeLists.txt b/robot_control_components/CMakeLists.txt new file mode 100644 index 0000000000..d0ab4c445d --- /dev/null +++ b/robot_control_components/CMakeLists.txt @@ -0,0 +1,75 @@ +cmake_minimum_required(VERSION 3.5) +project(robot_control_components) + +# Default to C99 +if(NOT CMAKE_C_STANDARD) + set(CMAKE_C_STANDARD 99) +endif() + +# Default to C++14 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) + +find_package(control_msgs REQUIRED) +find_package(pluginlib REQUIRED) +find_package(rclcpp REQUIRED) + + +## COMPILE +add_library(${PROJECT_NAME} SHARED + src/actuator.cpp + src/robot.cpp + src/sensor.cpp +) + +target_include_directories(${PROJECT_NAME} + PRIVATE include +) + +ament_target_dependencies(${PROJECT_NAME} + control_msgs + pluginlib + rclcpp +) + +## INSTALL +install(TARGETS ${PROJECT_NAME} + DESTINATION lib +) + +install(DIRECTORY include/ + DESTINATION include +) + +## TESTS +if(BUILD_TESTING) + find_package(ament_cmake_gtest REQUIRED) + + ament_add_gtest(test_dummy test/test_dummy.cpp) + target_include_directories(test_dummy PRIVATE include) +# ament_target_dependencies(test_dummy rcpputils) + + + # Uncomment this before creating a new PR to check code quality +# find_package(ament_lint_auto REQUIRED) +# ament_lint_auto_find_test_dependencies() +endif() + +## EXPORTS +ament_export_dependencies( + control_msgs + pluginlib + rclcpp +) +ament_export_libraries( + ${PROJECT_NAME} +) +ament_export_include_directories( +include +) +ament_package() + diff --git a/robot_control_components/include/robot_control_components/actuator.hpp b/robot_control_components/include/robot_control_components/actuator.hpp new file mode 100644 index 0000000000..a89c839651 --- /dev/null +++ b/robot_control_components/include/robot_control_components/actuator.hpp @@ -0,0 +1,46 @@ +// 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 ROBOT_CONTROL_COMPONENTS__ACTUATOR_HPP_ +#define ROBOT_CONTROL_COMPONENTS__ACTUATOR_HPP_ + +#include +#include + +#include "control_msgs/msg/interface_value.hpp" + +#include "rclcpp/macros.hpp" +#include "rclcpp/rclcpp.hpp" + +#include "robot_control_components/visibility_control.h" + + +namespace robot_control_components +{ + +class Actuator +{ +public: + RCLCPP_SHARED_PTR_DEFINITIONS(Actuator) + + ROS2_CONTROL_CORE_PUBLIC Actuator() = default; + + ROS2_CONTROL_CORE_PUBLIC virtual ~Actuator() = default; + +}; + +} // namespace robot_control_components + +#endif // ROBOT_CONTROL_COMPONENTS__ACTUATOR_HPP_ diff --git a/robot_control_components/include/robot_control_components/component.hpp b/robot_control_components/include/robot_control_components/component.hpp new file mode 100644 index 0000000000..6b5abe394f --- /dev/null +++ b/robot_control_components/include/robot_control_components/component.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 ROBOT_CONTROL_COMPONENTS__COMPONENTS_BASE_COMPONENT_HPP_ +#define ROBOT_CONTROL_COMPONENTS__COMPONENTS_BASE_COMPONENT_HPP_ + +#include + +#include "rclcpp/macros.hpp" +#include "rclcpp/rclcpp.hpp" + +#include "robot_control_components/hardware/component_hardware.hpp" + +#include "robot_control_components/ros2_control_types.h" +#include "robot_control_components/ros2_control_utils.hpp" +#include "robot_control_components/visibility_control.h" + + +namespace robot_control_components +{ + +template < typename ComponentHardwareType > +class Component +{ +public: + RCLCPP_SHARED_PTR_DEFINITIONS(Component) + + ROS2_CONTROL_CORE_PUBLIC Component() = default; + + ROS2_CONTROL_CORE_PUBLIC Component(std::string parameters_path, std::string type, const rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr logging_interface, const rclcpp::node_interfaces::NodeParametersInterface::SharedPtr parameters_interface, const rclcpp::node_interfaces::NodeServicesInterface::SharedPtr services_interface) + { + configure(parameters_path, type, logging_interface, parameters_interface, services_interface); + }; + + ROS2_CONTROL_CORE_PUBLIC virtual ~Component() = default; + + ros2_control_types::return_type virtual configure(std::string parameters_path, const rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr logging_interface, const rclcpp::node_interfaces::NodeParametersInterface::SharedPtr parameters_interface, const rclcpp::node_interfaces::NodeServicesInterface::SharedPtr services_interface) = 0; + + ros2_control_types::return_type configure(std::string parameters_path, std::string type, const rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr logging_interface, const rclcpp::node_interfaces::NodeParametersInterface::SharedPtr parameters_interface, const rclcpp::node_interfaces::NodeServicesInterface::SharedPtr services_interface) + { + parameters_path_ = parameters_path; + type_ = type; + logging_interface_ = logging_interface; + parameters_interface_ = parameters_interface; + services_interface_ = services_interface; + + parameters_interface_->declare_parameter(parameters_path_ + ".name"); + name_ = parameters_interface_->get_parameter(parameters_path_ + ".name").as_string(); + + parameters_interface_->declare_parameter(parameters_path_ + ".has_hardware", rclcpp::ParameterValue(false)); + has_hardware_ = parameters_interface_->get_parameter(parameters_path_ + ".has_hardware").as_bool(); + + RCLCPP_INFO(logging_interface_->get_logger(), "%s - %s: called configure!", type_.c_str(), name_.c_str()); + + return ros2_control_types::ROS2C_RETURN_OK; + }; + + ros2_control_types::return_type init() + { + ros2_control_types::return_type ret = ros2_control_types::ROS2C_RETURN_OK; + if (has_hardware_) + { + //FIXME:DEBUG + RCLCPP_INFO(logging_interface_->get_logger(), "'%s' calling hardware init.", name_.c_str()); + ret = hardware_->init(); + } + else + { + //FIXME:DEBUG + RCLCPP_INFO(logging_interface_->get_logger(), "'%s' component is initalized without hardware.", name_.c_str()); + } + return ret; + }; + + // ROS2_CONTROL_CORE_PUBLIC virtual ros2_control_types::return_type recover() = 0; + + // ROS2_CONTROL_CORE_PUBLIC virtual ros2_control_types::return_type stop() = 0; + + std::string name_; + +protected: + /** + * @brief Components parameter prefix. + * + */ + std::string parameters_path_; + std::string type_; + rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr logging_interface_; + rclcpp::node_interfaces::NodeParametersInterface::SharedPtr parameters_interface_; + rclcpp::node_interfaces::NodeServicesInterface::SharedPtr services_interface_; + + bool has_hardware_; + + uint n_dof_; +// ros2_control_types::component_state_type state_ = 0; + + std::shared_ptr hardware_; + + template + ros2_control_types::return_type load_hardware(ros2_control_utils::ROS2ControlLoaderPluginlib class_loader) + { + hardware_ = ros2_control_utils::load_component_from_parameter(parameters_path_ + "." + type_ + "Hardware.type", parameters_interface_, class_loader, logging_interface_->get_logger()); + + if (!hardware_) + { + RCLCPP_FATAL(logging_interface_->get_logger(), "%s: '%sHardware has to be defined if 'has_hardware=True'!", type_.c_str(), name_.c_str()); + return ros2_control_types::ROS2C_RETURN_ERROR; + } + return ros2_control_types::ROS2C_RETURN_OK; + }; + + template + ros2_control_types::return_type load_and_configure_hardware(ros2_control_utils::ROS2ControlLoaderPluginlib class_loader) + { + if (load_hardware(class_loader) == ros2_control_types::ROS2C_RETURN_OK) + { + hardware_->configure(parameters_path_ + "." + type_ + "Hardware", logging_interface_, parameters_interface_, services_interface_); + // FIXME: Make this general for all components - currently only for robot +// hardware_.set_components_for_data(); + return ros2_control_types::ROS2C_RETURN_OK; + } + return ros2_control_types::ROS2C_RETURN_ERROR; + }; + + +}; + +} // namespace robot_control_components + +#endif // ROBOT_CONTROL_COMPONENTS__COMPONENTS_BASE_COMPONENT_HPP_ diff --git a/robot_control_components/include/robot_control_components/robot.hpp b/robot_control_components/include/robot_control_components/robot.hpp new file mode 100644 index 0000000000..861110220f --- /dev/null +++ b/robot_control_components/include/robot_control_components/robot.hpp @@ -0,0 +1,47 @@ +// 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 ROBOT_CONTROL_COMPONENTS__ROBOT_HPP_ +#define ROBOT_CONTROL_COMPONENTS__ROBOT_HPP_ + +#include +#include + +#include "control_msgs/msg/dynamic_joint_state.hpp" +#include "control_msgs/msg/interface_value.hpp" + +#include "rclcpp/macros.hpp" +#include "rclcpp/rclcpp.hpp" + +#include "robot_control_components/visibility_control.h" + + +namespace robot_control_components +{ + +class Robot +{ +public: + RCLCPP_SHARED_PTR_DEFINITIONS(Robot) + + ROS2_CONTROL_CORE_PUBLIC Robot() = default; + + ROS2_CONTROL_CORE_PUBLIC ~Robot() = default; + +}; + +} // namespace robot_control_components + +#endif // ROBOT_CONTROL_COMPONENTS__ROBOT_HPP_ diff --git a/robot_control_components/include/robot_control_components/ros2_control_types.h b/robot_control_components/include/robot_control_components/ros2_control_types.h new file mode 100644 index 0000000000..0c5a9a1cab --- /dev/null +++ b/robot_control_components/include/robot_control_components/ros2_control_types.h @@ -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. + +/* This header must be included by all rclcpp headers which declare symbols + * which are defined in the rclcpp library. When not building the rclcpp + * library, i.e. when using the headers in other package's code, the contents + * of this header change the visibility of certain symbols which the rclcpp + * library cannot have, but the consuming code must have inorder to link. + */ + +#ifndef ROBOT_CONTROL_COMPONENTS__ROS2_CONTROL_TYPES_H_ +#define ROBOT_CONTROL_COMPONENTS__ROS2_CONTROL_TYPES_H_ + +#include +#include + +#include "rclcpp/rclcpp.hpp" + +#include "robot_control_components/visibility_control.h" + +#include "control_msgs/msg/interface_value.hpp" + + +// TODO: Do we need "ROS2_CONTROL_CORE_PUBLIC" before the variables? + +namespace robot_control_components +{ + +using components_ret_t = int; +static constexpr components_ret_t ROS2C_RETURN_OK = 0; +static constexpr components_ret_t ROS2C_RETURN_ERROR = 1; + +static constexpr components_ret_t ROS2C_RETURN_ACTUATOR_CLAIMED_ERROR = 10; +static constexpr components_ret_t ROS2C_RETURN_ACTUATOR_ALREADY_CLAIMED = 11; +static constexpr components_ret_t ROS2C_RETURN_ACTUATOR_NOT_CLAIMED = 11; +static constexpr components_ret_t ROS2C_RETURN_ACTUATOR_UNATHORIZED_UNCLAIM = 13; +static constexpr components_ret_t ROS2C_RETURN_ACTUATOR_CAN_NOT_READ = 20; + +// TODO: Check if needed or lifecycle component can do this +// using component_state_type = uint; +// static constexpr component_state_type ROS2C_COMPONENT_STATE_LOADED = 1; +// static constexpr component_state_type ROS2C_COMPONENT_STATE_CONFIGURED = 2; +// static constexpr component_state_type ROS2C_COMPONENT_STATE_INITIALIZED = 3; +// static constexpr component_state_type ROS2C_COMPONENT_STATE_STARTED = 4; +// static constexpr component_state_type ROS2C_COMPONENT_STATE_STOPPED = 5; +// static constexpr component_state_type ROS2C_COMPONENT_STATE_HALTED = 6; + + +} // namespace robot_control_components + +#endif // ROBOT_CONTROL_COMPONENTS__ROS2_CONTROL_TYPES_H_ diff --git a/robot_control_components/include/robot_control_components/ros2_control_utils.hpp b/robot_control_components/include/robot_control_components/ros2_control_utils.hpp new file mode 100644 index 0000000000..b00dee7f78 --- /dev/null +++ b/robot_control_components/include/robot_control_components/ros2_control_utils.hpp @@ -0,0 +1,69 @@ +// 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. + +/* This header must be included by all rclcpp headers which declare symbols + * which are defined in the rclcpp library. When not building the rclcpp + * library, i.e. when using the headers in other package's code, the contents + * of this header change the visibility of certain symbols which the rclcpp + * library cannot have, but the consuming code must have inorder to link. + */ + +#ifndef ROBOT_CONTROL_COMPONENTS__ROS2_CONTROL_UTILS_H_ +#define ROBOT_CONTROL_COMPONENTS__ROS2_CONTROL_UTILS_H_ + +#include +#include + +#include "pluginlib/class_loader.hpp" +#include "rclcpp/rclcpp.hpp" +#include "robot_control_components/visibility_control.h" + +// TODO: Create util library instead of inline functions? + +namespace ros2_control_utils +{ +// Classes +template < typename T > +class ROS2ControlLoaderPluginlib +{ +public: + ROS2_CONTROL_CORE_PUBLIC ROS2ControlLoaderPluginlib(const std::string package, const std::string base_type) : loader_(std::make_shared>(package, base_type)) + { + }; + + ROS2_CONTROL_CORE_PUBLIC + virtual ~ROS2ControlLoaderPluginlib() = default; + + // TODO: Add try-catch + ROS2_CONTROL_CORE_PUBLIC + std::shared_ptr create(const std::string & type) + { + return std::shared_ptr(loader_->createUnmanagedInstance(type)); + }; + + ROS2_CONTROL_CORE_PUBLIC + bool is_available(const std::string & type) + { + return loader_->isClassAvailable(type); + }; + +private: + std::shared_ptr> loader_; +}; + + + +} // namespace ros2_control_utils + +#endif // ROBOT_CONTROL_COMPONENTS__ROS2_CONTROL_UTILS_H_ diff --git a/robot_control_components/include/robot_control_components/sensor.hpp b/robot_control_components/include/robot_control_components/sensor.hpp new file mode 100644 index 0000000000..5a325e1bfd --- /dev/null +++ b/robot_control_components/include/robot_control_components/sensor.hpp @@ -0,0 +1,44 @@ +// 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 ROBOT_CONTROL_COMPONENTS__SENSOR_HPP_ +#define ROBOT_CONTROL_COMPONENTS__SENSOR_HPP_ + +#include + +#include "control_msgs/msg/interface_value.hpp" + +#include "rclcpp/macros.hpp" +#include "rclcpp/rclcpp.hpp" + +#include "robot_control_components/visibility_control.h" + +namespace robot_control_components +{ + +class Sensor +{ +public: + RCLCPP_SHARED_PTR_DEFINITIONS(Sensor); + + ROS2_CONTROL_CORE_PUBLIC Sensor() = default; + + ROS2_CONTROL_CORE_PUBLIC virtual ~Sensor() = default; + +}; + +} // namespace robot_control_components + +#endif // ROBOT_CONTROL_COMPONENTS__SENSOR_HPP_ diff --git a/robot_control_components/include/robot_control_components/simple_component.hpp b/robot_control_components/include/robot_control_components/simple_component.hpp new file mode 100644 index 0000000000..c7109b7199 --- /dev/null +++ b/robot_control_components/include/robot_control_components/simple_component.hpp @@ -0,0 +1,70 @@ +// 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 ROBOT_CONTROL_COMPONENTS__COMPONENTS_COMPONENT_HPP_ +#define ROBOT_CONTROL_COMPONENTS__COMPONENTS_COMPONENT_HPP_ + +#include +#include +#include + +#include "control_msgs/msg/interface_value.hpp" + +#include "robot_control_components/components/component.hpp" + +#include "robot_control_components/ros2_control_types.h" +#include "robot_control_components/visibility_control.h" + + +namespace robot_control_components +{ + +template < typename ComponentHardwareType > +class SimpleComponent : public Component< ComponentHardwareType > +{ +public: + ROS2_CONTROL_CORE_PUBLIC SimpleComponent() = default; + + ROS2_CONTROL_CORE_PUBLIC virtual ~SimpleComponent() = default; + + ROS2_CONTROL_CORE_PUBLIC ros2_control_types::return_type configure(const std::string parameters_path, const std::string type, const rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr logging_interface, const rclcpp::node_interfaces::NodeParametersInterface::SharedPtr parameters_interface, const rclcpp::node_interfaces::NodeServicesInterface::SharedPtr services_interface) + { + ros2_control_types::return_type ret = ros2_control_types::ROS2C_RETURN_OK; + ret = Component< ComponentHardwareType >::configure(parameters_path, type, logging_interface, parameters_interface, services_interface); + + parameters_interface->declare_parameter(parameters_path + ".interface_names"); + parameters_interface->declare_parameter(parameters_path + ".n_dof", rclcpp::ParameterValue(1)); + parameters_interface->declare_parameter(parameters_path + ".min_values"); + parameters_interface->declare_parameter(parameters_path + ".max_values"); + + return ret; + }; + + //TODO: this is simplest implementation. We need to do this more complex to support complex configurations of robots + std::vector get_interface_names() + { + return interface_names_; + } + +protected: + std::vector valid_interface_names_; + //TODO: should we use set for easier search? + std::vector interface_names_; + control_msgs::msg::InterfaceValue read_values_; +}; + +} // namespace robot_control_components + +#endif // ROBOT_CONTROL_COMPONENTS__COMPONENTS_COMPONENT_HPP_ diff --git a/robot_control_components/include/robot_control_components/visibility_control.h b/robot_control_components/include/robot_control_components/visibility_control.h new file mode 100644 index 0000000000..9331ef1117 --- /dev/null +++ b/robot_control_components/include/robot_control_components/visibility_control.h @@ -0,0 +1,64 @@ +// 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. + +/* This header must be included by all rclcpp headers which declare symbols + * which are defined in the rclcpp library. When not building the rclcpp + * library, i.e. when using the headers in other package's code, the contents + * of this header change the visibility of certain symbols which the rclcpp + * library cannot have, but the consuming code must have inorder to link. + */ + +#ifndef ROBOT_CONTROL_COMPONENTS__VISIBILITY_CONTROL_H_ +#define ROBOT_CONTROL_COMPONENTS__VISIBILITY_CONTROL_H_ + +// This logic was borrowed (then namespaced) from the examples on the gcc wiki: +// https://gcc.gnu.org/wiki/Visibility + +#if defined _WIN32 || defined __CYGWIN__ +#ifdef __GNUC__ +#define ROS2_CONTROL_CORE_EXPORT \ + __attribute__((dllexport)) +#define ROS2_CONTROL_CORE_IMPORT \ + __attribute__((dllimport)) +#else +#define ROS2_CONTROL_CORE_EXPORT __declspec(dllexport) +#define ROS2_CONTROL_CORE_IMPORT __declspec(dllimport) +#endif +#ifdef ROS2_CONTROL_CORE_BUILDING_DLL +#define ROS2_CONTROL_CORE_PUBLIC \ + ROS2_CONTROL_CORE_EXPORT +#else +#define ROS2_CONTROL_CORE_PUBLIC \ + ROS2_CONTROL_CORE_IMPORT +#endif +#define ROS2_CONTROL_CORE_PUBLIC_TYPE \ + ROS2_CONTROL_CORE_PUBLIC +#define ROS2_CONTROL_CORE_LOCAL +#else +#define ROS2_CONTROL_CORE_EXPORT \ + __attribute__((visibility("default"))) +#define ROS2_CONTROL_CORE_IMPORT +#if __GNUC__ >= 4 +#define ROS2_CONTROL_CORE_PUBLIC \ + __attribute__((visibility("default"))) +#define ROS2_CONTROL_CORE_LOCAL \ + __attribute__((visibility("hidden"))) +#else +#define ROS2_CONTROL_CORE_PUBLIC +#define ROS2_CONTROL_CORE_LOCAL +#endif +#define ROS2_CONTROL_CORE_PUBLIC_TYPE +#endif + +#endif // ROBOT_CONTROL_COMPONENTS__VISIBILITY_CONTROL_H_ diff --git a/robot_control_components/package.xml b/robot_control_components/package.xml new file mode 100644 index 0000000000..4031b47f66 --- /dev/null +++ b/robot_control_components/package.xml @@ -0,0 +1,21 @@ + + + + robot_control_components + 0.0.0 + The main package for `ros2_control`-concept testing. The package implements the most important classes and tests them with `demo_robot` to enable functionality and test driven development. + stogl + TODO: License declaration + + Denis Štogl + + control_msgs + pluginlib + rclcpp + + ament_cmake_gtest + + + ament_cmake + + diff --git a/robot_control_components/src/actuator.cpp b/robot_control_components/src/actuator.cpp new file mode 100644 index 0000000000..c3a95f6076 --- /dev/null +++ b/robot_control_components/src/actuator.cpp @@ -0,0 +1,22 @@ +// 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 "robot_control_components/actuator.hpp" + +namespace robot_control_components +{ + + + +} // namespace robot_control_components diff --git a/robot_control_components/src/robot.cpp b/robot_control_components/src/robot.cpp new file mode 100644 index 0000000000..c57743feda --- /dev/null +++ b/robot_control_components/src/robot.cpp @@ -0,0 +1,22 @@ +// 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 "robot_control_components/robot.hpp" + +namespace robot_control_components +{ + + + +} // namespace robot_control_components diff --git a/robot_control_components/src/sensor.cpp b/robot_control_components/src/sensor.cpp new file mode 100644 index 0000000000..c9f920dcfe --- /dev/null +++ b/robot_control_components/src/sensor.cpp @@ -0,0 +1,22 @@ +// 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 "robot_control_components/sensor.hpp" + +namespace robot_control_components +{ + + + +} // namespace robot_control_components diff --git a/robot_control_components/test/test_dummy.cpp b/robot_control_components/test/test_dummy.cpp new file mode 100644 index 0000000000..13b68d810e --- /dev/null +++ b/robot_control_components/test/test_dummy.cpp @@ -0,0 +1,23 @@ +// Copyright 2017 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +class TestDummy : public ::testing::Test +{ +protected: + static void SetUpTestCase() + { + } +}; From 3ceb98cd8615c6a515ce486e1250e2ff43b2019b Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Tue, 16 Jun 2020 17:29:36 +0200 Subject: [PATCH 02/29] Add control component interfaces. --- hardware_interface/CMakeLists.txt | 14 +- .../hardware_interface/component_info.hpp | 103 +++++++++++++ .../component_interfaces/joint_interface.hpp | 70 +++++++++ .../component_interfaces/sensor_interface.hpp | 63 ++++++++ .../component_interfaces/system_interface.hpp | 70 +++++++++ .../include/hardware_interface/joint.hpp | 89 +++++++++++ .../include/hardware_interface/sensor.hpp | 77 ++++++++++ .../include/hardware_interface/system.hpp | 88 +++++++++++ .../types/hardware_interface_type_values.hpp | 15 +- robot_control_components/CMakeLists.txt | 75 --------- .../robot_control_components/actuator.hpp | 46 ------ .../robot_control_components/component.hpp | 142 ------------------ .../robot_control_components/robot.hpp | 47 ------ .../ros2_control_types.h | 62 -------- .../ros2_control_utils.hpp | 69 --------- .../robot_control_components/sensor.hpp | 44 ------ .../simple_component.hpp | 70 --------- .../visibility_control.h | 64 -------- robot_control_components/package.xml | 21 --- robot_control_components/src/robot.cpp | 22 --- robot_control_components/src/sensor.cpp | 22 --- robot_control_components/test/test_dummy.cpp | 23 --- 22 files changed, 579 insertions(+), 717 deletions(-) create mode 100644 hardware_interface/include/hardware_interface/component_info.hpp create mode 100644 hardware_interface/include/hardware_interface/component_interfaces/joint_interface.hpp create mode 100644 hardware_interface/include/hardware_interface/component_interfaces/sensor_interface.hpp create mode 100644 hardware_interface/include/hardware_interface/component_interfaces/system_interface.hpp create mode 100644 hardware_interface/include/hardware_interface/joint.hpp create mode 100644 hardware_interface/include/hardware_interface/sensor.hpp create mode 100644 hardware_interface/include/hardware_interface/system.hpp rename robot_control_components/src/actuator.cpp => hardware_interface/include/hardware_interface/types/hardware_interface_type_values.hpp (54%) delete mode 100644 robot_control_components/CMakeLists.txt delete mode 100644 robot_control_components/include/robot_control_components/actuator.hpp delete mode 100644 robot_control_components/include/robot_control_components/component.hpp delete mode 100644 robot_control_components/include/robot_control_components/robot.hpp delete mode 100644 robot_control_components/include/robot_control_components/ros2_control_types.h delete mode 100644 robot_control_components/include/robot_control_components/ros2_control_utils.hpp delete mode 100644 robot_control_components/include/robot_control_components/sensor.hpp delete mode 100644 robot_control_components/include/robot_control_components/simple_component.hpp delete mode 100644 robot_control_components/include/robot_control_components/visibility_control.h delete mode 100644 robot_control_components/package.xml delete mode 100644 robot_control_components/src/robot.cpp delete mode 100644 robot_control_components/src/sensor.cpp delete mode 100644 robot_control_components/test/test_dummy.cpp diff --git a/hardware_interface/CMakeLists.txt b/hardware_interface/CMakeLists.txt index 04763b5e96..6554107797 100644 --- a/hardware_interface/CMakeLists.txt +++ b/hardware_interface/CMakeLists.txt @@ -26,14 +26,14 @@ add_library( 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") @@ -76,11 +76,17 @@ if(BUILD_TESTING) endif() +ament_export_dependencies( + rclcpp + rcpputils +) ament_export_include_directories( include ) ament_export_libraries( - hardware_interface) + hardware_interface +) ament_export_dependencies( - control_msgs) + control_msgs +) ament_package() diff --git a/hardware_interface/include/hardware_interface/component_info.hpp b/hardware_interface/include/hardware_interface/component_info.hpp new file mode 100644 index 0000000000..a1d82170be --- /dev/null +++ b/hardware_interface/include/hardware_interface/component_info.hpp @@ -0,0 +1,103 @@ +// 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__COMPONENT_INFO_HPP_ +#define HARDWARE_INTERFACE__COMPONENT_INFO_HPP_ + +#include +#include +#include + +namespace hardware_interface +{ + +/** + * \brief constants for types of components. + */ +constexpr const auto robotType = "robot"; +constexpr const auto actuatorType = "actuator"; +constexpr const auto sensorType = "sensor"; + +/** + * \brief This structure stores information about components defined in a 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 joint where component is placed. + */ + std::string joint; + /** + * \brief name of the interface, e.g. "position", "velocity", etc. for meaning of data this component holds. + */ + std::vector interface_names; + /** + * \brief (optional) key-value pairs of components parameters. + */ + std::unordered_map parameters; + + /** + * \brief (optional) hardware class of the component that will be dynamically loaded. If not defined, the system's hardware class has to be defined. + */ + std::string hardware_class_type; + /** + * \brief (optional) key-value pairs for components hardware. + */ + std::unordered_map hardware_parameters; +}; + +/** + * \brief This structure stores informations about system defined in robot's URDF, i.e. "ros2_control"-tag. + */ +struct SystemInfo +{ + /** + * \brief name of the system. + */ + std::string name; + /** + * \brief type of the system: robot, actuator or sensor. Note: URDF always needs a "robot" tag, nevertheless in terms of ros2_control, it can hold a definition for an actuator or sensor. + */ + std::string type; + /** + * \brief (optional) hardware class of the system, which will be dynamically loaded. If not defined, a hardware class for each subcomponent has to be defined. + */ + std::string hardware_class_type; + /** + * \brief (optional) key-value pairs for systems hardware. + */ + std::unordered_map hardware_parameters; + + /** + * \brief list of subcomponents in the system, i.e., list of sensors and actuators. + */ + std::vector subcomponents; +}; + +} // namespace hardware_interface + +#endif // HARDWARE_INTERFACE__COMPONENT_INFO_HPP_ diff --git a/hardware_interface/include/hardware_interface/component_interfaces/joint_interface.hpp b/hardware_interface/include/hardware_interface/component_interfaces/joint_interface.hpp new file mode 100644 index 0000000000..e40f97c797 --- /dev/null +++ b/hardware_interface/include/hardware_interface/component_interfaces/joint_interface.hpp @@ -0,0 +1,70 @@ +// 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__COMPONENT_INTERFACES__JOINT_INTERFACE_HPP_ +#define HARDWARE_INTERFACE__COMPONENT_INTERFACES__JOINT_INTERFACE_HPP_ + +#include +#include +#include +#include + +#include "hardware_interface/component_info.hpp" +#include "hardware_interface/types/hardware_interface_return_values.hpp" +#include "hardware_interface/visibility_control.h" + +namespace hardware_interface +{ + +class JointInterface +{ +public: + HARDWARE_INTERFACE_PUBLIC + JointInterface() = default; + + HARDWARE_INTERFACE_PUBLIC + virtual + ~JointInterface() = default; + + HARDWARE_INTERFACE_PUBLIC + virtual + return_type configure(const ComponentInfo & actuator_info) = 0; + + HARDWARE_INTERFACE_PUBLIC + virtual + std::string get_interface_name() const = 0; + + HARDWARE_INTERFACE_PUBLIC + virtual + return_type start() = 0; + + HARDWARE_INTERFACE_PUBLIC + virtual + return_type stop() = 0; + + HARDWARE_INTERFACE_PUBLIC + virtual + bool is_started() const = 0; + + HARDWARE_INTERFACE_PUBLIC + virtual + return_type read(double & data) = 0; + + HARDWARE_INTERFACE_PUBLIC + virtual + return_type write(const double & data) = 0; +}; + +} // namespace hardware_interface +#endif // HARDWARE_INTERFACE__COMPONENT_INTERFACES__JOINT_INTERFACE_HPP_ diff --git a/hardware_interface/include/hardware_interface/component_interfaces/sensor_interface.hpp b/hardware_interface/include/hardware_interface/component_interfaces/sensor_interface.hpp new file mode 100644 index 0000000000..22d195acf2 --- /dev/null +++ b/hardware_interface/include/hardware_interface/component_interfaces/sensor_interface.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__COMPONENT_INTERFACES__SENSOR_INTERFACE_HPP_ +#define HARDWARE_INTERFACE__COMPONENT_INTERFACES__SENSOR_INTERFACE_HPP_ + +#include + +#include "hardware_interface/component_info.hpp" +#include "hardware_interface/types/hardware_interface_return_values.hpp" +#include "hardware_interface/visibility_control.h" + +namespace hardware_interface +{ + +class SensorInterface +{ +public: + HARDWARE_INTERFACE_PUBLIC + SensorInterface() = default; + + HARDWARE_INTERFACE_PUBLIC + virtual + ~SensorInterface() = default; + + HARDWARE_INTERFACE_PUBLIC + virtual + return_type configure(const ComponentInfo & sensor_info) = 0; + + HARDWARE_INTERFACE_PUBLIC + virtual + std::string get_interface_name() const = 0; + + HARDWARE_INTERFACE_PUBLIC + virtual + return_type start() = 0; + + HARDWARE_INTERFACE_PUBLIC + virtual + return_type stop() = 0; + + HARDWARE_INTERFACE_PUBLIC + virtual + bool is_started() const = 0; + + HARDWARE_INTERFACE_PUBLIC + virtual + return_type read(double & data) = 0; +}; + +} // namespace hardware_interface +#endif // HARDWARE_INTERFACE__COMPONENT_INTERFACES__SENSOR_INTERFACE_HPP_ diff --git a/hardware_interface/include/hardware_interface/component_interfaces/system_interface.hpp b/hardware_interface/include/hardware_interface/component_interfaces/system_interface.hpp new file mode 100644 index 0000000000..6b460357be --- /dev/null +++ b/hardware_interface/include/hardware_interface/component_interfaces/system_interface.hpp @@ -0,0 +1,70 @@ +// 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__COMPONENT_INTERFACES__SYSTEM_INTERFACE_HPP_ +#define HARDWARE_INTERFACE__COMPONENT_INTERFACES__SYSTEM_INTERFACE_HPP_ + +#include +#include + +#include "hardware_interface/component_info.hpp" +#include "hardware_interface/types/hardware_interface_return_values.hpp" +#include "hardware_interface/visibility_control.h" + +namespace hardware_interface +{ + +class SystemInterface +{ +public: + HARDWARE_INTERFACE_PUBLIC + SystemInterface() = default; + + HARDWARE_INTERFACE_PUBLIC + virtual + ~SystemInterface() = default; + + HARDWARE_INTERFACE_PUBLIC + virtual + return_type configure(const SystemInfo & system_info) = 0; + + HARDWARE_INTERFACE_PUBLIC + virtual + std::vector get_interface_names() const = 0; + + HARDWARE_INTERFACE_PUBLIC + virtual + return_type start() = 0; + + HARDWARE_INTERFACE_PUBLIC + virtual + return_type stop() = 0; + + HARDWARE_INTERFACE_PUBLIC + virtual + bool is_started() const = 0; + + HARDWARE_INTERFACE_PUBLIC + virtual + return_type read(std::vector & data) = 0; + + return_type + virtual + write(const std::vector & data) = 0; +}; + +typedef SystemInterface RobotInterface; + +} // namespace hardware_interface +#endif // HARDWARE_INTERFACE__COMPONENT_INTERFACES__SYSTEM_INTERFACE_HPP_ diff --git a/hardware_interface/include/hardware_interface/joint.hpp b/hardware_interface/include/hardware_interface/joint.hpp new file mode 100644 index 0000000000..068cd55df3 --- /dev/null +++ b/hardware_interface/include/hardware_interface/joint.hpp @@ -0,0 +1,89 @@ +// 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__JOINT_HPP_ +#define HARDWARE_INTERFACE__JOINT_HPP_ + +#include +#include +#include +#include + +#include "hardware_interface/component_info.hpp" +#include "hardware_interface/component_interfaces/joint_interface.hpp" +#include "hardware_interface/types/hardware_interface_return_values.hpp" +#include "hardware_interface/visibility_control.h" + +namespace hardware_interface +{ + +class Joint final +{ +public: + Joint() = default; + + explicit Joint(std::unique_ptr & impl) + : impl_(std::move(impl)) + {} + + ~Joint() = default; + + HARDWARE_INTERFACE_PUBLIC + return_type configure(const ComponentInfo & actuator_info) + { + return impl_->configure(actuator_info); + } + + HARDWARE_INTERFACE_PUBLIC + return_type start() + { + return impl_->start(); + } + + HARDWARE_INTERFACE_PUBLIC + return_type stop() + { + return impl_->stop(); + } + + HARDWARE_INTERFACE_PUBLIC + bool is_started() const + { + return impl_->is_started(); + } + + HARDWARE_INTERFACE_PUBLIC + return_type read(double & data) + { + return impl_->read(data); + } + + HARDWARE_INTERFACE_PUBLIC + return_type write(const double & data) + { + return impl_->write(data); + } + + HARDWARE_INTERFACE_PUBLIC + std::string get_interface_name() const + { + return impl_->get_interface_name(); + } + +private: + std::unique_ptr impl_; +}; + +} // namespace hardware_interface +#endif // HARDWARE_INTERFACE__JOINT_HPP_ diff --git a/hardware_interface/include/hardware_interface/sensor.hpp b/hardware_interface/include/hardware_interface/sensor.hpp new file mode 100644 index 0000000000..6578f5d422 --- /dev/null +++ b/hardware_interface/include/hardware_interface/sensor.hpp @@ -0,0 +1,77 @@ +// 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_HPP_ +#define HARDWARE_INTERFACE__SENSOR_HPP_ + +#include +#include +#include + +#include "hardware_interface/component_info.hpp" +#include "hardware_interface/component_interfaces/sensor_interface.hpp" +#include "hardware_interface/types/hardware_interface_return_values.hpp" +#include "hardware_interface/visibility_control.h" + +namespace hardware_interface +{ + +class Sensor final +{ +public: + Sensor() = default; + + explicit Sensor(std::unique_ptr impl) + : impl_(std::move(impl)) + {} + + virtual ~Sensor() = default; + + HARDWARE_INTERFACE_PUBLIC + return_type configure(const ComponentInfo & sensor_info) + { + return impl_->configure(sensor_info); + } + + HARDWARE_INTERFACE_PUBLIC + return_type start() + { + return impl_->start(); + } + + HARDWARE_INTERFACE_PUBLIC + return_type stop() + { + return impl_->stop(); + } + + HARDWARE_INTERFACE_PUBLIC + return_type read(double & data) + { + return impl_->read(data); + } + + HARDWARE_INTERFACE_PUBLIC + std::string get_interface_name() + { + return impl_->get_interface_name(); + } + +private: + std::unique_ptr impl_; +}; + +} // namespace hardware_interface +#endif // HARDWARE_INTERFACE__SENSOR_HPP_ diff --git a/hardware_interface/include/hardware_interface/system.hpp b/hardware_interface/include/hardware_interface/system.hpp new file mode 100644 index 0000000000..eecf5b68b5 --- /dev/null +++ b/hardware_interface/include/hardware_interface/system.hpp @@ -0,0 +1,88 @@ +// 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_HPP_ +#define HARDWARE_INTERFACE__SYSTEM_HPP_ + +#include +#include +#include +#include + +#include "hardware_interface/component_info.hpp" +#include "hardware_interface/component_interfaces/system_interface.hpp" +#include "hardware_interface/visibility_control.h" + +namespace hardware_interface +{ + +class System final +{ +public: + explicit System(std::unique_ptr impl) + : impl_(std::move(impl)) + {} + + virtual ~System() = default; + + HARDWARE_INTERFACE_PUBLIC + return_type configure(const SystemInfo & system_info) + { + return impl_->configure(system_info); + } + + HARDWARE_INTERFACE_PUBLIC + std::vector get_interface_names() + { + return impl_->get_interface_names(); + } + + HARDWARE_INTERFACE_PUBLIC + return_type start() + { + return impl_->start(); + } + + HARDWARE_INTERFACE_PUBLIC + return_type stop() + { + return impl_->stop(); + } + + HARDWARE_INTERFACE_PUBLIC + bool is_started() + { + return impl_->is_started(); + } + + HARDWARE_INTERFACE_PUBLIC + return_type read(std::vector & data) + { + return impl_->read(data); + } + + HARDWARE_INTERFACE_PUBLIC + return_type write(const std::vector & data) + { + return impl_->write(data); + } + +private: + std::unique_ptr impl_; +}; + +typedef System Robot; + +} // namespace hardware_interface +#endif // HARDWARE_INTERFACE__SYSTEM_HPP_ diff --git a/robot_control_components/src/actuator.cpp b/hardware_interface/include/hardware_interface/types/hardware_interface_type_values.hpp similarity index 54% rename from robot_control_components/src/actuator.cpp rename to hardware_interface/include/hardware_interface/types/hardware_interface_type_values.hpp index c3a95f6076..7292f28229 100644 --- a/robot_control_components/src/actuator.cpp +++ b/hardware_interface/include/hardware_interface/types/hardware_interface_type_values.hpp @@ -1,4 +1,4 @@ -// Copyright 2020 ROS2-Control Development Team +// 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. @@ -12,11 +12,14 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "robot_control_components/actuator.hpp" +#ifndef HARDWARE_INTERFACE__TYPES__HARDWARE_INTERFACE_TYPE_VALUES_HPP_ +#define HARDWARE_INTERFACE__TYPES__HARDWARE_INTERFACE_TYPE_VALUES_HPP_ -namespace robot_control_components +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 - - -} // namespace robot_control_components +#endif // HARDWARE_INTERFACE__TYPES__HARDWARE_INTERFACE_TYPE_VALUES_HPP_ diff --git a/robot_control_components/CMakeLists.txt b/robot_control_components/CMakeLists.txt deleted file mode 100644 index d0ab4c445d..0000000000 --- a/robot_control_components/CMakeLists.txt +++ /dev/null @@ -1,75 +0,0 @@ -cmake_minimum_required(VERSION 3.5) -project(robot_control_components) - -# Default to C99 -if(NOT CMAKE_C_STANDARD) - set(CMAKE_C_STANDARD 99) -endif() - -# Default to C++14 -if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 14) -endif() - -# find dependencies -find_package(ament_cmake REQUIRED) - -find_package(control_msgs REQUIRED) -find_package(pluginlib REQUIRED) -find_package(rclcpp REQUIRED) - - -## COMPILE -add_library(${PROJECT_NAME} SHARED - src/actuator.cpp - src/robot.cpp - src/sensor.cpp -) - -target_include_directories(${PROJECT_NAME} - PRIVATE include -) - -ament_target_dependencies(${PROJECT_NAME} - control_msgs - pluginlib - rclcpp -) - -## INSTALL -install(TARGETS ${PROJECT_NAME} - DESTINATION lib -) - -install(DIRECTORY include/ - DESTINATION include -) - -## TESTS -if(BUILD_TESTING) - find_package(ament_cmake_gtest REQUIRED) - - ament_add_gtest(test_dummy test/test_dummy.cpp) - target_include_directories(test_dummy PRIVATE include) -# ament_target_dependencies(test_dummy rcpputils) - - - # Uncomment this before creating a new PR to check code quality -# find_package(ament_lint_auto REQUIRED) -# ament_lint_auto_find_test_dependencies() -endif() - -## EXPORTS -ament_export_dependencies( - control_msgs - pluginlib - rclcpp -) -ament_export_libraries( - ${PROJECT_NAME} -) -ament_export_include_directories( -include -) -ament_package() - diff --git a/robot_control_components/include/robot_control_components/actuator.hpp b/robot_control_components/include/robot_control_components/actuator.hpp deleted file mode 100644 index a89c839651..0000000000 --- a/robot_control_components/include/robot_control_components/actuator.hpp +++ /dev/null @@ -1,46 +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 ROBOT_CONTROL_COMPONENTS__ACTUATOR_HPP_ -#define ROBOT_CONTROL_COMPONENTS__ACTUATOR_HPP_ - -#include -#include - -#include "control_msgs/msg/interface_value.hpp" - -#include "rclcpp/macros.hpp" -#include "rclcpp/rclcpp.hpp" - -#include "robot_control_components/visibility_control.h" - - -namespace robot_control_components -{ - -class Actuator -{ -public: - RCLCPP_SHARED_PTR_DEFINITIONS(Actuator) - - ROS2_CONTROL_CORE_PUBLIC Actuator() = default; - - ROS2_CONTROL_CORE_PUBLIC virtual ~Actuator() = default; - -}; - -} // namespace robot_control_components - -#endif // ROBOT_CONTROL_COMPONENTS__ACTUATOR_HPP_ diff --git a/robot_control_components/include/robot_control_components/component.hpp b/robot_control_components/include/robot_control_components/component.hpp deleted file mode 100644 index 6b5abe394f..0000000000 --- a/robot_control_components/include/robot_control_components/component.hpp +++ /dev/null @@ -1,142 +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 ROBOT_CONTROL_COMPONENTS__COMPONENTS_BASE_COMPONENT_HPP_ -#define ROBOT_CONTROL_COMPONENTS__COMPONENTS_BASE_COMPONENT_HPP_ - -#include - -#include "rclcpp/macros.hpp" -#include "rclcpp/rclcpp.hpp" - -#include "robot_control_components/hardware/component_hardware.hpp" - -#include "robot_control_components/ros2_control_types.h" -#include "robot_control_components/ros2_control_utils.hpp" -#include "robot_control_components/visibility_control.h" - - -namespace robot_control_components -{ - -template < typename ComponentHardwareType > -class Component -{ -public: - RCLCPP_SHARED_PTR_DEFINITIONS(Component) - - ROS2_CONTROL_CORE_PUBLIC Component() = default; - - ROS2_CONTROL_CORE_PUBLIC Component(std::string parameters_path, std::string type, const rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr logging_interface, const rclcpp::node_interfaces::NodeParametersInterface::SharedPtr parameters_interface, const rclcpp::node_interfaces::NodeServicesInterface::SharedPtr services_interface) - { - configure(parameters_path, type, logging_interface, parameters_interface, services_interface); - }; - - ROS2_CONTROL_CORE_PUBLIC virtual ~Component() = default; - - ros2_control_types::return_type virtual configure(std::string parameters_path, const rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr logging_interface, const rclcpp::node_interfaces::NodeParametersInterface::SharedPtr parameters_interface, const rclcpp::node_interfaces::NodeServicesInterface::SharedPtr services_interface) = 0; - - ros2_control_types::return_type configure(std::string parameters_path, std::string type, const rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr logging_interface, const rclcpp::node_interfaces::NodeParametersInterface::SharedPtr parameters_interface, const rclcpp::node_interfaces::NodeServicesInterface::SharedPtr services_interface) - { - parameters_path_ = parameters_path; - type_ = type; - logging_interface_ = logging_interface; - parameters_interface_ = parameters_interface; - services_interface_ = services_interface; - - parameters_interface_->declare_parameter(parameters_path_ + ".name"); - name_ = parameters_interface_->get_parameter(parameters_path_ + ".name").as_string(); - - parameters_interface_->declare_parameter(parameters_path_ + ".has_hardware", rclcpp::ParameterValue(false)); - has_hardware_ = parameters_interface_->get_parameter(parameters_path_ + ".has_hardware").as_bool(); - - RCLCPP_INFO(logging_interface_->get_logger(), "%s - %s: called configure!", type_.c_str(), name_.c_str()); - - return ros2_control_types::ROS2C_RETURN_OK; - }; - - ros2_control_types::return_type init() - { - ros2_control_types::return_type ret = ros2_control_types::ROS2C_RETURN_OK; - if (has_hardware_) - { - //FIXME:DEBUG - RCLCPP_INFO(logging_interface_->get_logger(), "'%s' calling hardware init.", name_.c_str()); - ret = hardware_->init(); - } - else - { - //FIXME:DEBUG - RCLCPP_INFO(logging_interface_->get_logger(), "'%s' component is initalized without hardware.", name_.c_str()); - } - return ret; - }; - - // ROS2_CONTROL_CORE_PUBLIC virtual ros2_control_types::return_type recover() = 0; - - // ROS2_CONTROL_CORE_PUBLIC virtual ros2_control_types::return_type stop() = 0; - - std::string name_; - -protected: - /** - * @brief Components parameter prefix. - * - */ - std::string parameters_path_; - std::string type_; - rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr logging_interface_; - rclcpp::node_interfaces::NodeParametersInterface::SharedPtr parameters_interface_; - rclcpp::node_interfaces::NodeServicesInterface::SharedPtr services_interface_; - - bool has_hardware_; - - uint n_dof_; -// ros2_control_types::component_state_type state_ = 0; - - std::shared_ptr hardware_; - - template - ros2_control_types::return_type load_hardware(ros2_control_utils::ROS2ControlLoaderPluginlib class_loader) - { - hardware_ = ros2_control_utils::load_component_from_parameter(parameters_path_ + "." + type_ + "Hardware.type", parameters_interface_, class_loader, logging_interface_->get_logger()); - - if (!hardware_) - { - RCLCPP_FATAL(logging_interface_->get_logger(), "%s: '%sHardware has to be defined if 'has_hardware=True'!", type_.c_str(), name_.c_str()); - return ros2_control_types::ROS2C_RETURN_ERROR; - } - return ros2_control_types::ROS2C_RETURN_OK; - }; - - template - ros2_control_types::return_type load_and_configure_hardware(ros2_control_utils::ROS2ControlLoaderPluginlib class_loader) - { - if (load_hardware(class_loader) == ros2_control_types::ROS2C_RETURN_OK) - { - hardware_->configure(parameters_path_ + "." + type_ + "Hardware", logging_interface_, parameters_interface_, services_interface_); - // FIXME: Make this general for all components - currently only for robot -// hardware_.set_components_for_data(); - return ros2_control_types::ROS2C_RETURN_OK; - } - return ros2_control_types::ROS2C_RETURN_ERROR; - }; - - -}; - -} // namespace robot_control_components - -#endif // ROBOT_CONTROL_COMPONENTS__COMPONENTS_BASE_COMPONENT_HPP_ diff --git a/robot_control_components/include/robot_control_components/robot.hpp b/robot_control_components/include/robot_control_components/robot.hpp deleted file mode 100644 index 861110220f..0000000000 --- a/robot_control_components/include/robot_control_components/robot.hpp +++ /dev/null @@ -1,47 +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 ROBOT_CONTROL_COMPONENTS__ROBOT_HPP_ -#define ROBOT_CONTROL_COMPONENTS__ROBOT_HPP_ - -#include -#include - -#include "control_msgs/msg/dynamic_joint_state.hpp" -#include "control_msgs/msg/interface_value.hpp" - -#include "rclcpp/macros.hpp" -#include "rclcpp/rclcpp.hpp" - -#include "robot_control_components/visibility_control.h" - - -namespace robot_control_components -{ - -class Robot -{ -public: - RCLCPP_SHARED_PTR_DEFINITIONS(Robot) - - ROS2_CONTROL_CORE_PUBLIC Robot() = default; - - ROS2_CONTROL_CORE_PUBLIC ~Robot() = default; - -}; - -} // namespace robot_control_components - -#endif // ROBOT_CONTROL_COMPONENTS__ROBOT_HPP_ diff --git a/robot_control_components/include/robot_control_components/ros2_control_types.h b/robot_control_components/include/robot_control_components/ros2_control_types.h deleted file mode 100644 index 0c5a9a1cab..0000000000 --- a/robot_control_components/include/robot_control_components/ros2_control_types.h +++ /dev/null @@ -1,62 +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. - -/* This header must be included by all rclcpp headers which declare symbols - * which are defined in the rclcpp library. When not building the rclcpp - * library, i.e. when using the headers in other package's code, the contents - * of this header change the visibility of certain symbols which the rclcpp - * library cannot have, but the consuming code must have inorder to link. - */ - -#ifndef ROBOT_CONTROL_COMPONENTS__ROS2_CONTROL_TYPES_H_ -#define ROBOT_CONTROL_COMPONENTS__ROS2_CONTROL_TYPES_H_ - -#include -#include - -#include "rclcpp/rclcpp.hpp" - -#include "robot_control_components/visibility_control.h" - -#include "control_msgs/msg/interface_value.hpp" - - -// TODO: Do we need "ROS2_CONTROL_CORE_PUBLIC" before the variables? - -namespace robot_control_components -{ - -using components_ret_t = int; -static constexpr components_ret_t ROS2C_RETURN_OK = 0; -static constexpr components_ret_t ROS2C_RETURN_ERROR = 1; - -static constexpr components_ret_t ROS2C_RETURN_ACTUATOR_CLAIMED_ERROR = 10; -static constexpr components_ret_t ROS2C_RETURN_ACTUATOR_ALREADY_CLAIMED = 11; -static constexpr components_ret_t ROS2C_RETURN_ACTUATOR_NOT_CLAIMED = 11; -static constexpr components_ret_t ROS2C_RETURN_ACTUATOR_UNATHORIZED_UNCLAIM = 13; -static constexpr components_ret_t ROS2C_RETURN_ACTUATOR_CAN_NOT_READ = 20; - -// TODO: Check if needed or lifecycle component can do this -// using component_state_type = uint; -// static constexpr component_state_type ROS2C_COMPONENT_STATE_LOADED = 1; -// static constexpr component_state_type ROS2C_COMPONENT_STATE_CONFIGURED = 2; -// static constexpr component_state_type ROS2C_COMPONENT_STATE_INITIALIZED = 3; -// static constexpr component_state_type ROS2C_COMPONENT_STATE_STARTED = 4; -// static constexpr component_state_type ROS2C_COMPONENT_STATE_STOPPED = 5; -// static constexpr component_state_type ROS2C_COMPONENT_STATE_HALTED = 6; - - -} // namespace robot_control_components - -#endif // ROBOT_CONTROL_COMPONENTS__ROS2_CONTROL_TYPES_H_ diff --git a/robot_control_components/include/robot_control_components/ros2_control_utils.hpp b/robot_control_components/include/robot_control_components/ros2_control_utils.hpp deleted file mode 100644 index b00dee7f78..0000000000 --- a/robot_control_components/include/robot_control_components/ros2_control_utils.hpp +++ /dev/null @@ -1,69 +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. - -/* This header must be included by all rclcpp headers which declare symbols - * which are defined in the rclcpp library. When not building the rclcpp - * library, i.e. when using the headers in other package's code, the contents - * of this header change the visibility of certain symbols which the rclcpp - * library cannot have, but the consuming code must have inorder to link. - */ - -#ifndef ROBOT_CONTROL_COMPONENTS__ROS2_CONTROL_UTILS_H_ -#define ROBOT_CONTROL_COMPONENTS__ROS2_CONTROL_UTILS_H_ - -#include -#include - -#include "pluginlib/class_loader.hpp" -#include "rclcpp/rclcpp.hpp" -#include "robot_control_components/visibility_control.h" - -// TODO: Create util library instead of inline functions? - -namespace ros2_control_utils -{ -// Classes -template < typename T > -class ROS2ControlLoaderPluginlib -{ -public: - ROS2_CONTROL_CORE_PUBLIC ROS2ControlLoaderPluginlib(const std::string package, const std::string base_type) : loader_(std::make_shared>(package, base_type)) - { - }; - - ROS2_CONTROL_CORE_PUBLIC - virtual ~ROS2ControlLoaderPluginlib() = default; - - // TODO: Add try-catch - ROS2_CONTROL_CORE_PUBLIC - std::shared_ptr create(const std::string & type) - { - return std::shared_ptr(loader_->createUnmanagedInstance(type)); - }; - - ROS2_CONTROL_CORE_PUBLIC - bool is_available(const std::string & type) - { - return loader_->isClassAvailable(type); - }; - -private: - std::shared_ptr> loader_; -}; - - - -} // namespace ros2_control_utils - -#endif // ROBOT_CONTROL_COMPONENTS__ROS2_CONTROL_UTILS_H_ diff --git a/robot_control_components/include/robot_control_components/sensor.hpp b/robot_control_components/include/robot_control_components/sensor.hpp deleted file mode 100644 index 5a325e1bfd..0000000000 --- a/robot_control_components/include/robot_control_components/sensor.hpp +++ /dev/null @@ -1,44 +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 ROBOT_CONTROL_COMPONENTS__SENSOR_HPP_ -#define ROBOT_CONTROL_COMPONENTS__SENSOR_HPP_ - -#include - -#include "control_msgs/msg/interface_value.hpp" - -#include "rclcpp/macros.hpp" -#include "rclcpp/rclcpp.hpp" - -#include "robot_control_components/visibility_control.h" - -namespace robot_control_components -{ - -class Sensor -{ -public: - RCLCPP_SHARED_PTR_DEFINITIONS(Sensor); - - ROS2_CONTROL_CORE_PUBLIC Sensor() = default; - - ROS2_CONTROL_CORE_PUBLIC virtual ~Sensor() = default; - -}; - -} // namespace robot_control_components - -#endif // ROBOT_CONTROL_COMPONENTS__SENSOR_HPP_ diff --git a/robot_control_components/include/robot_control_components/simple_component.hpp b/robot_control_components/include/robot_control_components/simple_component.hpp deleted file mode 100644 index c7109b7199..0000000000 --- a/robot_control_components/include/robot_control_components/simple_component.hpp +++ /dev/null @@ -1,70 +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 ROBOT_CONTROL_COMPONENTS__COMPONENTS_COMPONENT_HPP_ -#define ROBOT_CONTROL_COMPONENTS__COMPONENTS_COMPONENT_HPP_ - -#include -#include -#include - -#include "control_msgs/msg/interface_value.hpp" - -#include "robot_control_components/components/component.hpp" - -#include "robot_control_components/ros2_control_types.h" -#include "robot_control_components/visibility_control.h" - - -namespace robot_control_components -{ - -template < typename ComponentHardwareType > -class SimpleComponent : public Component< ComponentHardwareType > -{ -public: - ROS2_CONTROL_CORE_PUBLIC SimpleComponent() = default; - - ROS2_CONTROL_CORE_PUBLIC virtual ~SimpleComponent() = default; - - ROS2_CONTROL_CORE_PUBLIC ros2_control_types::return_type configure(const std::string parameters_path, const std::string type, const rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr logging_interface, const rclcpp::node_interfaces::NodeParametersInterface::SharedPtr parameters_interface, const rclcpp::node_interfaces::NodeServicesInterface::SharedPtr services_interface) - { - ros2_control_types::return_type ret = ros2_control_types::ROS2C_RETURN_OK; - ret = Component< ComponentHardwareType >::configure(parameters_path, type, logging_interface, parameters_interface, services_interface); - - parameters_interface->declare_parameter(parameters_path + ".interface_names"); - parameters_interface->declare_parameter(parameters_path + ".n_dof", rclcpp::ParameterValue(1)); - parameters_interface->declare_parameter(parameters_path + ".min_values"); - parameters_interface->declare_parameter(parameters_path + ".max_values"); - - return ret; - }; - - //TODO: this is simplest implementation. We need to do this more complex to support complex configurations of robots - std::vector get_interface_names() - { - return interface_names_; - } - -protected: - std::vector valid_interface_names_; - //TODO: should we use set for easier search? - std::vector interface_names_; - control_msgs::msg::InterfaceValue read_values_; -}; - -} // namespace robot_control_components - -#endif // ROBOT_CONTROL_COMPONENTS__COMPONENTS_COMPONENT_HPP_ diff --git a/robot_control_components/include/robot_control_components/visibility_control.h b/robot_control_components/include/robot_control_components/visibility_control.h deleted file mode 100644 index 9331ef1117..0000000000 --- a/robot_control_components/include/robot_control_components/visibility_control.h +++ /dev/null @@ -1,64 +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. - -/* This header must be included by all rclcpp headers which declare symbols - * which are defined in the rclcpp library. When not building the rclcpp - * library, i.e. when using the headers in other package's code, the contents - * of this header change the visibility of certain symbols which the rclcpp - * library cannot have, but the consuming code must have inorder to link. - */ - -#ifndef ROBOT_CONTROL_COMPONENTS__VISIBILITY_CONTROL_H_ -#define ROBOT_CONTROL_COMPONENTS__VISIBILITY_CONTROL_H_ - -// This logic was borrowed (then namespaced) from the examples on the gcc wiki: -// https://gcc.gnu.org/wiki/Visibility - -#if defined _WIN32 || defined __CYGWIN__ -#ifdef __GNUC__ -#define ROS2_CONTROL_CORE_EXPORT \ - __attribute__((dllexport)) -#define ROS2_CONTROL_CORE_IMPORT \ - __attribute__((dllimport)) -#else -#define ROS2_CONTROL_CORE_EXPORT __declspec(dllexport) -#define ROS2_CONTROL_CORE_IMPORT __declspec(dllimport) -#endif -#ifdef ROS2_CONTROL_CORE_BUILDING_DLL -#define ROS2_CONTROL_CORE_PUBLIC \ - ROS2_CONTROL_CORE_EXPORT -#else -#define ROS2_CONTROL_CORE_PUBLIC \ - ROS2_CONTROL_CORE_IMPORT -#endif -#define ROS2_CONTROL_CORE_PUBLIC_TYPE \ - ROS2_CONTROL_CORE_PUBLIC -#define ROS2_CONTROL_CORE_LOCAL -#else -#define ROS2_CONTROL_CORE_EXPORT \ - __attribute__((visibility("default"))) -#define ROS2_CONTROL_CORE_IMPORT -#if __GNUC__ >= 4 -#define ROS2_CONTROL_CORE_PUBLIC \ - __attribute__((visibility("default"))) -#define ROS2_CONTROL_CORE_LOCAL \ - __attribute__((visibility("hidden"))) -#else -#define ROS2_CONTROL_CORE_PUBLIC -#define ROS2_CONTROL_CORE_LOCAL -#endif -#define ROS2_CONTROL_CORE_PUBLIC_TYPE -#endif - -#endif // ROBOT_CONTROL_COMPONENTS__VISIBILITY_CONTROL_H_ diff --git a/robot_control_components/package.xml b/robot_control_components/package.xml deleted file mode 100644 index 4031b47f66..0000000000 --- a/robot_control_components/package.xml +++ /dev/null @@ -1,21 +0,0 @@ - - - - robot_control_components - 0.0.0 - The main package for `ros2_control`-concept testing. The package implements the most important classes and tests them with `demo_robot` to enable functionality and test driven development. - stogl - TODO: License declaration - - Denis Štogl - - control_msgs - pluginlib - rclcpp - - ament_cmake_gtest - - - ament_cmake - - diff --git a/robot_control_components/src/robot.cpp b/robot_control_components/src/robot.cpp deleted file mode 100644 index c57743feda..0000000000 --- a/robot_control_components/src/robot.cpp +++ /dev/null @@ -1,22 +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. - -#include "robot_control_components/robot.hpp" - -namespace robot_control_components -{ - - - -} // namespace robot_control_components diff --git a/robot_control_components/src/sensor.cpp b/robot_control_components/src/sensor.cpp deleted file mode 100644 index c9f920dcfe..0000000000 --- a/robot_control_components/src/sensor.cpp +++ /dev/null @@ -1,22 +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. - -#include "robot_control_components/sensor.hpp" - -namespace robot_control_components -{ - - - -} // namespace robot_control_components diff --git a/robot_control_components/test/test_dummy.cpp b/robot_control_components/test/test_dummy.cpp deleted file mode 100644 index 13b68d810e..0000000000 --- a/robot_control_components/test/test_dummy.cpp +++ /dev/null @@ -1,23 +0,0 @@ -// Copyright 2017 Open Source Robotics Foundation, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include - -class TestDummy : public ::testing::Test -{ -protected: - static void SetUpTestCase() - { - } -}; From 9ddc603f97c58f033867c33b7b45d9c2f5dd8470 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Mon, 27 Jul 2020 21:36:09 +0200 Subject: [PATCH 03/29] Added initial tests for Joint and JointInterface --- hardware_interface/CMakeLists.txt | 4 + .../hardware_interface/component_info.hpp | 2 +- .../component_interfaces/joint_interface.hpp | 2 +- .../include/hardware_interface/joint.hpp | 4 +- .../test/test_component_interfaces.cpp | 127 ++++++++++++++++++ 5 files changed, 135 insertions(+), 4 deletions(-) create mode 100644 hardware_interface/test/test_component_interfaces.cpp diff --git a/hardware_interface/CMakeLists.txt b/hardware_interface/CMakeLists.txt index 6554107797..07f8356338 100644 --- a/hardware_interface/CMakeLists.txt +++ b/hardware_interface/CMakeLists.txt @@ -74,6 +74,10 @@ 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 hardware_interface) + endif() ament_export_dependencies( diff --git a/hardware_interface/include/hardware_interface/component_info.hpp b/hardware_interface/include/hardware_interface/component_info.hpp index a1d82170be..293730e5b0 100644 --- a/hardware_interface/include/hardware_interface/component_info.hpp +++ b/hardware_interface/include/hardware_interface/component_info.hpp @@ -80,7 +80,7 @@ struct SystemInfo */ std::string name; /** - * \brief type of the system: robot, actuator or sensor. Note: URDF always needs a "robot" tag, nevertheless in terms of ros2_control, it can hold a definition for an actuator or sensor. + * \brief type of the system: robot, joints or sensor. Note: URDF always needs a "robot" tag, nevertheless in terms of ros2_control, it can hold a definition for an joint or sensor. */ std::string type; /** diff --git a/hardware_interface/include/hardware_interface/component_interfaces/joint_interface.hpp b/hardware_interface/include/hardware_interface/component_interfaces/joint_interface.hpp index e40f97c797..9589323ec5 100644 --- a/hardware_interface/include/hardware_interface/component_interfaces/joint_interface.hpp +++ b/hardware_interface/include/hardware_interface/component_interfaces/joint_interface.hpp @@ -39,7 +39,7 @@ class JointInterface HARDWARE_INTERFACE_PUBLIC virtual - return_type configure(const ComponentInfo & actuator_info) = 0; + return_type configure(const ComponentInfo & joint_info) = 0; HARDWARE_INTERFACE_PUBLIC virtual diff --git a/hardware_interface/include/hardware_interface/joint.hpp b/hardware_interface/include/hardware_interface/joint.hpp index 068cd55df3..ce4c95d918 100644 --- a/hardware_interface/include/hardware_interface/joint.hpp +++ b/hardware_interface/include/hardware_interface/joint.hpp @@ -40,9 +40,9 @@ class Joint final ~Joint() = default; HARDWARE_INTERFACE_PUBLIC - return_type configure(const ComponentInfo & actuator_info) + return_type configure(const ComponentInfo & joint_info) { - return impl_->configure(actuator_info); + return impl_->configure(joint_info); } HARDWARE_INTERFACE_PUBLIC diff --git a/hardware_interface/test/test_component_interfaces.cpp b/hardware_interface/test/test_component_interfaces.cpp new file mode 100644 index 0000000000..564687fae1 --- /dev/null +++ b/hardware_interface/test/test_component_interfaces.cpp @@ -0,0 +1,127 @@ +// 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 "hardware_interface/component_info.hpp" +#include "hardware_interface/component_interfaces/joint_interface.hpp" +#include "hardware_interface/component_interfaces/sensor_interface.hpp" +#include "hardware_interface/component_interfaces/system_interface.hpp" +#include "hardware_interface/joint.hpp" +#include "hardware_interface/sensor.hpp" +#include "hardware_interface/system.hpp" +#include "hardware_interface/types/hardware_interface_return_values.hpp" + +namespace hardware_interface +{ +namespace component_interfaces_test +{ +class DummyJoint : public JointInterface +{ + return_type configure(const ComponentInfo & joint_info) override + { + info = joint_info; + min_value = stod(info.parameters["min_value"]); + max_value = stod(info.parameters["max_value"]); + return return_type::OK; + } + + std::string get_interface_name() const override + { + return info.interface_names.front(); + } + + return_type start() override + { + started = true; + return return_type::OK; + } + + return_type stop() override + { + started = false; + return return_type::OK; + } + + bool is_started() const override + { + return started; + } + + return_type read(double & data) override + { + data = joint_value; + return return_type::OK; + } + + return_type write(const double & data) override + { + if (data > min_value && data < max_value) { + joint_value = data; + } else { + return return_type::ERROR; + } + return return_type::OK; + } + +private: + ComponentInfo info; + + double min_value, max_value; + + double joint_value; + bool started; +}; + +class DummySensor : public SensorInterface +{ +}; + +class DummySystem : public SystemInterface +{ +}; + +} // namespace component_interfaces_test +} // namespace hardware_interface + +using hardware_interface::ComponentInfo; +using hardware_interface::Joint; +using hardware_interface::JointInterface; +using hardware_interface::component_interfaces_test::DummyJoint; +using hardware_interface::return_type; + +TEST(TestJointInterface, joint_interfce_works) +{ + std::unique_ptr joint_interface(new DummyJoint); + Joint joint(joint_interface); + + ComponentInfo joint_info; + joint_info.name = "DummyJoint"; + joint_info.interface_names.push_back("dummy"); + joint_info.parameters["min_value"] = "-1"; + joint_info.parameters["max_value"] = "1"; + + joint.configure(joint_info); + EXPECT_EQ(joint.get_interface_name(), "dummy"); + EXPECT_EQ(joint.is_started(), false); + EXPECT_EQ(joint.start(), return_type::OK); + EXPECT_EQ(joint.is_started(), true); + EXPECT_EQ(joint.stop(), return_type::OK); + EXPECT_EQ(joint.write(2), return_type::ERROR); + EXPECT_EQ(joint.write(0.5), return_type::OK); + double output; + EXPECT_EQ(joint.read(output), return_type::OK); + EXPECT_DOUBLE_EQ(output, 0.5); +} From eeaff7e881603b0dcfc67da556a1821534fcf6c2 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Tue, 28 Jul 2020 15:30:51 +0200 Subject: [PATCH 04/29] Extended SensorInterface to work with hardware. Added Example for DummySensor and HW in tests. Added lifecycle management for components #107 --- .../component_interfaces/sensor_interface.hpp | 17 +- .../include/hardware_interface/sensor.hpp | 29 ++- .../types/hardware_interface_state_values.hpp | 34 +++ .../test/test_component_interfaces.cpp | 193 +++++++++++++++++- 4 files changed, 267 insertions(+), 6 deletions(-) create mode 100644 hardware_interface/include/hardware_interface/types/hardware_interface_state_values.hpp diff --git a/hardware_interface/include/hardware_interface/component_interfaces/sensor_interface.hpp b/hardware_interface/include/hardware_interface/component_interfaces/sensor_interface.hpp index 22d195acf2..39e5c8d5cf 100644 --- a/hardware_interface/include/hardware_interface/component_interfaces/sensor_interface.hpp +++ b/hardware_interface/include/hardware_interface/component_interfaces/sensor_interface.hpp @@ -19,6 +19,7 @@ #include "hardware_interface/component_info.hpp" #include "hardware_interface/types/hardware_interface_return_values.hpp" +#include "hardware_interface/types/hardware_interface_state_values.hpp" #include "hardware_interface/visibility_control.h" namespace hardware_interface @@ -40,7 +41,11 @@ class SensorInterface HARDWARE_INTERFACE_PUBLIC virtual - std::string get_interface_name() const = 0; + return_type initalize(bool auto_start) = 0; + + HARDWARE_INTERFACE_PUBLIC + virtual + return_type recover(bool auto_start) = 0; HARDWARE_INTERFACE_PUBLIC virtual @@ -52,11 +57,19 @@ class SensorInterface HARDWARE_INTERFACE_PUBLIC virtual - bool is_started() const = 0; + return_type halt() = 0; + + HARDWARE_INTERFACE_PUBLIC + virtual + component_state get_state() const = 0; HARDWARE_INTERFACE_PUBLIC virtual return_type read(double & data) = 0; + + HARDWARE_INTERFACE_PUBLIC + virtual + std::string get_interface_name() const = 0; }; } // namespace hardware_interface diff --git a/hardware_interface/include/hardware_interface/sensor.hpp b/hardware_interface/include/hardware_interface/sensor.hpp index 6578f5d422..b61431c45b 100644 --- a/hardware_interface/include/hardware_interface/sensor.hpp +++ b/hardware_interface/include/hardware_interface/sensor.hpp @@ -22,6 +22,7 @@ #include "hardware_interface/component_info.hpp" #include "hardware_interface/component_interfaces/sensor_interface.hpp" +#include "hardware_interface/types/hardware_interface_state_values.hpp" #include "hardware_interface/types/hardware_interface_return_values.hpp" #include "hardware_interface/visibility_control.h" @@ -33,11 +34,11 @@ class Sensor final public: Sensor() = default; - explicit Sensor(std::unique_ptr impl) + explicit Sensor(std::unique_ptr & impl) : impl_(std::move(impl)) {} - virtual ~Sensor() = default; + ~Sensor() = default; HARDWARE_INTERFACE_PUBLIC return_type configure(const ComponentInfo & sensor_info) @@ -45,6 +46,18 @@ class Sensor final return impl_->configure(sensor_info); } + HARDWARE_INTERFACE_PUBLIC + return_type initalize(bool auto_start) + { + return impl_->initalize(auto_start); + } + + HARDWARE_INTERFACE_PUBLIC + return_type recover(bool auto_start) + { + return impl_->recover(auto_start); + } + HARDWARE_INTERFACE_PUBLIC return_type start() { @@ -57,6 +70,18 @@ class Sensor final return impl_->stop(); } + HARDWARE_INTERFACE_PUBLIC + return_type halt() + { + return impl_->halt(); + } + + HARDWARE_INTERFACE_PUBLIC + component_state get_state() + { + return impl_->get_state(); + } + HARDWARE_INTERFACE_PUBLIC return_type read(double & data) { diff --git a/hardware_interface/include/hardware_interface/types/hardware_interface_state_values.hpp b/hardware_interface/include/hardware_interface/types/hardware_interface_state_values.hpp new file mode 100644 index 0000000000..a4d20991e7 --- /dev/null +++ b/hardware_interface/include/hardware_interface/types/hardware_interface_state_values.hpp @@ -0,0 +1,34 @@ +// 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_STATE_VALUES_HPP_ +#define HARDWARE_INTERFACE__TYPES__HARDWARE_INTERFACE_STATE_VALUES_HPP_ + +#include + +namespace hardware_interface +{ +enum class component_state : std::uint8_t +{ + UNKNOWN = 0, + CONFIGURED = 1, + INITIALIZED = 2, + STARTED = 3, + STOPPED = 4, + HALTED = 5, +}; + +} // namespace hardware_interface + +#endif // HARDWARE_INTERFACE__TYPES__HARDWARE_INTERFACE_STATE_VALUES_HPP_ diff --git a/hardware_interface/test/test_component_interfaces.cpp b/hardware_interface/test/test_component_interfaces.cpp index 564687fae1..c4b3fed91b 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 "hardware_interface/component_info.hpp" #include "hardware_interface/component_interfaces/joint_interface.hpp" #include "hardware_interface/component_interfaces/sensor_interface.hpp" @@ -23,6 +24,7 @@ #include "hardware_interface/sensor.hpp" #include "hardware_interface/system.hpp" #include "hardware_interface/types/hardware_interface_return_values.hpp" +#include "hardware_interface/types/hardware_interface_state_values.hpp" namespace hardware_interface { @@ -85,8 +87,152 @@ class DummyJoint : public JointInterface bool started; }; +class DummySensorHardware +{ +public: + DummySensorHardware() = default; + + ~DummySensorHardware() = default; + + return_type configure_hardware(std::unordered_map & parameters) + { + if (parameters.count("start_value") > 0) { + value = stoi(parameters["start_value"]); + return return_type::OK; + } + return return_type::ERROR; + } + + return_type initalize_hardware(bool auto_start) + { + (void) auto_start; + return return_type::OK; + } + + return_type recover_hardware(bool auto_start) + { + (void) auto_start; + return return_type::OK; + } + + return_type start_hardware() + { + return return_type::OK; + } + + return_type stop_hardware() + { + return return_type::OK; + } + + return_type halt_hardware() + { + return return_type::OK; + } + + return_type read_from_hardware(int & data) + { + data = value + 128; + return return_type::OK; + } + +private: + int value = 0; +}; + class DummySensor : public SensorInterface { + return_type configure(const ComponentInfo & sensor_info) override + { + return_type ret; + info = sensor_info; + binary_to_voltage_factor = stod(info.parameters["binary_to_voltage_factor"]); + + // Normaly crate dynamically SensorHardware Object from sensor hardware_class_type + if (hardware.configure_hardware(info.hardware_parameters) == return_type::OK) { + state = component_state::CONFIGURED; + ret = return_type::OK; + } else { + state = component_state::UNKNOWN; + ret = return_type::ERROR; + } + return ret; + } + + std::string get_interface_name() const override + { + return info.interface_names.front(); + } + + return_type initalize(bool auto_start) override + { + if (hardware.initalize_hardware(auto_start) == return_type::OK) { + if (auto_start) { + start(); + } else { + state = component_state::INITIALIZED; + } + } + return return_type::OK; + } + + return_type recover(bool auto_start) override + { + if (hardware.recover_hardware(auto_start) == return_type::OK) { + if (auto_start) { + start(); + } else { + state = component_state::INITIALIZED; + } + } + return return_type::OK; + } + + return_type start() override + { + if (hardware.start_hardware() == return_type::OK) { + state = component_state::STARTED; + } + return return_type::OK; + } + + return_type stop() override + { + if (hardware.stop_hardware() == return_type::OK) { + state = component_state::STOPPED; + } + return return_type::OK; + } + + return_type halt() override + { + if (hardware.halt_hardware() == return_type::OK) { + state = component_state::HALTED; + } + return return_type::OK; + } + + component_state get_state() const override + { + return state; + } + + return_type read(double & data) override + { + int hw_data; + hardware.read_from_hardware(hw_data); + data = hw_data * binary_to_voltage_factor; + return return_type::OK; + } + +private: + ComponentInfo info; + + component_state state = component_state::UNKNOWN; + + DummySensorHardware hardware; + + double binary_to_voltage_factor; }; class DummySystem : public SystemInterface @@ -96,11 +242,16 @@ class DummySystem : public SystemInterface } // namespace component_interfaces_test } // namespace hardware_interface +using hardware_interface::return_type; using hardware_interface::ComponentInfo; using hardware_interface::Joint; using hardware_interface::JointInterface; +using hardware_interface::Sensor; +using hardware_interface::SensorInterface; + using hardware_interface::component_interfaces_test::DummyJoint; -using hardware_interface::return_type; +using hardware_interface::component_interfaces_test::DummySensor; +using hardware_interface::component_state; TEST(TestJointInterface, joint_interfce_works) { @@ -113,7 +264,7 @@ TEST(TestJointInterface, joint_interfce_works) joint_info.parameters["min_value"] = "-1"; joint_info.parameters["max_value"] = "1"; - joint.configure(joint_info); + EXPECT_EQ(joint.configure(joint_info), return_type::OK); EXPECT_EQ(joint.get_interface_name(), "dummy"); EXPECT_EQ(joint.is_started(), false); EXPECT_EQ(joint.start(), return_type::OK); @@ -125,3 +276,41 @@ TEST(TestJointInterface, joint_interfce_works) EXPECT_EQ(joint.read(output), return_type::OK); EXPECT_DOUBLE_EQ(output, 0.5); } + +TEST(TestSensorInterfaceWithHardware, sensor_interface_with_hardware_works) +{ + std::unique_ptr sensor_interface(new DummySensor); + Sensor sensor(sensor_interface); + + ComponentInfo sensor_info; + sensor_info.name = "DummySensor"; + sensor_info.interface_names.push_back("dummyS"); + sensor_info.parameters["binary_to_voltage_factor"] = "0.0048828125"; + sensor_info.hardware_class_type = "DummySensorHardware"; + + EXPECT_EQ(sensor.configure(sensor_info), return_type::ERROR); + EXPECT_EQ(sensor.get_state(), component_state::UNKNOWN); + + sensor_info.hardware_parameters["start_value"] = "128"; + + EXPECT_EQ(sensor.configure(sensor_info), return_type::OK); + EXPECT_EQ(sensor.get_state(), component_state::CONFIGURED); + EXPECT_EQ(sensor.get_interface_name(), "dummyS"); + EXPECT_EQ(sensor.initalize(false), return_type::OK); + EXPECT_EQ(sensor.get_state(), component_state::INITIALIZED); + EXPECT_EQ(sensor.initalize(true), return_type::OK); + EXPECT_EQ(sensor.get_state(), component_state::STARTED); + EXPECT_EQ(sensor.stop(), return_type::OK); + EXPECT_EQ(sensor.get_state(), component_state::STOPPED); + EXPECT_EQ(sensor.start(), return_type::OK); + EXPECT_EQ(sensor.get_state(), component_state::STARTED); + EXPECT_EQ(sensor.halt(), return_type::OK); + EXPECT_EQ(sensor.get_state(), component_state::HALTED); + EXPECT_EQ(sensor.recover(false), return_type::OK); + EXPECT_EQ(sensor.get_state(), component_state::INITIALIZED); + EXPECT_EQ(sensor.start(), return_type::OK); + EXPECT_EQ(sensor.get_state(), component_state::STARTED); + double output; + EXPECT_EQ(sensor.read(output), return_type::OK); + EXPECT_DOUBLE_EQ(output, 0.0048828125 * (128 + 128)); +} From d7eae16bbf9c7b627016c27b557318eb8089d849 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Tue, 28 Jul 2020 15:35:48 +0200 Subject: [PATCH 05/29] Remove component types since they are not used yet. --- .../include/hardware_interface/component_info.hpp | 7 ------- 1 file changed, 7 deletions(-) diff --git a/hardware_interface/include/hardware_interface/component_info.hpp b/hardware_interface/include/hardware_interface/component_info.hpp index 293730e5b0..d37228895c 100644 --- a/hardware_interface/include/hardware_interface/component_info.hpp +++ b/hardware_interface/include/hardware_interface/component_info.hpp @@ -23,13 +23,6 @@ namespace hardware_interface { -/** - * \brief constants for types of components. - */ -constexpr const auto robotType = "robot"; -constexpr const auto actuatorType = "actuator"; -constexpr const auto sensorType = "sensor"; - /** * \brief This structure stores information about components defined in a robot's URDF. */ From 818846d420eca7b30a28a7ccf2efdb67d34d6e57 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Wed, 29 Jul 2020 14:42:25 +0200 Subject: [PATCH 06/29] Added tests for all interfaces and implemented review comments --- .../hardware_interface/component_info.hpp | 32 +-- .../component_interfaces/joint_interface.hpp | 3 +- .../component_interfaces/sensor_interface.hpp | 14 +- .../component_interfaces/system_interface.hpp | 5 +- .../include/hardware_interface/joint.hpp | 19 +- .../include/hardware_interface/sensor.hpp | 27 +- .../include/hardware_interface/system.hpp | 8 +- .../types/hardware_interface_state_values.hpp | 2 - .../test/test_component_interfaces.cpp | 234 ++++++++++++------ 9 files changed, 182 insertions(+), 162 deletions(-) diff --git a/hardware_interface/include/hardware_interface/component_info.hpp b/hardware_interface/include/hardware_interface/component_info.hpp index d37228895c..eae15dd123 100644 --- a/hardware_interface/include/hardware_interface/component_info.hpp +++ b/hardware_interface/include/hardware_interface/component_info.hpp @@ -43,7 +43,7 @@ struct ComponentInfo /** * \brief joint where component is placed. */ - std::string joint; + std::vector joints; /** * \brief name of the interface, e.g. "position", "velocity", etc. for meaning of data this component holds. */ @@ -54,7 +54,7 @@ struct ComponentInfo std::unordered_map parameters; /** - * \brief (optional) hardware class of the component that will be dynamically loaded. If not defined, the system's hardware class has to be defined. + * \brief hardware class of the component that will be dynamically loaded. */ std::string hardware_class_type; /** @@ -63,34 +63,6 @@ struct ComponentInfo std::unordered_map hardware_parameters; }; -/** - * \brief This structure stores informations about system defined in robot's URDF, i.e. "ros2_control"-tag. - */ -struct SystemInfo -{ - /** - * \brief name of the system. - */ - std::string name; - /** - * \brief type of the system: robot, joints or sensor. Note: URDF always needs a "robot" tag, nevertheless in terms of ros2_control, it can hold a definition for an joint or sensor. - */ - std::string type; - /** - * \brief (optional) hardware class of the system, which will be dynamically loaded. If not defined, a hardware class for each subcomponent has to be defined. - */ - std::string hardware_class_type; - /** - * \brief (optional) key-value pairs for systems hardware. - */ - std::unordered_map hardware_parameters; - - /** - * \brief list of subcomponents in the system, i.e., list of sensors and actuators. - */ - std::vector subcomponents; -}; - } // namespace hardware_interface #endif // HARDWARE_INTERFACE__COMPONENT_INFO_HPP_ diff --git a/hardware_interface/include/hardware_interface/component_interfaces/joint_interface.hpp b/hardware_interface/include/hardware_interface/component_interfaces/joint_interface.hpp index 9589323ec5..7b0bbba830 100644 --- a/hardware_interface/include/hardware_interface/component_interfaces/joint_interface.hpp +++ b/hardware_interface/include/hardware_interface/component_interfaces/joint_interface.hpp @@ -22,6 +22,7 @@ #include "hardware_interface/component_info.hpp" #include "hardware_interface/types/hardware_interface_return_values.hpp" +#include "hardware_interface/types/hardware_interface_state_values.hpp" #include "hardware_interface/visibility_control.h" namespace hardware_interface @@ -55,7 +56,7 @@ class JointInterface HARDWARE_INTERFACE_PUBLIC virtual - bool is_started() const = 0; + component_state get_state() const = 0; HARDWARE_INTERFACE_PUBLIC virtual diff --git a/hardware_interface/include/hardware_interface/component_interfaces/sensor_interface.hpp b/hardware_interface/include/hardware_interface/component_interfaces/sensor_interface.hpp index 39e5c8d5cf..b65d9beb86 100644 --- a/hardware_interface/include/hardware_interface/component_interfaces/sensor_interface.hpp +++ b/hardware_interface/include/hardware_interface/component_interfaces/sensor_interface.hpp @@ -41,11 +41,7 @@ class SensorInterface HARDWARE_INTERFACE_PUBLIC virtual - return_type initalize(bool auto_start) = 0; - - HARDWARE_INTERFACE_PUBLIC - virtual - return_type recover(bool auto_start) = 0; + std::string get_interface_name() const = 0; HARDWARE_INTERFACE_PUBLIC virtual @@ -55,10 +51,6 @@ class SensorInterface virtual return_type stop() = 0; - HARDWARE_INTERFACE_PUBLIC - virtual - return_type halt() = 0; - HARDWARE_INTERFACE_PUBLIC virtual component_state get_state() const = 0; @@ -66,10 +58,6 @@ class SensorInterface HARDWARE_INTERFACE_PUBLIC virtual return_type read(double & data) = 0; - - HARDWARE_INTERFACE_PUBLIC - virtual - std::string get_interface_name() const = 0; }; } // namespace hardware_interface diff --git a/hardware_interface/include/hardware_interface/component_interfaces/system_interface.hpp b/hardware_interface/include/hardware_interface/component_interfaces/system_interface.hpp index 6b460357be..6ed894f5b4 100644 --- a/hardware_interface/include/hardware_interface/component_interfaces/system_interface.hpp +++ b/hardware_interface/include/hardware_interface/component_interfaces/system_interface.hpp @@ -20,6 +20,7 @@ #include "hardware_interface/component_info.hpp" #include "hardware_interface/types/hardware_interface_return_values.hpp" +#include "hardware_interface/types/hardware_interface_state_values.hpp" #include "hardware_interface/visibility_control.h" namespace hardware_interface @@ -37,7 +38,7 @@ class SystemInterface HARDWARE_INTERFACE_PUBLIC virtual - return_type configure(const SystemInfo & system_info) = 0; + return_type configure(const ComponentInfo & system_info) = 0; HARDWARE_INTERFACE_PUBLIC virtual @@ -53,7 +54,7 @@ class SystemInterface HARDWARE_INTERFACE_PUBLIC virtual - bool is_started() const = 0; + component_state get_state() const = 0; HARDWARE_INTERFACE_PUBLIC virtual diff --git a/hardware_interface/include/hardware_interface/joint.hpp b/hardware_interface/include/hardware_interface/joint.hpp index ce4c95d918..9af5ce4a3d 100644 --- a/hardware_interface/include/hardware_interface/joint.hpp +++ b/hardware_interface/include/hardware_interface/joint.hpp @@ -23,6 +23,7 @@ #include "hardware_interface/component_info.hpp" #include "hardware_interface/component_interfaces/joint_interface.hpp" #include "hardware_interface/types/hardware_interface_return_values.hpp" +#include "hardware_interface/types/hardware_interface_state_values.hpp" #include "hardware_interface/visibility_control.h" namespace hardware_interface @@ -33,7 +34,7 @@ class Joint final public: Joint() = default; - explicit Joint(std::unique_ptr & impl) + explicit Joint(std::unique_ptr impl) : impl_(std::move(impl)) {} @@ -45,6 +46,12 @@ class Joint final return impl_->configure(joint_info); } + HARDWARE_INTERFACE_PUBLIC + std::string get_interface_name() const + { + return impl_->get_interface_name(); + } + HARDWARE_INTERFACE_PUBLIC return_type start() { @@ -58,9 +65,9 @@ class Joint final } HARDWARE_INTERFACE_PUBLIC - bool is_started() const + component_state get_state() const { - return impl_->is_started(); + return impl_->get_state(); } HARDWARE_INTERFACE_PUBLIC @@ -75,12 +82,6 @@ class Joint final return impl_->write(data); } - HARDWARE_INTERFACE_PUBLIC - std::string get_interface_name() const - { - return impl_->get_interface_name(); - } - private: std::unique_ptr impl_; }; diff --git a/hardware_interface/include/hardware_interface/sensor.hpp b/hardware_interface/include/hardware_interface/sensor.hpp index b61431c45b..5010f5375b 100644 --- a/hardware_interface/include/hardware_interface/sensor.hpp +++ b/hardware_interface/include/hardware_interface/sensor.hpp @@ -22,8 +22,8 @@ #include "hardware_interface/component_info.hpp" #include "hardware_interface/component_interfaces/sensor_interface.hpp" -#include "hardware_interface/types/hardware_interface_state_values.hpp" #include "hardware_interface/types/hardware_interface_return_values.hpp" +#include "hardware_interface/types/hardware_interface_state_values.hpp" #include "hardware_interface/visibility_control.h" namespace hardware_interface @@ -34,7 +34,7 @@ class Sensor final public: Sensor() = default; - explicit Sensor(std::unique_ptr & impl) + explicit Sensor(std::unique_ptr impl) : impl_(std::move(impl)) {} @@ -47,16 +47,11 @@ class Sensor final } HARDWARE_INTERFACE_PUBLIC - return_type initalize(bool auto_start) + std::string get_interface_name() const { - return impl_->initalize(auto_start); + return impl_->get_interface_name(); } - HARDWARE_INTERFACE_PUBLIC - return_type recover(bool auto_start) - { - return impl_->recover(auto_start); - } HARDWARE_INTERFACE_PUBLIC return_type start() @@ -71,13 +66,7 @@ class Sensor final } HARDWARE_INTERFACE_PUBLIC - return_type halt() - { - return impl_->halt(); - } - - HARDWARE_INTERFACE_PUBLIC - component_state get_state() + component_state get_state() const { return impl_->get_state(); } @@ -88,12 +77,6 @@ class Sensor final return impl_->read(data); } - HARDWARE_INTERFACE_PUBLIC - std::string get_interface_name() - { - return impl_->get_interface_name(); - } - private: std::unique_ptr impl_; }; diff --git a/hardware_interface/include/hardware_interface/system.hpp b/hardware_interface/include/hardware_interface/system.hpp index eecf5b68b5..0f5acf2303 100644 --- a/hardware_interface/include/hardware_interface/system.hpp +++ b/hardware_interface/include/hardware_interface/system.hpp @@ -22,6 +22,8 @@ #include "hardware_interface/component_info.hpp" #include "hardware_interface/component_interfaces/system_interface.hpp" +#include "hardware_interface/types/hardware_interface_return_values.hpp" +#include "hardware_interface/types/hardware_interface_state_values.hpp" #include "hardware_interface/visibility_control.h" namespace hardware_interface @@ -37,7 +39,7 @@ class System final virtual ~System() = default; HARDWARE_INTERFACE_PUBLIC - return_type configure(const SystemInfo & system_info) + return_type configure(const ComponentInfo & system_info) { return impl_->configure(system_info); } @@ -61,9 +63,9 @@ class System final } HARDWARE_INTERFACE_PUBLIC - bool is_started() + component_state get_state() const { - return impl_->is_started(); + return impl_->get_state(); } HARDWARE_INTERFACE_PUBLIC diff --git a/hardware_interface/include/hardware_interface/types/hardware_interface_state_values.hpp b/hardware_interface/include/hardware_interface/types/hardware_interface_state_values.hpp index a4d20991e7..9f181ee825 100644 --- a/hardware_interface/include/hardware_interface/types/hardware_interface_state_values.hpp +++ b/hardware_interface/include/hardware_interface/types/hardware_interface_state_values.hpp @@ -23,10 +23,8 @@ enum class component_state : std::uint8_t { UNKNOWN = 0, CONFIGURED = 1, - INITIALIZED = 2, STARTED = 3, STOPPED = 4, - HALTED = 5, }; } // namespace hardware_interface diff --git a/hardware_interface/test/test_component_interfaces.cpp b/hardware_interface/test/test_component_interfaces.cpp index c4b3fed91b..0bab63f878 100644 --- a/hardware_interface/test/test_component_interfaces.cpp +++ b/hardware_interface/test/test_component_interfaces.cpp @@ -16,6 +16,7 @@ #include #include #include +#include #include "hardware_interface/component_info.hpp" #include "hardware_interface/component_interfaces/joint_interface.hpp" #include "hardware_interface/component_interfaces/sensor_interface.hpp" @@ -37,6 +38,7 @@ class DummyJoint : public JointInterface info = joint_info; min_value = stod(info.parameters["min_value"]); max_value = stod(info.parameters["max_value"]); + state = component_state::CONFIGURED; return return_type::OK; } @@ -47,19 +49,27 @@ class DummyJoint : public JointInterface return_type start() override { - started = true; + if (state == component_state::CONFIGURED || state == component_state::STOPPED) { + state = component_state::STARTED; + } else { + return return_type::ERROR; + } return return_type::OK; } return_type stop() override { - started = false; + if (state == component_state::STARTED) { + state = component_state::STOPPED; + } else { + return return_type::ERROR; + } return return_type::OK; } - bool is_started() const override + component_state get_state() const override { - return started; + return state; } return_type read(double & data) override @@ -80,11 +90,9 @@ class DummyJoint : public JointInterface private: ComponentInfo info; - + component_state state = component_state::UNKNOWN; double min_value, max_value; - double joint_value; - bool started; }; class DummySensorHardware @@ -103,18 +111,6 @@ class DummySensorHardware return return_type::ERROR; } - return_type initalize_hardware(bool auto_start) - { - (void) auto_start; - return return_type::OK; - } - - return_type recover_hardware(bool auto_start) - { - (void) auto_start; - return return_type::OK; - } - return_type start_hardware() { return return_type::OK; @@ -125,11 +121,6 @@ class DummySensorHardware return return_type::OK; } - return_type halt_hardware() - { - return return_type::OK; - } - return_type read_from_hardware(int & data) { data = value + 128; @@ -164,50 +155,96 @@ class DummySensor : public SensorInterface return info.interface_names.front(); } - return_type initalize(bool auto_start) override + return_type start() override { - if (hardware.initalize_hardware(auto_start) == return_type::OK) { - if (auto_start) { - start(); - } else { - state = component_state::INITIALIZED; + if (state == component_state::CONFIGURED || state == component_state::STOPPED) { + if (hardware.start_hardware() == return_type::OK) { + state = component_state::STARTED; } + } else { + return return_type::ERROR; } return return_type::OK; } - return_type recover(bool auto_start) override + return_type stop() override { - if (hardware.recover_hardware(auto_start) == return_type::OK) { - if (auto_start) { - start(); - } else { - state = component_state::INITIALIZED; + if (state == component_state::STARTED) { + if (hardware.stop_hardware() == return_type::OK) { + state = component_state::STOPPED; } + } else { + return return_type::ERROR; } return return_type::OK; } - return_type start() override + component_state get_state() const override { - if (hardware.start_hardware() == return_type::OK) { - state = component_state::STARTED; + return state; + } + + return_type read(double & data) override + { + int hw_data; + hardware.read_from_hardware(hw_data); + data = hw_data * binary_to_voltage_factor; + return return_type::OK; + } + +private: + ComponentInfo info; + component_state state = component_state::UNKNOWN; + DummySensorHardware hardware; + double binary_to_voltage_factor; +}; + +class DummySystem : public SystemInterface +{ + return_type configure(const ComponentInfo & system_info) override + { + info = system_info; + n_dof = info.joints.size(); + min_value.resize(n_dof); + max_value.resize(n_dof); + std::size_t current, previous = 0; + const char delim = ','; + for (uint i = 0; i < n_dof; i++) { + current = info.parameters["min_value"].find(delim, previous); + min_value[i] = stod(info.parameters["min_value"].substr(previous, current - previous)); + previous = current + 1; + } + for (uint i = 0; i < n_dof; i++) { + current = info.parameters["max_value"].find(delim, previous); + max_value[i] = stod(info.parameters["max_value"].substr(previous, current - previous)); + previous = current + 1; } + joint_value.resize(n_dof); + state = component_state::CONFIGURED; return return_type::OK; } - return_type stop() override + std::vector get_interface_names() const override { - if (hardware.stop_hardware() == return_type::OK) { - state = component_state::STOPPED; + return info.interface_names; + } + + return_type start() override + { + if (state == component_state::CONFIGURED || state == component_state::STOPPED) { + state = component_state::STARTED; + } else { + return return_type::ERROR; } return return_type::OK; } - return_type halt() override + return_type stop() override { - if (hardware.halt_hardware() == return_type::OK) { - state = component_state::HALTED; + if (state == component_state::STARTED) { + state = component_state::STOPPED; + } else { + return return_type::ERROR; } return return_type::OK; } @@ -217,26 +254,35 @@ class DummySensor : public SensorInterface return state; } - return_type read(double & data) override + return_type read(std::vector & data) override { - int hw_data; - hardware.read_from_hardware(hw_data); - data = hw_data * binary_to_voltage_factor; + data = joint_value; return return_type::OK; } + return_type write(const std::vector & data) override + { + return_type ret = return_type::OK; + if (data.size() == n_dof) { + for (uint i = 0; i < n_dof; i++) { + if (data[i] > min_value[i] && data[i] < max_value[i]) { + joint_value[i] = data[i] / 2; + } else { + ret = return_type::ERROR; + } + } + } else { + ret = return_type::ERROR; + } + return ret; + } + private: ComponentInfo info; - - component_state state = component_state::UNKNOWN; - - DummySensorHardware hardware; - - double binary_to_voltage_factor; -}; - -class DummySystem : public SystemInterface -{ + component_state state; + uint n_dof; + std::vector min_value, max_value; + std::vector joint_value; }; } // namespace component_interfaces_test @@ -248,15 +294,17 @@ using hardware_interface::Joint; using hardware_interface::JointInterface; using hardware_interface::Sensor; using hardware_interface::SensorInterface; +using hardware_interface::System; +using hardware_interface::SystemInterface; using hardware_interface::component_interfaces_test::DummyJoint; using hardware_interface::component_interfaces_test::DummySensor; +using hardware_interface::component_interfaces_test::DummySystem; using hardware_interface::component_state; -TEST(TestJointInterface, joint_interfce_works) +TEST(TestJointInterface, joint_interface_works) { - std::unique_ptr joint_interface(new DummyJoint); - Joint joint(joint_interface); + Joint joint(std::make_unique()); ComponentInfo joint_info; joint_info.name = "DummyJoint"; @@ -266,25 +314,25 @@ TEST(TestJointInterface, joint_interfce_works) EXPECT_EQ(joint.configure(joint_info), return_type::OK); EXPECT_EQ(joint.get_interface_name(), "dummy"); - EXPECT_EQ(joint.is_started(), false); + EXPECT_EQ(joint.get_state(), component_state::CONFIGURED); EXPECT_EQ(joint.start(), return_type::OK); - EXPECT_EQ(joint.is_started(), true); - EXPECT_EQ(joint.stop(), return_type::OK); + EXPECT_EQ(joint.get_state(), component_state::STARTED); EXPECT_EQ(joint.write(2), return_type::ERROR); EXPECT_EQ(joint.write(0.5), return_type::OK); - double output; + double output = 0.0; EXPECT_EQ(joint.read(output), return_type::OK); EXPECT_DOUBLE_EQ(output, 0.5); + EXPECT_EQ(joint.stop(), return_type::OK); + EXPECT_EQ(joint.get_state(), component_state::STOPPED); } TEST(TestSensorInterfaceWithHardware, sensor_interface_with_hardware_works) { - std::unique_ptr sensor_interface(new DummySensor); - Sensor sensor(sensor_interface); + Sensor sensor(std::make_unique()); ComponentInfo sensor_info; sensor_info.name = "DummySensor"; - sensor_info.interface_names.push_back("dummyS"); + sensor_info.interface_names.push_back("dummySensor"); sensor_info.parameters["binary_to_voltage_factor"] = "0.0048828125"; sensor_info.hardware_class_type = "DummySensorHardware"; @@ -295,22 +343,48 @@ TEST(TestSensorInterfaceWithHardware, sensor_interface_with_hardware_works) EXPECT_EQ(sensor.configure(sensor_info), return_type::OK); EXPECT_EQ(sensor.get_state(), component_state::CONFIGURED); - EXPECT_EQ(sensor.get_interface_name(), "dummyS"); - EXPECT_EQ(sensor.initalize(false), return_type::OK); - EXPECT_EQ(sensor.get_state(), component_state::INITIALIZED); - EXPECT_EQ(sensor.initalize(true), return_type::OK); + EXPECT_EQ(sensor.get_interface_name(), "dummySensor"); + EXPECT_EQ(sensor.start(), return_type::OK); EXPECT_EQ(sensor.get_state(), component_state::STARTED); EXPECT_EQ(sensor.stop(), return_type::OK); EXPECT_EQ(sensor.get_state(), component_state::STOPPED); EXPECT_EQ(sensor.start(), return_type::OK); EXPECT_EQ(sensor.get_state(), component_state::STARTED); - EXPECT_EQ(sensor.halt(), return_type::OK); - EXPECT_EQ(sensor.get_state(), component_state::HALTED); - EXPECT_EQ(sensor.recover(false), return_type::OK); - EXPECT_EQ(sensor.get_state(), component_state::INITIALIZED); - EXPECT_EQ(sensor.start(), return_type::OK); - EXPECT_EQ(sensor.get_state(), component_state::STARTED); - double output; + double output = 0.0; EXPECT_EQ(sensor.read(output), return_type::OK); EXPECT_DOUBLE_EQ(output, 0.0048828125 * (128 + 128)); } + +TEST(TestJointInterface, system_interface_works) +{ + System system(std::make_unique()); + + ComponentInfo system_info; + system_info.name = "DummyJoint"; + system_info.interface_names.push_back("dummy_position"); + system_info.interface_names.push_back("dummy_velocity"); + system_info.joints.push_back("joint1"); + system_info.joints.push_back("joint2"); + system_info.parameters["min_value"] = "0, 0"; + system_info.parameters["max_value"] = "4, 4"; + + EXPECT_EQ(system.configure(system_info), return_type::OK); + EXPECT_EQ(system.get_interface_names()[0], "dummy_position"); + EXPECT_EQ(system.get_interface_names()[1], "dummy_velocity"); + EXPECT_EQ(system.get_state(), component_state::CONFIGURED); + EXPECT_EQ(system.start(), return_type::OK); + EXPECT_EQ(system.get_state(), component_state::STARTED); + std::vector input; + input.push_back(2); + EXPECT_EQ(system.write(input), return_type::ERROR); + input.push_back(3); + EXPECT_EQ(system.write(input), return_type::OK); + std::vector output; + output.push_back(0.0); + output.push_back(0.0); + EXPECT_EQ(system.read(output), return_type::OK); + EXPECT_DOUBLE_EQ(output[0], 1.0); + EXPECT_DOUBLE_EQ(output[1], 1.5); + EXPECT_EQ(system.stop(), return_type::OK); + EXPECT_EQ(system.get_state(), component_state::STOPPED); +} From 3fead1f88f13070626d99f734353d4e12c409431 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Mon, 17 Aug 2020 14:17:41 +0200 Subject: [PATCH 07/29] Added HardwareInfo as the main structure in the robot's URDF file --- .../hardware_interface/component_info.hpp | 68 ------------- .../hardware_and_component_info.hpp | 99 +++++++++++++++++++ 2 files changed, 99 insertions(+), 68 deletions(-) delete mode 100644 hardware_interface/include/hardware_interface/component_info.hpp create mode 100644 hardware_interface/include/hardware_interface/hardware_and_component_info.hpp diff --git a/hardware_interface/include/hardware_interface/component_info.hpp b/hardware_interface/include/hardware_interface/component_info.hpp deleted file mode 100644 index eae15dd123..0000000000 --- a/hardware_interface/include/hardware_interface/component_info.hpp +++ /dev/null @@ -1,68 +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__COMPONENT_INFO_HPP_ -#define HARDWARE_INTERFACE__COMPONENT_INFO_HPP_ - -#include -#include -#include - -namespace hardware_interface -{ - -/** - * \brief This structure stores information about components defined in a 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 joint where component is placed. - */ - std::vector joints; - /** - * \brief name of the interface, e.g. "position", "velocity", etc. for meaning of data this component holds. - */ - std::vector interface_names; - /** - * \brief (optional) key-value pairs of components parameters. - */ - std::unordered_map parameters; - - /** - * \brief hardware class of the component that will be dynamically loaded. - */ - std::string hardware_class_type; - /** - * \brief (optional) key-value pairs for components hardware. - */ - std::unordered_map hardware_parameters; -}; - -} // namespace hardware_interface - -#endif // HARDWARE_INTERFACE__COMPONENT_INFO_HPP_ diff --git a/hardware_interface/include/hardware_interface/hardware_and_component_info.hpp b/hardware_interface/include/hardware_interface/hardware_and_component_info.hpp new file mode 100644 index 0000000000..600037aa95 --- /dev/null +++ b/hardware_interface/include/hardware_interface/hardware_and_component_info.hpp @@ -0,0 +1,99 @@ +// 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_AND_COMPONENT_INFO_HPP_ +#define HARDWARE_INTERFACE__HARDWARE_AND_COMPONENT_INFO_HPP_ + +#include +#include +#include + +namespace hardware_interface +{ + +/** + * \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; +}; + +/** + * \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_AND_COMPONENT_INFO_HPP_ From 2fea78dbc1272caf2f5ef610c238d89a78652c7b Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Mon, 17 Aug 2020 14:18:49 +0200 Subject: [PATCH 08/29] Add Hardware and Component base classes and tests. --- .../hardware_interface/actuator_hardware.hpp | 85 +++ .../actuator_hardware_interface.hpp | 108 ++++ .../component_interfaces/joint_interface.hpp | 71 --- .../component_interfaces/sensor_interface.hpp | 64 -- .../component_interfaces/system_interface.hpp | 71 --- .../include/hardware_interface/joint.hpp | 90 ++- .../include/hardware_interface/sensor.hpp | 72 +-- .../hardware_interface/sensor_hardware.hpp | 80 +++ .../sensor_hardware_interface.hpp | 96 +++ .../{system.hpp => system_hardware.hpp} | 48 +- .../system_hardware_interface.hpp | 122 ++++ ...p => hardware_interface_status_values.hpp} | 8 +- .../test/test_component_interfaces.cpp | 603 ++++++++++++------ 13 files changed, 976 insertions(+), 542 deletions(-) create mode 100644 hardware_interface/include/hardware_interface/actuator_hardware.hpp create mode 100644 hardware_interface/include/hardware_interface/actuator_hardware_interface.hpp delete mode 100644 hardware_interface/include/hardware_interface/component_interfaces/joint_interface.hpp delete mode 100644 hardware_interface/include/hardware_interface/component_interfaces/sensor_interface.hpp delete mode 100644 hardware_interface/include/hardware_interface/component_interfaces/system_interface.hpp create mode 100644 hardware_interface/include/hardware_interface/sensor_hardware.hpp create mode 100644 hardware_interface/include/hardware_interface/sensor_hardware_interface.hpp rename hardware_interface/include/hardware_interface/{system.hpp => system_hardware.hpp} (56%) create mode 100644 hardware_interface/include/hardware_interface/system_hardware_interface.hpp rename hardware_interface/include/hardware_interface/types/{hardware_interface_state_values.hpp => hardware_interface_status_values.hpp} (73%) 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..8dbcfca823 --- /dev/null +++ b/hardware_interface/include/hardware_interface/actuator_hardware.hpp @@ -0,0 +1,85 @@ +// 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 +#include +#include + +#include "hardware_interface/hardware_and_component_info.hpp" +#include "hardware_interface/actuator_hardware_interface.hpp" +#include "hardware_interface/joint.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 +{ + +class ActuatorHardware final +{ +public: + ActuatorHardware() = default; + + explicit ActuatorHardware(std::unique_ptr impl) + : impl_(std::move(impl)) + {} + + ~ActuatorHardware() = default; + + HARDWARE_INTERFACE_PUBLIC + return_type configure(const HardwareInfo & actuator_info) + { + return impl_->configure(actuator_info); + } + + HARDWARE_INTERFACE_PUBLIC + return_type start() + { + return impl_->start(); + } + + HARDWARE_INTERFACE_PUBLIC + return_type stop() + { + return impl_->stop(); + } + + HARDWARE_INTERFACE_PUBLIC + hardware_interface_status get_status() + { + return impl_->get_status(); + } + + HARDWARE_INTERFACE_PUBLIC + return_type read_joint(Joint & joint) + { + return impl_->read_joint(joint); + } + + HARDWARE_INTERFACE_PUBLIC + return_type write(const Joint & joint) + { + return impl_->write(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..78dfa4d7b0 --- /dev/null +++ b/hardware_interface/include/hardware_interface/actuator_hardware_interface.hpp @@ -0,0 +1,108 @@ +// 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 +#include +#include + +#include "hardware_interface/hardware_and_component_info.hpp" +#include "hardware_interface/joint.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 RessourceManger. + * + * @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(Joint & 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 RessourceManger. + * + * @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(const Joint & joint) = 0; +}; + +} // namespace hardware_interface +#endif // HARDWARE_INTERFACE__ACTUATOR_HARDWARE_INTERFACE_HPP_ diff --git a/hardware_interface/include/hardware_interface/component_interfaces/joint_interface.hpp b/hardware_interface/include/hardware_interface/component_interfaces/joint_interface.hpp deleted file mode 100644 index 7b0bbba830..0000000000 --- a/hardware_interface/include/hardware_interface/component_interfaces/joint_interface.hpp +++ /dev/null @@ -1,71 +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__COMPONENT_INTERFACES__JOINT_INTERFACE_HPP_ -#define HARDWARE_INTERFACE__COMPONENT_INTERFACES__JOINT_INTERFACE_HPP_ - -#include -#include -#include -#include - -#include "hardware_interface/component_info.hpp" -#include "hardware_interface/types/hardware_interface_return_values.hpp" -#include "hardware_interface/types/hardware_interface_state_values.hpp" -#include "hardware_interface/visibility_control.h" - -namespace hardware_interface -{ - -class JointInterface -{ -public: - HARDWARE_INTERFACE_PUBLIC - JointInterface() = default; - - HARDWARE_INTERFACE_PUBLIC - virtual - ~JointInterface() = default; - - HARDWARE_INTERFACE_PUBLIC - virtual - return_type configure(const ComponentInfo & joint_info) = 0; - - HARDWARE_INTERFACE_PUBLIC - virtual - std::string get_interface_name() const = 0; - - HARDWARE_INTERFACE_PUBLIC - virtual - return_type start() = 0; - - HARDWARE_INTERFACE_PUBLIC - virtual - return_type stop() = 0; - - HARDWARE_INTERFACE_PUBLIC - virtual - component_state get_state() const = 0; - - HARDWARE_INTERFACE_PUBLIC - virtual - return_type read(double & data) = 0; - - HARDWARE_INTERFACE_PUBLIC - virtual - return_type write(const double & data) = 0; -}; - -} // namespace hardware_interface -#endif // HARDWARE_INTERFACE__COMPONENT_INTERFACES__JOINT_INTERFACE_HPP_ diff --git a/hardware_interface/include/hardware_interface/component_interfaces/sensor_interface.hpp b/hardware_interface/include/hardware_interface/component_interfaces/sensor_interface.hpp deleted file mode 100644 index b65d9beb86..0000000000 --- a/hardware_interface/include/hardware_interface/component_interfaces/sensor_interface.hpp +++ /dev/null @@ -1,64 +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__COMPONENT_INTERFACES__SENSOR_INTERFACE_HPP_ -#define HARDWARE_INTERFACE__COMPONENT_INTERFACES__SENSOR_INTERFACE_HPP_ - -#include - -#include "hardware_interface/component_info.hpp" -#include "hardware_interface/types/hardware_interface_return_values.hpp" -#include "hardware_interface/types/hardware_interface_state_values.hpp" -#include "hardware_interface/visibility_control.h" - -namespace hardware_interface -{ - -class SensorInterface -{ -public: - HARDWARE_INTERFACE_PUBLIC - SensorInterface() = default; - - HARDWARE_INTERFACE_PUBLIC - virtual - ~SensorInterface() = default; - - HARDWARE_INTERFACE_PUBLIC - virtual - return_type configure(const ComponentInfo & sensor_info) = 0; - - HARDWARE_INTERFACE_PUBLIC - virtual - std::string get_interface_name() const = 0; - - HARDWARE_INTERFACE_PUBLIC - virtual - return_type start() = 0; - - HARDWARE_INTERFACE_PUBLIC - virtual - return_type stop() = 0; - - HARDWARE_INTERFACE_PUBLIC - virtual - component_state get_state() const = 0; - - HARDWARE_INTERFACE_PUBLIC - virtual - return_type read(double & data) = 0; -}; - -} // namespace hardware_interface -#endif // HARDWARE_INTERFACE__COMPONENT_INTERFACES__SENSOR_INTERFACE_HPP_ diff --git a/hardware_interface/include/hardware_interface/component_interfaces/system_interface.hpp b/hardware_interface/include/hardware_interface/component_interfaces/system_interface.hpp deleted file mode 100644 index 6ed894f5b4..0000000000 --- a/hardware_interface/include/hardware_interface/component_interfaces/system_interface.hpp +++ /dev/null @@ -1,71 +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__COMPONENT_INTERFACES__SYSTEM_INTERFACE_HPP_ -#define HARDWARE_INTERFACE__COMPONENT_INTERFACES__SYSTEM_INTERFACE_HPP_ - -#include -#include - -#include "hardware_interface/component_info.hpp" -#include "hardware_interface/types/hardware_interface_return_values.hpp" -#include "hardware_interface/types/hardware_interface_state_values.hpp" -#include "hardware_interface/visibility_control.h" - -namespace hardware_interface -{ - -class SystemInterface -{ -public: - HARDWARE_INTERFACE_PUBLIC - SystemInterface() = default; - - HARDWARE_INTERFACE_PUBLIC - virtual - ~SystemInterface() = default; - - HARDWARE_INTERFACE_PUBLIC - virtual - return_type configure(const ComponentInfo & system_info) = 0; - - HARDWARE_INTERFACE_PUBLIC - virtual - std::vector get_interface_names() const = 0; - - HARDWARE_INTERFACE_PUBLIC - virtual - return_type start() = 0; - - HARDWARE_INTERFACE_PUBLIC - virtual - return_type stop() = 0; - - HARDWARE_INTERFACE_PUBLIC - virtual - component_state get_state() const = 0; - - HARDWARE_INTERFACE_PUBLIC - virtual - return_type read(std::vector & data) = 0; - - return_type - virtual - write(const std::vector & data) = 0; -}; - -typedef SystemInterface RobotInterface; - -} // namespace hardware_interface -#endif // HARDWARE_INTERFACE__COMPONENT_INTERFACES__SYSTEM_INTERFACE_HPP_ diff --git a/hardware_interface/include/hardware_interface/joint.hpp b/hardware_interface/include/hardware_interface/joint.hpp index 9af5ce4a3d..5862391330 100644 --- a/hardware_interface/include/hardware_interface/joint.hpp +++ b/hardware_interface/include/hardware_interface/joint.hpp @@ -15,75 +15,65 @@ #ifndef HARDWARE_INTERFACE__JOINT_HPP_ #define HARDWARE_INTERFACE__JOINT_HPP_ -#include -#include #include -#include +#include -#include "hardware_interface/component_info.hpp" -#include "hardware_interface/component_interfaces/joint_interface.hpp" +#include "hardware_interface/hardware_and_component_info.hpp" #include "hardware_interface/types/hardware_interface_return_values.hpp" -#include "hardware_interface/types/hardware_interface_state_values.hpp" #include "hardware_interface/visibility_control.h" namespace hardware_interface { -class Joint final +class Joint { public: - Joint() = default; - - explicit Joint(std::unique_ptr impl) - : impl_(std::move(impl)) - {} - - ~Joint() = default; - - HARDWARE_INTERFACE_PUBLIC - return_type configure(const ComponentInfo & joint_info) - { - return impl_->configure(joint_info); - } - - HARDWARE_INTERFACE_PUBLIC - std::string get_interface_name() const - { - return impl_->get_interface_name(); - } - HARDWARE_INTERFACE_PUBLIC - return_type start() - { - return impl_->start(); - } + Joint() = default; HARDWARE_INTERFACE_PUBLIC - return_type stop() - { - return impl_->stop(); - } + virtual + ~Joint() = default; HARDWARE_INTERFACE_PUBLIC - component_state get_state() const - { - return impl_->get_state(); - } + virtual + return_type configure(const ComponentInfo & joint_info) = 0; HARDWARE_INTERFACE_PUBLIC - return_type read(double & data) - { - return impl_->read(data); - } + virtual + std::vector get_command_interfaces() const = 0; HARDWARE_INTERFACE_PUBLIC - return_type write(const double & data) - { - return impl_->write(data); - } - -private: - std::unique_ptr impl_; + virtual + std::vector get_state_interfaces() const = 0; + + HARDWARE_INTERFACE_EXPORT + virtual + return_type get_command( + std::vector & command, + std::vector & interfaces) const = 0; + + HARDWARE_INTERFACE_EXPORT + virtual + return_type set_command( + const std::vector command, + std::vector interfaces = std::vector()) = 0; + + HARDWARE_INTERFACE_EXPORT + virtual + return_type get_state( + std::vector & state, + std::vector & interfaces) const = 0; + + HARDWARE_INTERFACE_EXPORT + virtual + return_type set_state( + const std::vector & state, + std::vector interfaces = std::vector()) = 0; + +protected: + std::vector command_interfaces; + std::vector state_interfaces; }; } // namespace hardware_interface diff --git a/hardware_interface/include/hardware_interface/sensor.hpp b/hardware_interface/include/hardware_interface/sensor.hpp index 5010f5375b..e324edfa1f 100644 --- a/hardware_interface/include/hardware_interface/sensor.hpp +++ b/hardware_interface/include/hardware_interface/sensor.hpp @@ -12,73 +12,51 @@ // See the License for the specific language governing permissions and // limitations under the License. - #ifndef HARDWARE_INTERFACE__SENSOR_HPP_ #define HARDWARE_INTERFACE__SENSOR_HPP_ -#include #include -#include +#include -#include "hardware_interface/component_info.hpp" -#include "hardware_interface/component_interfaces/sensor_interface.hpp" +#include "hardware_interface/hardware_and_component_info.hpp" #include "hardware_interface/types/hardware_interface_return_values.hpp" -#include "hardware_interface/types/hardware_interface_state_values.hpp" #include "hardware_interface/visibility_control.h" namespace hardware_interface { -class Sensor final +class Sensor { public: - Sensor() = default; - - explicit Sensor(std::unique_ptr impl) - : impl_(std::move(impl)) - {} - - ~Sensor() = default; - - HARDWARE_INTERFACE_PUBLIC - return_type configure(const ComponentInfo & sensor_info) - { - return impl_->configure(sensor_info); - } - - HARDWARE_INTERFACE_PUBLIC - std::string get_interface_name() const - { - return impl_->get_interface_name(); - } - - HARDWARE_INTERFACE_PUBLIC - return_type start() - { - return impl_->start(); - } + Sensor() = default; HARDWARE_INTERFACE_PUBLIC - return_type stop() - { - return impl_->stop(); - } + virtual + ~Sensor() = default; HARDWARE_INTERFACE_PUBLIC - component_state get_state() const - { - return impl_->get_state(); - } + virtual + return_type configure(const ComponentInfo & sensor_info) = 0; HARDWARE_INTERFACE_PUBLIC - return_type read(double & data) - { - return impl_->read(data); - } - -private: - std::unique_ptr impl_; + virtual + std::vector get_interfaces() const = 0; + + HARDWARE_INTERFACE_EXPORT + virtual + return_type get_state( + std::vector & state, + std::vector & interfaces) const = 0; + + HARDWARE_INTERFACE_EXPORT + virtual + return_type set_state( + const std::vector & state, + std::vector interfaces = std::vector()) = 0; + +protected: + std::vector interfaces; }; } // namespace hardware_interface 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..a6d51b66b0 --- /dev/null +++ b/hardware_interface/include/hardware_interface/sensor_hardware.hpp @@ -0,0 +1,80 @@ +// 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_and_component_info.hpp" +#include "hardware_interface/sensor.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 +{ + +class SensorHardware final +{ +public: + SensorHardware() = default; + + explicit SensorHardware(std::unique_ptr impl) + : impl_(std::move(impl)) + {} + + ~SensorHardware() = default; + + HARDWARE_INTERFACE_PUBLIC + return_type configure(const HardwareInfo & sensor_info) + { + return impl_->configure(sensor_info); + } + + HARDWARE_INTERFACE_PUBLIC + return_type start() + { + return impl_->start(); + } + + HARDWARE_INTERFACE_PUBLIC + return_type stop() + { + return impl_->stop(); + } + + HARDWARE_INTERFACE_PUBLIC + hardware_interface_status get_status() + { + return impl_->get_status(); + } + + HARDWARE_INTERFACE_PUBLIC + return_type read_sensors(std::vector> & sensors) + { + return impl_->read_sensors(sensors); + } + +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..150fcec52d --- /dev/null +++ b/hardware_interface/include/hardware_interface/sensor_hardware_interface.hpp @@ -0,0 +1,96 @@ +// 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 +#include + +#include "hardware_interface/hardware_and_component_info.hpp" +#include "hardware_interface/sensor.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 RessourceManger. + * + * @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(const std::vector> & sensors) const = 0; +}; + +} // namespace hardware_interface +#endif // HARDWARE_INTERFACE__SENSOR_HARDWARE_INTERFACE_HPP_ diff --git a/hardware_interface/include/hardware_interface/system.hpp b/hardware_interface/include/hardware_interface/system_hardware.hpp similarity index 56% rename from hardware_interface/include/hardware_interface/system.hpp rename to hardware_interface/include/hardware_interface/system_hardware.hpp index 0f5acf2303..8ab22ed66d 100644 --- a/hardware_interface/include/hardware_interface/system.hpp +++ b/hardware_interface/include/hardware_interface/system_hardware.hpp @@ -12,44 +12,38 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef HARDWARE_INTERFACE__SYSTEM_HPP_ -#define HARDWARE_INTERFACE__SYSTEM_HPP_ +#ifndef HARDWARE_INTERFACE__SYSTEM_HARDWARE_HPP_ +#define HARDWARE_INTERFACE__SYSTEM_HARDWARE_HPP_ #include #include #include #include -#include "hardware_interface/component_info.hpp" -#include "hardware_interface/component_interfaces/system_interface.hpp" +#include "hardware_interface/hardware_and_component_info.hpp" +#include "hardware_interface/system_hardware_interface.hpp" #include "hardware_interface/types/hardware_interface_return_values.hpp" -#include "hardware_interface/types/hardware_interface_state_values.hpp" +#include "hardware_interface/types/hardware_interface_status_values.hpp" #include "hardware_interface/visibility_control.h" namespace hardware_interface { -class System final +class SystemHardware final { public: - explicit System(std::unique_ptr impl) + explicit SystemHardware(std::unique_ptr impl) : impl_(std::move(impl)) {} - virtual ~System() = default; + virtual ~SystemHardware() = default; HARDWARE_INTERFACE_PUBLIC - return_type configure(const ComponentInfo & system_info) + return_type configure(const HardwareInfo & system_info) { return impl_->configure(system_info); } - HARDWARE_INTERFACE_PUBLIC - std::vector get_interface_names() - { - return impl_->get_interface_names(); - } - HARDWARE_INTERFACE_PUBLIC return_type start() { @@ -63,28 +57,32 @@ class System final } HARDWARE_INTERFACE_PUBLIC - component_state get_state() const + hardware_interface_status get_status() const { - return impl_->get_state(); + return impl_->get_status(); } HARDWARE_INTERFACE_PUBLIC - return_type read(std::vector & data) + return_type read_sensors(std::vector> & sensors) { - return impl_->read(data); + return impl_->read_sensors(sensors); } HARDWARE_INTERFACE_PUBLIC - return_type write(const std::vector & data) + return_type read_joints(std::vector> & joints) { - return impl_->write(data); + return impl_->read_joints(joints); + } + + HARDWARE_INTERFACE_PUBLIC + return_type write(const std::vector> & joints) + { + return impl_->write(joints); } private: - std::unique_ptr impl_; + std::unique_ptr impl_; }; -typedef System Robot; - } // namespace hardware_interface -#endif // HARDWARE_INTERFACE__SYSTEM_HPP_ +#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..7edc00db07 --- /dev/null +++ b/hardware_interface/include/hardware_interface/system_hardware_interface.hpp @@ -0,0 +1,122 @@ +// 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 + +#include "hardware_interface/hardware_and_component_info.hpp" +#include "hardware_interface/joint.hpp" +#include "hardware_interface/sensor.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 RessourceManger 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 RessourceManger. + * + * @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 RessourceManger. + * + * @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(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_state_values.hpp b/hardware_interface/include/hardware_interface/types/hardware_interface_status_values.hpp similarity index 73% rename from hardware_interface/include/hardware_interface/types/hardware_interface_state_values.hpp rename to hardware_interface/include/hardware_interface/types/hardware_interface_status_values.hpp index 9f181ee825..7aff376a2f 100644 --- a/hardware_interface/include/hardware_interface/types/hardware_interface_state_values.hpp +++ b/hardware_interface/include/hardware_interface/types/hardware_interface_status_values.hpp @@ -12,14 +12,14 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef HARDWARE_INTERFACE__TYPES__HARDWARE_INTERFACE_STATE_VALUES_HPP_ -#define HARDWARE_INTERFACE__TYPES__HARDWARE_INTERFACE_STATE_VALUES_HPP_ +#ifndef HARDWARE_INTERFACE__TYPES__HARDWARE_INTERFACE_STATUS_VALUES_HPP_ +#define HARDWARE_INTERFACE__TYPES__HARDWARE_INTERFACE_STATUS_VALUES_HPP_ #include namespace hardware_interface { -enum class component_state : std::uint8_t +enum class hardware_interface_status : std::uint8_t { UNKNOWN = 0, CONFIGURED = 1, @@ -29,4 +29,4 @@ enum class component_state : std::uint8_t } // namespace hardware_interface -#endif // HARDWARE_INTERFACE__TYPES__HARDWARE_INTERFACE_STATE_VALUES_HPP_ +#endif // HARDWARE_INTERFACE__TYPES__HARDWARE_INTERFACE_STATUS_VALUES_HPP_ diff --git a/hardware_interface/test/test_component_interfaces.cpp b/hardware_interface/test/test_component_interfaces.cpp index 0bab63f878..15271fd545 100644 --- a/hardware_interface/test/test_component_interfaces.cpp +++ b/hardware_interface/test/test_component_interfaces.cpp @@ -17,150 +17,230 @@ #include #include #include -#include "hardware_interface/component_info.hpp" -#include "hardware_interface/component_interfaces/joint_interface.hpp" -#include "hardware_interface/component_interfaces/sensor_interface.hpp" -#include "hardware_interface/component_interfaces/system_interface.hpp" +#include "hardware_interface/hardware_and_component_info.hpp" +#include "hardware_interface/actuator_hardware_interface.hpp" +#include "hardware_interface/actuator_hardware.hpp" #include "hardware_interface/joint.hpp" +#include "hardware_interface/sensor_hardware_interface.hpp" +#include "hardware_interface/sensor_hardware.hpp" #include "hardware_interface/sensor.hpp" -#include "hardware_interface/system.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_state_values.hpp" +#include "hardware_interface/types/hardware_interface_status_values.hpp" + +using namespace ::testing; // NOLINT namespace hardware_interface { -namespace component_interfaces_test +namespace hardware_interfaces_components_test { -class DummyJoint : public JointInterface +class DummyPositionJoint : public Joint { +public: return_type configure(const ComponentInfo & joint_info) override { info = joint_info; - min_value = stod(info.parameters["min_value"]); - max_value = stod(info.parameters["max_value"]); - state = component_state::CONFIGURED; + 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("position"); + } + if (info.state_interfaces.size() == 0) { + info.state_interfaces.push_back("position"); + } + max_position = stod(info.parameters["max_position"]); + min_position = stod(info.parameters["min_position"]); return return_type::OK; } - std::string get_interface_name() const override + std::vector get_command_interfaces() const override { - return info.interface_names.front(); + return info.command_interfaces; } - return_type start() override + std::vector get_state_interfaces() const override { - if (state == component_state::CONFIGURED || state == component_state::STOPPED) { - state = component_state::STARTED; - } else { + return info.state_interfaces; + } + + return_type set_command( + const std::vector command, + std::vector interfaces = std::vector()) override + { + if (interfaces.size() != 0) { return return_type::ERROR; } + position_command = command[0]; return return_type::OK; } - return_type stop() override + return_type get_command( + std::vector & command, + std::vector & interfaces) const override + { + command.push_back(position_command); + interfaces = info.command_interfaces; + return return_type::OK; + } + + return_type set_state( + const std::vector & state, + std::vector interfaces = std::vector()) override { - if (state == component_state::STARTED) { - state = component_state::STOPPED; + return_type ret = return_type::OK; + if (interfaces.size() != 0) { + ret = return_type::ERROR; + } + if (state[0] > min_position && state[0] < max_position) { + position_state = state[0]; } else { + ret = return_type::ERROR; + } + return ret; + } + + return_type get_state( + std::vector & state, + std::vector & interfaces) const override + { + state.push_back(position_state); + interfaces = info.command_interfaces; + return return_type::OK; + } + +private: + ComponentInfo info; + double position_command; + double position_state; + double max_position, min_position; +}; + +class DummyForceTorqueSensor : public Sensor +{ +public: + return_type configure(const ComponentInfo & sensor_info) override + { + info = sensor_info; + 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"); + } + ft_values.resize(6); + ft_values = {1.34, 5.67, 8.21, 5.63, 5.99, 4.32}; return return_type::OK; } - component_state get_state() const override + std::vector get_interfaces() const override { - return state; + return info.state_interfaces; } - return_type read(double & data) override + return_type get_state( + std::vector & state, + std::vector & interfaces) const override { - data = joint_value; + interfaces = info.state_interfaces; + state = ft_values; return return_type::OK; } - return_type write(const double & data) override + return_type set_state( + const std::vector & state, + std::vector interfaces) override { - if (data > min_value && data < max_value) { - joint_value = data; - } else { + if (interfaces.size() != 0) { return return_type::ERROR; } + ft_values = state; return return_type::OK; } private: ComponentInfo info; - component_state state = component_state::UNKNOWN; - double min_value, max_value; - double joint_value; + std::vector ft_values; }; -class DummySensorHardware +class DummyActuatorHardware : public ActuatorHardwareInterface { -public: - DummySensorHardware() = default; - - ~DummySensorHardware() = default; + 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 configure_hardware(std::unordered_map & parameters) + return_type start() override { - if (parameters.count("start_value") > 0) { - value = stoi(parameters["start_value"]); - return return_type::OK; + if (status == hardware_interface_status::CONFIGURED || + status == hardware_interface_status::STOPPED) + { + status = hardware_interface_status::STARTED; + } else { + return return_type::ERROR; } - return return_type::ERROR; + return return_type::OK; } - return_type start_hardware() + return_type stop() override { + if (status == hardware_interface_status::STARTED) { + status = hardware_interface_status::STOPPED; + } else { + return return_type::ERROR; + } return return_type::OK; } - return_type stop_hardware() + hardware_interface_status get_status() const override { - return return_type::OK; + return status; } - return_type read_from_hardware(int & data) + return_type read_joint(Joint & joint) const override { - data = value + 128; - return return_type::OK; + return joint.set_state(hw_values); + } + + return_type write(const Joint & joint) override + { + return joint.get_command(hw_values, interfaces); } private: - int value = 0; + HardwareInfo info; + hardware_interface_status status = hardware_interface_status::UNKNOWN; + std::vector hw_values = {1.2}; + std::vector interfaces; + double hw_read_time, hw_write_time; }; -class DummySensor : public SensorInterface +class DummySensorHardware : public SensorHardwareInterface { - return_type configure(const ComponentInfo & sensor_info) override + return_type configure(const HardwareInfo & sensor_info) override { - return_type ret; info = sensor_info; - binary_to_voltage_factor = stod(info.parameters["binary_to_voltage_factor"]); - - // Normaly crate dynamically SensorHardware Object from sensor hardware_class_type - if (hardware.configure_hardware(info.hardware_parameters) == return_type::OK) { - state = component_state::CONFIGURED; - ret = return_type::OK; - } else { - state = component_state::UNKNOWN; - ret = return_type::ERROR; - } - return ret; - } - - std::string get_interface_name() const override - { - return info.interface_names.front(); + 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 (state == component_state::CONFIGURED || state == component_state::STOPPED) { - if (hardware.start_hardware() == return_type::OK) { - state = component_state::STARTED; - } + if (status == hardware_interface_status::CONFIGURED || + status == hardware_interface_status::STOPPED) + { + status = hardware_interface_status::STARTED; } else { return return_type::ERROR; } @@ -169,70 +249,56 @@ class DummySensor : public SensorInterface return_type stop() override { - if (state == component_state::STARTED) { - if (hardware.stop_hardware() == return_type::OK) { - state = component_state::STOPPED; - } + if (status == hardware_interface_status::STARTED) { + status = hardware_interface_status::STOPPED; } else { return return_type::ERROR; } return return_type::OK; } - component_state get_state() const override + hardware_interface_status get_status() const override { - return state; + return status; } - return_type read(double & data) override + return_type read_sensors(const std::vector> & sensors) const override { - int hw_data; - hardware.read_from_hardware(hw_data); - data = hw_data * binary_to_voltage_factor; - return return_type::OK; + 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; } private: - ComponentInfo info; - component_state state = component_state::UNKNOWN; - DummySensorHardware hardware; + 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 DummySystem : public SystemInterface +class DummySystemHardware : public SystemHardwareInterface { - return_type configure(const ComponentInfo & system_info) override + return_type configure(const HardwareInfo & system_info) override { info = system_info; - n_dof = info.joints.size(); - min_value.resize(n_dof); - max_value.resize(n_dof); - std::size_t current, previous = 0; - const char delim = ','; - for (uint i = 0; i < n_dof; i++) { - current = info.parameters["min_value"].find(delim, previous); - min_value[i] = stod(info.parameters["min_value"].substr(previous, current - previous)); - previous = current + 1; - } - for (uint i = 0; i < n_dof; i++) { - current = info.parameters["max_value"].find(delim, previous); - max_value[i] = stod(info.parameters["max_value"].substr(previous, current - previous)); - previous = current + 1; - } - joint_value.resize(n_dof); - state = component_state::CONFIGURED; + 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; } - std::vector get_interface_names() const override - { - return info.interface_names; - } - return_type start() override { - if (state == component_state::CONFIGURED || state == component_state::STOPPED) { - state = component_state::STARTED; + if (status == hardware_interface_status::CONFIGURED || + status == hardware_interface_status::STOPPED) + { + status = hardware_interface_status::STARTED; } else { return return_type::ERROR; } @@ -241,150 +307,267 @@ class DummySystem : public SystemInterface return_type stop() override { - if (state == component_state::STARTED) { - state = component_state::STOPPED; + if (status == hardware_interface_status::STARTED) { + status = hardware_interface_status::STOPPED; } else { return return_type::ERROR; } return return_type::OK; } - component_state get_state() const override + hardware_interface_status get_status() const override { - return state; + return status; } - return_type read(std::vector & data) override + return_type read_sensors(std::vector> & sensors) const override { - data = joint_value; - return return_type::OK; + 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 write(const std::vector & data) override + return_type read_joints(std::vector> & joints) const override { return_type ret = return_type::OK; - if (data.size() == n_dof) { - for (uint i = 0; i < n_dof; i++) { - if (data[i] > min_value[i] && data[i] < max_value[i]) { - joint_value[i] = data[i] / 2; - } else { - ret = return_type::ERROR; - } + std::vector joint_values; + for (uint i = 0; i < joints.size(); i++) { + joint_values.clear(); + joint_values.push_back(joints_hw_values[i]); + ret = joints[i]->set_state(joint_values); + if (ret != return_type::OK) { + break; + } + } + return ret; + } + + return_type write(const std::vector> & joints) override + { + return_type ret = return_type::OK; + for (const auto & joint : joints) { + std::vector values; + std::vector interfaces; + ret = joint->set_state(values, interfaces); + if (ret != return_type::OK) { + break; } - } else { - ret = return_type::ERROR; } return ret; } private: - ComponentInfo info; - component_state state; - uint n_dof; - std::vector min_value, max_value; - std::vector joint_value; + 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 component_interfaces_test +} // namespace hardware_interfaces_components_test } // namespace hardware_interface using hardware_interface::return_type; using hardware_interface::ComponentInfo; +using hardware_interface::HardwareInfo; +using hardware_interface::ActuatorHardware; +using hardware_interface::ActuatorHardwareInterface; using hardware_interface::Joint; -using hardware_interface::JointInterface; using hardware_interface::Sensor; -using hardware_interface::SensorInterface; -using hardware_interface::System; -using hardware_interface::SystemInterface; +using hardware_interface::SensorHardware; +using hardware_interface::SensorHardwareInterface; +using hardware_interface::SystemHardware; +using hardware_interface::SystemHardwareInterface; +using hardware_interface::hardware_interface_status; -using hardware_interface::component_interfaces_test::DummyJoint; -using hardware_interface::component_interfaces_test::DummySensor; -using hardware_interface::component_interfaces_test::DummySystem; -using hardware_interface::component_state; +using hardware_interface::hardware_interfaces_components_test::DummyPositionJoint; +using hardware_interface::hardware_interfaces_components_test::DummyForceTorqueSensor; -TEST(TestJointInterface, joint_interface_works) -{ - Joint joint(std::make_unique()); +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; - joint_info.name = "DummyJoint"; - joint_info.interface_names.push_back("dummy"); - joint_info.parameters["min_value"] = "-1"; - joint_info.parameters["max_value"] = "1"; + ComponentInfo sensor_info; + + void SetUp() override + { + joint_info.name = "DummyPositionJoint"; + joint_info.parameters["max_position"] = "1.742"; + joint_info.parameters["min_position"] = "-1.742"; + + 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); - EXPECT_EQ(joint.get_interface_name(), "dummy"); - EXPECT_EQ(joint.get_state(), component_state::CONFIGURED); - EXPECT_EQ(joint.start(), return_type::OK); - EXPECT_EQ(joint.get_state(), component_state::STARTED); - EXPECT_EQ(joint.write(2), return_type::ERROR); - EXPECT_EQ(joint.write(0.5), return_type::OK); - double output = 0.0; - EXPECT_EQ(joint.read(output), return_type::OK); - EXPECT_DOUBLE_EQ(output, 0.5); - EXPECT_EQ(joint.stop(), return_type::OK); - EXPECT_EQ(joint.get_state(), component_state::STOPPED); + ASSERT_THAT(joint.get_command_interfaces(), SizeIs(1)); + EXPECT_EQ(joint.get_command_interfaces()[0], "position"); + ASSERT_THAT(joint.get_state_interfaces(), SizeIs(1)); + EXPECT_EQ(joint.get_state_interfaces()[0], "position"); + std::vector interfaces; + std::vector input; + input.push_back(2.1); + 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], 2.1); + ASSERT_THAT(interfaces, SizeIs(1)); + EXPECT_EQ(interfaces[0], "position"); + + joint_info.command_interfaces.push_back("position"); + joint_info.command_interfaces.push_back("velocity"); + EXPECT_EQ(joint.configure(joint_info), return_type::ERROR); } -TEST(TestSensorInterfaceWithHardware, sensor_interface_with_hardware_works) +TEST_F(TestComponentInterfaces, sensor_example_component_works) { - Sensor sensor(std::make_unique()); - - ComponentInfo sensor_info; - sensor_info.name = "DummySensor"; - sensor_info.interface_names.push_back("dummySensor"); - sensor_info.parameters["binary_to_voltage_factor"] = "0.0048828125"; - sensor_info.hardware_class_type = "DummySensorHardware"; + DummyForceTorqueSensor sensor; + EXPECT_EQ(sensor.configure(sensor_info), return_type::OK); + ASSERT_THAT(sensor.get_interfaces(), SizeIs(6)); + EXPECT_EQ(sensor.get_interfaces()[0], "force_x"); + EXPECT_EQ(sensor.get_interfaces()[5], "torque_z"); + std::vector input = {5, 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::OK); + EXPECT_EQ(output[1], 5.67); + ASSERT_THAT(interfaces, SizeIs(6)); + EXPECT_EQ(interfaces[0], "force_x"); + interfaces.clear(); + EXPECT_EQ(sensor.set_state(input, interfaces), return_type::OK); + EXPECT_EQ(sensor.get_state(output, interfaces), return_type::OK); + EXPECT_EQ(output[5], 12.3); + + sensor_info.parameters.clear(); EXPECT_EQ(sensor.configure(sensor_info), return_type::ERROR); - EXPECT_EQ(sensor.get_state(), component_state::UNKNOWN); +} + +TEST_F(TestComponentInterfaces, actuator_hardware_interface_works) +{ + ActuatorHardware actuator_hw(std::make_unique()); + DummyPositionJoint joint; - sensor_info.hardware_parameters["start_value"] = "128"; + 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(sensor.configure(sensor_info), return_type::OK); - EXPECT_EQ(sensor.get_state(), component_state::CONFIGURED); - EXPECT_EQ(sensor.get_interface_name(), "dummySensor"); - EXPECT_EQ(sensor.start(), return_type::OK); - EXPECT_EQ(sensor.get_state(), component_state::STARTED); - EXPECT_EQ(sensor.stop(), return_type::OK); - EXPECT_EQ(sensor.get_state(), component_state::STOPPED); - EXPECT_EQ(sensor.start(), return_type::OK); - EXPECT_EQ(sensor.get_state(), component_state::STARTED); - double output = 0.0; - EXPECT_EQ(sensor.read(output), return_type::OK); - EXPECT_DOUBLE_EQ(output, 0.0048828125 * (128 + 128)); + 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; + 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], "position"); + EXPECT_EQ(actuator_hw.write(joint), return_type::OK); + EXPECT_EQ(actuator_hw.stop(), return_type::OK); + EXPECT_EQ(actuator_hw.get_status(), hardware_interface_status::STOPPED); } -TEST(TestJointInterface, system_interface_works) +TEST_F(TestComponentInterfaces, sensor_interface_with_hardware_works) { - System system(std::make_unique()); - - ComponentInfo system_info; - system_info.name = "DummyJoint"; - system_info.interface_names.push_back("dummy_position"); - system_info.interface_names.push_back("dummy_velocity"); - system_info.joints.push_back("joint1"); - system_info.joints.push_back("joint2"); - system_info.parameters["min_value"] = "0, 0"; - system_info.parameters["max_value"] = "4, 4"; - - EXPECT_EQ(system.configure(system_info), return_type::OK); - EXPECT_EQ(system.get_interface_names()[0], "dummy_position"); - EXPECT_EQ(system.get_interface_names()[1], "dummy_velocity"); - EXPECT_EQ(system.get_state(), component_state::CONFIGURED); + SensorHardware sensor_hw(std::make_unique()); + std::shared_ptr sensor(new DummyForceTorqueSensor); + + 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); + std::vector> sensors; + sensors.push_back(sensor); + EXPECT_EQ(sensor_hw.read_sensors(sensors), return_type::OK); + std::vector output; + std::vector 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()); + std::shared_ptr joint1(new DummyPositionJoint()); + std::shared_ptr joint2(new DummyPositionJoint()); + std::vector> joints; + joints.push_back(joint1); + joints.push_back(joint2); + + std::shared_ptr sensor(new DummyForceTorqueSensor); + 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_state(), component_state::STARTED); - std::vector input; - input.push_back(2); - EXPECT_EQ(system.write(input), return_type::ERROR); - input.push_back(3); - EXPECT_EQ(system.write(input), return_type::OK); + EXPECT_EQ(system.get_status(), hardware_interface_status::STARTED); + + EXPECT_EQ(system.read_sensors(sensors), return_type::OK); std::vector output; - output.push_back(0.0); - output.push_back(0.0); - EXPECT_EQ(system.read(output), return_type::OK); - EXPECT_DOUBLE_EQ(output[0], 1.0); - EXPECT_DOUBLE_EQ(output[1], 1.5); + std::vector 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); + 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], "position"); + output.clear(); + interfaces.clear(); + 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], "position"); + EXPECT_EQ(system.stop(), return_type::OK); - EXPECT_EQ(system.get_state(), component_state::STOPPED); + EXPECT_EQ(system.get_status(), hardware_interface_status::STOPPED); } From 182d7f159ea7914463bbf956daae4d2bb7075264 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Tue, 18 Aug 2020 09:11:43 +0200 Subject: [PATCH 09/29] Update hardware_interface/include/hardware_interface/sensor_hardware_interface.hpp Co-authored-by: Karsten Knese --- .../include/hardware_interface/sensor_hardware_interface.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/hardware_interface/include/hardware_interface/sensor_hardware_interface.hpp b/hardware_interface/include/hardware_interface/sensor_hardware_interface.hpp index 150fcec52d..50c92eb4cf 100644 --- a/hardware_interface/include/hardware_interface/sensor_hardware_interface.hpp +++ b/hardware_interface/include/hardware_interface/sensor_hardware_interface.hpp @@ -82,7 +82,7 @@ class SensorHardwareInterface /** * @brief Read data from the hardware into sensors using "set_state" function in the Sensor class. - * This function is always called by the RessourceManger. + * This function is always called by the resource manager. * * @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. From 90b6ee2c30fc053a93a1f9cbb35479cbfef8eb68 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Tue, 18 Aug 2020 09:11:57 +0200 Subject: [PATCH 10/29] Update hardware_interface/include/hardware_interface/system_hardware_interface.hpp Co-authored-by: Karsten Knese --- .../include/hardware_interface/system_hardware_interface.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/hardware_interface/include/hardware_interface/system_hardware_interface.hpp b/hardware_interface/include/hardware_interface/system_hardware_interface.hpp index 7edc00db07..702e0445be 100644 --- a/hardware_interface/include/hardware_interface/system_hardware_interface.hpp +++ b/hardware_interface/include/hardware_interface/system_hardware_interface.hpp @@ -85,7 +85,7 @@ class SystemHardwareInterface /** * @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 RessourceManger is therefore optional. + * 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. From 53e83c4b9801206cdda33a4e596045e7bb9ad815 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Tue, 18 Aug 2020 09:12:04 +0200 Subject: [PATCH 11/29] Update hardware_interface/include/hardware_interface/system_hardware_interface.hpp Co-authored-by: Karsten Knese --- .../include/hardware_interface/system_hardware_interface.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/hardware_interface/include/hardware_interface/system_hardware_interface.hpp b/hardware_interface/include/hardware_interface/system_hardware_interface.hpp index 702e0445be..28b2c2fe44 100644 --- a/hardware_interface/include/hardware_interface/system_hardware_interface.hpp +++ b/hardware_interface/include/hardware_interface/system_hardware_interface.hpp @@ -96,7 +96,7 @@ class SystemHardwareInterface /** * @brief Read data fromt the hardware into joints using "set_state" function of the Joint class. - * This function is always called by the RessourceManger. + * 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. From 7f922ddf04c024c8af1274649f1ce2d12fef49f4 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Tue, 18 Aug 2020 09:12:14 +0200 Subject: [PATCH 12/29] Update hardware_interface/include/hardware_interface/system_hardware_interface.hpp Co-authored-by: Karsten Knese --- .../include/hardware_interface/system_hardware_interface.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/hardware_interface/include/hardware_interface/system_hardware_interface.hpp b/hardware_interface/include/hardware_interface/system_hardware_interface.hpp index 28b2c2fe44..8a3f7dd056 100644 --- a/hardware_interface/include/hardware_interface/system_hardware_interface.hpp +++ b/hardware_interface/include/hardware_interface/system_hardware_interface.hpp @@ -107,7 +107,7 @@ class SystemHardwareInterface /** * @brief Write data from the joints to the hardware using "get_command" function of the Joint class. - * This function is always called by the RessourceManger. + * 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. From ef5a25968cb63c9424d280255b8b8c8b6409b8b9 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Tue, 18 Aug 2020 09:24:06 +0200 Subject: [PATCH 13/29] Split Component and Hardware Infos into two files --- .../hardware_interface/component_info.hpp | 61 +++++++++++++++++++ ...d_component_info.hpp => hardware_info.hpp} | 38 +----------- 2 files changed, 63 insertions(+), 36 deletions(-) create mode 100644 hardware_interface/include/hardware_interface/component_info.hpp rename hardware_interface/include/hardware_interface/{hardware_and_component_info.hpp => hardware_info.hpp} (65%) diff --git a/hardware_interface/include/hardware_interface/component_info.hpp b/hardware_interface/include/hardware_interface/component_info.hpp new file mode 100644 index 0000000000..d921b4f67a --- /dev/null +++ b/hardware_interface/include/hardware_interface/component_info.hpp @@ -0,0 +1,61 @@ +// 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__COMPONENT_INFO_HPP_ +#define HARDWARE_INTERFACE__COMPONENT_INFO_HPP_ + +#include +#include +#include + +namespace hardware_interface +{ + +/** + * \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 hardware_interface +#endif // HARDWARE_INTERFACE__COMPONENT_INFO_HPP_ diff --git a/hardware_interface/include/hardware_interface/hardware_and_component_info.hpp b/hardware_interface/include/hardware_interface/hardware_info.hpp similarity index 65% rename from hardware_interface/include/hardware_interface/hardware_and_component_info.hpp rename to hardware_interface/include/hardware_interface/hardware_info.hpp index 600037aa95..ff96cedd03 100644 --- a/hardware_interface/include/hardware_interface/hardware_and_component_info.hpp +++ b/hardware_interface/include/hardware_interface/hardware_info.hpp @@ -13,8 +13,8 @@ // limitations under the License. -#ifndef HARDWARE_INTERFACE__HARDWARE_AND_COMPONENT_INFO_HPP_ -#define HARDWARE_INTERFACE__HARDWARE_AND_COMPONENT_INFO_HPP_ +#ifndef HARDWARE_INTERFACE__HARDWARE_INFO_HPP_ +#define HARDWARE_INTERFACE__HARDWARE_INFO_HPP_ #include #include @@ -23,40 +23,6 @@ namespace hardware_interface { -/** - * \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; -}; - /** * \brief This structure stores information about hardware defined in a robot's URDF. */ From 374aeb00fd5ce7d2f0a31d4985f7d447f82d0d96 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Tue, 18 Aug 2020 14:48:32 +0200 Subject: [PATCH 14/29] Use separated headers for components_info and hardware_info --- .../include/hardware_interface/actuator_hardware.hpp | 3 ++- .../hardware_interface/actuator_hardware_interface.hpp | 3 ++- .../include/hardware_interface/hardware_info.hpp | 2 +- hardware_interface/include/hardware_interface/joint.hpp | 3 ++- hardware_interface/include/hardware_interface/sensor.hpp | 3 ++- .../include/hardware_interface/sensor_hardware.hpp | 3 ++- .../hardware_interface/sensor_hardware_interface.hpp | 3 ++- .../include/hardware_interface/system_hardware.hpp | 3 ++- .../hardware_interface/system_hardware_interface.hpp | 3 ++- hardware_interface/test/test_component_interfaces.cpp | 6 ++++-- 10 files changed, 21 insertions(+), 11 deletions(-) diff --git a/hardware_interface/include/hardware_interface/actuator_hardware.hpp b/hardware_interface/include/hardware_interface/actuator_hardware.hpp index 8dbcfca823..a2b9566153 100644 --- a/hardware_interface/include/hardware_interface/actuator_hardware.hpp +++ b/hardware_interface/include/hardware_interface/actuator_hardware.hpp @@ -20,8 +20,9 @@ #include #include -#include "hardware_interface/hardware_and_component_info.hpp" #include "hardware_interface/actuator_hardware_interface.hpp" +#include "hardware_interface/component_info.hpp" +#include "hardware_interface/hardware_info.hpp" #include "hardware_interface/joint.hpp" #include "hardware_interface/types/hardware_interface_return_values.hpp" #include "hardware_interface/types/hardware_interface_status_values.hpp" diff --git a/hardware_interface/include/hardware_interface/actuator_hardware_interface.hpp b/hardware_interface/include/hardware_interface/actuator_hardware_interface.hpp index 78dfa4d7b0..1885eb9e8c 100644 --- a/hardware_interface/include/hardware_interface/actuator_hardware_interface.hpp +++ b/hardware_interface/include/hardware_interface/actuator_hardware_interface.hpp @@ -20,7 +20,8 @@ #include #include -#include "hardware_interface/hardware_and_component_info.hpp" +#include "hardware_interface/component_info.hpp" +#include "hardware_interface/hardware_info.hpp" #include "hardware_interface/joint.hpp" #include "hardware_interface/types/hardware_interface_return_values.hpp" #include "hardware_interface/types/hardware_interface_status_values.hpp" diff --git a/hardware_interface/include/hardware_interface/hardware_info.hpp b/hardware_interface/include/hardware_interface/hardware_info.hpp index ff96cedd03..39ba04ed49 100644 --- a/hardware_interface/include/hardware_interface/hardware_info.hpp +++ b/hardware_interface/include/hardware_interface/hardware_info.hpp @@ -62,4 +62,4 @@ struct HardwareInfo }; } // namespace hardware_interface -#endif // HARDWARE_INTERFACE__HARDWARE_AND_COMPONENT_INFO_HPP_ +#endif // HARDWARE_INTERFACE__HARDWARE_INFO_HPP_ diff --git a/hardware_interface/include/hardware_interface/joint.hpp b/hardware_interface/include/hardware_interface/joint.hpp index 5862391330..df469c9705 100644 --- a/hardware_interface/include/hardware_interface/joint.hpp +++ b/hardware_interface/include/hardware_interface/joint.hpp @@ -18,7 +18,8 @@ #include #include -#include "hardware_interface/hardware_and_component_info.hpp" +#include "hardware_interface/component_info.hpp" +#include "hardware_interface/hardware_info.hpp" #include "hardware_interface/types/hardware_interface_return_values.hpp" #include "hardware_interface/visibility_control.h" diff --git a/hardware_interface/include/hardware_interface/sensor.hpp b/hardware_interface/include/hardware_interface/sensor.hpp index e324edfa1f..783b4976c5 100644 --- a/hardware_interface/include/hardware_interface/sensor.hpp +++ b/hardware_interface/include/hardware_interface/sensor.hpp @@ -18,7 +18,8 @@ #include #include -#include "hardware_interface/hardware_and_component_info.hpp" +#include "hardware_interface/component_info.hpp" +#include "hardware_interface/hardware_info.hpp" #include "hardware_interface/types/hardware_interface_return_values.hpp" #include "hardware_interface/visibility_control.h" diff --git a/hardware_interface/include/hardware_interface/sensor_hardware.hpp b/hardware_interface/include/hardware_interface/sensor_hardware.hpp index a6d51b66b0..059ecc6d2d 100644 --- a/hardware_interface/include/hardware_interface/sensor_hardware.hpp +++ b/hardware_interface/include/hardware_interface/sensor_hardware.hpp @@ -21,7 +21,8 @@ #include #include -#include "hardware_interface/hardware_and_component_info.hpp" +#include "hardware_interface/component_info.hpp" +#include "hardware_interface/hardware_info.hpp" #include "hardware_interface/sensor.hpp" #include "hardware_interface/sensor_hardware_interface.hpp" #include "hardware_interface/types/hardware_interface_return_values.hpp" diff --git a/hardware_interface/include/hardware_interface/sensor_hardware_interface.hpp b/hardware_interface/include/hardware_interface/sensor_hardware_interface.hpp index 50c92eb4cf..278fa97001 100644 --- a/hardware_interface/include/hardware_interface/sensor_hardware_interface.hpp +++ b/hardware_interface/include/hardware_interface/sensor_hardware_interface.hpp @@ -19,7 +19,8 @@ #include #include -#include "hardware_interface/hardware_and_component_info.hpp" +#include "hardware_interface/component_info.hpp" +#include "hardware_interface/hardware_info.hpp" #include "hardware_interface/sensor.hpp" #include "hardware_interface/types/hardware_interface_return_values.hpp" #include "hardware_interface/types/hardware_interface_status_values.hpp" diff --git a/hardware_interface/include/hardware_interface/system_hardware.hpp b/hardware_interface/include/hardware_interface/system_hardware.hpp index 8ab22ed66d..a4a38122b1 100644 --- a/hardware_interface/include/hardware_interface/system_hardware.hpp +++ b/hardware_interface/include/hardware_interface/system_hardware.hpp @@ -20,7 +20,8 @@ #include #include -#include "hardware_interface/hardware_and_component_info.hpp" +#include "hardware_interface/component_info.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" diff --git a/hardware_interface/include/hardware_interface/system_hardware_interface.hpp b/hardware_interface/include/hardware_interface/system_hardware_interface.hpp index 8a3f7dd056..86f8880e5a 100644 --- a/hardware_interface/include/hardware_interface/system_hardware_interface.hpp +++ b/hardware_interface/include/hardware_interface/system_hardware_interface.hpp @@ -19,7 +19,8 @@ #include #include -#include "hardware_interface/hardware_and_component_info.hpp" +#include "hardware_interface/component_info.hpp" +#include "hardware_interface/hardware_info.hpp" #include "hardware_interface/joint.hpp" #include "hardware_interface/sensor.hpp" #include "hardware_interface/types/hardware_interface_return_values.hpp" diff --git a/hardware_interface/test/test_component_interfaces.cpp b/hardware_interface/test/test_component_interfaces.cpp index 15271fd545..c063b19680 100644 --- a/hardware_interface/test/test_component_interfaces.cpp +++ b/hardware_interface/test/test_component_interfaces.cpp @@ -17,9 +17,11 @@ #include #include #include -#include "hardware_interface/hardware_and_component_info.hpp" -#include "hardware_interface/actuator_hardware_interface.hpp" + #include "hardware_interface/actuator_hardware.hpp" +#include "hardware_interface/actuator_hardware_interface.hpp" +#include "hardware_interface/component_info.hpp" +#include "hardware_interface/hardware_info.hpp" #include "hardware_interface/joint.hpp" #include "hardware_interface/sensor_hardware_interface.hpp" #include "hardware_interface/sensor_hardware.hpp" From daaac84523be2d0410d18857636c0b36a9a3d3c2 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Tue, 18 Aug 2020 14:57:10 +0200 Subject: [PATCH 15/29] Style corrections --- .../hardware_interface/actuator_hardware.hpp | 4 +- .../actuator_hardware_interface.hpp | 36 +++++++++--------- .../sensor_hardware_interface.hpp | 24 ++++++------ .../hardware_interface/system_hardware.hpp | 4 +- .../system_hardware_interface.hpp | 38 +++++++++---------- .../test/test_component_interfaces.cpp | 10 +++-- 6 files changed, 59 insertions(+), 57 deletions(-) diff --git a/hardware_interface/include/hardware_interface/actuator_hardware.hpp b/hardware_interface/include/hardware_interface/actuator_hardware.hpp index a2b9566153..10aa4890a2 100644 --- a/hardware_interface/include/hardware_interface/actuator_hardware.hpp +++ b/hardware_interface/include/hardware_interface/actuator_hardware.hpp @@ -73,9 +73,9 @@ class ActuatorHardware final } HARDWARE_INTERFACE_PUBLIC - return_type write(const Joint & joint) + return_type write_joint(const Joint & joint) { - return impl_->write(joint); + return impl_->write_joint(joint); } private: diff --git a/hardware_interface/include/hardware_interface/actuator_hardware_interface.hpp b/hardware_interface/include/hardware_interface/actuator_hardware_interface.hpp index 1885eb9e8c..b4e6222308 100644 --- a/hardware_interface/include/hardware_interface/actuator_hardware_interface.hpp +++ b/hardware_interface/include/hardware_interface/actuator_hardware_interface.hpp @@ -45,10 +45,10 @@ class ActuatorHardwareInterface ~ActuatorHardwareInterface() = default; /** - * @brief Configuration of the actuator from data parsed from the robot's URDF. + * \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, + * \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 @@ -56,53 +56,53 @@ class ActuatorHardwareInterface return_type configure(const HardwareInfo & actuator_info) = 0; /** - * @brief Start exchange data with the hardware. + * \brief Start exchange data with the hardware. * - * @return return_type:OK if everything worked as expected, return_type::ERROR otherwise. + * \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. + * \brief Stop exchange data with the hardware. * - * @return return_type:OK if everything worked as expected, return_type::ERROR otherwise. + * \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. + * \brief Get current state of the system hardware. * - * @return hardware_interface_status current status. + * \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 RessourceManger. + * \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. + * \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(Joint & 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 RessourceManger. + * \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. + * \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(const Joint & joint) = 0; + return_type write_joint(const Joint & joint) = 0; }; } // namespace hardware_interface diff --git a/hardware_interface/include/hardware_interface/sensor_hardware_interface.hpp b/hardware_interface/include/hardware_interface/sensor_hardware_interface.hpp index 278fa97001..1d8bb4bdbb 100644 --- a/hardware_interface/include/hardware_interface/sensor_hardware_interface.hpp +++ b/hardware_interface/include/hardware_interface/sensor_hardware_interface.hpp @@ -44,10 +44,10 @@ class SensorHardwareInterface ~SensorHardwareInterface() = default; /** - * @brief Configuration of the sensor from data parsed from the robot's URDF. + * \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, + * \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 @@ -55,38 +55,38 @@ class SensorHardwareInterface return_type configure(const HardwareInfo & sensor_info) = 0; /** - * @brief Start exchange data with the hardware. + * \brief Start exchange data with the hardware. * - * @return return_type:OK if everything worked as expected, return_type::ERROR otherwise. + * \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. + * \brief Stop exchange data with the hardware. * - * @return return_type:OK if everything worked as expected, return_type::ERROR otherwise. + * \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. + * \brief Get current state of the system hardware. * - * @return hardware_interface_status current status. + * \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. + * \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 list of sensors where data from the hardware are stored. - * @return return_type:OK if everything worked as expected, return_type::ERROR otherwise. + * \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 diff --git a/hardware_interface/include/hardware_interface/system_hardware.hpp b/hardware_interface/include/hardware_interface/system_hardware.hpp index a4a38122b1..52e196f6d3 100644 --- a/hardware_interface/include/hardware_interface/system_hardware.hpp +++ b/hardware_interface/include/hardware_interface/system_hardware.hpp @@ -76,9 +76,9 @@ class SystemHardware final } HARDWARE_INTERFACE_PUBLIC - return_type write(const std::vector> & joints) + return_type write_joints(const std::vector> & joints) { - return impl_->write(joints); + return impl_->write_joints(joints); } private: diff --git a/hardware_interface/include/hardware_interface/system_hardware_interface.hpp b/hardware_interface/include/hardware_interface/system_hardware_interface.hpp index 86f8880e5a..41c0456e18 100644 --- a/hardware_interface/include/hardware_interface/system_hardware_interface.hpp +++ b/hardware_interface/include/hardware_interface/system_hardware_interface.hpp @@ -46,10 +46,10 @@ class SystemHardwareInterface ~SystemHardwareInterface() = default; /** - * @brief Configuration of the system from data parsed from the robot's URDF. + * \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, + * \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 @@ -57,66 +57,66 @@ class SystemHardwareInterface return_type configure(const HardwareInfo & system_info) = 0; /** - * @brief Start exchange data with the hardware. + * \brief Start exchange data with the hardware. * - * @return return_type:OK if everything worked as expected, return_type::ERROR otherwise. + * \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. + * \brief Stop exchange data with the hardware. * - * @return return_type:OK if everything worked as expected, return_type::ERROR otherwise. + * \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. + * \brief Get current state of the system hardware. * - * @return hardware_interface_status current status. + * \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. + * \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. + * \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. + * \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. + * \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. + * \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. + * \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(const std::vector> & joints) = 0; + write_joints(const std::vector> & joints) = 0; }; } // namespace hardware_interface diff --git a/hardware_interface/test/test_component_interfaces.cpp b/hardware_interface/test/test_component_interfaces.cpp index c063b19680..ca4ce7339d 100644 --- a/hardware_interface/test/test_component_interfaces.cpp +++ b/hardware_interface/test/test_component_interfaces.cpp @@ -35,8 +35,10 @@ using namespace ::testing; // NOLINT namespace hardware_interface { + namespace hardware_interfaces_components_test { + class DummyPositionJoint : public Joint { public: @@ -214,7 +216,7 @@ class DummyActuatorHardware : public ActuatorHardwareInterface return joint.set_state(hw_values); } - return_type write(const Joint & joint) override + return_type write_joint(const Joint & joint) override { return joint.get_command(hw_values, interfaces); } @@ -349,7 +351,7 @@ class DummySystemHardware : public SystemHardwareInterface return ret; } - return_type write(const std::vector> & joints) override + return_type write_joints(const std::vector> & joints) override { return_type ret = return_type::OK; for (const auto & joint : joints) { @@ -483,7 +485,7 @@ TEST_F(TestComponentInterfaces, actuator_hardware_interface_works) ASSERT_THAT(output, SizeIs(1)); EXPECT_EQ(output[0], 1.2); EXPECT_EQ(interfaces[0], "position"); - EXPECT_EQ(actuator_hw.write(joint), return_type::OK); + 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); } @@ -539,7 +541,7 @@ TEST_F(TestComponentInterfaces, system_interface_with_hardware_works) 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); From 5e4ed739a9fb5c8d39ca1f3f157f79af7d1c9f69 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Tue, 18 Aug 2020 17:34:20 +0200 Subject: [PATCH 16/29] Add documentation for joint and sensor components --- .../include/hardware_interface/joint.hpp | 68 +++++++++++++++++-- .../include/hardware_interface/sensor.hpp | 42 ++++++++++-- .../test/test_component_interfaces.cpp | 8 +-- 3 files changed, 106 insertions(+), 12 deletions(-) diff --git a/hardware_interface/include/hardware_interface/joint.hpp b/hardware_interface/include/hardware_interface/joint.hpp index df469c9705..1937dadd6e 100644 --- a/hardware_interface/include/hardware_interface/joint.hpp +++ b/hardware_interface/include/hardware_interface/joint.hpp @@ -26,6 +26,12 @@ namespace hardware_interface { +/** + * \brief Virtual Class for the "Joint" component used as a basic build 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). + * The lists of command and state interfaces define this. + */ class Joint { public: @@ -36,45 +42,99 @@ class Joint virtual ~Joint() = default; + /** + * @brief Configure joint 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 virtual return_type configure(const ComponentInfo & joint_info) = 0; + /** + * @brief Provide the list of command interfaces configured for the joint. + * + * @return string list with command interfaces. + */ HARDWARE_INTERFACE_PUBLIC virtual std::vector get_command_interfaces() const = 0; + /** + * @brief Provide the list of state interfaces configured for the joint. + * + * @return string list with state interfaces. + */ HARDWARE_INTERFACE_PUBLIC virtual std::vector get_state_interfaces() const = 0; + /** + * @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::OK the interfaces exist for the joints and the values, are set into + * commands list, otherwise return_type::ERROR. + */ HARDWARE_INTERFACE_EXPORT virtual return_type get_command( std::vector & command, std::vector & interfaces) const = 0; + /** + * @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::OK the interfaces exist for the joints and the values, are set valid + * for the joint, otherwise return_type::ERROR. + */ HARDWARE_INTERFACE_EXPORT virtual return_type set_command( const std::vector command, std::vector interfaces = std::vector()) = 0; + /** + * @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::OK the interfaces exist for the joints and the values are set into + * state list, otherwise return_type::ERROR. + */ HARDWARE_INTERFACE_EXPORT virtual return_type get_state( std::vector & state, std::vector & interfaces) const = 0; + /** + * @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::OK the interfaces exist for the joints and the values are set from the + * state list, otherwise return_type::ERROR. + */ HARDWARE_INTERFACE_EXPORT virtual return_type set_state( const std::vector & state, std::vector interfaces = std::vector()) = 0; - -protected: - std::vector command_interfaces; - std::vector state_interfaces; }; } // namespace hardware_interface diff --git a/hardware_interface/include/hardware_interface/sensor.hpp b/hardware_interface/include/hardware_interface/sensor.hpp index 783b4976c5..2cf810b814 100644 --- a/hardware_interface/include/hardware_interface/sensor.hpp +++ b/hardware_interface/include/hardware_interface/sensor.hpp @@ -26,6 +26,11 @@ namespace hardware_interface { +/** + * \brief Virtual 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: @@ -36,28 +41,57 @@ class Sensor virtual ~Sensor() = default; + /** + * @brief Configure senosr based on the description in the robot's URDF file. + * + * @param sensor_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 virtual return_type configure(const ComponentInfo & sensor_info) = 0; + /** + * @brief Provide the list of state interfaces configured for the sensor. + * + * @return string list with state interfaces. + */ HARDWARE_INTERFACE_PUBLIC virtual - std::vector get_interfaces() const = 0; + std::vector get_state_interfaces() const = 0; + /** + * @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::OK the interfaces exist for the sensor and the values are set into + * state list, otherwise return_type::ERROR. + */ HARDWARE_INTERFACE_EXPORT virtual return_type get_state( std::vector & state, std::vector & interfaces) const = 0; + /** + * @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::OK the interfaces exist for the sensor and the values are set from the + * state list, otherwise return_type::ERROR. + */ HARDWARE_INTERFACE_EXPORT virtual return_type set_state( const std::vector & state, std::vector interfaces = std::vector()) = 0; - -protected: - std::vector interfaces; }; } // namespace hardware_interface diff --git a/hardware_interface/test/test_component_interfaces.cpp b/hardware_interface/test/test_component_interfaces.cpp index ca4ce7339d..5331a62372 100644 --- a/hardware_interface/test/test_component_interfaces.cpp +++ b/hardware_interface/test/test_component_interfaces.cpp @@ -143,7 +143,7 @@ class DummyForceTorqueSensor : public Sensor return return_type::OK; } - std::vector get_interfaces() const override + std::vector get_state_interfaces() const override { return info.state_interfaces; } @@ -443,9 +443,9 @@ TEST_F(TestComponentInterfaces, sensor_example_component_works) DummyForceTorqueSensor sensor; EXPECT_EQ(sensor.configure(sensor_info), return_type::OK); - ASSERT_THAT(sensor.get_interfaces(), SizeIs(6)); - EXPECT_EQ(sensor.get_interfaces()[0], "force_x"); - EXPECT_EQ(sensor.get_interfaces()[5], "torque_z"); + 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, 6.7, 2.5, 3.8, 8.9, 12.3}; std::vector output; std::vector interfaces; From 3053824d3003ae3cdd8f758c549f67733b55788c Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Tue, 18 Aug 2020 19:02:15 +0200 Subject: [PATCH 17/29] Update hardware_interface/test/test_component_interfaces.cpp Co-authored-by: Karsten Knese --- hardware_interface/test/test_component_interfaces.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/hardware_interface/test/test_component_interfaces.cpp b/hardware_interface/test/test_component_interfaces.cpp index 5331a62372..7908491b83 100644 --- a/hardware_interface/test/test_component_interfaces.cpp +++ b/hardware_interface/test/test_component_interfaces.cpp @@ -169,7 +169,7 @@ class DummyForceTorqueSensor : public Sensor } private: - ComponentInfo info; + ComponentInfo info_; std::vector ft_values; }; From 7089abe27893151b7cdb2aaf021bd09ede493046 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Tue, 18 Aug 2020 21:40:43 +0200 Subject: [PATCH 18/29] Style corrections --- hardware_interface/CMakeLists.txt | 1 - .../include/hardware_interface/joint.hpp | 48 ++--- .../include/hardware_interface/sensor.hpp | 26 +-- .../test/test_component_interfaces.cpp | 189 +++++++++--------- 4 files changed, 132 insertions(+), 132 deletions(-) diff --git a/hardware_interface/CMakeLists.txt b/hardware_interface/CMakeLists.txt index 07f8356338..41118b1893 100644 --- a/hardware_interface/CMakeLists.txt +++ b/hardware_interface/CMakeLists.txt @@ -77,7 +77,6 @@ if(BUILD_TESTING) 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 hardware_interface) - endif() ament_export_dependencies( diff --git a/hardware_interface/include/hardware_interface/joint.hpp b/hardware_interface/include/hardware_interface/joint.hpp index 1937dadd6e..05f063bcd0 100644 --- a/hardware_interface/include/hardware_interface/joint.hpp +++ b/hardware_interface/include/hardware_interface/joint.hpp @@ -43,10 +43,10 @@ class Joint ~Joint() = default; /** - * @brief Configure joint based on the description in the robot's URDF file. + * \brief Configure joint 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, + * \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 @@ -54,32 +54,32 @@ class Joint return_type configure(const ComponentInfo & joint_info) = 0; /** - * @brief Provide the list of command interfaces configured for the joint. + * \brief Provide the list of command interfaces configured for the joint. * - * @return string list with command interfaces. + * \return string list with command interfaces. */ HARDWARE_INTERFACE_PUBLIC virtual std::vector get_command_interfaces() const = 0; /** - * @brief Provide the list of state interfaces configured for the joint. + * \brief Provide the list of state interfaces configured for the joint. * - * @return string list with state interfaces. + * \return string list with state interfaces. */ HARDWARE_INTERFACE_PUBLIC virtual std::vector get_state_interfaces() const = 0; /** - * @brief Get command list from the joint. This function is used in the write function of the + * \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::OK the interfaces exist for the joints and the values, are set into + * \param command list of doubles with commands for the hardware. + * \param interfaces list of interfaces on which commands have to set. + * \return return_type::OK the interfaces exist for the joints and the values, are set into * commands list, otherwise return_type::ERROR. */ HARDWARE_INTERFACE_EXPORT @@ -89,29 +89,29 @@ class Joint std::vector & interfaces) const = 0; /** - * @brief Set command list for the joint. This function is used by the controller to set the goal + * \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::OK the interfaces exist for the joints and the values, are set valid + * \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::OK the interfaces exist for the joints and the values, are set valid * for the joint, otherwise return_type::ERROR. */ HARDWARE_INTERFACE_EXPORT virtual return_type set_command( - const std::vector command, + const std::vector command, std::vector interfaces = std::vector()) = 0; /** - * @brief Get state list from the joint. This function is used by the controller to get the + * \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::OK the interfaces exist for the joints and the values are set into + * \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::OK the interfaces exist for the joints and the values are set into * state list, otherwise return_type::ERROR. */ HARDWARE_INTERFACE_EXPORT @@ -121,13 +121,13 @@ class Joint std::vector & interfaces) const = 0; /** - * @brief Set state list for the joint. This function is used by the hardware to set its actual + * \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::OK the interfaces exist for the joints and the values are set from the + * \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::OK the interfaces exist for the joints and the values are set from the * state list, otherwise return_type::ERROR. */ HARDWARE_INTERFACE_EXPORT diff --git a/hardware_interface/include/hardware_interface/sensor.hpp b/hardware_interface/include/hardware_interface/sensor.hpp index 2cf810b814..f2f29df15d 100644 --- a/hardware_interface/include/hardware_interface/sensor.hpp +++ b/hardware_interface/include/hardware_interface/sensor.hpp @@ -42,10 +42,10 @@ class Sensor ~Sensor() = default; /** - * @brief Configure senosr based on the description in the robot's URDF file. + * \brief Configure senosr based on the description in the robot's URDF file. * - * @param sensor_info structure with data from URDF. - * @return return_type::OK if required data are provided and is successfully parsed, + * \param sensor_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 @@ -53,22 +53,22 @@ class Sensor return_type configure(const ComponentInfo & sensor_info) = 0; /** - * @brief Provide the list of state interfaces configured for the sensor. + * \brief Provide the list of state interfaces configured for the sensor. * - * @return string list with state interfaces. + * \return string list with state interfaces. */ HARDWARE_INTERFACE_PUBLIC virtual std::vector get_state_interfaces() const = 0; /** - * @brief Get state list from the sensor. This function is used by the controller to get the + * \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::OK the interfaces exist for the sensor and the values are set into + * \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::OK the interfaces exist for the sensor and the values are set into * state list, otherwise return_type::ERROR. */ HARDWARE_INTERFACE_EXPORT @@ -78,13 +78,13 @@ class Sensor std::vector & interfaces) const = 0; /** - * @brief Set state list for the sensor. This function is used by the hardware to set its actual + * \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::OK the interfaces exist for the sensor and the values are set from the + * \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::OK the interfaces exist for the sensor and the values are set from the * state list, otherwise return_type::ERROR. */ HARDWARE_INTERFACE_EXPORT diff --git a/hardware_interface/test/test_component_interfaces.cpp b/hardware_interface/test/test_component_interfaces.cpp index 7908491b83..db110f6e96 100644 --- a/hardware_interface/test/test_component_interfaces.cpp +++ b/hardware_interface/test/test_component_interfaces.cpp @@ -22,6 +22,7 @@ #include "hardware_interface/actuator_hardware_interface.hpp" #include "hardware_interface/component_info.hpp" #include "hardware_interface/hardware_info.hpp" +#include "hardware_interface/types/hardware_interface_type_values.hpp" #include "hardware_interface/joint.hpp" #include "hardware_interface/sensor_hardware_interface.hpp" #include "hardware_interface/sensor_hardware.hpp" @@ -44,29 +45,29 @@ class DummyPositionJoint : public Joint public: return_type configure(const ComponentInfo & joint_info) override { - info = joint_info; - if (info.command_interfaces.size() > 1 || info.state_interfaces.size() > 1) { + info_ = joint_info; + 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("position"); + if (info_.command_interfaces.size() == 0) { + info_.command_interfaces.push_back(HW_IF_POSITION); } - if (info.state_interfaces.size() == 0) { - info.state_interfaces.push_back("position"); + if (info_.state_interfaces.size() == 0) { + info_.state_interfaces.push_back(HW_IF_POSITION); } - max_position = stod(info.parameters["max_position"]); - min_position = stod(info.parameters["min_position"]); + max_position_ = stod(info_.parameters["max_position"]); + min_position_ = stod(info_.parameters["min_position"]); return return_type::OK; } std::vector get_command_interfaces() const override { - return info.command_interfaces; + return info_.command_interfaces; } std::vector get_state_interfaces() const override { - return info.state_interfaces; + return info_.state_interfaces; } return_type set_command( @@ -76,7 +77,7 @@ class DummyPositionJoint : public Joint if (interfaces.size() != 0) { return return_type::ERROR; } - position_command = command[0]; + position_command_ = command[0]; return return_type::OK; } @@ -84,8 +85,8 @@ class DummyPositionJoint : public Joint std::vector & command, std::vector & interfaces) const override { - command.push_back(position_command); - interfaces = info.command_interfaces; + command.push_back(position_command_); + interfaces = info_.command_interfaces; return return_type::OK; } @@ -97,8 +98,8 @@ class DummyPositionJoint : public Joint if (interfaces.size() != 0) { ret = return_type::ERROR; } - if (state[0] > min_position && state[0] < max_position) { - position_state = state[0]; + if (state[0] > min_position_ && state[0] < max_position_) { + position_state_ = state[0]; } else { ret = return_type::ERROR; } @@ -109,16 +110,16 @@ class DummyPositionJoint : public Joint std::vector & state, std::vector & interfaces) const override { - state.push_back(position_state); - interfaces = info.command_interfaces; + state.push_back(position_state_); + interfaces = info_.command_interfaces; return return_type::OK; } private: - ComponentInfo info; - double position_command; - double position_state; - double max_position, min_position; + ComponentInfo info_; + double position_command_; + double position_state_; + double max_position_, min_position_; }; class DummyForceTorqueSensor : public Sensor @@ -126,34 +127,34 @@ class DummyForceTorqueSensor : public Sensor public: return_type configure(const ComponentInfo & sensor_info) override { - info = sensor_info; - if (info.parameters["frame_id"] == "") { + info_ = sensor_info; + 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"); + 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"); } - ft_values.resize(6); - ft_values = {1.34, 5.67, 8.21, 5.63, 5.99, 4.32}; + ft_values_.resize(6); + ft_values_ = {1.34, 5.67, 8.21, 5.63, 5.99, 4.32}; return return_type::OK; } std::vector get_state_interfaces() const override { - return info.state_interfaces; + return info_.state_interfaces; } return_type get_state( std::vector & state, std::vector & interfaces) const override { - interfaces = info.state_interfaces; - state = ft_values; + interfaces = info_.state_interfaces; + state = ft_values_; return return_type::OK; } @@ -164,32 +165,32 @@ class DummyForceTorqueSensor : public Sensor if (interfaces.size() != 0) { return return_type::ERROR; } - ft_values = state; + ft_values_ = state; return return_type::OK; } private: ComponentInfo info_; - std::vector ft_values; + std::vector ft_values_; }; 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; + 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) + if (status_ == hardware_interface_status::CONFIGURED || + status_ == hardware_interface_status::STOPPED) { - status = hardware_interface_status::STARTED; + status_ = hardware_interface_status::STARTED; } else { return return_type::ERROR; } @@ -198,8 +199,8 @@ class DummyActuatorHardware : public ActuatorHardwareInterface return_type stop() override { - if (status == hardware_interface_status::STARTED) { - status = hardware_interface_status::STOPPED; + if (status_ == hardware_interface_status::STARTED) { + status_ = hardware_interface_status::STOPPED; } else { return return_type::ERROR; } @@ -208,43 +209,43 @@ class DummyActuatorHardware : public ActuatorHardwareInterface hardware_interface_status get_status() const override { - return status; + return status_; } return_type read_joint(Joint & joint) const override { - return joint.set_state(hw_values); + return joint.set_state(hw_values_); } return_type write_joint(const Joint & joint) override { - return joint.get_command(hw_values, 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}; - std::vector interfaces; - double hw_read_time, hw_write_time; + HardwareInfo info_; + hardware_interface_status status_ = hardware_interface_status::UNKNOWN; + std::vector hw_values_ = {1.2}; + std::vector interfaces_; + 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; + 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) + if (status_ == hardware_interface_status::CONFIGURED || + status_ == hardware_interface_status::STOPPED) { - status = hardware_interface_status::STARTED; + status_ = hardware_interface_status::STARTED; } else { return return_type::ERROR; } @@ -253,8 +254,8 @@ class DummySensorHardware : public SensorHardwareInterface return_type stop() override { - if (status == hardware_interface_status::STARTED) { - status = hardware_interface_status::STOPPED; + if (status_ == hardware_interface_status::STARTED) { + status_ = hardware_interface_status::STOPPED; } else { return return_type::ERROR; } @@ -263,14 +264,14 @@ class DummySensorHardware : public SensorHardwareInterface hardware_interface_status get_status() const override { - return status; + return status_; } return_type read_sensors(const std::vector> & sensors) const override { return_type ret = return_type::OK; for (const auto & sensor : sensors) { - ret = sensor->set_state(ft_hw_values); + ret = sensor->set_state(ft_hw_values_); if (ret != return_type::OK) { break; } @@ -279,30 +280,30 @@ class DummySensorHardware : public SensorHardwareInterface } 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}; + 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; + 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) + if (status_ == hardware_interface_status::CONFIGURED || + status_ == hardware_interface_status::STOPPED) { - status = hardware_interface_status::STARTED; + status_ = hardware_interface_status::STARTED; } else { return return_type::ERROR; } @@ -311,8 +312,8 @@ class DummySystemHardware : public SystemHardwareInterface return_type stop() override { - if (status == hardware_interface_status::STARTED) { - status = hardware_interface_status::STOPPED; + if (status_ == hardware_interface_status::STARTED) { + status_ = hardware_interface_status::STOPPED; } else { return return_type::ERROR; } @@ -321,14 +322,14 @@ class DummySystemHardware : public SystemHardwareInterface hardware_interface_status get_status() const override { - return status; + 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); + ret = sensor->set_state(ft_hw_values_); if (ret != return_type::OK) { break; } @@ -342,7 +343,7 @@ class DummySystemHardware : public SystemHardwareInterface std::vector joint_values; for (uint i = 0; i < joints.size(); i++) { joint_values.clear(); - joint_values.push_back(joints_hw_values[i]); + joint_values.push_back(joints_hw_values_[i]); ret = joints[i]->set_state(joint_values); if (ret != return_type::OK) { break; @@ -366,11 +367,11 @@ class DummySystemHardware : public SystemHardwareInterface } 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}; + 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 @@ -419,9 +420,9 @@ TEST_F(TestComponentInterfaces, joint_example_component_works) EXPECT_EQ(joint.configure(joint_info), return_type::OK); ASSERT_THAT(joint.get_command_interfaces(), SizeIs(1)); - EXPECT_EQ(joint.get_command_interfaces()[0], "position"); + 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], "position"); + EXPECT_EQ(joint.get_state_interfaces()[0], hardware_interface::HW_IF_POSITION); std::vector interfaces; std::vector input; input.push_back(2.1); @@ -431,10 +432,10 @@ TEST_F(TestComponentInterfaces, joint_example_component_works) ASSERT_THAT(output, SizeIs(1)); EXPECT_EQ(output[0], 2.1); ASSERT_THAT(interfaces, SizeIs(1)); - EXPECT_EQ(interfaces[0], "position"); + EXPECT_EQ(interfaces[0], hardware_interface::HW_IF_POSITION); - joint_info.command_interfaces.push_back("position"); - joint_info.command_interfaces.push_back("velocity"); + 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); } @@ -484,7 +485,7 @@ TEST_F(TestComponentInterfaces, actuator_hardware_interface_works) 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], "position"); + 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); @@ -563,14 +564,14 @@ TEST_F(TestComponentInterfaces, system_interface_with_hardware_works) ASSERT_THAT(output, SizeIs(1)); EXPECT_EQ(output[0], -1.575); ASSERT_THAT(interfaces, SizeIs(1)); - EXPECT_EQ(interfaces[0], "position"); + EXPECT_EQ(interfaces[0], hardware_interface::HW_IF_POSITION); output.clear(); interfaces.clear(); 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], "position"); + EXPECT_EQ(interfaces[0], hardware_interface::HW_IF_POSITION); EXPECT_EQ(system.stop(), return_type::OK); EXPECT_EQ(system.get_status(), hardware_interface_status::STOPPED); From f9c1d15111e18e4304283066be2510131989e085 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Wed, 19 Aug 2020 00:49:29 +0200 Subject: [PATCH 19/29] The base classes for Joints and Sensors are implemented --- .../include/hardware_interface/joint.hpp | 266 +++++++++++-- .../include/hardware_interface/sensor.hpp | 128 ++++++- .../hardware_interface_return_values.hpp | 6 + .../test/test_component_interfaces.cpp | 349 +++++++++++++----- 4 files changed, 623 insertions(+), 126 deletions(-) diff --git a/hardware_interface/include/hardware_interface/joint.hpp b/hardware_interface/include/hardware_interface/joint.hpp index 05f063bcd0..d6851787d5 100644 --- a/hardware_interface/include/hardware_interface/joint.hpp +++ b/hardware_interface/include/hardware_interface/joint.hpp @@ -27,10 +27,9 @@ namespace hardware_interface { /** - * \brief Virtual Class for the "Joint" component used as a basic build block for a robot. + * \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). - * The lists of command and state interfaces define this. */ class Joint { @@ -59,8 +58,10 @@ class Joint * \return string list with command interfaces. */ HARDWARE_INTERFACE_PUBLIC - virtual - std::vector get_command_interfaces() const = 0; + std::vector get_command_interfaces() const + { + return info_.command_interfaces; + } /** * \brief Provide the list of state interfaces configured for the joint. @@ -68,8 +69,10 @@ class Joint * \return string list with state interfaces. */ HARDWARE_INTERFACE_PUBLIC - virtual - std::vector get_state_interfaces() const = 0; + std::vector get_state_interfaces() const + { + return info_.state_interfaces; + } /** * \brief Get command list from the joint. This function is used in the write function of the @@ -79,14 +82,54 @@ class Joint * * \param command list of doubles with commands for the hardware. * \param interfaces list of interfaces on which commands have to set. - * \return return_type::OK the interfaces exist for the joints and the values, are set into - * commands list, otherwise return_type::ERROR. + * \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_type::OK otherwise. */ HARDWARE_INTERFACE_EXPORT - virtual return_type get_command( std::vector & command, - std::vector & interfaces) const = 0; + const std::vector & interfaces) const + { + if (interfaces.size() == 0) { + return return_type::INTERFACE_NOT_PROVIDED; + } + return_type ret = return_type::OK; + bool found; + + for (const auto & interface : interfaces) { + found = false; + for (uint i = 0; i < info_.command_interfaces.size(); i++) { + if (!interface.compare(info_.command_interfaces[i])) { + command.push_back(commands_[i]); + found = true; + break; + } + } + if (!found) { + ret = return_type::INTERFACE_NOT_FOUND; + command.clear(); + break; + } + } + return ret; + } + + /** + * \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. + * + * \param command list of doubles with commands for the hardware. + */ + HARDWARE_INTERFACE_EXPORT + void get_complete_command(std::vector & command) const + { + command.clear(); + for (const auto & internal_command : commands_) { + command.push_back(internal_command); + } + } /** * \brief Set command list for the joint. This function is used by the controller to set the goal @@ -95,14 +138,74 @@ class Joint * * \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::OK the interfaces exist for the joints and the values, are set valid - * for the joint, otherwise return_type::ERROR. + * \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. */ HARDWARE_INTERFACE_EXPORT - virtual return_type set_command( - const std::vector command, - std::vector interfaces = std::vector()) = 0; + const std::vector & command, + const std::vector & interfaces) + { + // check sizes + if (command.size() != interfaces.size()) { + return return_type::INTERFACE_VALUE_SIZE_NOT_EQUAL; + } + + return_type ret = return_type::OK; + bool found; + for (uint i = 0; i < interfaces.size(); i++) { + found = false; + for (uint j = 0; j < info_.command_interfaces.size(); j++) { + if (!interfaces[i].compare(info_.command_interfaces[j])) { + found = true; + if (check_command_limits(command[i], interfaces[i]) == return_type::OK) { + commands_[j] = command[i]; + } else { + ret = return_type::COMMAND_OUT_OF_LIMITS; + } + break; + } + } + if (!found) { + ret = return_type::INTERFACE_NOT_FOUND; + break; + } else if (ret != return_type::OK) { + break; + } + } + return ret; + } + + /** + * \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_complete_command(const std::vector & command) + { + return_type ret = return_type::OK; + if (command.size() == commands_.size()) { + for (uint i = 0; i < commands_.size(); i++) { + if (check_command_limits(command[i], info_.command_interfaces[i]) == return_type::OK) { + commands_[i] = command[i]; + } else { + ret = return_type::COMMAND_OUT_OF_LIMITS; + break; + } + } + } else { + ret = return_type::INTERFACE_VALUE_SIZE_NOT_EQUAL; + } + return ret; + } /** * \brief Get state list from the joint. This function is used by the controller to get the @@ -111,14 +214,54 @@ class Joint * * \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::OK the interfaces exist for the joints and the values are set into - * state list, otherwise return_type::ERROR. + * \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 - virtual return_type get_state( std::vector & state, - std::vector & interfaces) const = 0; + const std::vector & interfaces) const + { + if (interfaces.size() == 0) { + return return_type::INTERFACE_NOT_PROVIDED; + } + return_type ret = return_type::OK; + bool found; + + for (const auto & interface : interfaces) { + found = false; + for (uint i = 0; i < info_.state_interfaces.size(); i++) { + if (!interface.compare(info_.state_interfaces[i])) { + state.push_back(states_[i]); + found = true; + break; + } + } + if (!found) { + ret = return_type::INTERFACE_NOT_FOUND; + state.clear(); + break; + } + } + return ret; + } + + /** + * \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. + * + * \param state list of doubles with states of the hardware. + */ + HARDWARE_INTERFACE_EXPORT + void get_complete_state(std::vector & state) const + { + state.clear(); + for (const auto & internal_state : states_) { + state.push_back(internal_state); + } + } /** * \brief Set state list for the joint. This function is used by the hardware to set its actual @@ -127,14 +270,91 @@ class Joint * * \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::OK the interfaces exist for the joints and the values are set from the - * state list, otherwise return_type::ERROR. + * \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 - virtual return_type set_state( const std::vector & state, - std::vector interfaces = std::vector()) = 0; + const std::vector & interfaces) + { + if (state.size() != interfaces.size()) { + return return_type::INTERFACE_VALUE_SIZE_NOT_EQUAL; + } + return_type ret = return_type::OK; + bool found; + + for (uint i = 0; i < interfaces.size(); i++) { + found = false; + for (uint j = 0; j < info_.state_interfaces.size(); j++) { + if (!interfaces[i].compare(info_.state_interfaces[j])) { + states_[j] = state[i]; + found = true; + break; + } + } + if (!found) { + ret = return_type::INTERFACE_NOT_FOUND; + break; + } + } + return ret; + } + + /** + * \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_complete_state(const std::vector & state) + { + if (state.size() == states_.size()) { + for (uint i = 0; i < states_.size(); i++) { + states_[i] = state[i]; + } + } else { + return return_type::INTERFACE_VALUE_SIZE_NOT_EQUAL; + } + return return_type::OK; + } + +protected: + ComponentInfo info_; + std::vector commands_; + std::vector states_; + + /** + * @brief Specific joint logic for enforsing limits on the command. + * + * @return return_type::OK if all limits are respected; return_type::ERROR otherwise + */ + HARDWARE_INTERFACE_PUBLIC + virtual + return_type check_command_limits(const double & command, const std::string & interface) const = 0; + + /** + * \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 + */ + return_type configure_base(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; + } }; } // namespace hardware_interface diff --git a/hardware_interface/include/hardware_interface/sensor.hpp b/hardware_interface/include/hardware_interface/sensor.hpp index f2f29df15d..bcddea1740 100644 --- a/hardware_interface/include/hardware_interface/sensor.hpp +++ b/hardware_interface/include/hardware_interface/sensor.hpp @@ -27,7 +27,7 @@ namespace hardware_interface { /** - * \brief Virtual Class for "Sensor" component used as a basic build block for the devices which + * \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. */ @@ -58,8 +58,10 @@ class Sensor * \return string list with state interfaces. */ HARDWARE_INTERFACE_PUBLIC - virtual - std::vector get_state_interfaces() const = 0; + std::vector get_state_interfaces() + { + return info_.state_interfaces; + } /** * \brief Get state list from the sensor. This function is used by the controller to get the @@ -68,14 +70,54 @@ class Sensor * * \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::OK the interfaces exist for the sensor and the values are set into - * state list, otherwise return_type::ERROR. + * \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 - virtual return_type get_state( std::vector & state, - std::vector & interfaces) const = 0; + const std::vector & interfaces) const + { + if (interfaces.size() == 0) { + return return_type::INTERFACE_NOT_PROVIDED; + } + return_type ret = return_type::OK; + bool found; + + for (const auto & interface : interfaces) { + found = false; + for (uint i = 0; i < info_.state_interfaces.size(); i++) { + if (!interface.compare(info_.state_interfaces[i])) { + state.push_back(states_[i]); + found = true; + break; + } + } + if (!found) { + ret = return_type::INTERFACE_NOT_FOUND; + state.clear(); + break; + } + } + return ret; + } + + /** + * \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. + * + * \param state list of doubles with states of the hardware. + */ + HARDWARE_INTERFACE_EXPORT + void get_complete_state(std::vector & state) const + { + state.clear(); + for (const auto & internal_state : states_) { + state.push_back(internal_state); + } + } /** * \brief Set state list for the sensor. This function is used by the hardware to set its actual @@ -84,14 +126,78 @@ class Sensor * * \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::OK the interfaces exist for the sensor and the values are set from the - * state list, otherwise return_type::ERROR. + * \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 - virtual return_type set_state( const std::vector & state, - std::vector interfaces = std::vector()) = 0; + const std::vector & interfaces) + { + if (state.size() != interfaces.size()) { + return return_type::INTERFACE_VALUE_SIZE_NOT_EQUAL; + } + return_type ret = return_type::OK; + bool found; + + for (uint i = 0; i < info_.state_interfaces.size(); i++) { + found = false; + for (uint j = 0; j < info_.state_interfaces.size(); j++) { + if (!interfaces[i].compare(info_.state_interfaces[j])) { + states_[j] = state[i]; + found = true; + break; + } + } + if (!found) { + ret = return_type::INTERFACE_NOT_FOUND; + break; + } + } + return ret; + } + + /** + * \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 command size is not equal to number of + * joint's state interfaces, return_type::OK otherwise. + */ + HARDWARE_INTERFACE_EXPORT + return_type set_complete_state(const std::vector & state) + { + if (state.size() == states_.size()) { + for (uint i = 0; i < states_.size(); i++) { + states_[i] = state[i]; + } + } else { + return return_type::INTERFACE_VALUE_SIZE_NOT_EQUAL; + } + return return_type::OK; + } + +protected: + ComponentInfo info_; + std::vector states_; + + /** + * \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 + */ + return_type configure_base(const ComponentInfo & joint_info) + { + info_ = joint_info; + if (info_.state_interfaces.size() > 0) { + states_.resize(info_.state_interfaces.size()); + } + return return_type::OK; + } }; } // namespace hardware_interface 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/test/test_component_interfaces.cpp b/hardware_interface/test/test_component_interfaces.cpp index db110f6e96..6058c87369 100644 --- a/hardware_interface/test/test_component_interfaces.cpp +++ b/hardware_interface/test/test_component_interfaces.cpp @@ -45,81 +45,84 @@ class DummyPositionJoint : public Joint public: return_type configure(const ComponentInfo & joint_info) override { - info_ = joint_info; + if (configure_base(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; } - std::vector get_command_interfaces() const override + return_type check_command_limits( + const double & command, + const std::string & interface) const override { - return info_.command_interfaces; + return_type ret = return_type::ERROR; + if (!interface.compare(HW_IF_POSITION)) { + if (command >= min_position_ && command <= max_position_) { + ret = return_type::OK; + } + } + return ret; } - std::vector get_state_interfaces() const override - { - return info_.state_interfaces; - } +private: + double max_position_, min_position_; +}; - return_type set_command( - const std::vector command, - std::vector interfaces = std::vector()) override +class DummyMultiJoint : public Joint +{ +public: + return_type configure(const ComponentInfo & joint_info) override { - if (interfaces.size() != 0) { + if (configure_base(joint_info) != return_type::OK) { return return_type::ERROR; } - position_command_ = command[0]; - return return_type::OK; - } - return_type get_command( - std::vector & command, - std::vector & interfaces) const override - { - command.push_back(position_command_); - interfaces = info_.command_interfaces; + 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; } - return_type set_state( - const std::vector & state, - std::vector interfaces = std::vector()) override + return_type check_command_limits( + const double & command, + const std::string & interface) const override { - return_type ret = return_type::OK; - if (interfaces.size() != 0) { - ret = return_type::ERROR; - } - if (state[0] > min_position_ && state[0] < max_position_) { - position_state_ = state[0]; - } else { - ret = return_type::ERROR; + return_type ret = return_type::ERROR; + if (!interface.compare(HW_IF_POSITION)) { + if (command >= min_position_ && command <= max_position_) { + ret = return_type::OK; + } + } else if (!interface.compare(HW_IF_VELOCITY)) { + if (command >= min_velocity_ && command <= max_velocity_) { + ret = return_type::OK; + } } return ret; } - return_type get_state( - std::vector & state, - std::vector & interfaces) const override - { - state.push_back(position_state_); - interfaces = info_.command_interfaces; - return return_type::OK; - } - private: - ComponentInfo info_; - double position_command_; - double position_state_; double max_position_, min_position_; + double max_velocity_, min_velocity_; }; class DummyForceTorqueSensor : public Sensor @@ -127,7 +130,10 @@ class DummyForceTorqueSensor : public Sensor public: return_type configure(const ComponentInfo & sensor_info) override { - info_ = sensor_info; + if (configure_base(sensor_info) != return_type::OK) { + return return_type::ERROR; + } + if (info_.parameters["frame_id"] == "") { return return_type::ERROR; } @@ -139,39 +145,9 @@ class DummyForceTorqueSensor : public Sensor info_.state_interfaces.push_back("torque_y"); info_.state_interfaces.push_back("torque_z"); } - ft_values_.resize(6); - ft_values_ = {1.34, 5.67, 8.21, 5.63, 5.99, 4.32}; - return return_type::OK; - } - - std::vector get_state_interfaces() const override - { - return info_.state_interfaces; - } - - return_type get_state( - std::vector & state, - std::vector & interfaces) const override - { - interfaces = info_.state_interfaces; - state = ft_values_; + states_ = {1.34, 5.67, 8.21, 5.63, 5.99, 4.32}; return return_type::OK; } - - return_type set_state( - const std::vector & state, - std::vector interfaces) override - { - if (interfaces.size() != 0) { - return return_type::ERROR; - } - ft_values_ = state; - return return_type::OK; - } - -private: - ComponentInfo info_; - std::vector ft_values_; }; class DummyActuatorHardware : public ActuatorHardwareInterface @@ -214,19 +190,20 @@ class DummyActuatorHardware : public ActuatorHardwareInterface return_type read_joint(Joint & joint) const override { - return joint.set_state(hw_values_); + std::vector interfaces = joint.get_state_interfaces(); + return joint.set_state(hw_values_, interfaces); } return_type write_joint(const Joint & joint) override { - return joint.get_command(hw_values_, interfaces_); + 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}; - std::vector interfaces_; double hw_read_time_, hw_write_time_; }; @@ -271,7 +248,7 @@ class DummySensorHardware : public SensorHardwareInterface { return_type ret = return_type::OK; for (const auto & sensor : sensors) { - ret = sensor->set_state(ft_hw_values_); + ret = sensor->set_complete_state(ft_hw_values_); if (ret != return_type::OK) { break; } @@ -329,7 +306,7 @@ class DummySystemHardware : public SystemHardwareInterface { return_type ret = return_type::OK; for (const auto & sensor : sensors) { - ret = sensor->set_state(ft_hw_values_); + ret = sensor->set_complete_state(ft_hw_values_); if (ret != return_type::OK) { break; } @@ -340,11 +317,13 @@ class DummySystemHardware : public SystemHardwareInterface 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]); - ret = joints[i]->set_state(joint_values); + interfaces = joints[i]->get_state_interfaces(); + ret = joints[i]->set_state(joint_values, interfaces); if (ret != return_type::OK) { break; } @@ -391,6 +370,7 @@ using hardware_interface::SystemHardwareInterface; using hardware_interface::hardware_interface_status; using hardware_interface::hardware_interfaces_components_test::DummyPositionJoint; +using hardware_interface::hardware_interfaces_components_test::DummyMultiJoint; using hardware_interface::hardware_interfaces_components_test::DummyForceTorqueSensor; using hardware_interface::hardware_interfaces_components_test::DummyActuatorHardware; @@ -406,8 +386,8 @@ class TestComponentInterfaces : public Test void SetUp() override { joint_info.name = "DummyPositionJoint"; - joint_info.parameters["max_position"] = "1.742"; - joint_info.parameters["min_position"] = "-1.742"; + 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"; @@ -423,22 +403,177 @@ TEST_F(TestComponentInterfaces, joint_example_component_works) 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_VALUE_SIZE_NOT_EQUAL); + interfaces.push_back(hardware_interface::HW_IF_VELOCITY); + EXPECT_EQ(joint.set_command(input, interfaces), return_type::INTERFACE_NOT_FOUND); + interfaces.clear(); + interfaces.push_back(hardware_interface::HW_IF_POSITION); + input.clear(); + input.push_back(20.21); + EXPECT_EQ(joint.set_command(input, interfaces), return_type::COMMAND_OUT_OF_LIMITS); + input.clear(); + input.push_back(1.2); 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.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_complete_command(input), return_type::INTERFACE_VALUE_SIZE_NOT_EQUAL); + input.push_back(5.77); + EXPECT_EQ(joint.set_complete_command(input), return_type::COMMAND_OUT_OF_LIMITS); + input.clear(); + input.push_back(2.1); + EXPECT_EQ(joint.set_complete_command(input), return_type::OK); + + joint.get_complete_command(output); + ASSERT_THAT(output, SizeIs(1)); EXPECT_EQ(output[0], 2.1); - ASSERT_THAT(interfaces, SizeIs(1)); - EXPECT_EQ(interfaces[0], hardware_interface::HW_IF_POSITION); + // State getters and setters + interfaces.clear(); + input.clear(); + input.push_back(2.1); + EXPECT_EQ(joint.set_state(input, interfaces), return_type::INTERFACE_VALUE_SIZE_NOT_EQUAL); + interfaces.push_back(hardware_interface::HW_IF_VELOCITY); + EXPECT_EQ(joint.set_state(input, interfaces), return_type::INTERFACE_NOT_FOUND); + 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_VELOCITY); + EXPECT_EQ(joint.get_state(output, interfaces), return_type::INTERFACE_NOT_FOUND); + + input.clear(); + EXPECT_EQ(joint.set_complete_state(input), return_type::INTERFACE_VALUE_SIZE_NOT_EQUAL); + input.push_back(2.1); + EXPECT_EQ(joint.set_complete_state(input), return_type::OK); + + joint.get_complete_state(output); + 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_VALUE_SIZE_NOT_EQUAL); + interfaces.push_back(hardware_interface::HW_IF_EFFORT); + EXPECT_EQ(joint.set_command(input, interfaces), return_type::INTERFACE_NOT_FOUND); + interfaces.clear(); + interfaces.push_back(hardware_interface::HW_IF_VELOCITY); + input.clear(); + input.push_back(20.21); + EXPECT_EQ(joint.set_command(input, interfaces), return_type::COMMAND_OUT_OF_LIMITS); + 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_complete_command(input), return_type::INTERFACE_VALUE_SIZE_NOT_EQUAL); + input.push_back(5.77); + EXPECT_EQ(joint.set_complete_command(input), return_type::INTERFACE_VALUE_SIZE_NOT_EQUAL); + input.push_back(2.1); + EXPECT_EQ(joint.set_complete_command(input), return_type::COMMAND_OUT_OF_LIMITS); + input.clear(); + input.push_back(1.2); + input.push_back(0.4); + EXPECT_EQ(joint.set_complete_command(input), return_type::OK); + + joint.get_complete_command(output); + 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_VALUE_SIZE_NOT_EQUAL); + interfaces.push_back(hardware_interface::HW_IF_EFFORT); + EXPECT_EQ(joint.set_state(input, interfaces), return_type::INTERFACE_NOT_FOUND); + 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_complete_state(input), return_type::INTERFACE_VALUE_SIZE_NOT_EQUAL); + input.push_back(2.1); + input.push_back(1.02); + EXPECT_EQ(joint.set_complete_state(input), return_type::OK); + + joint.get_complete_state(output); + ASSERT_THAT(output, SizeIs(2)); + EXPECT_EQ(output[0], 2.1); +} + TEST_F(TestComponentInterfaces, sensor_example_component_works) { DummyForceTorqueSensor sensor; @@ -447,16 +582,44 @@ TEST_F(TestComponentInterfaces, sensor_example_component_works) 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, 6.7, 2.5, 3.8, 8.9, 12.3}; + 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); - EXPECT_EQ(output[1], 5.67); - ASSERT_THAT(interfaces, SizeIs(6)); - EXPECT_EQ(interfaces[0], "force_x"); + 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_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); + 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_complete_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_complete_state(input), return_type::OK); + + sensor.get_complete_state(output); + ASSERT_THAT(output, SizeIs(6)); EXPECT_EQ(output[5], 12.3); sensor_info.parameters.clear(); @@ -480,7 +643,7 @@ TEST_F(TestComponentInterfaces, actuator_hardware_interface_works) 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; + 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)); @@ -510,7 +673,7 @@ TEST_F(TestComponentInterfaces, sensor_interface_with_hardware_works) sensors.push_back(sensor); EXPECT_EQ(sensor_hw.read_sensors(sensors), return_type::OK); std::vector output; - std::vector interfaces; + 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)); @@ -550,7 +713,7 @@ TEST_F(TestComponentInterfaces, system_interface_with_hardware_works) EXPECT_EQ(system.read_sensors(sensors), return_type::OK); std::vector output; - std::vector interfaces; + 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); @@ -560,6 +723,7 @@ TEST_F(TestComponentInterfaces, system_interface_with_hardware_works) 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); @@ -567,6 +731,7 @@ TEST_F(TestComponentInterfaces, system_interface_with_hardware_works) 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); From ed11e86a0e4c9a64127e4acd269a9a3d265100f2 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Wed, 19 Aug 2020 09:44:33 +0200 Subject: [PATCH 20/29] Add more tests to get better coverage --- .../test/test_component_interfaces.cpp | 12 ++++++++++-- 1 file changed, 10 insertions(+), 2 deletions(-) diff --git a/hardware_interface/test/test_component_interfaces.cpp b/hardware_interface/test/test_component_interfaces.cpp index 6058c87369..f6baaf4f99 100644 --- a/hardware_interface/test/test_component_interfaces.cpp +++ b/hardware_interface/test/test_component_interfaces.cpp @@ -336,8 +336,8 @@ class DummySystemHardware : public SystemHardwareInterface return_type ret = return_type::OK; for (const auto & joint : joints) { std::vector values; - std::vector interfaces; - ret = joint->set_state(values, interfaces); + std::vector interfaces = joint->get_command_interfaces(); + ret = joint->get_command(values, interfaces); if (ret != return_type::OK) { break; } @@ -421,6 +421,9 @@ TEST_F(TestComponentInterfaces, joint_example_component_works) 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); @@ -454,6 +457,9 @@ TEST_F(TestComponentInterfaces, joint_example_component_works) 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); @@ -738,6 +744,8 @@ TEST_F(TestComponentInterfaces, system_interface_with_hardware_works) 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); } From f6a731a1d441b207d5906d60efdc3f0ef8da9980 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Thu, 20 Aug 2020 00:31:16 +0200 Subject: [PATCH 21/29] Updated after comments and added helper header --- .../component_interface_management.hpp | 145 ++++++++++++++++++ .../include/hardware_interface/joint.hpp | 131 ++++------------ .../include/hardware_interface/sensor.hpp | 101 +++--------- .../test/test_component_interfaces.cpp | 71 +++++---- 4 files changed, 234 insertions(+), 214 deletions(-) create mode 100644 hardware_interface/include/hardware_interface/helpers/component_interface_management.hpp diff --git a/hardware_interface/include/hardware_interface/helpers/component_interface_management.hpp b/hardware_interface/include/hardware_interface/helpers/component_interface_management.hpp new file mode 100644 index 0000000000..be6cc3d41c --- /dev/null +++ b/hardware_interface/include/hardware_interface/helpers/component_interface_management.hpp @@ -0,0 +1,145 @@ +// 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__HELPERS__COMPONENTS_LISTS_MANAGEMENT_HPP_ +#define HARDWARE_INTERFACE__HELPERS__COMPONENTS_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 helpers +{ + +/** + * \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. + */ +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; + * + * \param values output list of values. + * \param int_values internal values of the component. + */ +void 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); + } +} + +/** + * \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) + */ +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. + */ +return_type set_internal_values( + const std::vector & values, std::vector & int_values) +{ + if (values.size() == int_values.size()) { + for (uint i = 0; i < int_values.size(); i++) { + int_values[i] = values[i]; + } + } else { + return return_type::INTERFACE_VALUE_SIZE_NOT_EQUAL; + } + return return_type::OK; + +} + +} // namespace helpers + +} // namespace hardware_interface +#endif // HARDWARE_INTERFACE__HELPERS__COMPONENTS_LISTS_MANAGEMENT_HPP_ diff --git a/hardware_interface/include/hardware_interface/joint.hpp b/hardware_interface/include/hardware_interface/joint.hpp index d6851787d5..da927ec3f3 100644 --- a/hardware_interface/include/hardware_interface/joint.hpp +++ b/hardware_interface/include/hardware_interface/joint.hpp @@ -20,6 +20,7 @@ #include "hardware_interface/component_info.hpp" #include "hardware_interface/hardware_info.hpp" +#include "hardware_interface/helpers/component_interface_management.hpp" #include "hardware_interface/types/hardware_interface_return_values.hpp" #include "hardware_interface/visibility_control.h" @@ -42,15 +43,23 @@ class Joint ~Joint() = default; /** - * \brief Configure joint based on the description in the robot's URDF file. + * \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 - virtual - return_type configure(const ComponentInfo & joint_info) = 0; + return_type 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; + } /** * \brief Provide the list of command interfaces configured for the joint. @@ -84,35 +93,15 @@ class Joint * \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_type::OK otherwise. + * 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 { - if (interfaces.size() == 0) { - return return_type::INTERFACE_NOT_PROVIDED; - } - return_type ret = return_type::OK; - bool found; - - for (const auto & interface : interfaces) { - found = false; - for (uint i = 0; i < info_.command_interfaces.size(); i++) { - if (!interface.compare(info_.command_interfaces[i])) { - command.push_back(commands_[i]); - found = true; - break; - } - } - if (!found) { - ret = return_type::INTERFACE_NOT_FOUND; - command.clear(); - break; - } - } - return ret; + return helpers::get_internal_values(command, interfaces, info_.command_interfaces, commands_); } /** @@ -123,12 +112,9 @@ class Joint * \param command list of doubles with commands for the hardware. */ HARDWARE_INTERFACE_EXPORT - void get_complete_command(std::vector & command) const + void get_command(std::vector & command) const { - command.clear(); - for (const auto & internal_command : commands_) { - command.push_back(internal_command); - } + helpers::get_internal_values(command, commands_); } /** @@ -142,6 +128,10 @@ class Joint * 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( @@ -189,7 +179,7 @@ class Joint * of limits; return_type::OK otherwise. */ HARDWARE_INTERFACE_EXPORT - return_type set_complete_command(const std::vector & command) + return_type set_command(const std::vector & command) { return_type ret = return_type::OK; if (command.size() == commands_.size()) { @@ -216,35 +206,15 @@ class Joint * \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. + * 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 { - if (interfaces.size() == 0) { - return return_type::INTERFACE_NOT_PROVIDED; - } - return_type ret = return_type::OK; - bool found; - - for (const auto & interface : interfaces) { - found = false; - for (uint i = 0; i < info_.state_interfaces.size(); i++) { - if (!interface.compare(info_.state_interfaces[i])) { - state.push_back(states_[i]); - found = true; - break; - } - } - if (!found) { - ret = return_type::INTERFACE_NOT_FOUND; - state.clear(); - break; - } - } - return ret; + return helpers::get_internal_values(state, interfaces, info_.state_interfaces, states_); } /** @@ -255,12 +225,9 @@ class Joint * \param state list of doubles with states of the hardware. */ HARDWARE_INTERFACE_EXPORT - void get_complete_state(std::vector & state) const + void get_state(std::vector & state) const { - state.clear(); - for (const auto & internal_state : states_) { - state.push_back(internal_state); - } + helpers::get_internal_values(state, states_); } /** @@ -279,27 +246,7 @@ class Joint const std::vector & state, const std::vector & interfaces) { - if (state.size() != interfaces.size()) { - return return_type::INTERFACE_VALUE_SIZE_NOT_EQUAL; - } - return_type ret = return_type::OK; - bool found; - - for (uint i = 0; i < interfaces.size(); i++) { - found = false; - for (uint j = 0; j < info_.state_interfaces.size(); j++) { - if (!interfaces[i].compare(info_.state_interfaces[j])) { - states_[j] = state[i]; - found = true; - break; - } - } - if (!found) { - ret = return_type::INTERFACE_NOT_FOUND; - break; - } - } - return ret; + return helpers::set_internal_values(state, interfaces, info_.state_interfaces, states_); } /** @@ -312,7 +259,7 @@ class Joint * joint's state interfaces, return_type::OK otherwise. */ HARDWARE_INTERFACE_EXPORT - return_type set_complete_state(const std::vector & state) + return_type set_state(const std::vector & state) { if (state.size() == states_.size()) { for (uint i = 0; i < states_.size(); i++) { @@ -337,24 +284,6 @@ class Joint HARDWARE_INTERFACE_PUBLIC virtual return_type check_command_limits(const double & command, const std::string & interface) const = 0; - - /** - * \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 - */ - return_type configure_base(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; - } }; } // namespace hardware_interface diff --git a/hardware_interface/include/hardware_interface/sensor.hpp b/hardware_interface/include/hardware_interface/sensor.hpp index bcddea1740..920a1e9102 100644 --- a/hardware_interface/include/hardware_interface/sensor.hpp +++ b/hardware_interface/include/hardware_interface/sensor.hpp @@ -20,6 +20,7 @@ #include "hardware_interface/component_info.hpp" #include "hardware_interface/hardware_info.hpp" +#include "hardware_interface/helpers/component_interface_management.hpp" #include "hardware_interface/types/hardware_interface_return_values.hpp" #include "hardware_interface/visibility_control.h" @@ -42,15 +43,20 @@ class Sensor ~Sensor() = default; /** - * \brief Configure senosr based on the description in the robot's URDF file. + * \brief Configure base sensor class based on the description in the robot's URDF file. * - * \param sensor_info structure with data from URDF. + * \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 - virtual - return_type configure(const ComponentInfo & sensor_info) = 0; + return_type configure(const ComponentInfo & joint_info) + { + info_ = joint_info; + if (info_.state_interfaces.size() > 0) { + states_.resize(info_.state_interfaces.size()); + } + return return_type::OK; + } /** * \brief Provide the list of state interfaces configured for the sensor. @@ -72,35 +78,15 @@ class Sensor * \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. + * 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 { - if (interfaces.size() == 0) { - return return_type::INTERFACE_NOT_PROVIDED; - } - return_type ret = return_type::OK; - bool found; - - for (const auto & interface : interfaces) { - found = false; - for (uint i = 0; i < info_.state_interfaces.size(); i++) { - if (!interface.compare(info_.state_interfaces[i])) { - state.push_back(states_[i]); - found = true; - break; - } - } - if (!found) { - ret = return_type::INTERFACE_NOT_FOUND; - state.clear(); - break; - } - } - return ret; + return helpers::get_internal_values(state, interfaces, info_.state_interfaces, states_); } /** @@ -111,12 +97,9 @@ class Sensor * \param state list of doubles with states of the hardware. */ HARDWARE_INTERFACE_EXPORT - void get_complete_state(std::vector & state) const + void get_state(std::vector & state) const { - state.clear(); - for (const auto & internal_state : states_) { - state.push_back(internal_state); - } + helpers::get_internal_values(state, states_); } /** @@ -135,27 +118,7 @@ class Sensor const std::vector & state, const std::vector & interfaces) { - if (state.size() != interfaces.size()) { - return return_type::INTERFACE_VALUE_SIZE_NOT_EQUAL; - } - return_type ret = return_type::OK; - bool found; - - for (uint i = 0; i < info_.state_interfaces.size(); i++) { - found = false; - for (uint j = 0; j < info_.state_interfaces.size(); j++) { - if (!interfaces[i].compare(info_.state_interfaces[j])) { - states_[j] = state[i]; - found = true; - break; - } - } - if (!found) { - ret = return_type::INTERFACE_NOT_FOUND; - break; - } - } - return ret; + return helpers::set_internal_values(state, interfaces, info_.state_interfaces, states_); } /** @@ -164,40 +127,18 @@ class Sensor * 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. + * \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_complete_state(const std::vector & state) + return_type set_state(const std::vector & state) { - if (state.size() == states_.size()) { - for (uint i = 0; i < states_.size(); i++) { - states_[i] = state[i]; - } - } else { - return return_type::INTERFACE_VALUE_SIZE_NOT_EQUAL; - } - return return_type::OK; + return helpers::set_internal_values(state, states_); } protected: ComponentInfo info_; std::vector states_; - - /** - * \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 - */ - return_type configure_base(const ComponentInfo & joint_info) - { - info_ = joint_info; - if (info_.state_interfaces.size() > 0) { - states_.resize(info_.state_interfaces.size()); - } - return return_type::OK; - } }; } // namespace hardware_interface diff --git a/hardware_interface/test/test_component_interfaces.cpp b/hardware_interface/test/test_component_interfaces.cpp index f6baaf4f99..4ca0f7ca7e 100644 --- a/hardware_interface/test/test_component_interfaces.cpp +++ b/hardware_interface/test/test_component_interfaces.cpp @@ -43,9 +43,9 @@ namespace hardware_interfaces_components_test class DummyPositionJoint : public Joint { public: - return_type configure(const ComponentInfo & joint_info) override + return_type configure(const ComponentInfo & joint_info) { - if (configure_base(joint_info) != return_type::OK) { + if (Joint::configure(joint_info) != return_type::OK) { return return_type::ERROR; } @@ -86,9 +86,9 @@ class DummyPositionJoint : public Joint class DummyMultiJoint : public Joint { public: - return_type configure(const ComponentInfo & joint_info) override + return_type configure(const ComponentInfo & joint_info) { - if (configure_base(joint_info) != return_type::OK) { + if (Joint::configure(joint_info) != return_type::OK) { return return_type::ERROR; } @@ -128,9 +128,9 @@ class DummyMultiJoint : public Joint class DummyForceTorqueSensor : public Sensor { public: - return_type configure(const ComponentInfo & sensor_info) override + return_type configure(const ComponentInfo & sensor_info) { - if (configure_base(sensor_info) != return_type::OK) { + if (Sensor::configure(sensor_info) != return_type::OK) { return return_type::ERROR; } @@ -248,7 +248,7 @@ class DummySensorHardware : public SensorHardwareInterface { return_type ret = return_type::OK; for (const auto & sensor : sensors) { - ret = sensor->set_complete_state(ft_hw_values_); + ret = sensor->set_state(ft_hw_values_); if (ret != return_type::OK) { break; } @@ -306,7 +306,7 @@ class DummySystemHardware : public SystemHardwareInterface { return_type ret = return_type::OK; for (const auto & sensor : sensors) { - ret = sensor->set_complete_state(ft_hw_values_); + ret = sensor->set_state(ft_hw_values_); if (ret != return_type::OK) { break; } @@ -432,14 +432,14 @@ TEST_F(TestComponentInterfaces, joint_example_component_works) EXPECT_EQ(joint.get_command(output, interfaces), return_type::INTERFACE_NOT_FOUND); input.clear(); - EXPECT_EQ(joint.set_complete_command(input), return_type::INTERFACE_VALUE_SIZE_NOT_EQUAL); + EXPECT_EQ(joint.set_command(input), return_type::INTERFACE_VALUE_SIZE_NOT_EQUAL); input.push_back(5.77); - EXPECT_EQ(joint.set_complete_command(input), return_type::COMMAND_OUT_OF_LIMITS); + EXPECT_EQ(joint.set_command(input), return_type::COMMAND_OUT_OF_LIMITS); input.clear(); input.push_back(2.1); - EXPECT_EQ(joint.set_complete_command(input), return_type::OK); + EXPECT_EQ(joint.set_command(input), return_type::OK); - joint.get_complete_command(output); + joint.get_command(output); ASSERT_THAT(output, SizeIs(1)); EXPECT_EQ(output[0], 2.1); @@ -447,9 +447,11 @@ TEST_F(TestComponentInterfaces, joint_example_component_works) interfaces.clear(); input.clear(); input.push_back(2.1); - EXPECT_EQ(joint.set_state(input, interfaces), return_type::INTERFACE_VALUE_SIZE_NOT_EQUAL); + 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(); @@ -468,11 +470,11 @@ TEST_F(TestComponentInterfaces, joint_example_component_works) EXPECT_EQ(joint.get_state(output, interfaces), return_type::INTERFACE_NOT_FOUND); input.clear(); - EXPECT_EQ(joint.set_complete_state(input), return_type::INTERFACE_VALUE_SIZE_NOT_EQUAL); + EXPECT_EQ(joint.set_state(input), return_type::INTERFACE_VALUE_SIZE_NOT_EQUAL); input.push_back(2.1); - EXPECT_EQ(joint.set_complete_state(input), return_type::OK); + EXPECT_EQ(joint.set_state(input), return_type::OK); - joint.get_complete_state(output); + joint.get_state(output); ASSERT_THAT(output, SizeIs(1)); EXPECT_EQ(output[0], 2.1); @@ -534,17 +536,17 @@ TEST_F(TestComponentInterfaces, multi_joint_example_component_works) EXPECT_EQ(joint.get_command(output, interfaces), return_type::INTERFACE_NOT_FOUND); input.clear(); - EXPECT_EQ(joint.set_complete_command(input), return_type::INTERFACE_VALUE_SIZE_NOT_EQUAL); + EXPECT_EQ(joint.set_command(input), return_type::INTERFACE_VALUE_SIZE_NOT_EQUAL); input.push_back(5.77); - EXPECT_EQ(joint.set_complete_command(input), return_type::INTERFACE_VALUE_SIZE_NOT_EQUAL); + EXPECT_EQ(joint.set_command(input), return_type::INTERFACE_VALUE_SIZE_NOT_EQUAL); input.push_back(2.1); - EXPECT_EQ(joint.set_complete_command(input), return_type::COMMAND_OUT_OF_LIMITS); + EXPECT_EQ(joint.set_command(input), return_type::COMMAND_OUT_OF_LIMITS); input.clear(); input.push_back(1.2); input.push_back(0.4); - EXPECT_EQ(joint.set_complete_command(input), return_type::OK); + EXPECT_EQ(joint.set_command(input), return_type::OK); - joint.get_complete_command(output); + joint.get_command(output); ASSERT_THAT(output, SizeIs(2)); EXPECT_EQ(output[1], 0.4); @@ -552,9 +554,11 @@ TEST_F(TestComponentInterfaces, multi_joint_example_component_works) interfaces.clear(); input.clear(); input.push_back(2.1); - EXPECT_EQ(joint.set_state(input, interfaces), return_type::INTERFACE_VALUE_SIZE_NOT_EQUAL); + 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(); @@ -570,12 +574,12 @@ TEST_F(TestComponentInterfaces, multi_joint_example_component_works) EXPECT_EQ(joint.get_state(output, interfaces), return_type::INTERFACE_NOT_FOUND); input.clear(); - EXPECT_EQ(joint.set_complete_state(input), return_type::INTERFACE_VALUE_SIZE_NOT_EQUAL); + 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_complete_state(input), return_type::OK); + EXPECT_EQ(joint.set_state(input), return_type::OK); - joint.get_complete_state(output); + joint.get_state(output); ASSERT_THAT(output, SizeIs(2)); EXPECT_EQ(output[0], 2.1); } @@ -599,10 +603,11 @@ TEST_F(TestComponentInterfaces, sensor_example_component_works) // State getters and setters interfaces.clear(); - EXPECT_EQ(sensor.set_state(input, interfaces), return_type::INTERFACE_VALUE_SIZE_NOT_EQUAL); + 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); @@ -620,11 +625,11 @@ TEST_F(TestComponentInterfaces, sensor_example_component_works) EXPECT_EQ(sensor.get_state(output, interfaces), return_type::INTERFACE_NOT_FOUND); input.clear(); - EXPECT_EQ(sensor.set_complete_state(input), return_type::INTERFACE_VALUE_SIZE_NOT_EQUAL); + 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_complete_state(input), return_type::OK); + EXPECT_EQ(sensor.set_state(input), return_type::OK); - sensor.get_complete_state(output); + sensor.get_state(output); ASSERT_THAT(output, SizeIs(6)); EXPECT_EQ(output[5], 12.3); @@ -663,7 +668,7 @@ TEST_F(TestComponentInterfaces, actuator_hardware_interface_works) TEST_F(TestComponentInterfaces, sensor_interface_with_hardware_works) { SensorHardware sensor_hw(std::make_unique()); - std::shared_ptr sensor(new DummyForceTorqueSensor); + std::shared_ptr sensor(std::make_shared()); HardwareInfo sensor_hw_info; sensor_hw_info.name = "DummySensor"; @@ -692,13 +697,13 @@ TEST_F(TestComponentInterfaces, sensor_interface_with_hardware_works) TEST_F(TestComponentInterfaces, system_interface_with_hardware_works) { SystemHardware system(std::make_unique()); - std::shared_ptr joint1(new DummyPositionJoint()); - std::shared_ptr joint2(new DummyPositionJoint()); + std::shared_ptr joint1(std::make_shared()); + std::shared_ptr joint2(std::make_shared()); std::vector> joints; joints.push_back(joint1); joints.push_back(joint2); - std::shared_ptr sensor(new DummyForceTorqueSensor); + std::shared_ptr sensor(std::make_shared()); std::vector> sensors; sensors.push_back(sensor); From 8dbe0bc08cbf65c90d40aec313ff1b2f12a23f74 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Thu, 20 Aug 2020 00:57:42 +0200 Subject: [PATCH 22/29] Implementation moved to cpp files --- hardware_interface/CMakeLists.txt | 26 +++- ...ent.hpp => component_lists_management.hpp} | 68 +-------- .../include/hardware_interface/joint.hpp | 112 ++------------- .../include/hardware_interface/sensor.hpp | 37 +---- .../helpers/component_lists_management.cpp | 95 +++++++++++++ hardware_interface/src/joint.cpp | 133 ++++++++++++++++++ hardware_interface/src/sensor.cpp | 62 ++++++++ 7 files changed, 341 insertions(+), 192 deletions(-) rename hardware_interface/include/hardware_interface/helpers/{component_interface_management.hpp => component_lists_management.hpp} (66%) create mode 100644 hardware_interface/src/helpers/component_lists_management.cpp create mode 100644 hardware_interface/src/joint.cpp create mode 100644 hardware_interface/src/sensor.cpp diff --git a/hardware_interface/CMakeLists.txt b/hardware_interface/CMakeLists.txt index 41118b1893..7e72fdb596 100644 --- a/hardware_interface/CMakeLists.txt +++ b/hardware_interface/CMakeLists.txt @@ -38,6 +38,28 @@ ament_target_dependencies( # which is appropriate when building the dll but not consuming it. target_compile_definitions(hardware_interface PRIVATE "HARDWARE_INTERFACE_BUILDING_DLL") +add_library( + control_components + SHARED + src/helpers/component_lists_management.cpp + src/joint.cpp + src/sensor.cpp +) +target_include_directories( + control_components + PUBLIC + include +) +# ament_target_dependencies( +# control_components +# 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(control_components PRIVATE "HARDWARE_INTERFACE_BUILDING_DLL") + install( DIRECTORY include/ DESTINATION include @@ -45,6 +67,7 @@ install( install( TARGETS + control_components hardware_interface RUNTIME DESTINATION bin ARCHIVE DESTINATION lib @@ -76,7 +99,7 @@ if(BUILD_TESTING) 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 hardware_interface) + target_link_libraries(test_component_interfaces control_components hardware_interface) endif() ament_export_dependencies( @@ -87,6 +110,7 @@ ament_export_include_directories( include ) ament_export_libraries( + control_components hardware_interface ) ament_export_dependencies( diff --git a/hardware_interface/include/hardware_interface/helpers/component_interface_management.hpp b/hardware_interface/include/hardware_interface/helpers/component_lists_management.hpp similarity index 66% rename from hardware_interface/include/hardware_interface/helpers/component_interface_management.hpp rename to hardware_interface/include/hardware_interface/helpers/component_lists_management.hpp index be6cc3d41c..e5e2f97d28 100644 --- a/hardware_interface/include/hardware_interface/helpers/component_interface_management.hpp +++ b/hardware_interface/include/hardware_interface/helpers/component_lists_management.hpp @@ -12,10 +12,9 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef HARDWARE_INTERFACE__HELPERS__COMPONENTS_LISTS_MANAGEMENT_HPP_ -#define HARDWARE_INTERFACE__HELPERS__COMPONENTS_LISTS_MANAGEMENT_HPP_ +#ifndef HARDWARE_INTERFACE__HELPERS__COMPONENT_LISTS_MANAGEMENT_HPP_ +#define HARDWARE_INTERFACE__HELPERS__COMPONENT_LISTS_MANAGEMENT_HPP_ -#include #include #include @@ -43,24 +42,7 @@ namespace helpers */ 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; -} + const std::vector & int_interfaces, const std::vector & int_values); /** * \brief Set all internal values to to other vector; @@ -69,13 +51,7 @@ return_type get_internal_values( * \param int_values internal values of the component. */ void 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); - } -} + std::vector & values, const std::vector & int_values); /** * \brief set values for queried_interfaces to the int_values. int_values data structure matches @@ -96,26 +72,7 @@ void get_internal_values( */ 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; -} + const std::vector & int_interfaces, std::vector & int_values); /** * \brief set all values to compoenents internal values. @@ -126,20 +83,9 @@ return_type set_internal_values( * return_type::OK otherwise. */ return_type set_internal_values( - const std::vector & values, std::vector & int_values) -{ - if (values.size() == int_values.size()) { - for (uint i = 0; i < int_values.size(); i++) { - int_values[i] = values[i]; - } - } else { - return return_type::INTERFACE_VALUE_SIZE_NOT_EQUAL; - } - return return_type::OK; - -} + const std::vector & values, std::vector & int_values); } // namespace helpers } // namespace hardware_interface -#endif // HARDWARE_INTERFACE__HELPERS__COMPONENTS_LISTS_MANAGEMENT_HPP_ +#endif // HARDWARE_INTERFACE__HELPERS__COMPONENT_LISTS_MANAGEMENT_HPP_ diff --git a/hardware_interface/include/hardware_interface/joint.hpp b/hardware_interface/include/hardware_interface/joint.hpp index da927ec3f3..401ee7e349 100644 --- a/hardware_interface/include/hardware_interface/joint.hpp +++ b/hardware_interface/include/hardware_interface/joint.hpp @@ -19,8 +19,6 @@ #include #include "hardware_interface/component_info.hpp" -#include "hardware_interface/hardware_info.hpp" -#include "hardware_interface/helpers/component_interface_management.hpp" #include "hardware_interface/types/hardware_interface_return_values.hpp" #include "hardware_interface/visibility_control.h" @@ -49,17 +47,8 @@ class Joint * \return return_type::OK if required data are provided and is successfully parsed, * return_type::ERROR otherwise. */ - return_type 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; - } + HARDWARE_INTERFACE_PUBLIC + return_type configure(const ComponentInfo & joint_info); /** * \brief Provide the list of command interfaces configured for the joint. @@ -67,10 +56,7 @@ class Joint * \return string list with command interfaces. */ HARDWARE_INTERFACE_PUBLIC - std::vector get_command_interfaces() const - { - return info_.command_interfaces; - } + std::vector get_command_interfaces() const; /** * \brief Provide the list of state interfaces configured for the joint. @@ -78,10 +64,7 @@ class Joint * \return string list with state interfaces. */ HARDWARE_INTERFACE_PUBLIC - std::vector get_state_interfaces() const - { - return info_.state_interfaces; - } + std::vector get_state_interfaces() const; /** * \brief Get command list from the joint. This function is used in the write function of the @@ -99,10 +82,7 @@ class Joint HARDWARE_INTERFACE_EXPORT return_type get_command( std::vector & command, - const std::vector & interfaces) const - { - return helpers::get_internal_values(command, interfaces, info_.command_interfaces, commands_); - } + const std::vector & interfaces) const; /** * \brief Get complete command list for the joint. This function is used by the hardware to get @@ -112,10 +92,7 @@ class Joint * \param command list of doubles with commands for the hardware. */ HARDWARE_INTERFACE_EXPORT - void get_command(std::vector & command) const - { - helpers::get_internal_values(command, commands_); - } + void get_command(std::vector & command) const; /** * \brief Set command list for the joint. This function is used by the controller to set the goal @@ -136,37 +113,7 @@ class Joint HARDWARE_INTERFACE_EXPORT return_type set_command( const std::vector & command, - const std::vector & interfaces) - { - // check sizes - if (command.size() != interfaces.size()) { - return return_type::INTERFACE_VALUE_SIZE_NOT_EQUAL; - } - - return_type ret = return_type::OK; - bool found; - for (uint i = 0; i < interfaces.size(); i++) { - found = false; - for (uint j = 0; j < info_.command_interfaces.size(); j++) { - if (!interfaces[i].compare(info_.command_interfaces[j])) { - found = true; - if (check_command_limits(command[i], interfaces[i]) == return_type::OK) { - commands_[j] = command[i]; - } else { - ret = return_type::COMMAND_OUT_OF_LIMITS; - } - break; - } - } - if (!found) { - ret = return_type::INTERFACE_NOT_FOUND; - break; - } else if (ret != return_type::OK) { - break; - } - } - return ret; - } + const std::vector & interfaces); /** * \brief Get complete state list from the joint. This function is used by the hardware to get @@ -179,23 +126,7 @@ class Joint * of limits; return_type::OK otherwise. */ HARDWARE_INTERFACE_EXPORT - return_type set_command(const std::vector & command) - { - return_type ret = return_type::OK; - if (command.size() == commands_.size()) { - for (uint i = 0; i < commands_.size(); i++) { - if (check_command_limits(command[i], info_.command_interfaces[i]) == return_type::OK) { - commands_[i] = command[i]; - } else { - ret = return_type::COMMAND_OUT_OF_LIMITS; - break; - } - } - } else { - ret = return_type::INTERFACE_VALUE_SIZE_NOT_EQUAL; - } - return ret; - } + 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 @@ -212,10 +143,7 @@ class Joint HARDWARE_INTERFACE_EXPORT return_type get_state( std::vector & state, - const std::vector & interfaces) const - { - return helpers::get_internal_values(state, interfaces, info_.state_interfaces, states_); - } + const std::vector & interfaces) const; /** * \brief Get complete state list from the joint. This function is used by the controller to get @@ -225,10 +153,7 @@ class Joint * \param state list of doubles with states of the hardware. */ HARDWARE_INTERFACE_EXPORT - void get_state(std::vector & state) const - { - helpers::get_internal_values(state, states_); - } + void get_state(std::vector & state) const; /** * \brief Set state list for the joint. This function is used by the hardware to set its actual @@ -244,10 +169,7 @@ class Joint HARDWARE_INTERFACE_EXPORT return_type set_state( const std::vector & state, - const std::vector & interfaces) - { - return helpers::set_internal_values(state, interfaces, info_.state_interfaces, states_); - } + const std::vector & interfaces); /** * \brief Set complete state list from the joint.This function is used by the hardware to set its @@ -259,17 +181,7 @@ class Joint * joint's state interfaces, return_type::OK otherwise. */ HARDWARE_INTERFACE_EXPORT - return_type set_state(const std::vector & state) - { - if (state.size() == states_.size()) { - for (uint i = 0; i < states_.size(); i++) { - states_[i] = state[i]; - } - } else { - return return_type::INTERFACE_VALUE_SIZE_NOT_EQUAL; - } - return return_type::OK; - } + return_type set_state(const std::vector & state); protected: ComponentInfo info_; diff --git a/hardware_interface/include/hardware_interface/sensor.hpp b/hardware_interface/include/hardware_interface/sensor.hpp index 920a1e9102..13c9ba28ee 100644 --- a/hardware_interface/include/hardware_interface/sensor.hpp +++ b/hardware_interface/include/hardware_interface/sensor.hpp @@ -19,8 +19,6 @@ #include #include "hardware_interface/component_info.hpp" -#include "hardware_interface/hardware_info.hpp" -#include "hardware_interface/helpers/component_interface_management.hpp" #include "hardware_interface/types/hardware_interface_return_values.hpp" #include "hardware_interface/visibility_control.h" @@ -49,14 +47,8 @@ class Sensor * \return return_type::OK if required data are provided and is successfully parsed, * return_type::ERROR otherwise. */ - return_type configure(const ComponentInfo & joint_info) - { - info_ = joint_info; - if (info_.state_interfaces.size() > 0) { - states_.resize(info_.state_interfaces.size()); - } - return return_type::OK; - } + HARDWARE_INTERFACE_PUBLIC + return_type configure(const ComponentInfo & joint_info); /** * \brief Provide the list of state interfaces configured for the sensor. @@ -64,10 +56,7 @@ class Sensor * \return string list with state interfaces. */ HARDWARE_INTERFACE_PUBLIC - std::vector get_state_interfaces() - { - return info_.state_interfaces; - } + std::vector get_state_interfaces(); /** * \brief Get state list from the sensor. This function is used by the controller to get the @@ -84,10 +73,7 @@ class Sensor HARDWARE_INTERFACE_EXPORT return_type get_state( std::vector & state, - const std::vector & interfaces) const - { - return helpers::get_internal_values(state, interfaces, info_.state_interfaces, states_); - } + const std::vector & interfaces) const; /** * \brief Get complete state list from the sensor. This function is used by the controller to get @@ -97,10 +83,7 @@ class Sensor * \param state list of doubles with states of the hardware. */ HARDWARE_INTERFACE_EXPORT - void get_state(std::vector & state) const - { - helpers::get_internal_values(state, states_); - } + void get_state(std::vector & state) const; /** * \brief Set state list for the sensor. This function is used by the hardware to set its actual @@ -116,10 +99,7 @@ class Sensor HARDWARE_INTERFACE_EXPORT return_type set_state( const std::vector & state, - const std::vector & interfaces) - { - return helpers::set_internal_values(state, interfaces, info_.state_interfaces, states_); - } + const std::vector & interfaces); /** * \brief Set complete state list from the sensor.This function is used by the hardware to set its @@ -131,10 +111,7 @@ class Sensor * sensor's state interfaces, return_type::OK otherwise. */ HARDWARE_INTERFACE_EXPORT - return_type set_state(const std::vector & state) - { - return helpers::set_internal_values(state, states_); - } + return_type set_state(const std::vector & state); protected: ComponentInfo info_; diff --git a/hardware_interface/src/helpers/component_lists_management.cpp b/hardware_interface/src/helpers/component_lists_management.cpp new file mode 100644 index 0000000000..6e46db2638 --- /dev/null +++ b/hardware_interface/src/helpers/component_lists_management.cpp @@ -0,0 +1,95 @@ +// 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 "hardware_interface/helpers/component_lists_management.hpp" +#include "hardware_interface/types/hardware_interface_return_values.hpp" + +namespace hardware_interface +{ + +namespace helpers +{ + +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; +} + +void 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_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; +} + +return_type set_internal_values( + const std::vector & values, std::vector & int_values) +{ + if (values.size() == int_values.size()) { + for (uint i = 0; i < int_values.size(); i++) { + int_values[i] = values[i]; + } + } else { + return return_type::INTERFACE_VALUE_SIZE_NOT_EQUAL; + } + return return_type::OK; +} + +} // namespace helpers +} // namespace hardware_interface diff --git a/hardware_interface/src/joint.cpp b/hardware_interface/src/joint.cpp new file mode 100644 index 0000000000..e44287dbc4 --- /dev/null +++ b/hardware_interface/src/joint.cpp @@ -0,0 +1,133 @@ +// 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/component_info.hpp" +#include "hardware_interface/joint.hpp" +#include "hardware_interface/helpers/component_lists_management.hpp" +#include "hardware_interface/types/hardware_interface_return_values.hpp" + +namespace hardware_interface +{ + +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 helpers::get_internal_values(command, interfaces, info_.command_interfaces, commands_); +} + +void Joint::get_command(std::vector & command) const +{ + helpers::get_internal_values(command, commands_); +} + +return_type Joint::set_command( + const std::vector & command, + const std::vector & interfaces) +{ + // check sizes + if (command.size() != interfaces.size()) { + return return_type::INTERFACE_VALUE_SIZE_NOT_EQUAL; + } + + return_type ret = return_type::OK; + bool found; + for (uint i = 0; i < interfaces.size(); i++) { + found = false; + for (uint j = 0; j < info_.command_interfaces.size(); j++) { + if (!interfaces[i].compare(info_.command_interfaces[j])) { + found = true; + if (check_command_limits(command[i], interfaces[i]) == return_type::OK) { + commands_[j] = command[i]; + } else { + ret = return_type::COMMAND_OUT_OF_LIMITS; + } + break; + } + } + if (!found) { + ret = return_type::INTERFACE_NOT_FOUND; + break; + } else if (ret != return_type::OK) { + break; + } + } + return ret; +} + +return_type Joint::set_command(const std::vector & command) +{ + return_type ret = return_type::OK; + if (command.size() == commands_.size()) { + for (uint i = 0; i < commands_.size(); i++) { + if (check_command_limits(command[i], info_.command_interfaces[i]) == return_type::OK) { + commands_[i] = command[i]; + } else { + ret = return_type::COMMAND_OUT_OF_LIMITS; + break; + } + } + } else { + ret = return_type::INTERFACE_VALUE_SIZE_NOT_EQUAL; + } + return ret; +} + +return_type Joint::get_state( + std::vector & state, const std::vector & interfaces) const +{ + return helpers::get_internal_values(state, interfaces, info_.state_interfaces, states_); +} + +void Joint::get_state(std::vector & state) const +{ + helpers::get_internal_values(state, states_); +} + +return_type Joint::set_state( + const std::vector & state, const std::vector & interfaces) +{ + return helpers::set_internal_values(state, interfaces, info_.state_interfaces, states_); +} + +return_type Joint::set_state(const std::vector & state) +{ + return helpers::set_internal_values(state, states_); +} + +} // namespace hardware_interface diff --git a/hardware_interface/src/sensor.cpp b/hardware_interface/src/sensor.cpp new file mode 100644 index 0000000000..8b3ff76a92 --- /dev/null +++ b/hardware_interface/src/sensor.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 "hardware_interface/component_info.hpp" +#include "hardware_interface/helpers/component_lists_management.hpp" +#include "hardware_interface/sensor.hpp" +#include "hardware_interface/types/hardware_interface_return_values.hpp" + +namespace hardware_interface +{ + +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 helpers::get_internal_values(state, interfaces, info_.state_interfaces, states_); +} + +void Sensor::get_state(std::vector & state) const +{ + helpers::get_internal_values(state, states_); +} + +return_type Sensor::set_state( + const std::vector & state, const std::vector & interfaces) +{ + return helpers::set_internal_values(state, interfaces, info_.state_interfaces, states_); +} + +return_type Sensor::set_state(const std::vector & state) +{ + return helpers::set_internal_values(state, states_); +} + +} // namespace hardware_interface From f871d5e7dfe156c810fb44f946abb2c015695636 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Thu, 20 Aug 2020 01:09:03 +0200 Subject: [PATCH 23/29] Removed virtual function for checking limits --- .../include/hardware_interface/joint.hpp | 9 ---- hardware_interface/src/joint.cpp | 44 +---------------- .../test/test_component_interfaces.cpp | 49 +++---------------- 3 files changed, 8 insertions(+), 94 deletions(-) diff --git a/hardware_interface/include/hardware_interface/joint.hpp b/hardware_interface/include/hardware_interface/joint.hpp index 401ee7e349..c5442c44ad 100644 --- a/hardware_interface/include/hardware_interface/joint.hpp +++ b/hardware_interface/include/hardware_interface/joint.hpp @@ -187,15 +187,6 @@ class Joint ComponentInfo info_; std::vector commands_; std::vector states_; - - /** - * @brief Specific joint logic for enforsing limits on the command. - * - * @return return_type::OK if all limits are respected; return_type::ERROR otherwise - */ - HARDWARE_INTERFACE_PUBLIC - virtual - return_type check_command_limits(const double & command, const std::string & interface) const = 0; }; } // namespace hardware_interface diff --git a/hardware_interface/src/joint.cpp b/hardware_interface/src/joint.cpp index e44287dbc4..d24eb04b03 100644 --- a/hardware_interface/src/joint.cpp +++ b/hardware_interface/src/joint.cpp @@ -60,52 +60,12 @@ return_type Joint::set_command( const std::vector & command, const std::vector & interfaces) { - // check sizes - if (command.size() != interfaces.size()) { - return return_type::INTERFACE_VALUE_SIZE_NOT_EQUAL; - } - - return_type ret = return_type::OK; - bool found; - for (uint i = 0; i < interfaces.size(); i++) { - found = false; - for (uint j = 0; j < info_.command_interfaces.size(); j++) { - if (!interfaces[i].compare(info_.command_interfaces[j])) { - found = true; - if (check_command_limits(command[i], interfaces[i]) == return_type::OK) { - commands_[j] = command[i]; - } else { - ret = return_type::COMMAND_OUT_OF_LIMITS; - } - break; - } - } - if (!found) { - ret = return_type::INTERFACE_NOT_FOUND; - break; - } else if (ret != return_type::OK) { - break; - } - } - return ret; + return helpers::set_internal_values(command, interfaces, info_.command_interfaces, commands_); } return_type Joint::set_command(const std::vector & command) { - return_type ret = return_type::OK; - if (command.size() == commands_.size()) { - for (uint i = 0; i < commands_.size(); i++) { - if (check_command_limits(command[i], info_.command_interfaces[i]) == return_type::OK) { - commands_[i] = command[i]; - } else { - ret = return_type::COMMAND_OUT_OF_LIMITS; - break; - } - } - } else { - ret = return_type::INTERFACE_VALUE_SIZE_NOT_EQUAL; - } - return ret; + return helpers::set_internal_values(command, commands_); } return_type Joint::get_state( diff --git a/hardware_interface/test/test_component_interfaces.cpp b/hardware_interface/test/test_component_interfaces.cpp index 4ca0f7ca7e..2816f4357a 100644 --- a/hardware_interface/test/test_component_interfaces.cpp +++ b/hardware_interface/test/test_component_interfaces.cpp @@ -66,19 +66,6 @@ class DummyPositionJoint : public Joint return return_type::OK; } - return_type check_command_limits( - const double & command, - const std::string & interface) const override - { - return_type ret = return_type::ERROR; - if (!interface.compare(HW_IF_POSITION)) { - if (command >= min_position_ && command <= max_position_) { - ret = return_type::OK; - } - } - return ret; - } - private: double max_position_, min_position_; }; @@ -103,23 +90,6 @@ class DummyMultiJoint : public Joint return return_type::OK; } - return_type check_command_limits( - const double & command, - const std::string & interface) const override - { - return_type ret = return_type::ERROR; - if (!interface.compare(HW_IF_POSITION)) { - if (command >= min_position_ && command <= max_position_) { - ret = return_type::OK; - } - } else if (!interface.compare(HW_IF_VELOCITY)) { - if (command >= min_velocity_ && command <= max_velocity_) { - ret = return_type::OK; - } - } - return ret; - } - private: double max_position_, min_position_; double max_velocity_, min_velocity_; @@ -408,15 +378,14 @@ TEST_F(TestComponentInterfaces, joint_example_component_works) std::vector interfaces; std::vector input; input.push_back(2.1); - EXPECT_EQ(joint.set_command(input, interfaces), return_type::INTERFACE_VALUE_SIZE_NOT_EQUAL); + 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(20.21); - EXPECT_EQ(joint.set_command(input, interfaces), return_type::COMMAND_OUT_OF_LIMITS); - input.clear(); input.push_back(1.2); EXPECT_EQ(joint.set_command(input, interfaces), return_type::OK); @@ -433,9 +402,6 @@ TEST_F(TestComponentInterfaces, joint_example_component_works) 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::COMMAND_OUT_OF_LIMITS); - input.clear(); input.push_back(2.1); EXPECT_EQ(joint.set_command(input), return_type::OK); @@ -515,15 +481,14 @@ TEST_F(TestComponentInterfaces, multi_joint_example_component_works) std::vector interfaces; std::vector input; input.push_back(2.1); - EXPECT_EQ(joint.set_command(input, interfaces), return_type::INTERFACE_VALUE_SIZE_NOT_EQUAL); + 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(20.21); - EXPECT_EQ(joint.set_command(input, interfaces), return_type::COMMAND_OUT_OF_LIMITS); - input.clear(); input.push_back(1.02); EXPECT_EQ(joint.set_command(input, interfaces), return_type::OK); @@ -539,8 +504,6 @@ TEST_F(TestComponentInterfaces, multi_joint_example_component_works) 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.push_back(2.1); - EXPECT_EQ(joint.set_command(input), return_type::COMMAND_OUT_OF_LIMITS); input.clear(); input.push_back(1.2); input.push_back(0.4); From fbed6017a7a7b47e0afa655c6bbd8578a1af774f Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Thu, 20 Aug 2020 09:06:35 +0200 Subject: [PATCH 24/29] Use inline functions in helper header and make API-consistent return values --- hardware_interface/CMakeLists.txt | 7 -- .../helpers/component_lists_management.hpp | 80 +++++++++++++--- .../include/hardware_interface/joint.hpp | 10 +- .../include/hardware_interface/sensor.hpp | 5 +- .../helpers/component_lists_management.cpp | 95 ------------------- hardware_interface/src/joint.cpp | 8 +- hardware_interface/src/sensor.cpp | 4 +- .../test/test_component_interfaces.cpp | 10 +- 8 files changed, 87 insertions(+), 132 deletions(-) delete mode 100644 hardware_interface/src/helpers/component_lists_management.cpp diff --git a/hardware_interface/CMakeLists.txt b/hardware_interface/CMakeLists.txt index 7e72fdb596..67217e4177 100644 --- a/hardware_interface/CMakeLists.txt +++ b/hardware_interface/CMakeLists.txt @@ -41,7 +41,6 @@ target_compile_definitions(hardware_interface PRIVATE "HARDWARE_INTERFACE_BUILDI add_library( control_components SHARED - src/helpers/component_lists_management.cpp src/joint.cpp src/sensor.cpp ) @@ -50,12 +49,6 @@ target_include_directories( PUBLIC include ) -# ament_target_dependencies( -# control_components -# 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(control_components PRIVATE "HARDWARE_INTERFACE_BUILDING_DLL") diff --git a/hardware_interface/include/hardware_interface/helpers/component_lists_management.hpp b/hardware_interface/include/hardware_interface/helpers/component_lists_management.hpp index e5e2f97d28..aa39830589 100644 --- a/hardware_interface/include/hardware_interface/helpers/component_lists_management.hpp +++ b/hardware_interface/include/hardware_interface/helpers/component_lists_management.hpp @@ -12,9 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef HARDWARE_INTERFACE__HELPERS__COMPONENT_LISTS_MANAGEMENT_HPP_ -#define HARDWARE_INTERFACE__HELPERS__COMPONENT_LISTS_MANAGEMENT_HPP_ +#ifndef HARDWARE_INTERFACE__HELPERS__COMPONENTS_LISTS_MANAGEMENT_HPP_ +#define HARDWARE_INTERFACE__HELPERS__COMPONENTS_LISTS_MANAGEMENT_HPP_ +#include #include #include @@ -40,18 +41,43 @@ namespace helpers * not defined in int_interfaces; return return_type::INTERFACE_NOT_PROVIDED if queried_interfaces * list is is empty; return_type::OK otherwise. */ -return_type get_internal_values( +inline return_type get_internal_values( std::vector & values, const std::vector & queried_interfaces, - const std::vector & int_interfaces, const std::vector & int_values); + 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; + * \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. */ -void get_internal_values( - std::vector & values, const std::vector & int_values); +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 @@ -70,9 +96,28 @@ void get_internal_values( * for different interfaces. This should be changed in the future. * (see: https://github.com/ros-controls/ros2_control/issues/129) */ -return_type set_internal_values( +inline return_type set_internal_values( const std::vector & values, const std::vector & queried_interfaces, - const std::vector & int_interfaces, std::vector & int_values); + 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. @@ -82,10 +127,19 @@ return_type set_internal_values( * \return return_type::INTERFACE_VALUE_SIZE_NOT_EQUAL if the size of the arguments is not equal, * return_type::OK otherwise. */ -return_type set_internal_values( - const std::vector & values, std::vector & int_values); +inline return_type set_internal_values( + const std::vector & values, std::vector & int_values) +{ + if (values.size() == int_values.size()) { + for (uint i = 0; i < int_values.size(); i++) { + int_values[i] = values[i]; + } + } else { + return return_type::INTERFACE_VALUE_SIZE_NOT_EQUAL; + } + return return_type::OK; +} } // namespace helpers - } // namespace hardware_interface -#endif // HARDWARE_INTERFACE__HELPERS__COMPONENT_LISTS_MANAGEMENT_HPP_ +#endif // HARDWARE_INTERFACE__HELPERS__COMPONENTS_LISTS_MANAGEMENT_HPP_ diff --git a/hardware_interface/include/hardware_interface/joint.hpp b/hardware_interface/include/hardware_interface/joint.hpp index c5442c44ad..9184d906c7 100644 --- a/hardware_interface/include/hardware_interface/joint.hpp +++ b/hardware_interface/include/hardware_interface/joint.hpp @@ -87,12 +87,13 @@ class Joint /** * \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. + * 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 - void get_command(std::vector & command) const; + 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 @@ -148,12 +149,13 @@ class Joint /** * \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. + * 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 - void get_state(std::vector & state) const; + 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 diff --git a/hardware_interface/include/hardware_interface/sensor.hpp b/hardware_interface/include/hardware_interface/sensor.hpp index 13c9ba28ee..d68343ea1a 100644 --- a/hardware_interface/include/hardware_interface/sensor.hpp +++ b/hardware_interface/include/hardware_interface/sensor.hpp @@ -78,12 +78,13 @@ class Sensor /** * \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. + * 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 - void get_state(std::vector & state) const; + 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 diff --git a/hardware_interface/src/helpers/component_lists_management.cpp b/hardware_interface/src/helpers/component_lists_management.cpp deleted file mode 100644 index 6e46db2638..0000000000 --- a/hardware_interface/src/helpers/component_lists_management.cpp +++ /dev/null @@ -1,95 +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. - -#include -#include -#include - -#include "hardware_interface/helpers/component_lists_management.hpp" -#include "hardware_interface/types/hardware_interface_return_values.hpp" - -namespace hardware_interface -{ - -namespace helpers -{ - -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; -} - -void 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_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; -} - -return_type set_internal_values( - const std::vector & values, std::vector & int_values) -{ - if (values.size() == int_values.size()) { - for (uint i = 0; i < int_values.size(); i++) { - int_values[i] = values[i]; - } - } else { - return return_type::INTERFACE_VALUE_SIZE_NOT_EQUAL; - } - return return_type::OK; -} - -} // namespace helpers -} // namespace hardware_interface diff --git a/hardware_interface/src/joint.cpp b/hardware_interface/src/joint.cpp index d24eb04b03..e5d5ef35c6 100644 --- a/hardware_interface/src/joint.cpp +++ b/hardware_interface/src/joint.cpp @@ -51,9 +51,9 @@ return_type Joint::get_command( return helpers::get_internal_values(command, interfaces, info_.command_interfaces, commands_); } -void Joint::get_command(std::vector & command) const +return_type Joint::get_command(std::vector & command) const { - helpers::get_internal_values(command, commands_); + return helpers::get_internal_values(command, commands_); } return_type Joint::set_command( @@ -74,9 +74,9 @@ return_type Joint::get_state( return helpers::get_internal_values(state, interfaces, info_.state_interfaces, states_); } -void Joint::get_state(std::vector & state) const +return_type Joint::get_state(std::vector & state) const { - helpers::get_internal_values(state, states_); + return helpers::get_internal_values(state, states_); } return_type Joint::set_state( diff --git a/hardware_interface/src/sensor.cpp b/hardware_interface/src/sensor.cpp index 8b3ff76a92..6723e709ce 100644 --- a/hardware_interface/src/sensor.cpp +++ b/hardware_interface/src/sensor.cpp @@ -43,9 +43,9 @@ return_type Sensor::get_state( return helpers::get_internal_values(state, interfaces, info_.state_interfaces, states_); } -void Sensor::get_state(std::vector & state) const +return_type Sensor::get_state(std::vector & state) const { - helpers::get_internal_values(state, states_); + return helpers::get_internal_values(state, states_); } return_type Sensor::set_state( diff --git a/hardware_interface/test/test_component_interfaces.cpp b/hardware_interface/test/test_component_interfaces.cpp index 2816f4357a..c415ffb264 100644 --- a/hardware_interface/test/test_component_interfaces.cpp +++ b/hardware_interface/test/test_component_interfaces.cpp @@ -405,7 +405,7 @@ TEST_F(TestComponentInterfaces, joint_example_component_works) input.push_back(2.1); EXPECT_EQ(joint.set_command(input), return_type::OK); - joint.get_command(output); + EXPECT_EQ(joint.get_command(output), return_type::OK); ASSERT_THAT(output, SizeIs(1)); EXPECT_EQ(output[0], 2.1); @@ -440,7 +440,7 @@ TEST_F(TestComponentInterfaces, joint_example_component_works) input.push_back(2.1); EXPECT_EQ(joint.set_state(input), return_type::OK); - joint.get_state(output); + EXPECT_EQ(joint.get_state(output), return_type::OK); ASSERT_THAT(output, SizeIs(1)); EXPECT_EQ(output[0], 2.1); @@ -509,7 +509,7 @@ TEST_F(TestComponentInterfaces, multi_joint_example_component_works) input.push_back(0.4); EXPECT_EQ(joint.set_command(input), return_type::OK); - joint.get_command(output); + EXPECT_EQ(joint.get_command(output), return_type::OK); ASSERT_THAT(output, SizeIs(2)); EXPECT_EQ(output[1], 0.4); @@ -542,7 +542,7 @@ TEST_F(TestComponentInterfaces, multi_joint_example_component_works) input.push_back(1.02); EXPECT_EQ(joint.set_state(input), return_type::OK); - joint.get_state(output); + EXPECT_EQ(joint.get_state(output), return_type::OK); ASSERT_THAT(output, SizeIs(2)); EXPECT_EQ(output[0], 2.1); } @@ -592,7 +592,7 @@ TEST_F(TestComponentInterfaces, sensor_example_component_works) input = {5.23, 6.7, 2.5, 3.8, 8.9, 12.3}; EXPECT_EQ(sensor.set_state(input), return_type::OK); - sensor.get_state(output); + EXPECT_EQ(sensor.get_state(output), return_type::OK); ASSERT_THAT(output, SizeIs(6)); EXPECT_EQ(output[5], 12.3); From 62cf9abfe9cbcd18e8137cb8e3fb72deff41f1ca Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Thu, 20 Aug 2020 09:16:36 +0200 Subject: [PATCH 25/29] Correct faulty header guard --- .../helpers/component_lists_management.hpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/hardware_interface/include/hardware_interface/helpers/component_lists_management.hpp b/hardware_interface/include/hardware_interface/helpers/component_lists_management.hpp index aa39830589..b1841c539f 100644 --- a/hardware_interface/include/hardware_interface/helpers/component_lists_management.hpp +++ b/hardware_interface/include/hardware_interface/helpers/component_lists_management.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef HARDWARE_INTERFACE__HELPERS__COMPONENTS_LISTS_MANAGEMENT_HPP_ -#define HARDWARE_INTERFACE__HELPERS__COMPONENTS_LISTS_MANAGEMENT_HPP_ +#ifndef HARDWARE_INTERFACE__HELPERS__COMPONENT_LISTS_MANAGEMENT_HPP_ +#define HARDWARE_INTERFACE__HELPERS__COMPONENT_LISTS_MANAGEMENT_HPP_ #include #include @@ -142,4 +142,4 @@ inline return_type set_internal_values( } // namespace helpers } // namespace hardware_interface -#endif // HARDWARE_INTERFACE__HELPERS__COMPONENTS_LISTS_MANAGEMENT_HPP_ +#endif // HARDWARE_INTERFACE__HELPERS__COMPONENT_LISTS_MANAGEMENT_HPP_ From 567b774b3e99fd61793187966b549cb9b6f84ac8 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Thu, 20 Aug 2020 20:30:58 +0200 Subject: [PATCH 26/29] Corrected depency export and sorting of includes and namespaces in test file --- hardware_interface/CMakeLists.txt | 8 ++++---- hardware_interface/test/test_component_interfaces.cpp | 8 ++++---- 2 files changed, 8 insertions(+), 8 deletions(-) diff --git a/hardware_interface/CMakeLists.txt b/hardware_interface/CMakeLists.txt index 67217e4177..aa2d1624dd 100644 --- a/hardware_interface/CMakeLists.txt +++ b/hardware_interface/CMakeLists.txt @@ -95,10 +95,6 @@ if(BUILD_TESTING) target_link_libraries(test_component_interfaces control_components hardware_interface) endif() -ament_export_dependencies( - rclcpp - rcpputils -) ament_export_include_directories( include ) @@ -108,5 +104,9 @@ ament_export_libraries( ) ament_export_dependencies( control_msgs + rclcpp + rcpputils + tinyxml2_vendor + TinyXML2 ) ament_package() diff --git a/hardware_interface/test/test_component_interfaces.cpp b/hardware_interface/test/test_component_interfaces.cpp index c415ffb264..baf60a7005 100644 --- a/hardware_interface/test/test_component_interfaces.cpp +++ b/hardware_interface/test/test_component_interfaces.cpp @@ -22,7 +22,6 @@ #include "hardware_interface/actuator_hardware_interface.hpp" #include "hardware_interface/component_info.hpp" #include "hardware_interface/hardware_info.hpp" -#include "hardware_interface/types/hardware_interface_type_values.hpp" #include "hardware_interface/joint.hpp" #include "hardware_interface/sensor_hardware_interface.hpp" #include "hardware_interface/sensor_hardware.hpp" @@ -31,6 +30,7 @@ #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 @@ -326,7 +326,6 @@ class DummySystemHardware : public SystemHardwareInterface } // namespace hardware_interfaces_components_test } // namespace hardware_interface -using hardware_interface::return_type; using hardware_interface::ComponentInfo; using hardware_interface::HardwareInfo; using hardware_interface::ActuatorHardware; @@ -338,10 +337,11 @@ using hardware_interface::SensorHardwareInterface; using hardware_interface::SystemHardware; using hardware_interface::SystemHardwareInterface; using hardware_interface::hardware_interface_status; +using hardware_interface::return_type; -using hardware_interface::hardware_interfaces_components_test::DummyPositionJoint; -using hardware_interface::hardware_interfaces_components_test::DummyMultiJoint; 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; From 71aafc2cbeec606de296be7c8046cf7d311eeb85 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Fri, 21 Aug 2020 08:53:51 +0200 Subject: [PATCH 27/29] remove helpers namespace --- .../helpers/component_lists_management.hpp | 4 ---- hardware_interface/src/joint.cpp | 16 ++++++++-------- hardware_interface/src/sensor.cpp | 8 ++++---- 3 files changed, 12 insertions(+), 16 deletions(-) diff --git a/hardware_interface/include/hardware_interface/helpers/component_lists_management.hpp b/hardware_interface/include/hardware_interface/helpers/component_lists_management.hpp index b1841c539f..df72c09f45 100644 --- a/hardware_interface/include/hardware_interface/helpers/component_lists_management.hpp +++ b/hardware_interface/include/hardware_interface/helpers/component_lists_management.hpp @@ -25,9 +25,6 @@ namespace hardware_interface { -namespace helpers -{ - /** * \brief Get values for queried_interfaces from the int_values. int_values data structure matches * int_interfaces vector. @@ -140,6 +137,5 @@ inline return_type set_internal_values( return return_type::OK; } -} // namespace helpers } // namespace hardware_interface #endif // HARDWARE_INTERFACE__HELPERS__COMPONENT_LISTS_MANAGEMENT_HPP_ diff --git a/hardware_interface/src/joint.cpp b/hardware_interface/src/joint.cpp index e5d5ef35c6..295b3d396e 100644 --- a/hardware_interface/src/joint.cpp +++ b/hardware_interface/src/joint.cpp @@ -48,46 +48,46 @@ std::vector Joint::get_state_interfaces() const return_type Joint::get_command( std::vector & command, const std::vector & interfaces) const { - return helpers::get_internal_values(command, interfaces, info_.command_interfaces, commands_); + return get_internal_values(command, interfaces, info_.command_interfaces, commands_); } return_type Joint::get_command(std::vector & command) const { - return helpers::get_internal_values(command, commands_); + return get_internal_values(command, commands_); } return_type Joint::set_command( const std::vector & command, const std::vector & interfaces) { - return helpers::set_internal_values(command, interfaces, info_.command_interfaces, commands_); + return set_internal_values(command, interfaces, info_.command_interfaces, commands_); } return_type Joint::set_command(const std::vector & command) { - return helpers::set_internal_values(command, commands_); + return set_internal_values(command, commands_); } return_type Joint::get_state( std::vector & state, const std::vector & interfaces) const { - return helpers::get_internal_values(state, interfaces, info_.state_interfaces, states_); + return get_internal_values(state, interfaces, info_.state_interfaces, states_); } return_type Joint::get_state(std::vector & state) const { - return helpers::get_internal_values(state, states_); + return get_internal_values(state, states_); } return_type Joint::set_state( const std::vector & state, const std::vector & interfaces) { - return helpers::set_internal_values(state, interfaces, info_.state_interfaces, states_); + return set_internal_values(state, interfaces, info_.state_interfaces, states_); } return_type Joint::set_state(const std::vector & state) { - return helpers::set_internal_values(state, states_); + return set_internal_values(state, states_); } } // namespace hardware_interface diff --git a/hardware_interface/src/sensor.cpp b/hardware_interface/src/sensor.cpp index 6723e709ce..eae5fd1301 100644 --- a/hardware_interface/src/sensor.cpp +++ b/hardware_interface/src/sensor.cpp @@ -40,23 +40,23 @@ std::vector Sensor::get_state_interfaces() return_type Sensor::get_state( std::vector & state, const std::vector & interfaces) const { - return helpers::get_internal_values(state, interfaces, info_.state_interfaces, states_); + return get_internal_values(state, interfaces, info_.state_interfaces, states_); } return_type Sensor::get_state(std::vector & state) const { - return helpers::get_internal_values(state, states_); + return get_internal_values(state, states_); } return_type Sensor::set_state( const std::vector & state, const std::vector & interfaces) { - return helpers::set_internal_values(state, interfaces, info_.state_interfaces, states_); + return set_internal_values(state, interfaces, info_.state_interfaces, states_); } return_type Sensor::set_state(const std::vector & state) { - return helpers::set_internal_values(state, states_); + return set_internal_values(state, states_); } } // namespace hardware_interface From 3d5f7a50895eb2ef7f78c8c7093035857d5df5cf Mon Sep 17 00:00:00 2001 From: Karsten Knese Date: Fri, 21 Aug 2020 14:33:05 -0700 Subject: [PATCH 28/29] osx touchups Signed-off-by: Karsten Knese --- .../helpers => src}/component_lists_management.hpp | 10 ++++------ hardware_interface/src/joint.cpp | 3 ++- hardware_interface/src/robot_hardware.cpp | 9 +++++---- hardware_interface/src/sensor.cpp | 3 ++- hardware_interface/test/test_macros.cpp | 1 + 5 files changed, 14 insertions(+), 12 deletions(-) rename hardware_interface/{include/hardware_interface/helpers => src}/component_lists_management.hpp (94%) diff --git a/hardware_interface/include/hardware_interface/helpers/component_lists_management.hpp b/hardware_interface/src/component_lists_management.hpp similarity index 94% rename from hardware_interface/include/hardware_interface/helpers/component_lists_management.hpp rename to hardware_interface/src/component_lists_management.hpp index df72c09f45..740b0a30e1 100644 --- a/hardware_interface/include/hardware_interface/helpers/component_lists_management.hpp +++ b/hardware_interface/src/component_lists_management.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef HARDWARE_INTERFACE__HELPERS__COMPONENT_LISTS_MANAGEMENT_HPP_ -#define HARDWARE_INTERFACE__HELPERS__COMPONENT_LISTS_MANAGEMENT_HPP_ +#ifndef COMPONENT_LISTS_MANAGEMENT_HPP_ +#define COMPONENT_LISTS_MANAGEMENT_HPP_ #include #include @@ -128,9 +128,7 @@ inline return_type set_internal_values( const std::vector & values, std::vector & int_values) { if (values.size() == int_values.size()) { - for (uint i = 0; i < int_values.size(); i++) { - int_values[i] = values[i]; - } + int_values = values; } else { return return_type::INTERFACE_VALUE_SIZE_NOT_EQUAL; } @@ -138,4 +136,4 @@ inline return_type set_internal_values( } } // namespace hardware_interface -#endif // HARDWARE_INTERFACE__HELPERS__COMPONENT_LISTS_MANAGEMENT_HPP_ +#endif // COMPONENT_LISTS_MANAGEMENT_HPP_ diff --git a/hardware_interface/src/joint.cpp b/hardware_interface/src/joint.cpp index 295b3d396e..f744f76ff5 100644 --- a/hardware_interface/src/joint.cpp +++ b/hardware_interface/src/joint.cpp @@ -17,9 +17,10 @@ #include "hardware_interface/component_info.hpp" #include "hardware_interface/joint.hpp" -#include "hardware_interface/helpers/component_lists_management.hpp" #include "hardware_interface/types/hardware_interface_return_values.hpp" +#include "./component_lists_management.hpp" + 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.cpp b/hardware_interface/src/sensor.cpp index eae5fd1301..4022c511d0 100644 --- a/hardware_interface/src/sensor.cpp +++ b/hardware_interface/src/sensor.cpp @@ -16,10 +16,11 @@ #include #include "hardware_interface/component_info.hpp" -#include "hardware_interface/helpers/component_lists_management.hpp" #include "hardware_interface/sensor.hpp" #include "hardware_interface/types/hardware_interface_return_values.hpp" +#include "./component_lists_management.hpp" + namespace hardware_interface { 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; From 00b73ee933418807ac33f9c0ec6b7286ad7b8bea Mon Sep 17 00:00:00 2001 From: Karsten Knese Date: Fri, 21 Aug 2020 16:46:21 -0700 Subject: [PATCH 29/29] move components into components folder Signed-off-by: Karsten Knese --- hardware_interface/CMakeLists.txt | 19 +++-- .../hardware_interface/actuator_hardware.hpp | 62 +++++---------- .../actuator_hardware_interface.hpp | 12 +-- .../{ => components}/component_info.hpp | 10 ++- .../{ => components}/joint.hpp | 11 ++- .../{ => components}/sensor.hpp | 11 ++- .../hardware_interface/hardware_info.hpp | 8 +- .../hardware_interface/sensor_hardware.hpp | 39 +++------- .../sensor_hardware_interface.hpp | 9 +-- .../hardware_interface/system_hardware.hpp | 49 ++++-------- .../system_hardware_interface.hpp | 12 ++- hardware_interface/src/actuator_hardware.cpp | 67 ++++++++++++++++ .../component_lists_management.hpp | 9 ++- .../src/{ => components}/joint.cpp | 8 +- .../src/{ => components}/sensor.cpp | 8 +- hardware_interface/src/sensor_hardware.cpp | 62 +++++++++++++++ hardware_interface/src/system_hardware.cpp | 74 ++++++++++++++++++ .../test/test_component_interfaces.cpp | 78 +++++++++---------- 18 files changed, 350 insertions(+), 198 deletions(-) rename hardware_interface/include/hardware_interface/{ => components}/component_info.hpp (87%) rename hardware_interface/include/hardware_interface/{ => components}/joint.hpp (96%) rename hardware_interface/include/hardware_interface/{ => components}/sensor.hpp (94%) create mode 100644 hardware_interface/src/actuator_hardware.cpp rename hardware_interface/src/{ => components}/component_lists_management.hpp (96%) rename hardware_interface/src/{ => components}/joint.cpp (94%) rename hardware_interface/src/{ => components}/sensor.cpp (91%) create mode 100644 hardware_interface/src/sensor_hardware.cpp create mode 100644 hardware_interface/src/system_hardware.cpp diff --git a/hardware_interface/CMakeLists.txt b/hardware_interface/CMakeLists.txt index aa2d1624dd..ac7cd5a5f9 100644 --- a/hardware_interface/CMakeLists.txt +++ b/hardware_interface/CMakeLists.txt @@ -18,10 +18,13 @@ 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 @@ -39,19 +42,19 @@ ament_target_dependencies( target_compile_definitions(hardware_interface PRIVATE "HARDWARE_INTERFACE_BUILDING_DLL") add_library( - control_components + components SHARED - src/joint.cpp - src/sensor.cpp + src/components/joint.cpp + src/components/sensor.cpp ) target_include_directories( - control_components + 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(control_components PRIVATE "HARDWARE_INTERFACE_BUILDING_DLL") +target_compile_definitions(components PRIVATE "HARDWARE_INTERFACE_BUILDING_DLL") install( DIRECTORY include/ @@ -60,7 +63,7 @@ install( install( TARGETS - control_components + components hardware_interface RUNTIME DESTINATION bin ARCHIVE DESTINATION lib @@ -92,14 +95,14 @@ if(BUILD_TESTING) 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 control_components hardware_interface) + target_link_libraries(test_component_interfaces components hardware_interface) endif() ament_export_include_directories( include ) ament_export_libraries( - control_components + components hardware_interface ) ament_export_dependencies( diff --git a/hardware_interface/include/hardware_interface/actuator_hardware.hpp b/hardware_interface/include/hardware_interface/actuator_hardware.hpp index 10aa4890a2..1bcfbafe60 100644 --- a/hardware_interface/include/hardware_interface/actuator_hardware.hpp +++ b/hardware_interface/include/hardware_interface/actuator_hardware.hpp @@ -15,15 +15,9 @@ #ifndef HARDWARE_INTERFACE__ACTUATOR_HARDWARE_HPP_ #define HARDWARE_INTERFACE__ACTUATOR_HARDWARE_HPP_ -#include #include -#include -#include -#include "hardware_interface/actuator_hardware_interface.hpp" -#include "hardware_interface/component_info.hpp" #include "hardware_interface/hardware_info.hpp" -#include "hardware_interface/joint.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" @@ -31,52 +25,32 @@ namespace hardware_interface { +namespace components +{ +class Joint; +} // namespace components +class ActuatorHardwareInterface; + class ActuatorHardware final { public: ActuatorHardware() = default; - explicit ActuatorHardware(std::unique_ptr impl) - : impl_(std::move(impl)) - {} + explicit ActuatorHardware(std::unique_ptr impl); ~ActuatorHardware() = default; - HARDWARE_INTERFACE_PUBLIC - return_type configure(const HardwareInfo & actuator_info) - { - return impl_->configure(actuator_info); - } - - HARDWARE_INTERFACE_PUBLIC - return_type start() - { - return impl_->start(); - } - - HARDWARE_INTERFACE_PUBLIC - return_type stop() - { - return impl_->stop(); - } - - HARDWARE_INTERFACE_PUBLIC - hardware_interface_status get_status() - { - return impl_->get_status(); - } - - HARDWARE_INTERFACE_PUBLIC - return_type read_joint(Joint & joint) - { - return impl_->read_joint(joint); - } - - HARDWARE_INTERFACE_PUBLIC - return_type write_joint(const Joint & joint) - { - return impl_->write_joint(joint); - } + 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_; diff --git a/hardware_interface/include/hardware_interface/actuator_hardware_interface.hpp b/hardware_interface/include/hardware_interface/actuator_hardware_interface.hpp index b4e6222308..cf7ae22ee5 100644 --- a/hardware_interface/include/hardware_interface/actuator_hardware_interface.hpp +++ b/hardware_interface/include/hardware_interface/actuator_hardware_interface.hpp @@ -15,14 +15,10 @@ #ifndef HARDWARE_INTERFACE__ACTUATOR_HARDWARE_INTERFACE_HPP_ #define HARDWARE_INTERFACE__ACTUATOR_HARDWARE_INTERFACE_HPP_ -#include -#include -#include -#include +#include -#include "hardware_interface/component_info.hpp" +#include "hardware_interface/components/joint.hpp" #include "hardware_interface/hardware_info.hpp" -#include "hardware_interface/joint.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" @@ -91,7 +87,7 @@ class ActuatorHardwareInterface */ HARDWARE_INTERFACE_PUBLIC virtual - return_type read_joint(Joint & joint) const = 0; + 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. @@ -102,7 +98,7 @@ class ActuatorHardwareInterface */ HARDWARE_INTERFACE_PUBLIC virtual - return_type write_joint(const Joint & joint) = 0; + return_type write_joint(const std::shared_ptr joint) = 0; }; } // namespace hardware_interface diff --git a/hardware_interface/include/hardware_interface/component_info.hpp b/hardware_interface/include/hardware_interface/components/component_info.hpp similarity index 87% rename from hardware_interface/include/hardware_interface/component_info.hpp rename to hardware_interface/include/hardware_interface/components/component_info.hpp index d921b4f67a..0dc1b78dd2 100644 --- a/hardware_interface/include/hardware_interface/component_info.hpp +++ b/hardware_interface/include/hardware_interface/components/component_info.hpp @@ -12,9 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. - -#ifndef HARDWARE_INTERFACE__COMPONENT_INFO_HPP_ -#define HARDWARE_INTERFACE__COMPONENT_INFO_HPP_ +#ifndef HARDWARE_INTERFACE__COMPONENTS__COMPONENT_INFO_HPP_ +#define HARDWARE_INTERFACE__COMPONENTS__COMPONENT_INFO_HPP_ #include #include @@ -22,6 +21,8 @@ namespace hardware_interface { +namespace components +{ /** * \brief This structure stores information about components defined for a specific hardware @@ -57,5 +58,6 @@ struct ComponentInfo std::unordered_map parameters; }; +} // namespace components } // namespace hardware_interface -#endif // HARDWARE_INTERFACE__COMPONENT_INFO_HPP_ +#endif // HARDWARE_INTERFACE__COMPONENTS__COMPONENT_INFO_HPP_ diff --git a/hardware_interface/include/hardware_interface/joint.hpp b/hardware_interface/include/hardware_interface/components/joint.hpp similarity index 96% rename from hardware_interface/include/hardware_interface/joint.hpp rename to hardware_interface/include/hardware_interface/components/joint.hpp index 9184d906c7..64ede0a2bc 100644 --- a/hardware_interface/include/hardware_interface/joint.hpp +++ b/hardware_interface/include/hardware_interface/components/joint.hpp @@ -12,18 +12,20 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef HARDWARE_INTERFACE__JOINT_HPP_ -#define HARDWARE_INTERFACE__JOINT_HPP_ +#ifndef HARDWARE_INTERFACE__COMPONENTS__JOINT_HPP_ +#define HARDWARE_INTERFACE__COMPONENTS__JOINT_HPP_ #include #include -#include "hardware_interface/component_info.hpp" +#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. @@ -191,5 +193,6 @@ class Joint std::vector states_; }; +} // namespace components } // namespace hardware_interface -#endif // HARDWARE_INTERFACE__JOINT_HPP_ +#endif // HARDWARE_INTERFACE__COMPONENTS__JOINT_HPP_ diff --git a/hardware_interface/include/hardware_interface/sensor.hpp b/hardware_interface/include/hardware_interface/components/sensor.hpp similarity index 94% rename from hardware_interface/include/hardware_interface/sensor.hpp rename to hardware_interface/include/hardware_interface/components/sensor.hpp index d68343ea1a..a99c85eec9 100644 --- a/hardware_interface/include/hardware_interface/sensor.hpp +++ b/hardware_interface/include/hardware_interface/components/sensor.hpp @@ -12,18 +12,20 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef HARDWARE_INTERFACE__SENSOR_HPP_ -#define HARDWARE_INTERFACE__SENSOR_HPP_ +#ifndef HARDWARE_INTERFACE__COMPONENTS__SENSOR_HPP_ +#define HARDWARE_INTERFACE__COMPONENTS__SENSOR_HPP_ #include #include -#include "hardware_interface/component_info.hpp" +#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 @@ -119,5 +121,6 @@ class Sensor std::vector states_; }; +} // namespace components } // namespace hardware_interface -#endif // HARDWARE_INTERFACE__SENSOR_HPP_ +#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 index 39ba04ed49..a8cf6901af 100644 --- a/hardware_interface/include/hardware_interface/hardware_info.hpp +++ b/hardware_interface/include/hardware_interface/hardware_info.hpp @@ -20,6 +20,8 @@ #include #include +#include "hardware_interface/components/component_info.hpp" + namespace hardware_interface { @@ -48,17 +50,17 @@ struct HardwareInfo * \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; + 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; + 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; + std::unordered_map transmissions; }; } // namespace hardware_interface diff --git a/hardware_interface/include/hardware_interface/sensor_hardware.hpp b/hardware_interface/include/hardware_interface/sensor_hardware.hpp index 059ecc6d2d..fde489abc2 100644 --- a/hardware_interface/include/hardware_interface/sensor_hardware.hpp +++ b/hardware_interface/include/hardware_interface/sensor_hardware.hpp @@ -12,7 +12,6 @@ // 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_ @@ -21,10 +20,7 @@ #include #include -#include "hardware_interface/component_info.hpp" #include "hardware_interface/hardware_info.hpp" -#include "hardware_interface/sensor.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" @@ -32,46 +28,35 @@ namespace hardware_interface { +namespace components +{ +class Sensor; +} // namespace components +class SensorHardwareInterface; + class SensorHardware final { public: SensorHardware() = default; - explicit SensorHardware(std::unique_ptr impl) - : impl_(std::move(impl)) - {} + explicit SensorHardware(std::unique_ptr impl); ~SensorHardware() = default; HARDWARE_INTERFACE_PUBLIC - return_type configure(const HardwareInfo & sensor_info) - { - return impl_->configure(sensor_info); - } + return_type configure(const HardwareInfo & sensor_info); HARDWARE_INTERFACE_PUBLIC - return_type start() - { - return impl_->start(); - } + return_type start(); HARDWARE_INTERFACE_PUBLIC - return_type stop() - { - return impl_->stop(); - } + return_type stop(); HARDWARE_INTERFACE_PUBLIC - hardware_interface_status get_status() - { - return impl_->get_status(); - } + hardware_interface_status get_status() const; HARDWARE_INTERFACE_PUBLIC - return_type read_sensors(std::vector> & sensors) - { - return impl_->read_sensors(sensors); - } + return_type read_sensor(std::shared_ptr sensor); private: std::unique_ptr impl_; diff --git a/hardware_interface/include/hardware_interface/sensor_hardware_interface.hpp b/hardware_interface/include/hardware_interface/sensor_hardware_interface.hpp index 1d8bb4bdbb..7a4cd7abc8 100644 --- a/hardware_interface/include/hardware_interface/sensor_hardware_interface.hpp +++ b/hardware_interface/include/hardware_interface/sensor_hardware_interface.hpp @@ -16,12 +16,9 @@ #define HARDWARE_INTERFACE__SENSOR_HARDWARE_INTERFACE_HPP_ #include -#include -#include -#include "hardware_interface/component_info.hpp" +#include "hardware_interface/components/sensor.hpp" #include "hardware_interface/hardware_info.hpp" -#include "hardware_interface/sensor.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" @@ -85,12 +82,12 @@ class SensorHardwareInterface * \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 list of sensors where data from the hardware are stored. + * \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_sensors(const std::vector> & sensors) const = 0; + return_type read_sensor(std::shared_ptr sensor) const = 0; }; } // namespace hardware_interface diff --git a/hardware_interface/include/hardware_interface/system_hardware.hpp b/hardware_interface/include/hardware_interface/system_hardware.hpp index 52e196f6d3..5a64148c29 100644 --- a/hardware_interface/include/hardware_interface/system_hardware.hpp +++ b/hardware_interface/include/hardware_interface/system_hardware.hpp @@ -20,9 +20,7 @@ #include #include -#include "hardware_interface/component_info.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" @@ -30,56 +28,41 @@ namespace hardware_interface { +namespace components +{ +class Joint; +class Sensor; +} // namespace components +class SystemHardwareInterface; + class SystemHardware final { public: - explicit SystemHardware(std::unique_ptr impl) - : impl_(std::move(impl)) - {} + HARDWARE_INTERFACE_PUBLIC + explicit SystemHardware(std::unique_ptr impl); virtual ~SystemHardware() = default; HARDWARE_INTERFACE_PUBLIC - return_type configure(const HardwareInfo & system_info) - { - return impl_->configure(system_info); - } + return_type configure(const HardwareInfo & system_info); HARDWARE_INTERFACE_PUBLIC - return_type start() - { - return impl_->start(); - } + return_type start(); HARDWARE_INTERFACE_PUBLIC - return_type stop() - { - return impl_->stop(); - } + return_type stop(); HARDWARE_INTERFACE_PUBLIC - hardware_interface_status get_status() const - { - return impl_->get_status(); - } + hardware_interface_status get_status() const; HARDWARE_INTERFACE_PUBLIC - return_type read_sensors(std::vector> & sensors) - { - return impl_->read_sensors(sensors); - } + return_type read_sensors(std::vector> & sensors); HARDWARE_INTERFACE_PUBLIC - return_type read_joints(std::vector> & joints) - { - return impl_->read_joints(joints); - } + return_type read_joints(std::vector> & joints); HARDWARE_INTERFACE_PUBLIC - return_type write_joints(const std::vector> & joints) - { - return impl_->write_joints(joints); - } + return_type write_joints(const std::vector> & joints); private: std::unique_ptr impl_; diff --git a/hardware_interface/include/hardware_interface/system_hardware_interface.hpp b/hardware_interface/include/hardware_interface/system_hardware_interface.hpp index 41c0456e18..722c75c435 100644 --- a/hardware_interface/include/hardware_interface/system_hardware_interface.hpp +++ b/hardware_interface/include/hardware_interface/system_hardware_interface.hpp @@ -16,13 +16,11 @@ #define HARDWARE_INTERFACE__SYSTEM_HARDWARE_INTERFACE_HPP_ #include -#include #include -#include "hardware_interface/component_info.hpp" +#include "hardware_interface/components/joint.hpp" +#include "hardware_interface/components/sensor.hpp" #include "hardware_interface/hardware_info.hpp" -#include "hardware_interface/joint.hpp" -#include "hardware_interface/sensor.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" @@ -93,7 +91,7 @@ class SystemHardwareInterface */ HARDWARE_INTERFACE_PUBLIC virtual - return_type read_sensors(std::vector> & sensors) const = 0; + 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. @@ -104,7 +102,7 @@ class SystemHardwareInterface */ HARDWARE_INTERFACE_PUBLIC virtual - return_type read_joints(std::vector> & joints) const = 0; + 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. @@ -116,7 +114,7 @@ class SystemHardwareInterface HARDWARE_INTERFACE_PUBLIC return_type virtual - write_joints(const std::vector> & joints) = 0; + write_joints(const std::vector> & joints) = 0; }; } // namespace hardware_interface 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/component_lists_management.hpp b/hardware_interface/src/components/component_lists_management.hpp similarity index 96% rename from hardware_interface/src/component_lists_management.hpp rename to hardware_interface/src/components/component_lists_management.hpp index 740b0a30e1..deb949a980 100644 --- a/hardware_interface/src/component_lists_management.hpp +++ b/hardware_interface/src/components/component_lists_management.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef COMPONENT_LISTS_MANAGEMENT_HPP_ -#define COMPONENT_LISTS_MANAGEMENT_HPP_ +#ifndef COMPONENTS__COMPONENT_LISTS_MANAGEMENT_HPP_ +#define COMPONENTS__COMPONENT_LISTS_MANAGEMENT_HPP_ #include #include @@ -24,6 +24,8 @@ namespace hardware_interface { +namespace components +{ /** * \brief Get values for queried_interfaces from the int_values. int_values data structure matches @@ -135,5 +137,6 @@ inline return_type set_internal_values( return return_type::OK; } +} // namespace components } // namespace hardware_interface -#endif // COMPONENT_LISTS_MANAGEMENT_HPP_ +#endif // COMPONENTS__COMPONENT_LISTS_MANAGEMENT_HPP_ diff --git a/hardware_interface/src/joint.cpp b/hardware_interface/src/components/joint.cpp similarity index 94% rename from hardware_interface/src/joint.cpp rename to hardware_interface/src/components/joint.cpp index f744f76ff5..badd4d3e9a 100644 --- a/hardware_interface/src/joint.cpp +++ b/hardware_interface/src/components/joint.cpp @@ -15,14 +15,17 @@ #include #include -#include "hardware_interface/component_info.hpp" -#include "hardware_interface/joint.hpp" +#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) { @@ -91,4 +94,5 @@ 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/sensor.cpp b/hardware_interface/src/components/sensor.cpp similarity index 91% rename from hardware_interface/src/sensor.cpp rename to hardware_interface/src/components/sensor.cpp index 4022c511d0..0229e8b14f 100644 --- a/hardware_interface/src/sensor.cpp +++ b/hardware_interface/src/components/sensor.cpp @@ -15,14 +15,17 @@ #include #include -#include "hardware_interface/component_info.hpp" -#include "hardware_interface/sensor.hpp" +#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) { @@ -60,4 +63,5 @@ 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/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 index baf60a7005..a1e694a053 100644 --- a/hardware_interface/test/test_component_interfaces.cpp +++ b/hardware_interface/test/test_component_interfaces.cpp @@ -20,12 +20,12 @@ #include "hardware_interface/actuator_hardware.hpp" #include "hardware_interface/actuator_hardware_interface.hpp" -#include "hardware_interface/component_info.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/joint.hpp" #include "hardware_interface/sensor_hardware_interface.hpp" #include "hardware_interface/sensor_hardware.hpp" -#include "hardware_interface/sensor.hpp" #include "hardware_interface/system_hardware_interface.hpp" #include "hardware_interface/system_hardware.hpp" #include "hardware_interface/types/hardware_interface_return_values.hpp" @@ -40,10 +40,10 @@ namespace hardware_interface namespace hardware_interfaces_components_test { -class DummyPositionJoint : public Joint +class DummyPositionJoint : public components::Joint { public: - return_type configure(const ComponentInfo & joint_info) + return_type configure(const components::ComponentInfo & joint_info) { if (Joint::configure(joint_info) != return_type::OK) { return return_type::ERROR; @@ -70,10 +70,10 @@ class DummyPositionJoint : public Joint double max_position_, min_position_; }; -class DummyMultiJoint : public Joint +class DummyMultiJoint : public components::Joint { public: - return_type configure(const ComponentInfo & joint_info) + return_type configure(const components::ComponentInfo & joint_info) { if (Joint::configure(joint_info) != return_type::OK) { return return_type::ERROR; @@ -95,10 +95,10 @@ class DummyMultiJoint : public Joint double max_velocity_, min_velocity_; }; -class DummyForceTorqueSensor : public Sensor +class DummyForceTorqueSensor : public components::Sensor { public: - return_type configure(const ComponentInfo & sensor_info) + return_type configure(const components::ComponentInfo & sensor_info) { if (Sensor::configure(sensor_info) != return_type::OK) { return return_type::ERROR; @@ -158,16 +158,16 @@ class DummyActuatorHardware : public ActuatorHardwareInterface return status_; } - return_type read_joint(Joint & joint) const override + return_type read_joint(std::shared_ptr joint) const override { - std::vector interfaces = joint.get_state_interfaces(); - return joint.set_state(hw_values_, interfaces); + std::vector interfaces = joint->get_state_interfaces(); + return joint->set_state(hw_values_, interfaces); } - return_type write_joint(const Joint & joint) override + return_type write_joint(const std::shared_ptr joint) override { - std::vector interfaces = joint.get_command_interfaces(); - return joint.get_command(hw_values_, interfaces); + std::vector interfaces = joint->get_command_interfaces(); + return joint->get_command(hw_values_, interfaces); } private: @@ -214,16 +214,9 @@ class DummySensorHardware : public SensorHardwareInterface return status_; } - return_type read_sensors(const std::vector> & sensors) const override + return_type read_sensor(std::shared_ptr sensor) 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 sensor->set_state(ft_hw_values_); } private: @@ -272,7 +265,8 @@ class DummySystemHardware : public SystemHardwareInterface return status_; } - return_type read_sensors(std::vector> & sensors) const override + return_type read_sensors(std::vector> & sensors) const + override { return_type ret = return_type::OK; for (const auto & sensor : sensors) { @@ -284,7 +278,7 @@ class DummySystemHardware : public SystemHardwareInterface return ret; } - return_type read_joints(std::vector> & joints) const override + return_type read_joints(std::vector> & joints) const override { return_type ret = return_type::OK; std::vector interfaces; @@ -301,7 +295,7 @@ class DummySystemHardware : public SystemHardwareInterface return ret; } - return_type write_joints(const std::vector> & joints) override + return_type write_joints(const std::vector> & joints) override { return_type ret = return_type::OK; for (const auto & joint : joints) { @@ -326,18 +320,18 @@ class DummySystemHardware : public SystemHardwareInterface } // namespace hardware_interfaces_components_test } // namespace hardware_interface -using hardware_interface::ComponentInfo; -using hardware_interface::HardwareInfo; +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::Joint; -using hardware_interface::Sensor; +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_interface_status; -using hardware_interface::return_type; using hardware_interface::hardware_interfaces_components_test::DummyForceTorqueSensor; using hardware_interface::hardware_interfaces_components_test::DummyMultiJoint; @@ -603,23 +597,23 @@ TEST_F(TestComponentInterfaces, sensor_example_component_works) TEST_F(TestComponentInterfaces, actuator_hardware_interface_works) { ActuatorHardware actuator_hw(std::make_unique()); - DummyPositionJoint joint; + 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(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 interfaces = joint->get_state_interfaces(); std::vector output; - EXPECT_EQ(joint.get_state(output, interfaces), return_type::OK); + 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); @@ -631,7 +625,7 @@ TEST_F(TestComponentInterfaces, actuator_hardware_interface_works) TEST_F(TestComponentInterfaces, sensor_interface_with_hardware_works) { SensorHardware sensor_hw(std::make_unique()); - std::shared_ptr sensor(std::make_shared()); + auto sensor = std::make_shared(); HardwareInfo sensor_hw_info; sensor_hw_info.name = "DummySensor"; @@ -643,9 +637,7 @@ TEST_F(TestComponentInterfaces, sensor_interface_with_hardware_works) 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); - std::vector> sensors; - sensors.push_back(sensor); - EXPECT_EQ(sensor_hw.read_sensors(sensors), return_type::OK); + 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); @@ -660,8 +652,8 @@ TEST_F(TestComponentInterfaces, sensor_interface_with_hardware_works) TEST_F(TestComponentInterfaces, system_interface_with_hardware_works) { SystemHardware system(std::make_unique()); - std::shared_ptr joint1(std::make_shared()); - std::shared_ptr joint2(std::make_shared()); + auto joint1 = std::make_shared(); + auto joint2 = std::make_shared(); std::vector> joints; joints.push_back(joint1); joints.push_back(joint2);