diff --git a/hardware_interface/src/components/component_lists_management.hpp b/hardware_interface/include/hardware_interface/components/component_lists_management.hpp similarity index 57% rename from hardware_interface/src/components/component_lists_management.hpp rename to hardware_interface/include/hardware_interface/components/component_lists_management.hpp index deb949a980..49809bb403 100644 --- a/hardware_interface/src/components/component_lists_management.hpp +++ b/hardware_interface/include/hardware_interface/components/component_lists_management.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef COMPONENTS__COMPONENT_LISTS_MANAGEMENT_HPP_ -#define COMPONENTS__COMPONENT_LISTS_MANAGEMENT_HPP_ +#ifndef HARDWARE_INTERFACE__COMPONENTS__COMPONENT_LISTS_MANAGEMENT_HPP_ +#define HARDWARE_INTERFACE__COMPONENTS__COMPONENT_LISTS_MANAGEMENT_HPP_ #include #include @@ -118,6 +118,58 @@ inline return_type set_internal_values( return return_type::OK; } +/** + * \brief set values for queried_interfaces to the int_values considering the value limits. + * int_values, lower_limits and upper_limits 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. + * \param lower_limits list of lower limits. + * \param upper_limits list of upper limits. + * \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::VALUE_OUT_OF_LIMITS if a + * value is not in the limits; return_type::INTERFACE_NOT_FOUND if + * one of queried_interfaces is not defined in int_interfaces; return_type::OK otherwise. + * + * \todo The error handling in this function could lead to incosistant command or state variables + * for different interfaces. This should be changed in the future. + * (see: https://github.com/ros-controls/ros2_control/issues/129) + */ +inline return_type set_internal_values_with_limits( + const std::vector & values, const std::vector & queried_interfaces, + const std::vector & int_interfaces, std::vector & int_values, + const std::vector & lower_limits, const std::vector & upper_limits) +{ + 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()) { + if (values[std::distance(queried_interfaces.begin(), q_it)] >= + lower_limits[std::distance(int_interfaces.begin(), it)] && + values[std::distance(queried_interfaces.begin(), q_it)] <= + upper_limits[std::distance(int_interfaces.begin(), it)]) + { + int_values[std::distance(int_interfaces.begin(), it)] = + values[std::distance(queried_interfaces.begin(), q_it)]; + } else { + return return_type::VALUE_OUT_OF_LIMITS; + } + } else { + return return_type::INTERFACE_NOT_FOUND; + } + } + return return_type::OK; +} + /** * \brief set all values to compoenents internal values. * @@ -137,6 +189,35 @@ inline return_type set_internal_values( return return_type::OK; } +/** + * \brief set all values to compoenents internal values considering limits. + * int_values, lower_limits and upper_limits have the same data structure. + * + * \param values values to set. + * \param int_values internal values of a component. + * \param lower_limits list of lower limits. + * \param upper_limits list of upper limits. + * \return return_type::INTERFACE_VALUE_SIZE_NOT_EQUAL if the size of the arguments is not equal; + * return_type::VALUE_OUT_OF_LIMITS if a value is not in the limits; return_type::OK otherwise. + */ +inline return_type set_internal_values_with_limits( + const std::vector & values, std::vector & int_values, + const std::vector & lower_limits, const std::vector & upper_limits) +{ + if (values.size() == int_values.size()) { + for (uint i = 0; i < int_values.size(); i++) { + if (values[i] >= lower_limits[i] && values[i] <= upper_limits[i]) { + int_values[i] = values[i]; + } else { + return return_type::VALUE_OUT_OF_LIMITS; + } + } + } else { + return return_type::INTERFACE_VALUE_SIZE_NOT_EQUAL; + } + return return_type::OK; +} + } // namespace components } // namespace hardware_interface -#endif // COMPONENTS__COMPONENT_LISTS_MANAGEMENT_HPP_ +#endif // HARDWARE_INTERFACE__COMPONENTS__COMPONENT_LISTS_MANAGEMENT_HPP_ diff --git a/hardware_interface/include/hardware_interface/types/hardware_interface_return_values.hpp b/hardware_interface/include/hardware_interface/types/hardware_interface_return_values.hpp index d09fea6e1a..6d3e6a12af 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 @@ -36,7 +36,12 @@ enum class return_type : std::uint8_t INTERFACE_VALUE_SIZE_NOT_EQUAL = 31, INTERFACE_NOT_PROVIDED = 32, - COMMAND_OUT_OF_LIMITS = 40, + VALUE_OUT_OF_LIMITS = 40, + + COMPONENT_TOO_MANY_INTERFACES = 51, + COMPONENT_WRONG_INTERFACE = 52, + COMPONENT_ONLY_STATE_DEFINED = 53, + COMPONENT_MISSING_PARAMETER = 54, }; using hardware_interface_ret_t = return_type; diff --git a/hardware_interface/include/hardware_interface/types/hardware_interface_type_values.hpp b/hardware_interface/include/hardware_interface/types/hardware_interface_type_values.hpp index 7292f28229..09575f82fe 100644 --- a/hardware_interface/include/hardware_interface/types/hardware_interface_type_values.hpp +++ b/hardware_interface/include/hardware_interface/types/hardware_interface_type_values.hpp @@ -17,8 +17,17 @@ namespace hardware_interface { +/** + * Constant defining position interface + */ constexpr const auto HW_IF_POSITION = "position"; +/** + * Constant defining velocity interface + */ constexpr const auto HW_IF_VELOCITY = "velocity"; +/** + * Constant defining effort interface + */ constexpr const auto HW_IF_EFFORT = "effort"; } // namespace hardware_interface diff --git a/hardware_interface/src/components/joint.cpp b/hardware_interface/src/components/joint.cpp index 8aaa0c661d..c6a2754dde 100644 --- a/hardware_interface/src/components/joint.cpp +++ b/hardware_interface/src/components/joint.cpp @@ -17,10 +17,9 @@ #include "hardware_interface/components/joint.hpp" #include "hardware_interface/components/component_info.hpp" +#include "hardware_interface/components/component_lists_management.hpp" #include "hardware_interface/types/hardware_interface_return_values.hpp" -#include "./component_lists_management.hpp" - namespace hardware_interface { namespace components diff --git a/hardware_interface/src/components/sensor.cpp b/hardware_interface/src/components/sensor.cpp index 397bee648d..7519847cd4 100644 --- a/hardware_interface/src/components/sensor.cpp +++ b/hardware_interface/src/components/sensor.cpp @@ -17,10 +17,9 @@ #include "hardware_interface/components/sensor.hpp" #include "hardware_interface/components/component_info.hpp" +#include "hardware_interface/components/component_lists_management.hpp" #include "hardware_interface/types/hardware_interface_return_values.hpp" -#include "./component_lists_management.hpp" - namespace hardware_interface { namespace components diff --git a/hardware_interface/test/test_component_interfaces.cpp b/hardware_interface/test/test_component_interfaces.cpp index 004b9de8a1..114ac6e67b 100644 --- a/hardware_interface/test/test_component_interfaces.cpp +++ b/hardware_interface/test/test_component_interfaces.cpp @@ -43,7 +43,7 @@ namespace hardware_interfaces_components_test class DummyPositionJoint : public components::Joint { public: - return_type configure(const components::ComponentInfo & joint_info) + return_type configure(const components::ComponentInfo & joint_info) override { if (Joint::configure(joint_info) != return_type::OK) { return return_type::ERROR; @@ -73,7 +73,7 @@ class DummyPositionJoint : public components::Joint class DummyMultiJoint : public components::Joint { public: - return_type configure(const components::ComponentInfo & joint_info) + return_type configure(const components::ComponentInfo & joint_info) override { if (Joint::configure(joint_info) != return_type::OK) { return return_type::ERROR; @@ -98,7 +98,7 @@ class DummyMultiJoint : public components::Joint class DummyForceTorqueSensor : public components::Sensor { public: - return_type configure(const components::ComponentInfo & sensor_info) + return_type configure(const components::ComponentInfo & sensor_info) override { if (Sensor::configure(sensor_info) != return_type::OK) { return return_type::ERROR; diff --git a/ros2_control/package.xml b/ros2_control/package.xml index e4673a0bbe..bb4b9953e7 100644 --- a/ros2_control/package.xml +++ b/ros2_control/package.xml @@ -14,6 +14,7 @@ controller_interface controller_manager hardware_interface + ros2_control_components transmission_interface test_robot_hardware diff --git a/ros2_control_components/CMakeLists.txt b/ros2_control_components/CMakeLists.txt new file mode 100644 index 0000000000..d348851560 --- /dev/null +++ b/ros2_control_components/CMakeLists.txt @@ -0,0 +1,57 @@ +cmake_minimum_required(VERSION 3.5) +project(ros2_control_components) + +# Default to C++14 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +find_package(hardware_interface REQUIRED) +find_package(pluginlib REQUIRED) + +add_library( + ros2_control_components + SHARED + # joints + src/position_joint.cpp + # sensors +) +ament_target_dependencies( + ros2_control_components + hardware_interface + pluginlib +) +# prevent pluginlib from using boost +target_compile_definitions(ros2_control_components PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS") + +pluginlib_export_plugin_description_file( + hardware_interface ros2_control_components_plugins.xml) + +install(TARGETS + ros2_control_components + DESTINATION lib +) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() + + find_package(ament_cmake_gmock REQUIRED) + + ament_add_gmock(test_position_joint test/test_position_joint.cpp) + ament_target_dependencies(test_position_joint hardware_interface pluginlib) + # prevent pluginlib from using boost + target_compile_definitions(test_position_joint PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS") +endif() + +ament_export_libraries( + ros2_control_components +) +ament_export_dependencies( + control_components + hardware_interface + pluginlib +) +ament_package() diff --git a/ros2_control_components/package.xml b/ros2_control_components/package.xml new file mode 100644 index 0000000000..031f3db2bf --- /dev/null +++ b/ros2_control_components/package.xml @@ -0,0 +1,20 @@ + + + + ros2_control_components + 0.0.2 + The package implements control components, i.e., joints and sensors, the logical building blocks of ros2_control system. + + Denis Štogl + Apache License 2.0 + + hardware_interface + pluginlib + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/ros2_control_components/ros2_control_components_plugins.xml b/ros2_control_components/ros2_control_components_plugins.xml new file mode 100644 index 0000000000..f4e2fb4dbe --- /dev/null +++ b/ros2_control_components/ros2_control_components_plugins.xml @@ -0,0 +1,7 @@ + + + + The position joint provides functionality of position-only joint with minimum and maximum limits. + + + diff --git a/ros2_control_components/src/position_joint.cpp b/ros2_control_components/src/position_joint.cpp new file mode 100644 index 0000000000..8f12a5c970 --- /dev/null +++ b/ros2_control_components/src/position_joint.cpp @@ -0,0 +1,106 @@ +// Copyright 2020 ros2_control Development Team +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include + +#include "hardware_interface/components/joint.hpp" +#include "hardware_interface/components/component_lists_management.hpp" +#include "hardware_interface/types/hardware_interface_return_values.hpp" +#include "hardware_interface/types/hardware_interface_type_values.hpp" + +using hardware_interface::components::ComponentInfo; +using hardware_interface::components::Joint; +using hardware_interface::return_type; + +namespace ros2_control_components +{ + +class PositionJoint : public Joint +{ +public: + return_type configure(const ComponentInfo & joint_info) override + { + if (Joint::configure(joint_info) != return_type::OK) { + return return_type::ERROR; + } + + // has to provide exactly one command interface if defined + if (info_.command_interfaces.size() > 1 || info_.state_interfaces.size() > 1) { + return return_type::COMPONENT_TOO_MANY_INTERFACES; + } + + if (info_.command_interfaces.size() == 1 && + info_.command_interfaces[0].compare(hardware_interface::HW_IF_POSITION)) + { + return return_type::COMPONENT_WRONG_INTERFACE; + } + + if (info_.state_interfaces.size() == 1 && + info_.state_interfaces[0].compare(hardware_interface::HW_IF_POSITION)) + { + return return_type::COMPONENT_WRONG_INTERFACE; + } + + // set default values is not interface defined in URDF. + // Set state interface to default only if command interface is also not defined, otherwise + // return error code. + if (info_.command_interfaces.size() == 0) { + info_.command_interfaces = {hardware_interface::HW_IF_POSITION}; + commands_.resize(1); + if (info_.state_interfaces.size() == 0) { + info_.state_interfaces = {hardware_interface::HW_IF_POSITION}; + states_.resize(1); + } else { + return return_type::COMPONENT_ONLY_STATE_DEFINED; + } + } + + if (info_.parameters.find("min") == info_.parameters.end()) { + return return_type::COMPONENT_MISSING_PARAMETER; + } + if (info_.parameters.find("max") == info_.parameters.end()) { + return return_type::COMPONENT_MISSING_PARAMETER; + } + lower_limits_.resize(1); + lower_limits_[0] = stod(info_.parameters["min"]); + upper_limits_.resize(1); + upper_limits_[0] = stod(info_.parameters["max"]); + + return return_type::OK; + } + + return_type set_command( + const std::vector & command, + const std::vector & interfaces) override + { + return hardware_interface::components::set_internal_values_with_limits( + command, interfaces, info_.command_interfaces, commands_, lower_limits_, upper_limits_); + } + + return_type set_command(const std::vector & command) override + { + return hardware_interface::components::set_internal_values_with_limits( + command, commands_, lower_limits_, upper_limits_); + } + +private: + std::vector lower_limits_, upper_limits_; +}; + +} // namespace ros2_control_components + +#include "pluginlib/class_list_macros.hpp" +PLUGINLIB_EXPORT_CLASS( + ros2_control_components::PositionJoint, hardware_interface::components::Joint) diff --git a/ros2_control_components/test/test_position_joint.cpp b/ros2_control_components/test/test_position_joint.cpp new file mode 100644 index 0000000000..7c70f907ea --- /dev/null +++ b/ros2_control_components/test/test_position_joint.cpp @@ -0,0 +1,175 @@ +// Copyright 2020 ros2_control development team +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include +#include +#include + +#include "hardware_interface/components/component_info.hpp" +#include "hardware_interface/components/joint.hpp" +#include "hardware_interface/types/hardware_interface_return_values.hpp" +#include "hardware_interface/types/hardware_interface_status_values.hpp" +#include "hardware_interface/types/hardware_interface_type_values.hpp" +#include "pluginlib/class_loader.hpp" + +using namespace ::testing; // NOLINT + +using hardware_interface::components::ComponentInfo; +using hardware_interface::components::Joint; +using hardware_interface::hardware_interface_status; +using hardware_interface::return_type; + +TEST(PositionJointTest, position_joint_ok_test) +{ + ComponentInfo position_joint_info; + position_joint_info.name = "joint1"; + position_joint_info.parameters["min"] = "-3.14"; + position_joint_info.parameters["max"] = "3.14"; + + pluginlib::ClassLoader joint_component_loader( + "hardware_interface", "hardware_interface::components::Joint"); + + std::shared_ptr joint = + joint_component_loader.createSharedInstance("ros2_control_components/PositionJoint"); + + EXPECT_EQ(joint->configure(position_joint_info), return_type::OK); + ASSERT_THAT(joint->get_command_interfaces(), SizeIs(1)); + EXPECT_EQ(joint->get_command_interfaces()[0], hardware_interface::HW_IF_POSITION); + ASSERT_THAT(joint->get_state_interfaces(), SizeIs(1)); + EXPECT_EQ(joint->get_state_interfaces()[0], hardware_interface::HW_IF_POSITION); + + // Command setters - other commands are inherited from components::Joint + std::vector interfaces; + std::vector input; + + // good value + input.push_back(2.1); + interfaces.push_back(hardware_interface::HW_IF_POSITION); + EXPECT_EQ(joint->set_command(input), return_type::OK); + EXPECT_EQ(joint->set_command(input, interfaces), return_type::OK); + + // above max value + input[0] = 3.5; + EXPECT_EQ(joint->set_command(input), return_type::VALUE_OUT_OF_LIMITS); + EXPECT_EQ(joint->set_command(input, interfaces), return_type::VALUE_OUT_OF_LIMITS); + + // under min value + input[0] = -3.5; + EXPECT_EQ(joint->set_command(input), return_type::VALUE_OUT_OF_LIMITS); + EXPECT_EQ(joint->set_command(input, interfaces), return_type::VALUE_OUT_OF_LIMITS); +} + +TEST(PositionJointTest, position_joint_only_command_inteface_test) +{ + ComponentInfo position_joint_info; + position_joint_info.name = "joint1"; + position_joint_info.parameters["min"] = "-3.14"; + position_joint_info.parameters["max"] = "3.14"; + position_joint_info.command_interfaces.push_back(hardware_interface::HW_IF_POSITION); + + pluginlib::ClassLoader joint_component_loader( + "hardware_interface", "hardware_interface::components::Joint"); + + std::shared_ptr joint = + joint_component_loader.createSharedInstance("ros2_control_components/PositionJoint"); + + EXPECT_EQ(joint->configure(position_joint_info), return_type::OK); + ASSERT_THAT(joint->get_command_interfaces(), SizeIs(1)); + EXPECT_EQ(joint->get_command_interfaces()[0], hardware_interface::HW_IF_POSITION); + ASSERT_THAT(joint->get_state_interfaces(), SizeIs(0)); +} + +TEST(PositionJointTest, position_joint_only_state_error_test) +{ + ComponentInfo position_joint_info; + position_joint_info.name = "joint1"; + position_joint_info.parameters["min"] = "-3.14"; + position_joint_info.parameters["max"] = "3.14"; + position_joint_info.state_interfaces.push_back(hardware_interface::HW_IF_POSITION); + + pluginlib::ClassLoader joint_component_loader( + "hardware_interface", "hardware_interface::components::Joint"); + + std::shared_ptr joint = + joint_component_loader.createSharedInstance("ros2_control_components/PositionJoint"); + + EXPECT_EQ(joint->configure(position_joint_info), return_type::COMPONENT_ONLY_STATE_DEFINED); +} + +TEST(PositionJointTest, position_joint_missing_params_test) +{ + ComponentInfo position_joint_info; + position_joint_info.name = "joint1"; + + pluginlib::ClassLoader joint_component_loader( + "hardware_interface", "hardware_interface::components::Joint"); + + std::shared_ptr joint = + joint_component_loader.createSharedInstance("ros2_control_components/PositionJoint"); + + EXPECT_EQ(joint->configure(position_joint_info), return_type::COMPONENT_MISSING_PARAMETER); + + position_joint_info.parameters["min"] = "-3.14"; + EXPECT_EQ(joint->configure(position_joint_info), return_type::COMPONENT_MISSING_PARAMETER); + + position_joint_info.parameters["max"] = "3.14"; + EXPECT_EQ(joint->configure(position_joint_info), return_type::OK); +} + +TEST(PositionJointTest, position_joint_too_many_interfaces_test) +{ + pluginlib::ClassLoader joint_component_loader( + "hardware_interface", "hardware_interface::components::Joint"); + + std::shared_ptr joint = + joint_component_loader.createSharedInstance("ros2_control_components/PositionJoint"); + + ComponentInfo position_joint_info; + position_joint_info.name = "joint1"; + position_joint_info.command_interfaces.push_back(hardware_interface::HW_IF_POSITION); + position_joint_info.command_interfaces.push_back(hardware_interface::HW_IF_POSITION); + + EXPECT_EQ(joint->configure(position_joint_info), return_type::COMPONENT_TOO_MANY_INTERFACES); + + position_joint_info.command_interfaces.clear(); + position_joint_info.state_interfaces.push_back(hardware_interface::HW_IF_POSITION); + position_joint_info.state_interfaces.push_back(hardware_interface::HW_IF_POSITION); + + EXPECT_EQ(joint->configure(position_joint_info), return_type::COMPONENT_TOO_MANY_INTERFACES); +} + +TEST(PositionJointTest, position_joint_wrong_interface_test) +{ + ComponentInfo position_joint_info; + position_joint_info.name = "joint1"; + position_joint_info.parameters["min"] = "-3.14"; + position_joint_info.parameters["max"] = "3.14"; + position_joint_info.command_interfaces.push_back(hardware_interface::HW_IF_POSITION); + position_joint_info.state_interfaces.push_back(hardware_interface::HW_IF_EFFORT); + + pluginlib::ClassLoader joint_component_loader( + "hardware_interface", "hardware_interface::components::Joint"); + + std::shared_ptr joint = + joint_component_loader.createSharedInstance("ros2_control_components/PositionJoint"); + + EXPECT_EQ(joint->configure(position_joint_info), return_type::COMPONENT_WRONG_INTERFACE); + + position_joint_info.command_interfaces.clear(); + position_joint_info.command_interfaces.push_back(hardware_interface::HW_IF_EFFORT); + position_joint_info.state_interfaces.clear(); + EXPECT_EQ(joint->configure(position_joint_info), return_type::COMPONENT_WRONG_INTERFACE); +}