From 375984369218addc9333f62b61ddf0d4cc16c6e6 Mon Sep 17 00:00:00 2001 From: Karsten Knese Date: Fri, 25 Sep 2020 11:20:05 -0700 Subject: [PATCH 01/18] add main repos Signed-off-by: Karsten Knese --- .github/workflows/ci.yml | 1 + 1 file changed, 1 insertion(+) diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index 19cd6057be..b815a9e587 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -17,6 +17,7 @@ jobs: env: - {ROS_DISTRO: foxy, ROS_REPO: main} - {ROS_DISTRO: foxy, ROS_REPO: testing} + - {ROS_DISTRO: foxy, ROS_REPO: main} steps: - run: sudo apt-get update -qq && sudo apt-get upgrade - uses: actions/checkout@v1 From e11b77e660b4b572b45ad26065896d46193377a0 Mon Sep 17 00:00:00 2001 From: Karsten Knese Date: Fri, 25 Sep 2020 20:32:03 -0700 Subject: [PATCH 02/18] resource manager parses urdf Signed-off-by: Karsten Knese --- controller_manager/CMakeLists.txt | 25 ++- controller_manager/src/resource_manager.cpp | 143 ++++++++++++++++ controller_manager/src/resource_manager.hpp | 51 ++++++ .../test/test_hardware_resources.xml | 20 +++ .../test_actuator_interface.cpp | 62 +++++++ .../test_actuator_interface.hpp | 44 +++++ .../test_sensor_interface.cpp | 55 +++++++ .../test_sensor_interface.hpp | 43 +++++ .../test_system_interface.cpp | 71 ++++++++ .../test_system_interface.hpp | 51 ++++++ .../test/test_resource_manager.cpp | 152 ++++++++++++++++++ .../hardware_interface/actuator_hardware.hpp | 4 +- .../hardware_interface/sensor_hardware.hpp | 4 +- .../hardware_interface/system_hardware.hpp | 5 +- .../system_hardware_interface.hpp | 2 +- 15 files changed, 725 insertions(+), 7 deletions(-) create mode 100644 controller_manager/src/resource_manager.cpp create mode 100644 controller_manager/src/resource_manager.hpp create mode 100644 controller_manager/test/test_hardware_resources.xml create mode 100644 controller_manager/test/test_hardware_resources/test_actuator_interface.cpp create mode 100644 controller_manager/test/test_hardware_resources/test_actuator_interface.hpp create mode 100644 controller_manager/test/test_hardware_resources/test_sensor_interface.cpp create mode 100644 controller_manager/test/test_hardware_resources/test_sensor_interface.hpp create mode 100644 controller_manager/test/test_hardware_resources/test_system_interface.cpp create mode 100644 controller_manager/test/test_hardware_resources/test_system_interface.hpp create mode 100644 controller_manager/test/test_resource_manager.cpp diff --git a/controller_manager/CMakeLists.txt b/controller_manager/CMakeLists.txt index 0a93d78d94..621e4f9b06 100644 --- a/controller_manager/CMakeLists.txt +++ b/controller_manager/CMakeLists.txt @@ -21,6 +21,7 @@ find_package(rclcpp REQUIRED) add_library(controller_manager SHARED src/controller_manager.cpp + src/resource_manager.cpp ) target_include_directories(controller_manager PRIVATE include) ament_target_dependencies(controller_manager @@ -65,6 +66,10 @@ if(BUILD_TESTING) target_include_directories(test_controller PRIVATE include) target_link_libraries(test_controller controller_manager) target_compile_definitions(test_controller PRIVATE "CONTROLLER_MANAGER_BUILDING_DLL") + install(TARGETS test_controller + DESTINATION lib + ) + pluginlib_export_plugin_description_file(controller_interface test/test_controller.xml) ament_add_gmock( test_controller_manager @@ -101,11 +106,25 @@ if(BUILD_TESTING) test_robot_hardware ) - pluginlib_export_plugin_description_file(controller_interface test/test_controller.xml) - - install(TARGETS test_controller + add_library(test_hardware_resources SHARED + test/test_hardware_resources/test_actuator_interface.cpp + test/test_hardware_resources/test_sensor_interface.cpp + test/test_hardware_resources/test_system_interface.cpp) + ament_target_dependencies(test_hardware_resources + hardware_interface + pluginlib) + install(TARGETS test_hardware_resources DESTINATION lib ) + pluginlib_export_plugin_description_file( + hardware_interface test/test_hardware_resources.xml) + + ament_add_gtest( + test_resource_manager + test/test_resource_manager.cpp + ) + target_include_directories(test_resource_manager PRIVATE include src) + target_link_libraries(test_resource_manager controller_manager) endif() ament_export_libraries( diff --git a/controller_manager/src/resource_manager.cpp b/controller_manager/src/resource_manager.cpp new file mode 100644 index 0000000000..51ea06c2da --- /dev/null +++ b/controller_manager/src/resource_manager.cpp @@ -0,0 +1,143 @@ +// Copyright 2020 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 +#include +#include +#include + +#include "hardware_interface/components/component_info.hpp" +#include "hardware_interface/component_parser.hpp" +#include "hardware_interface/hardware_info.hpp" +#include "hardware_interface/actuator_hardware.hpp" +#include "hardware_interface/sensor_hardware.hpp" +#include "hardware_interface/system_hardware.hpp" + +#include "pluginlib/class_loader.hpp" + +#include "./resource_manager.hpp" + +namespace controller_manager +{ + +class ResourceStorage +{ + static constexpr const char * pkg_name = "hardware_interface"; + static constexpr const char * actuator_interface_name = + "hardware_interface::ActuatorHardwareInterface"; + static constexpr const char * sensor_interface_name = + "hardware_interface::SensorHardwareInterface"; + static constexpr const char * system_interface_name = + "hardware_interface::SystemHardwareInterface"; + +public: + ResourceStorage() + : actuator_loader_(pkg_name, actuator_interface_name), + sensor_loader_(pkg_name, sensor_interface_name), + system_loader_(pkg_name, system_interface_name) + {} + + ~ResourceStorage() = default; + + template + void initialize_hardware( + const hardware_interface::HardwareInfo & hardware_info, + pluginlib::ClassLoader & loader, + std::vector & container) + { + // hardware_class_type has to match class name in plugin xml description + auto interface = std::unique_ptr( + loader.createUnmanagedInstance(hardware_info.hardware_class_type)); + HardwareT actuator(std::move(interface)); + container.emplace_back(std::move(actuator)); + container.back().configure(hardware_info); + } + + void initialize_actuator(const hardware_interface::HardwareInfo & hardware_info) + { + using HardwareT = hardware_interface::ActuatorHardware; + using HardwareInterfaceT = hardware_interface::ActuatorHardwareInterface; + initialize_hardware( + hardware_info, actuator_loader_, actuators_); + } + + void initialize_sensor(const hardware_interface::HardwareInfo & hardware_info) + { + using HardwareT = hardware_interface::SensorHardware; + using HardwareInterfaceT = hardware_interface::SensorHardwareInterface; + initialize_hardware( + hardware_info, sensor_loader_, sensors_); + } + + void initialize_system(const hardware_interface::HardwareInfo & hardware_info) + { + using HardwareT = hardware_interface::SystemHardware; + using HardwareInterfaceT = hardware_interface::SystemHardwareInterface; + initialize_hardware( + hardware_info, system_loader_, systems_); + } + + pluginlib::ClassLoader actuator_loader_; + pluginlib::ClassLoader sensor_loader_; + pluginlib::ClassLoader system_loader_; + + std::vector actuators_; + std::vector sensors_; + std::vector systems_; +}; + +ResourceManager::ResourceManager() +: resource_storage_(std::make_unique()) +{} + +ResourceManager::~ResourceManager() = default; + +ResourceManager::ResourceManager(const std::string & urdf) +: resource_storage_(std::make_unique()) +{ + const std::string system_type = "system"; + const std::string sensor_type = "sensor"; + const std::string actuator_type = "actuator"; + + auto hardware_info = hardware_interface::parse_control_resources_from_urdf(urdf); + + for (const auto & hardware : hardware_info) { + if (hardware.type == actuator_type) { + resource_storage_->initialize_actuator(hardware); + } + if (hardware.type == sensor_type) { + resource_storage_->initialize_sensor(hardware); + } + if (hardware.type == system_type) { + resource_storage_->initialize_system(hardware); + } + } +} + +size_t ResourceManager::actuator_interfaces_size() const +{ + return resource_storage_->actuators_.size(); +} + +size_t ResourceManager::sensor_interfaces_size() const +{ + return resource_storage_->sensors_.size(); +} + +size_t ResourceManager::system_interfaces_size() const +{ + return resource_storage_->systems_.size(); +} + +} // namespace controller_manager diff --git a/controller_manager/src/resource_manager.hpp b/controller_manager/src/resource_manager.hpp new file mode 100644 index 0000000000..cddab65d8a --- /dev/null +++ b/controller_manager/src/resource_manager.hpp @@ -0,0 +1,51 @@ +// Copyright 2020 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. + +#ifndef RESOURCE_MANAGER_HPP_ +#define RESOURCE_MANAGER_HPP_ + +#include +#include + +namespace controller_manager +{ + +class ResourceStorage; + +class ResourceManager +{ +public: + ResourceManager(); + + explicit ResourceManager(const std::string & urdf); + + ResourceManager(const ResourceManager &) = delete; + + virtual ~ResourceManager(); + + size_t actuator_interfaces_size() const; + + size_t sensor_interfaces_size() const; + + size_t system_interfaces_size() const; + + // loan_joint(const std::string & name); + // loan_sensor(const std::string & name); + +private: + std::unique_ptr resource_storage_; +}; + +} // namespace controller_manager +#endif // RESOURCE_MANAGER_HPP_ diff --git a/controller_manager/test/test_hardware_resources.xml b/controller_manager/test/test_hardware_resources.xml new file mode 100644 index 0000000000..e630173706 --- /dev/null +++ b/controller_manager/test/test_hardware_resources.xml @@ -0,0 +1,20 @@ + + + + + Test Hardware Actuator + + + + + + Test Hardware Sensor + + + + + + Test Hardware System + + + diff --git a/controller_manager/test/test_hardware_resources/test_actuator_interface.cpp b/controller_manager/test/test_hardware_resources/test_actuator_interface.cpp new file mode 100644 index 0000000000..649505893a --- /dev/null +++ b/controller_manager/test/test_hardware_resources/test_actuator_interface.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 "./test_actuator_interface.hpp" + +hardware_interface::return_type +TestActuator::configure(const hardware_interface::HardwareInfo & actuator_info) +{ + fprintf(stderr, "configuring plugin with name %s\n", actuator_info.name.c_str()); + return hardware_interface::return_type::OK; +} + +hardware_interface::return_type +TestActuator::start() +{ + return hardware_interface::return_type::OK; +} + +hardware_interface::return_type +TestActuator::stop() +{ + return hardware_interface::return_type::OK; +} + +hardware_interface::hardware_interface_status +TestActuator::get_status() const +{ + return hardware_interface::hardware_interface_status::UNKNOWN; +} + +hardware_interface::return_type +TestActuator::read_joint( + std::shared_ptr joint) const +{ + (void) joint; + return hardware_interface::return_type::OK; +} + +hardware_interface::return_type +TestActuator::write_joint( + const std::shared_ptr joint) +{ + (void) joint; + return hardware_interface::return_type::OK; +} + +#include "pluginlib/class_list_macros.hpp" // NOLINT + +PLUGINLIB_EXPORT_CLASS(TestActuator, hardware_interface::ActuatorHardwareInterface) diff --git a/controller_manager/test/test_hardware_resources/test_actuator_interface.hpp b/controller_manager/test/test_hardware_resources/test_actuator_interface.hpp new file mode 100644 index 0000000000..88dc7e78f1 --- /dev/null +++ b/controller_manager/test/test_hardware_resources/test_actuator_interface.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 TEST_HARDWARE_RESOURCES__TEST_ACTUATOR_INTERFACE_HPP_ +#define TEST_HARDWARE_RESOURCES__TEST_ACTUATOR_INTERFACE_HPP_ + +#include + +#include "hardware_interface/actuator_hardware_interface.hpp" + +class TestActuator : public hardware_interface::ActuatorHardwareInterface +{ +public: + hardware_interface::return_type + configure(const hardware_interface::HardwareInfo & actuator_info) override; + + hardware_interface::return_type + start() override; + + hardware_interface::return_type + stop() override; + + hardware_interface::hardware_interface_status + get_status() const override; + + hardware_interface::return_type + read_joint(std::shared_ptr joint) const override; + + hardware_interface::return_type + write_joint(const std::shared_ptr joint) override; +}; + +#endif // TEST_HARDWARE_RESOURCES__TEST_ACTUATOR_INTERFACE_HPP_ diff --git a/controller_manager/test/test_hardware_resources/test_sensor_interface.cpp b/controller_manager/test/test_hardware_resources/test_sensor_interface.cpp new file mode 100644 index 0000000000..723446f30d --- /dev/null +++ b/controller_manager/test/test_hardware_resources/test_sensor_interface.cpp @@ -0,0 +1,55 @@ +// 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 "./test_sensor_interface.hpp" + +hardware_interface::return_type +TestSensor::configure(const hardware_interface::HardwareInfo & sensor_info) +{ + fprintf(stderr, "configuring plugin with name %s\n", sensor_info.name.c_str()); + return hardware_interface::return_type::OK; +} + +hardware_interface::return_type +TestSensor::start() +{ + return hardware_interface::return_type::OK; +} + +hardware_interface::return_type +TestSensor::stop() +{ + return hardware_interface::return_type::OK; +} + +hardware_interface::hardware_interface_status +TestSensor::get_status() const +{ + return hardware_interface::hardware_interface_status::UNKNOWN; +} + +hardware_interface::return_type +TestSensor::read_sensors( + const std::vector> & sensors) const +{ + (void) sensors; + return hardware_interface::return_type::OK; +} + +#include "pluginlib/class_list_macros.hpp" // NOLINT + +PLUGINLIB_EXPORT_CLASS(TestSensor, hardware_interface::SensorHardwareInterface) diff --git a/controller_manager/test/test_hardware_resources/test_sensor_interface.hpp b/controller_manager/test/test_hardware_resources/test_sensor_interface.hpp new file mode 100644 index 0000000000..cba55bc071 --- /dev/null +++ b/controller_manager/test/test_hardware_resources/test_sensor_interface.hpp @@ -0,0 +1,43 @@ +// 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 TEST_HARDWARE_RESOURCES__TEST_SENSOR_INTERFACE_HPP_ +#define TEST_HARDWARE_RESOURCES__TEST_SENSOR_INTERFACE_HPP_ + +#include +#include + +#include "hardware_interface/sensor_hardware_interface.hpp" + +class TestSensor : public hardware_interface::SensorHardwareInterface +{ +public: + hardware_interface::return_type + configure(const hardware_interface::HardwareInfo & sensor_info) override; + + hardware_interface::return_type + start() override; + + hardware_interface::return_type + stop() override; + + hardware_interface::hardware_interface_status + get_status() const override; + + hardware_interface::return_type + read_sensors(const std::vector> & sensors) + const override; +}; + +#endif // TEST_HARDWARE_RESOURCES__TEST_SENSOR_INTERFACE_HPP_ diff --git a/controller_manager/test/test_hardware_resources/test_system_interface.cpp b/controller_manager/test/test_hardware_resources/test_system_interface.cpp new file mode 100644 index 0000000000..56280e6e07 --- /dev/null +++ b/controller_manager/test/test_hardware_resources/test_system_interface.cpp @@ -0,0 +1,71 @@ +// 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 "./test_system_interface.hpp" + +hardware_interface::return_type +TestSystem::configure(const hardware_interface::HardwareInfo & system_info) +{ + fprintf(stderr, "configuring plugin with name %s\n", system_info.name.c_str()); + return hardware_interface::return_type::OK; +} + +hardware_interface::return_type +TestSystem::start() +{ + return hardware_interface::return_type::OK; +} + +hardware_interface::return_type +TestSystem::stop() +{ + return hardware_interface::return_type::OK; +} + +hardware_interface::hardware_interface_status +TestSystem::get_status() const +{ + return hardware_interface::hardware_interface_status::UNKNOWN; +} + +hardware_interface::return_type +TestSystem::read_sensors( + std::vector> & sensors) const +{ + (void) sensors; + return hardware_interface::return_type::OK; +} + +hardware_interface::return_type +TestSystem::read_joints( + std::vector> & joints) const +{ + (void) joints; + return hardware_interface::return_type::OK; +} + +hardware_interface::return_type +TestSystem::write_joints( + const std::vector> & joints) +{ + (void) joints; + return hardware_interface::return_type::OK; +} + +#include "pluginlib/class_list_macros.hpp" // NOLINT + +PLUGINLIB_EXPORT_CLASS(TestSystem, hardware_interface::SystemHardwareInterface) diff --git a/controller_manager/test/test_hardware_resources/test_system_interface.hpp b/controller_manager/test/test_hardware_resources/test_system_interface.hpp new file mode 100644 index 0000000000..344f91b37d --- /dev/null +++ b/controller_manager/test/test_hardware_resources/test_system_interface.hpp @@ -0,0 +1,51 @@ +// 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 TEST_HARDWARE_RESOURCES__TEST_SYSTEM_INTERFACE_HPP_ +#define TEST_HARDWARE_RESOURCES__TEST_SYSTEM_INTERFACE_HPP_ + +#include +#include + +#include "hardware_interface/system_hardware_interface.hpp" + +class TestSystem : public hardware_interface::SystemHardwareInterface +{ +public: + hardware_interface::return_type + configure(const hardware_interface::HardwareInfo & system_info) override; + + hardware_interface::return_type + start() override; + + hardware_interface::return_type + stop() override; + + hardware_interface::hardware_interface_status + get_status() const override; + + hardware_interface::return_type + read_sensors( + std::vector> & sensors) const override; + + hardware_interface::return_type + read_joints( + std::vector> & joints) const override; + + hardware_interface::return_type + write_joints( + const std::vector> & joints) override; +}; + +#endif // TEST_HARDWARE_RESOURCES__TEST_SYSTEM_INTERFACE_HPP_ diff --git a/controller_manager/test/test_resource_manager.cpp b/controller_manager/test/test_resource_manager.cpp new file mode 100644 index 0000000000..1d216e8f28 --- /dev/null +++ b/controller_manager/test/test_resource_manager.cpp @@ -0,0 +1,152 @@ +// 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 +#include +#include + +#include "resource_manager.hpp" + +class TestResourceManager : public ::testing::Test +{ +public: + static void SetUpTestCase() + { + } + + void SetUp() + { + urdf_head_ = + R"( + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +)"; + + urdf_tail_ = + R"( + +)"; + + // 1. Industrial Robots with only one interface + test_hardware_resource_system_ = + R"( + + + test_actuator + 2 + 2 + + + ros2_control_components/PositionJoint + -1 + 1 + + + + + test_sensor + 2 + 2 + + + ros2_control_components/PositionJoint + -1 + 1 + + + + + test_system + 2 + 2 + + + ros2_control_components/PositionJoint + -1 + 1 + + + ros2_control_components/PositionJoint + -1 + 1 + + +)"; + } + + std::string urdf_head_; + std::string test_hardware_resource_system_; + std::string urdf_tail_; +}; + +TEST_F(TestResourceManager, initialization_empty) { + controller_manager::ResourceManager rm; + EXPECT_EQ(0u, rm.actuator_interfaces_size()); + EXPECT_EQ(0u, rm.sensor_interfaces_size()); + EXPECT_EQ(0u, rm.system_interfaces_size()); +} + +TEST_F(TestResourceManager, initialization_with_urdf) { + auto urdf = urdf_head_ + test_hardware_resource_system_ + urdf_tail_; + controller_manager::ResourceManager rm(urdf); + EXPECT_EQ(1u, rm.actuator_interfaces_size()); + EXPECT_EQ(1u, rm.sensor_interfaces_size()); + EXPECT_EQ(1u, rm.system_interfaces_size()); +} diff --git a/hardware_interface/include/hardware_interface/actuator_hardware.hpp b/hardware_interface/include/hardware_interface/actuator_hardware.hpp index ecca54042d..da8db8d1b5 100644 --- a/hardware_interface/include/hardware_interface/actuator_hardware.hpp +++ b/hardware_interface/include/hardware_interface/actuator_hardware.hpp @@ -17,6 +17,7 @@ #include +#include "hardware_interface/actuator_hardware_interface.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" @@ -29,13 +30,14 @@ namespace components { class Joint; } // namespace components -class ActuatorHardwareInterface; class ActuatorHardware final { public: ActuatorHardware() = default; + explicit ActuatorHardware(ActuatorHardware && other) = default; + explicit ActuatorHardware(std::unique_ptr impl); ~ActuatorHardware() = default; diff --git a/hardware_interface/include/hardware_interface/sensor_hardware.hpp b/hardware_interface/include/hardware_interface/sensor_hardware.hpp index 52a363ea0e..bec1e33ab9 100644 --- a/hardware_interface/include/hardware_interface/sensor_hardware.hpp +++ b/hardware_interface/include/hardware_interface/sensor_hardware.hpp @@ -21,6 +21,7 @@ #include #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" @@ -32,13 +33,14 @@ namespace components { class Sensor; } // namespace components -class SensorHardwareInterface; class SensorHardware final { public: SensorHardware() = default; + explicit SensorHardware(SensorHardware && other) = default; + explicit SensorHardware(std::unique_ptr impl); ~SensorHardware() = default; diff --git a/hardware_interface/include/hardware_interface/system_hardware.hpp b/hardware_interface/include/hardware_interface/system_hardware.hpp index 113af16c72..6522bb1729 100644 --- a/hardware_interface/include/hardware_interface/system_hardware.hpp +++ b/hardware_interface/include/hardware_interface/system_hardware.hpp @@ -21,6 +21,7 @@ #include #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" @@ -33,7 +34,6 @@ namespace components class Joint; class Sensor; } // namespace components -class SystemHardwareInterface; class SystemHardware final { @@ -41,6 +41,9 @@ class SystemHardware final HARDWARE_INTERFACE_PUBLIC explicit SystemHardware(std::unique_ptr impl); + HARDWARE_INTERFACE_PUBLIC + explicit SystemHardware(SystemHardware && other) = default; + virtual ~SystemHardware() = default; HARDWARE_INTERFACE_PUBLIC diff --git a/hardware_interface/include/hardware_interface/system_hardware_interface.hpp b/hardware_interface/include/hardware_interface/system_hardware_interface.hpp index 1d9a3089aa..6c38c0a937 100644 --- a/hardware_interface/include/hardware_interface/system_hardware_interface.hpp +++ b/hardware_interface/include/hardware_interface/system_hardware_interface.hpp @@ -112,8 +112,8 @@ class SystemHardwareInterface * \return return_type:OK if everything worked as expected, return_type::ERROR otherwise. */ HARDWARE_INTERFACE_PUBLIC - return_type virtual + return_type write_joints(const std::vector> & joints) = 0; }; From 8d2a28b9b9f6d575c999d1f30938bbb1823d785a Mon Sep 17 00:00:00 2001 From: Karsten Knese Date: Fri, 25 Sep 2020 21:47:21 -0700 Subject: [PATCH 03/18] cleanup hardware_interface cmake Signed-off-by: Karsten Knese --- hardware_interface/CMakeLists.txt | 29 ++++++----------------------- 1 file changed, 6 insertions(+), 23 deletions(-) diff --git a/hardware_interface/CMakeLists.txt b/hardware_interface/CMakeLists.txt index 4e9f289919..d2467982be 100644 --- a/hardware_interface/CMakeLists.txt +++ b/hardware_interface/CMakeLists.txt @@ -41,32 +41,22 @@ 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( - component_parser - src/component_parser.cpp -) -target_include_directories( - component_parser - PUBLIC - include -) -ament_target_dependencies( - component_parser - TinyXML2 -) -target_compile_definitions(component_parser PRIVATE "HARDWARE_INTERFACE_BUILDING_DLL") - add_library( components SHARED src/components/joint.cpp src/components/sensor.cpp + src/component_parser.cpp ) target_include_directories( components PUBLIC include ) +ament_target_dependencies( + components + TinyXML2 +) # Causes the visibility macros to use dllexport rather than dllimport, # which is appropriate when building the dll but not consuming it. target_compile_definitions(components PRIVATE "HARDWARE_INTERFACE_BUILDING_DLL") @@ -78,7 +68,6 @@ install( install( TARGETS - component_parser components hardware_interface RUNTIME DESTINATION bin @@ -124,19 +113,13 @@ if(BUILD_TESTING) target_link_libraries(test_component_interfaces components hardware_interface) ament_add_gmock(test_component_parser test/test_component_parser.cpp) - target_link_libraries(test_component_parser component_parser) - ament_target_dependencies(test_component_parser TinyXML2) + target_link_libraries(test_component_parser components) endif() -ament_export_dependencies( - rclcpp - rcpputils -) ament_export_include_directories( include ) ament_export_libraries( - component_parser components hardware_interface ) From 9f07f430568283f880d3bea0aba3ef51f6f79b59 Mon Sep 17 00:00:00 2001 From: Karsten Knese Date: Sat, 26 Sep 2020 10:45:44 -0700 Subject: [PATCH 04/18] rebase Signed-off-by: Karsten Knese --- .github/workflows/ci.yml | 1 - 1 file changed, 1 deletion(-) diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index b815a9e587..19cd6057be 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -17,7 +17,6 @@ jobs: env: - {ROS_DISTRO: foxy, ROS_REPO: main} - {ROS_DISTRO: foxy, ROS_REPO: testing} - - {ROS_DISTRO: foxy, ROS_REPO: main} steps: - run: sudo apt-get update -qq && sudo apt-get upgrade - uses: actions/checkout@v1 From d349914b5db987a14a99e6b36c4976801d59a8cf Mon Sep 17 00:00:00 2001 From: Karsten Knese Date: Mon, 28 Sep 2020 12:32:13 -0700 Subject: [PATCH 05/18] renaming Signed-off-by: Karsten Knese --- controller_manager/CMakeLists.txt | 6 +- .../test/test_hardware_resources.xml | 6 +- .../test_actuator_interface.cpp | 62 ---------------- .../test_actuator_interface.hpp | 44 ------------ .../test_sensor_interface.cpp | 55 -------------- .../test_sensor_interface.hpp | 43 ----------- .../test_system_interface.cpp | 71 ------------------- .../test_system_interface.hpp | 51 ------------- .../test/test_resource_manager.cpp | 21 +++--- 9 files changed, 16 insertions(+), 343 deletions(-) delete mode 100644 controller_manager/test/test_hardware_resources/test_actuator_interface.cpp delete mode 100644 controller_manager/test/test_hardware_resources/test_actuator_interface.hpp delete mode 100644 controller_manager/test/test_hardware_resources/test_sensor_interface.cpp delete mode 100644 controller_manager/test/test_hardware_resources/test_sensor_interface.hpp delete mode 100644 controller_manager/test/test_hardware_resources/test_system_interface.cpp delete mode 100644 controller_manager/test/test_hardware_resources/test_system_interface.hpp diff --git a/controller_manager/CMakeLists.txt b/controller_manager/CMakeLists.txt index 621e4f9b06..d71ffcf6a1 100644 --- a/controller_manager/CMakeLists.txt +++ b/controller_manager/CMakeLists.txt @@ -107,9 +107,9 @@ if(BUILD_TESTING) ) add_library(test_hardware_resources SHARED - test/test_hardware_resources/test_actuator_interface.cpp - test/test_hardware_resources/test_sensor_interface.cpp - test/test_hardware_resources/test_system_interface.cpp) + test/test_hardware_resources/test_actuator_hardware.cpp + test/test_hardware_resources/test_sensor_hardware.cpp + test/test_hardware_resources/test_system_hardware.cpp) ament_target_dependencies(test_hardware_resources hardware_interface pluginlib) diff --git a/controller_manager/test/test_hardware_resources.xml b/controller_manager/test/test_hardware_resources.xml index e630173706..6c9dab8c0b 100644 --- a/controller_manager/test/test_hardware_resources.xml +++ b/controller_manager/test/test_hardware_resources.xml @@ -1,18 +1,18 @@ - + Test Hardware Actuator - + Test Hardware Sensor - + Test Hardware System diff --git a/controller_manager/test/test_hardware_resources/test_actuator_interface.cpp b/controller_manager/test/test_hardware_resources/test_actuator_interface.cpp deleted file mode 100644 index 649505893a..0000000000 --- a/controller_manager/test/test_hardware_resources/test_actuator_interface.cpp +++ /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. - -#include - -#include "./test_actuator_interface.hpp" - -hardware_interface::return_type -TestActuator::configure(const hardware_interface::HardwareInfo & actuator_info) -{ - fprintf(stderr, "configuring plugin with name %s\n", actuator_info.name.c_str()); - return hardware_interface::return_type::OK; -} - -hardware_interface::return_type -TestActuator::start() -{ - return hardware_interface::return_type::OK; -} - -hardware_interface::return_type -TestActuator::stop() -{ - return hardware_interface::return_type::OK; -} - -hardware_interface::hardware_interface_status -TestActuator::get_status() const -{ - return hardware_interface::hardware_interface_status::UNKNOWN; -} - -hardware_interface::return_type -TestActuator::read_joint( - std::shared_ptr joint) const -{ - (void) joint; - return hardware_interface::return_type::OK; -} - -hardware_interface::return_type -TestActuator::write_joint( - const std::shared_ptr joint) -{ - (void) joint; - return hardware_interface::return_type::OK; -} - -#include "pluginlib/class_list_macros.hpp" // NOLINT - -PLUGINLIB_EXPORT_CLASS(TestActuator, hardware_interface::ActuatorHardwareInterface) diff --git a/controller_manager/test/test_hardware_resources/test_actuator_interface.hpp b/controller_manager/test/test_hardware_resources/test_actuator_interface.hpp deleted file mode 100644 index 88dc7e78f1..0000000000 --- a/controller_manager/test/test_hardware_resources/test_actuator_interface.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 TEST_HARDWARE_RESOURCES__TEST_ACTUATOR_INTERFACE_HPP_ -#define TEST_HARDWARE_RESOURCES__TEST_ACTUATOR_INTERFACE_HPP_ - -#include - -#include "hardware_interface/actuator_hardware_interface.hpp" - -class TestActuator : public hardware_interface::ActuatorHardwareInterface -{ -public: - hardware_interface::return_type - configure(const hardware_interface::HardwareInfo & actuator_info) override; - - hardware_interface::return_type - start() override; - - hardware_interface::return_type - stop() override; - - hardware_interface::hardware_interface_status - get_status() const override; - - hardware_interface::return_type - read_joint(std::shared_ptr joint) const override; - - hardware_interface::return_type - write_joint(const std::shared_ptr joint) override; -}; - -#endif // TEST_HARDWARE_RESOURCES__TEST_ACTUATOR_INTERFACE_HPP_ diff --git a/controller_manager/test/test_hardware_resources/test_sensor_interface.cpp b/controller_manager/test/test_hardware_resources/test_sensor_interface.cpp deleted file mode 100644 index 723446f30d..0000000000 --- a/controller_manager/test/test_hardware_resources/test_sensor_interface.cpp +++ /dev/null @@ -1,55 +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 "./test_sensor_interface.hpp" - -hardware_interface::return_type -TestSensor::configure(const hardware_interface::HardwareInfo & sensor_info) -{ - fprintf(stderr, "configuring plugin with name %s\n", sensor_info.name.c_str()); - return hardware_interface::return_type::OK; -} - -hardware_interface::return_type -TestSensor::start() -{ - return hardware_interface::return_type::OK; -} - -hardware_interface::return_type -TestSensor::stop() -{ - return hardware_interface::return_type::OK; -} - -hardware_interface::hardware_interface_status -TestSensor::get_status() const -{ - return hardware_interface::hardware_interface_status::UNKNOWN; -} - -hardware_interface::return_type -TestSensor::read_sensors( - const std::vector> & sensors) const -{ - (void) sensors; - return hardware_interface::return_type::OK; -} - -#include "pluginlib/class_list_macros.hpp" // NOLINT - -PLUGINLIB_EXPORT_CLASS(TestSensor, hardware_interface::SensorHardwareInterface) diff --git a/controller_manager/test/test_hardware_resources/test_sensor_interface.hpp b/controller_manager/test/test_hardware_resources/test_sensor_interface.hpp deleted file mode 100644 index cba55bc071..0000000000 --- a/controller_manager/test/test_hardware_resources/test_sensor_interface.hpp +++ /dev/null @@ -1,43 +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 TEST_HARDWARE_RESOURCES__TEST_SENSOR_INTERFACE_HPP_ -#define TEST_HARDWARE_RESOURCES__TEST_SENSOR_INTERFACE_HPP_ - -#include -#include - -#include "hardware_interface/sensor_hardware_interface.hpp" - -class TestSensor : public hardware_interface::SensorHardwareInterface -{ -public: - hardware_interface::return_type - configure(const hardware_interface::HardwareInfo & sensor_info) override; - - hardware_interface::return_type - start() override; - - hardware_interface::return_type - stop() override; - - hardware_interface::hardware_interface_status - get_status() const override; - - hardware_interface::return_type - read_sensors(const std::vector> & sensors) - const override; -}; - -#endif // TEST_HARDWARE_RESOURCES__TEST_SENSOR_INTERFACE_HPP_ diff --git a/controller_manager/test/test_hardware_resources/test_system_interface.cpp b/controller_manager/test/test_hardware_resources/test_system_interface.cpp deleted file mode 100644 index 56280e6e07..0000000000 --- a/controller_manager/test/test_hardware_resources/test_system_interface.cpp +++ /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. - -#include -#include - -#include "./test_system_interface.hpp" - -hardware_interface::return_type -TestSystem::configure(const hardware_interface::HardwareInfo & system_info) -{ - fprintf(stderr, "configuring plugin with name %s\n", system_info.name.c_str()); - return hardware_interface::return_type::OK; -} - -hardware_interface::return_type -TestSystem::start() -{ - return hardware_interface::return_type::OK; -} - -hardware_interface::return_type -TestSystem::stop() -{ - return hardware_interface::return_type::OK; -} - -hardware_interface::hardware_interface_status -TestSystem::get_status() const -{ - return hardware_interface::hardware_interface_status::UNKNOWN; -} - -hardware_interface::return_type -TestSystem::read_sensors( - std::vector> & sensors) const -{ - (void) sensors; - return hardware_interface::return_type::OK; -} - -hardware_interface::return_type -TestSystem::read_joints( - std::vector> & joints) const -{ - (void) joints; - return hardware_interface::return_type::OK; -} - -hardware_interface::return_type -TestSystem::write_joints( - const std::vector> & joints) -{ - (void) joints; - return hardware_interface::return_type::OK; -} - -#include "pluginlib/class_list_macros.hpp" // NOLINT - -PLUGINLIB_EXPORT_CLASS(TestSystem, hardware_interface::SystemHardwareInterface) diff --git a/controller_manager/test/test_hardware_resources/test_system_interface.hpp b/controller_manager/test/test_hardware_resources/test_system_interface.hpp deleted file mode 100644 index 344f91b37d..0000000000 --- a/controller_manager/test/test_hardware_resources/test_system_interface.hpp +++ /dev/null @@ -1,51 +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 TEST_HARDWARE_RESOURCES__TEST_SYSTEM_INTERFACE_HPP_ -#define TEST_HARDWARE_RESOURCES__TEST_SYSTEM_INTERFACE_HPP_ - -#include -#include - -#include "hardware_interface/system_hardware_interface.hpp" - -class TestSystem : public hardware_interface::SystemHardwareInterface -{ -public: - hardware_interface::return_type - configure(const hardware_interface::HardwareInfo & system_info) override; - - hardware_interface::return_type - start() override; - - hardware_interface::return_type - stop() override; - - hardware_interface::hardware_interface_status - get_status() const override; - - hardware_interface::return_type - read_sensors( - std::vector> & sensors) const override; - - hardware_interface::return_type - read_joints( - std::vector> & joints) const override; - - hardware_interface::return_type - write_joints( - const std::vector> & joints) override; -}; - -#endif // TEST_HARDWARE_RESOURCES__TEST_SYSTEM_INTERFACE_HPP_ diff --git a/controller_manager/test/test_resource_manager.cpp b/controller_manager/test/test_resource_manager.cpp index 1d216e8f28..c1062bc878 100644 --- a/controller_manager/test/test_resource_manager.cpp +++ b/controller_manager/test/test_resource_manager.cpp @@ -84,46 +84,45 @@ class TestResourceManager : public ::testing::Test )"; - // 1. Industrial Robots with only one interface test_hardware_resource_system_ = R"( - + - test_actuator + test_actuator_hardware 2 2 - ros2_control_components/PositionJoint + test_joint_component -1 1 - + - test_sensor + test_sensor_hardware 2 2 - ros2_control_components/PositionJoint + test_sensor_component -1 1 - + - test_system + test_system_hardware 2 2 - ros2_control_components/PositionJoint + test_joint_component -1 1 - ros2_control_components/PositionJoint + test_joint_component -1 1 From c1067efcd89d97975c7b4e4685fcd1e82439c6d5 Mon Sep 17 00:00:00 2001 From: Karsten Knese Date: Mon, 28 Sep 2020 15:56:19 -0700 Subject: [PATCH 06/18] load hardware and components Signed-off-by: Karsten Knese --- controller_manager/CMakeLists.txt | 2 + controller_manager/src/resource_manager.cpp | 59 ++++++++++++++- controller_manager/src/resource_manager.hpp | 4 ++ .../test/test_hardware_resources.xml | 13 ++++ .../test_actuator_hardware.cpp | 62 ++++++++++++++++ .../test_actuator_hardware.hpp | 44 ++++++++++++ .../test_joint_component.cpp | 42 +++++++++++ .../test_joint_component.hpp | 35 +++++++++ .../test_sensor_component.cpp | 36 ++++++++++ .../test_sensor_component.hpp | 33 +++++++++ .../test_sensor_hardware.cpp | 55 ++++++++++++++ .../test_sensor_hardware.hpp | 43 +++++++++++ .../test_system_hardware.cpp | 71 +++++++++++++++++++ .../test_system_hardware.hpp | 51 +++++++++++++ .../test/test_resource_manager.cpp | 4 ++ .../hardware_interface/components/joint.hpp | 16 ++--- .../hardware_interface/components/sensor.hpp | 10 +-- 17 files changed, 564 insertions(+), 16 deletions(-) create mode 100644 controller_manager/test/test_hardware_resources/test_actuator_hardware.cpp create mode 100644 controller_manager/test/test_hardware_resources/test_actuator_hardware.hpp create mode 100644 controller_manager/test/test_hardware_resources/test_joint_component.cpp create mode 100644 controller_manager/test/test_hardware_resources/test_joint_component.hpp create mode 100644 controller_manager/test/test_hardware_resources/test_sensor_component.cpp create mode 100644 controller_manager/test/test_hardware_resources/test_sensor_component.hpp create mode 100644 controller_manager/test/test_hardware_resources/test_sensor_hardware.cpp create mode 100644 controller_manager/test/test_hardware_resources/test_sensor_hardware.hpp create mode 100644 controller_manager/test/test_hardware_resources/test_system_hardware.cpp create mode 100644 controller_manager/test/test_hardware_resources/test_system_hardware.hpp diff --git a/controller_manager/CMakeLists.txt b/controller_manager/CMakeLists.txt index d71ffcf6a1..0eef758640 100644 --- a/controller_manager/CMakeLists.txt +++ b/controller_manager/CMakeLists.txt @@ -108,6 +108,8 @@ if(BUILD_TESTING) add_library(test_hardware_resources SHARED test/test_hardware_resources/test_actuator_hardware.cpp + test/test_hardware_resources/test_joint_component.cpp + test/test_hardware_resources/test_sensor_component.cpp test/test_hardware_resources/test_sensor_hardware.cpp test/test_hardware_resources/test_system_hardware.cpp) ament_target_dependencies(test_hardware_resources diff --git a/controller_manager/src/resource_manager.cpp b/controller_manager/src/resource_manager.cpp index 51ea06c2da..4ffd758666 100644 --- a/controller_manager/src/resource_manager.cpp +++ b/controller_manager/src/resource_manager.cpp @@ -17,10 +17,10 @@ #include #include +#include "hardware_interface/actuator_hardware.hpp" #include "hardware_interface/components/component_info.hpp" #include "hardware_interface/component_parser.hpp" #include "hardware_interface/hardware_info.hpp" -#include "hardware_interface/actuator_hardware.hpp" #include "hardware_interface/sensor_hardware.hpp" #include "hardware_interface/system_hardware.hpp" @@ -34,6 +34,12 @@ namespace controller_manager class ResourceStorage { static constexpr const char * pkg_name = "hardware_interface"; + + static constexpr const char * joint_component_interface_name = + "hardware_interface::components::Joint"; + static constexpr const char * sensor_component_interface_name = + "hardware_interface::components::Sensor"; + static constexpr const char * actuator_interface_name = "hardware_interface::ActuatorHardwareInterface"; static constexpr const char * sensor_interface_name = @@ -43,13 +49,33 @@ class ResourceStorage public: ResourceStorage() - : actuator_loader_(pkg_name, actuator_interface_name), + : joint_component_loader_(pkg_name, joint_component_interface_name), + sensor_component_loader_(pkg_name, sensor_component_interface_name), + actuator_loader_(pkg_name, actuator_interface_name), sensor_loader_(pkg_name, sensor_interface_name), system_loader_(pkg_name, system_interface_name) {} ~ResourceStorage() = default; + void initialize_joint_component( + const hardware_interface::components::ComponentInfo & component_info) + { + joint_components_.emplace_back( + std::unique_ptr( + joint_component_loader_.createUnmanagedInstance(component_info.class_type))); + joint_components_.back()->configure(component_info); + } + + void initialize_sensor_component( + const hardware_interface::components::ComponentInfo & component_info) + { + sensor_components_.emplace_back( + std::unique_ptr( + sensor_component_loader_.createUnmanagedInstance(component_info.class_type))); + sensor_components_.back()->configure(component_info); + } + template void initialize_hardware( const hardware_interface::HardwareInfo & hardware_info, @@ -57,6 +83,8 @@ class ResourceStorage std::vector & container) { // hardware_class_type has to match class name in plugin xml description + // TODO(karsten1987) extract package from hardware_class_type + // e.g.: / auto interface = std::unique_ptr( loader.createUnmanagedInstance(hardware_info.hardware_class_type)); HardwareT actuator(std::move(interface)); @@ -88,6 +116,14 @@ class ResourceStorage hardware_info, system_loader_, systems_); } + // components plugins + pluginlib::ClassLoader joint_component_loader_; + pluginlib::ClassLoader sensor_component_loader_; + + std::vector> joint_components_; + std::vector> sensor_components_; + + // hardware plugins pluginlib::ClassLoader actuator_loader_; pluginlib::ClassLoader sensor_loader_; pluginlib::ClassLoader system_loader_; @@ -122,9 +158,27 @@ ResourceManager::ResourceManager(const std::string & urdf) if (hardware.type == system_type) { resource_storage_->initialize_system(hardware); } + + for (const auto & joint : hardware.joints) { + resource_storage_->initialize_joint_component(joint); + } + + for (const auto & sensor : hardware.sensors) { + resource_storage_->initialize_sensor_component(sensor); + } } } +size_t ResourceManager::joint_components_size() const +{ + return resource_storage_->joint_components_.size(); +} + +size_t ResourceManager::sensor_components_size() const +{ + return resource_storage_->sensor_components_.size(); +} + size_t ResourceManager::actuator_interfaces_size() const { return resource_storage_->actuators_.size(); @@ -139,5 +193,4 @@ size_t ResourceManager::system_interfaces_size() const { return resource_storage_->systems_.size(); } - } // namespace controller_manager diff --git a/controller_manager/src/resource_manager.hpp b/controller_manager/src/resource_manager.hpp index cddab65d8a..21c991680a 100644 --- a/controller_manager/src/resource_manager.hpp +++ b/controller_manager/src/resource_manager.hpp @@ -34,6 +34,10 @@ class ResourceManager virtual ~ResourceManager(); + size_t joint_components_size() const; + + size_t sensor_components_size() const; + size_t actuator_interfaces_size() const; size_t sensor_interfaces_size() const; diff --git a/controller_manager/test/test_hardware_resources.xml b/controller_manager/test/test_hardware_resources.xml index 6c9dab8c0b..d0494d45ec 100644 --- a/controller_manager/test/test_hardware_resources.xml +++ b/controller_manager/test/test_hardware_resources.xml @@ -17,4 +17,17 @@ Test Hardware System + + + + Test Joint Component + + + + + + Test Sensor Component + + + diff --git a/controller_manager/test/test_hardware_resources/test_actuator_hardware.cpp b/controller_manager/test/test_hardware_resources/test_actuator_hardware.cpp new file mode 100644 index 0000000000..f2f3616d6f --- /dev/null +++ b/controller_manager/test/test_hardware_resources/test_actuator_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 "./test_actuator_hardware.hpp" + +hardware_interface::return_type +TestActuatorHardware::configure(const hardware_interface::HardwareInfo & actuator_info) +{ + fprintf(stderr, "configuring plugin with name %s\n", actuator_info.name.c_str()); + return hardware_interface::return_type::OK; +} + +hardware_interface::return_type +TestActuatorHardware::start() +{ + return hardware_interface::return_type::OK; +} + +hardware_interface::return_type +TestActuatorHardware::stop() +{ + return hardware_interface::return_type::OK; +} + +hardware_interface::hardware_interface_status +TestActuatorHardware::get_status() const +{ + return hardware_interface::hardware_interface_status::UNKNOWN; +} + +hardware_interface::return_type +TestActuatorHardware::read_joint( + std::shared_ptr joint) const +{ + (void) joint; + return hardware_interface::return_type::OK; +} + +hardware_interface::return_type +TestActuatorHardware::write_joint( + const std::shared_ptr joint) +{ + (void) joint; + return hardware_interface::return_type::OK; +} + +#include "pluginlib/class_list_macros.hpp" // NOLINT + +PLUGINLIB_EXPORT_CLASS(TestActuatorHardware, hardware_interface::ActuatorHardwareInterface) diff --git a/controller_manager/test/test_hardware_resources/test_actuator_hardware.hpp b/controller_manager/test/test_hardware_resources/test_actuator_hardware.hpp new file mode 100644 index 0000000000..daaaded4fc --- /dev/null +++ b/controller_manager/test/test_hardware_resources/test_actuator_hardware.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 TEST_HARDWARE_RESOURCES__TEST_ACTUATOR_HARDWARE_HPP_ +#define TEST_HARDWARE_RESOURCES__TEST_ACTUATOR_HARDWARE_HPP_ + +#include + +#include "hardware_interface/actuator_hardware_interface.hpp" + +class TestActuatorHardware : public hardware_interface::ActuatorHardwareInterface +{ +public: + hardware_interface::return_type + configure(const hardware_interface::HardwareInfo & actuator_info) override; + + hardware_interface::return_type + start() override; + + hardware_interface::return_type + stop() override; + + hardware_interface::hardware_interface_status + get_status() const override; + + hardware_interface::return_type + read_joint(std::shared_ptr joint) const override; + + hardware_interface::return_type + write_joint(const std::shared_ptr joint) override; +}; + +#endif // TEST_HARDWARE_RESOURCES__TEST_ACTUATOR_HARDWARE_HPP_ diff --git a/controller_manager/test/test_hardware_resources/test_joint_component.cpp b/controller_manager/test/test_hardware_resources/test_joint_component.cpp new file mode 100644 index 0000000000..4e5661d808 --- /dev/null +++ b/controller_manager/test/test_hardware_resources/test_joint_component.cpp @@ -0,0 +1,42 @@ +// 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 "./test_joint_component.hpp" + +hardware_interface::return_type +TestJointComponent::configure(const hardware_interface::components::ComponentInfo & joint_info) +{ + fprintf(stderr, "configuring plugin with name %s\n", joint_info.name.c_str()); + return hardware_interface::return_type::OK; +} + +std::vector +TestJointComponent::get_command_interfaces() const +{ + return {"test_command_interface"}; +} + +std::vector +TestJointComponent::get_state_interfaces() const +{ + return {"test_state_interface"}; +} + +#include "pluginlib/class_list_macros.hpp" // NOLINT + +PLUGINLIB_EXPORT_CLASS(TestJointComponent, hardware_interface::components::Joint) diff --git a/controller_manager/test/test_hardware_resources/test_joint_component.hpp b/controller_manager/test/test_hardware_resources/test_joint_component.hpp new file mode 100644 index 0000000000..b754cfa993 --- /dev/null +++ b/controller_manager/test/test_hardware_resources/test_joint_component.hpp @@ -0,0 +1,35 @@ +// 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 TEST_HARDWARE_RESOURCES__TEST_JOINT_COMPONENT_HPP_ +#define TEST_HARDWARE_RESOURCES__TEST_JOINT_COMPONENT_HPP_ + +#include +#include +#include + +#include "hardware_interface/components/joint.hpp" + +class TestJointComponent : public hardware_interface::components::Joint +{ +public: + hardware_interface::return_type + configure(const hardware_interface::components::ComponentInfo & joint_info) override; + + std::vector get_command_interfaces() const override; + + std::vector get_state_interfaces() const override; +}; + +#endif // TEST_HARDWARE_RESOURCES__TEST_JOINT_COMPONENT_HPP_ diff --git a/controller_manager/test/test_hardware_resources/test_sensor_component.cpp b/controller_manager/test/test_hardware_resources/test_sensor_component.cpp new file mode 100644 index 0000000000..d614e91b65 --- /dev/null +++ b/controller_manager/test/test_hardware_resources/test_sensor_component.cpp @@ -0,0 +1,36 @@ +// 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 "./test_sensor_component.hpp" + +hardware_interface::return_type +TestSensorComponent::configure(const hardware_interface::components::ComponentInfo & sensor_info) +{ + fprintf(stderr, "configuring plugin with name %s\n", sensor_info.name.c_str()); + return hardware_interface::return_type::OK; +} + +std::vector +TestSensorComponent::get_state_interfaces() const +{ + return {"test_state_interface"}; +} + +#include "pluginlib/class_list_macros.hpp" // NOLINT + +PLUGINLIB_EXPORT_CLASS(TestSensorComponent, hardware_interface::components::Sensor) diff --git a/controller_manager/test/test_hardware_resources/test_sensor_component.hpp b/controller_manager/test/test_hardware_resources/test_sensor_component.hpp new file mode 100644 index 0000000000..ce7e8c1998 --- /dev/null +++ b/controller_manager/test/test_hardware_resources/test_sensor_component.hpp @@ -0,0 +1,33 @@ +// 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 TEST_HARDWARE_RESOURCES__TEST_SENSOR_COMPONENT_HPP_ +#define TEST_HARDWARE_RESOURCES__TEST_SENSOR_COMPONENT_HPP_ + +#include +#include +#include + +#include "hardware_interface/components/sensor.hpp" + +class TestSensorComponent : public hardware_interface::components::Sensor +{ +public: + hardware_interface::return_type + configure(const hardware_interface::components::ComponentInfo & sensor_info) override; + + std::vector get_state_interfaces() const override; +}; + +#endif // TEST_HARDWARE_RESOURCES__TEST_SENSOR_COMPONENT_HPP_ diff --git a/controller_manager/test/test_hardware_resources/test_sensor_hardware.cpp b/controller_manager/test/test_hardware_resources/test_sensor_hardware.cpp new file mode 100644 index 0000000000..addd1203ab --- /dev/null +++ b/controller_manager/test/test_hardware_resources/test_sensor_hardware.cpp @@ -0,0 +1,55 @@ +// 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 "./test_sensor_hardware.hpp" + +hardware_interface::return_type +TestSensorHardware::configure(const hardware_interface::HardwareInfo & sensor_info) +{ + fprintf(stderr, "configuring plugin with name %s\n", sensor_info.name.c_str()); + return hardware_interface::return_type::OK; +} + +hardware_interface::return_type +TestSensorHardware::start() +{ + return hardware_interface::return_type::OK; +} + +hardware_interface::return_type +TestSensorHardware::stop() +{ + return hardware_interface::return_type::OK; +} + +hardware_interface::hardware_interface_status +TestSensorHardware::get_status() const +{ + return hardware_interface::hardware_interface_status::UNKNOWN; +} + +hardware_interface::return_type +TestSensorHardware::read_sensors( + const std::vector> & sensors) const +{ + (void) sensors; + return hardware_interface::return_type::OK; +} + +#include "pluginlib/class_list_macros.hpp" // NOLINT + +PLUGINLIB_EXPORT_CLASS(TestSensorHardware, hardware_interface::SensorHardwareInterface) diff --git a/controller_manager/test/test_hardware_resources/test_sensor_hardware.hpp b/controller_manager/test/test_hardware_resources/test_sensor_hardware.hpp new file mode 100644 index 0000000000..23b82b7f2f --- /dev/null +++ b/controller_manager/test/test_hardware_resources/test_sensor_hardware.hpp @@ -0,0 +1,43 @@ +// 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 TEST_HARDWARE_RESOURCES__TEST_SENSOR_HARDWARE_HPP_ +#define TEST_HARDWARE_RESOURCES__TEST_SENSOR_HARDWARE_HPP_ + +#include +#include + +#include "hardware_interface/sensor_hardware_interface.hpp" + +class TestSensorHardware : public hardware_interface::SensorHardwareInterface +{ +public: + hardware_interface::return_type + configure(const hardware_interface::HardwareInfo & sensor_info) override; + + hardware_interface::return_type + start() override; + + hardware_interface::return_type + stop() override; + + hardware_interface::hardware_interface_status + get_status() const override; + + hardware_interface::return_type + read_sensors(const std::vector> & sensors) + const override; +}; + +#endif // TEST_HARDWARE_RESOURCES__TEST_SENSOR_HARDWARE_HPP_ diff --git a/controller_manager/test/test_hardware_resources/test_system_hardware.cpp b/controller_manager/test/test_hardware_resources/test_system_hardware.cpp new file mode 100644 index 0000000000..99ce9452dd --- /dev/null +++ b/controller_manager/test/test_hardware_resources/test_system_hardware.cpp @@ -0,0 +1,71 @@ +// 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 "./test_system_hardware.hpp" + +hardware_interface::return_type +TestSystemHardware::configure(const hardware_interface::HardwareInfo & system_info) +{ + fprintf(stderr, "configuring plugin with name %s\n", system_info.name.c_str()); + return hardware_interface::return_type::OK; +} + +hardware_interface::return_type +TestSystemHardware::start() +{ + return hardware_interface::return_type::OK; +} + +hardware_interface::return_type +TestSystemHardware::stop() +{ + return hardware_interface::return_type::OK; +} + +hardware_interface::hardware_interface_status +TestSystemHardware::get_status() const +{ + return hardware_interface::hardware_interface_status::UNKNOWN; +} + +hardware_interface::return_type +TestSystemHardware::read_sensors( + std::vector> & sensors) const +{ + (void) sensors; + return hardware_interface::return_type::OK; +} + +hardware_interface::return_type +TestSystemHardware::read_joints( + std::vector> & joints) const +{ + (void) joints; + return hardware_interface::return_type::OK; +} + +hardware_interface::return_type +TestSystemHardware::write_joints( + const std::vector> & joints) +{ + (void) joints; + return hardware_interface::return_type::OK; +} + +#include "pluginlib/class_list_macros.hpp" // NOLINT + +PLUGINLIB_EXPORT_CLASS(TestSystemHardware, hardware_interface::SystemHardwareInterface) diff --git a/controller_manager/test/test_hardware_resources/test_system_hardware.hpp b/controller_manager/test/test_hardware_resources/test_system_hardware.hpp new file mode 100644 index 0000000000..c2d62bd8e6 --- /dev/null +++ b/controller_manager/test/test_hardware_resources/test_system_hardware.hpp @@ -0,0 +1,51 @@ +// 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 TEST_HARDWARE_RESOURCES__TEST_SYSTEM_HARDWARE_HPP_ +#define TEST_HARDWARE_RESOURCES__TEST_SYSTEM_HARDWARE_HPP_ + +#include +#include + +#include "hardware_interface/system_hardware_interface.hpp" + +class TestSystemHardware : public hardware_interface::SystemHardwareInterface +{ +public: + hardware_interface::return_type + configure(const hardware_interface::HardwareInfo & system_info) override; + + hardware_interface::return_type + start() override; + + hardware_interface::return_type + stop() override; + + hardware_interface::hardware_interface_status + get_status() const override; + + hardware_interface::return_type + read_sensors( + std::vector> & sensors) const override; + + hardware_interface::return_type + read_joints( + std::vector> & joints) const override; + + hardware_interface::return_type + write_joints( + const std::vector> & joints) override; +}; + +#endif // TEST_HARDWARE_RESOURCES__TEST_SYSTEM_HARDWARE_HPP_ diff --git a/controller_manager/test/test_resource_manager.cpp b/controller_manager/test/test_resource_manager.cpp index c1062bc878..93d76ce0ac 100644 --- a/controller_manager/test/test_resource_manager.cpp +++ b/controller_manager/test/test_resource_manager.cpp @@ -145,6 +145,10 @@ TEST_F(TestResourceManager, initialization_empty) { TEST_F(TestResourceManager, initialization_with_urdf) { auto urdf = urdf_head_ + test_hardware_resource_system_ + urdf_tail_; controller_manager::ResourceManager rm(urdf); + + EXPECT_EQ(3u, rm.joint_components_size()); + EXPECT_EQ(1u, rm.sensor_components_size()); + EXPECT_EQ(1u, rm.actuator_interfaces_size()); EXPECT_EQ(1u, rm.sensor_interfaces_size()); EXPECT_EQ(1u, rm.system_interfaces_size()); diff --git a/hardware_interface/include/hardware_interface/components/joint.hpp b/hardware_interface/include/hardware_interface/components/joint.hpp index 0101afa6f0..be16bff8c3 100644 --- a/hardware_interface/include/hardware_interface/components/joint.hpp +++ b/hardware_interface/include/hardware_interface/components/joint.hpp @@ -84,7 +84,7 @@ class Joint * defined for the joint; return return_type::INTERFACE_NOT_PROVIDED if the list of interfaces * is empty; return_type::OK otherwise. */ - HARDWARE_INTERFACE_EXPORT + HARDWARE_INTERFACE_PUBLIC virtual return_type get_command( std::vector & command, const std::vector & interfaces) const; @@ -97,7 +97,7 @@ class Joint * \param command list of doubles with commands for the hardware. * \return return_type::OK always. */ - HARDWARE_INTERFACE_EXPORT + HARDWARE_INTERFACE_PUBLIC virtual return_type get_command(std::vector & command) const; @@ -117,7 +117,7 @@ class Joint * for different interfaces. This should be changed in the future. * (see: https://github.com/ros-controls/ros2_control/issues/129) */ - HARDWARE_INTERFACE_EXPORT + HARDWARE_INTERFACE_PUBLIC virtual return_type set_command( const std::vector & command, const std::vector & interfaces); @@ -132,7 +132,7 @@ class Joint * 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 + HARDWARE_INTERFACE_PUBLIC virtual return_type set_command(const std::vector & command); @@ -148,7 +148,7 @@ class Joint * defined for the joint; return return_type::INTERFACE_NOT_PROVIDED if the list of interfaces * is empty; return_type::OK otherwise. */ - HARDWARE_INTERFACE_EXPORT + HARDWARE_INTERFACE_PUBLIC virtual return_type get_state( std::vector & state, @@ -162,7 +162,7 @@ class Joint * \param state list of doubles with states of the hardware. * \return return_type::OK always. */ - HARDWARE_INTERFACE_EXPORT + HARDWARE_INTERFACE_PUBLIC virtual return_type get_state(std::vector & state) const; @@ -177,7 +177,7 @@ class Joint * 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 + HARDWARE_INTERFACE_PUBLIC virtual return_type set_state( const std::vector & state, const std::vector & interfaces); @@ -191,7 +191,7 @@ class Joint * \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 + HARDWARE_INTERFACE_PUBLIC virtual return_type set_state(const std::vector & state); diff --git a/hardware_interface/include/hardware_interface/components/sensor.hpp b/hardware_interface/include/hardware_interface/components/sensor.hpp index b44fb12718..f41d5f6da6 100644 --- a/hardware_interface/include/hardware_interface/components/sensor.hpp +++ b/hardware_interface/include/hardware_interface/components/sensor.hpp @@ -51,7 +51,7 @@ class Sensor */ HARDWARE_INTERFACE_PUBLIC virtual - return_type configure(const ComponentInfo & joint_info); + return_type configure(const ComponentInfo & sensor_info); /** * \brief Provide the list of state interfaces configured for the sensor. @@ -74,7 +74,7 @@ class Sensor * defined for the sensor; return return_type::INTERFACE_NOT_PROVIDED if the list of interfaces * is empty; return_type::OK otherwise. */ - HARDWARE_INTERFACE_EXPORT + HARDWARE_INTERFACE_PUBLIC virtual return_type get_state( std::vector & state, const std::vector & interfaces) const; @@ -87,7 +87,7 @@ class Sensor * \param state list of doubles with states of the hardware. * \return return_type::OK always. */ - HARDWARE_INTERFACE_EXPORT + HARDWARE_INTERFACE_PUBLIC virtual return_type get_state(std::vector & state) const; @@ -102,7 +102,7 @@ class Sensor * 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 + HARDWARE_INTERFACE_PUBLIC virtual return_type set_state( const std::vector & state, const std::vector & interfaces); @@ -116,7 +116,7 @@ class Sensor * \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 + HARDWARE_INTERFACE_PUBLIC virtual return_type set_state(const std::vector & state); From 3acf937160d69bacf6bef7a70cad0c6815c2000d Mon Sep 17 00:00:00 2001 From: Karsten Knese Date: Tue, 29 Sep 2020 11:19:59 -0700 Subject: [PATCH 07/18] review comments Signed-off-by: Karsten Knese --- controller_manager/src/resource_manager.cpp | 15 +++---- controller_manager/src/resource_manager.hpp | 2 +- .../test_actuator_hardware.cpp | 36 ++++++++-------- .../test_joint_component.cpp | 10 +++-- .../test_sensor_component.cpp | 10 +++-- .../test_sensor_hardware.cpp | 29 ++++++------- .../test_system_hardware.cpp | 41 +++++++++---------- 7 files changed, 72 insertions(+), 71 deletions(-) diff --git a/controller_manager/src/resource_manager.cpp b/controller_manager/src/resource_manager.cpp index 4ffd758666..a3efaed710 100644 --- a/controller_manager/src/resource_manager.cpp +++ b/controller_manager/src/resource_manager.cpp @@ -94,25 +94,22 @@ class ResourceStorage void initialize_actuator(const hardware_interface::HardwareInfo & hardware_info) { - using HardwareT = hardware_interface::ActuatorHardware; - using HardwareInterfaceT = hardware_interface::ActuatorHardwareInterface; - initialize_hardware( + initialize_hardware( hardware_info, actuator_loader_, actuators_); } void initialize_sensor(const hardware_interface::HardwareInfo & hardware_info) { - using HardwareT = hardware_interface::SensorHardware; - using HardwareInterfaceT = hardware_interface::SensorHardwareInterface; - initialize_hardware( + initialize_hardware( hardware_info, sensor_loader_, sensors_); } void initialize_system(const hardware_interface::HardwareInfo & hardware_info) { - using HardwareT = hardware_interface::SystemHardware; - using HardwareInterfaceT = hardware_interface::SystemHardwareInterface; - initialize_hardware( + initialize_hardware( hardware_info, system_loader_, systems_); } diff --git a/controller_manager/src/resource_manager.hpp b/controller_manager/src/resource_manager.hpp index 21c991680a..3646317619 100644 --- a/controller_manager/src/resource_manager.hpp +++ b/controller_manager/src/resource_manager.hpp @@ -32,7 +32,7 @@ class ResourceManager ResourceManager(const ResourceManager &) = delete; - virtual ~ResourceManager(); + ~ResourceManager(); size_t joint_components_size() const; diff --git a/controller_manager/test/test_hardware_resources/test_actuator_hardware.cpp b/controller_manager/test/test_hardware_resources/test_actuator_hardware.cpp index f2f3616d6f..68c6a30481 100644 --- a/controller_manager/test/test_hardware_resources/test_actuator_hardware.cpp +++ b/controller_manager/test/test_hardware_resources/test_actuator_hardware.cpp @@ -16,45 +16,45 @@ #include "./test_actuator_hardware.hpp" -hardware_interface::return_type -TestActuatorHardware::configure(const hardware_interface::HardwareInfo & actuator_info) +using hardware_interface::hardware_interface_status; +using hardware_interface::return_type; + +return_type +TestActuatorHardware::configure(const hardware_interface::HardwareInfo & /* actuator_info */) { - fprintf(stderr, "configuring plugin with name %s\n", actuator_info.name.c_str()); - return hardware_interface::return_type::OK; + return return_type::OK; } -hardware_interface::return_type +return_type TestActuatorHardware::start() { - return hardware_interface::return_type::OK; + return return_type::OK; } -hardware_interface::return_type +return_type TestActuatorHardware::stop() { - return hardware_interface::return_type::OK; + return return_type::OK; } -hardware_interface::hardware_interface_status +hardware_interface_status TestActuatorHardware::get_status() const { - return hardware_interface::hardware_interface_status::UNKNOWN; + return hardware_interface_status::UNKNOWN; } -hardware_interface::return_type +return_type TestActuatorHardware::read_joint( - std::shared_ptr joint) const + std::shared_ptr/* joint */) const { - (void) joint; - return hardware_interface::return_type::OK; + return return_type::OK; } -hardware_interface::return_type +return_type TestActuatorHardware::write_joint( - const std::shared_ptr joint) + const std::shared_ptr/* joint */) { - (void) joint; - return hardware_interface::return_type::OK; + return return_type::OK; } #include "pluginlib/class_list_macros.hpp" // NOLINT diff --git a/controller_manager/test/test_hardware_resources/test_joint_component.cpp b/controller_manager/test/test_hardware_resources/test_joint_component.cpp index 4e5661d808..3ab5858eb0 100644 --- a/controller_manager/test/test_hardware_resources/test_joint_component.cpp +++ b/controller_manager/test/test_hardware_resources/test_joint_component.cpp @@ -18,11 +18,13 @@ #include "./test_joint_component.hpp" -hardware_interface::return_type -TestJointComponent::configure(const hardware_interface::components::ComponentInfo & joint_info) +using hardware_interface::return_type; + +return_type +TestJointComponent::configure( + const hardware_interface::components::ComponentInfo & /* joint_info */) { - fprintf(stderr, "configuring plugin with name %s\n", joint_info.name.c_str()); - return hardware_interface::return_type::OK; + return return_type::OK; } std::vector diff --git a/controller_manager/test/test_hardware_resources/test_sensor_component.cpp b/controller_manager/test/test_hardware_resources/test_sensor_component.cpp index d614e91b65..7ee7c6f0c6 100644 --- a/controller_manager/test/test_hardware_resources/test_sensor_component.cpp +++ b/controller_manager/test/test_hardware_resources/test_sensor_component.cpp @@ -18,11 +18,13 @@ #include "./test_sensor_component.hpp" -hardware_interface::return_type -TestSensorComponent::configure(const hardware_interface::components::ComponentInfo & sensor_info) +using hardware_interface::return_type; + +return_type +TestSensorComponent::configure( + const hardware_interface::components::ComponentInfo & /* sensor_info */) { - fprintf(stderr, "configuring plugin with name %s\n", sensor_info.name.c_str()); - return hardware_interface::return_type::OK; + return return_type::OK; } std::vector diff --git a/controller_manager/test/test_hardware_resources/test_sensor_hardware.cpp b/controller_manager/test/test_hardware_resources/test_sensor_hardware.cpp index addd1203ab..b418c5b896 100644 --- a/controller_manager/test/test_hardware_resources/test_sensor_hardware.cpp +++ b/controller_manager/test/test_hardware_resources/test_sensor_hardware.cpp @@ -17,37 +17,38 @@ #include "./test_sensor_hardware.hpp" -hardware_interface::return_type -TestSensorHardware::configure(const hardware_interface::HardwareInfo & sensor_info) +using hardware_interface::hardware_interface_status; +using hardware_interface::return_type; + +return_type +TestSensorHardware::configure(const hardware_interface::HardwareInfo & /* sensor_info */) { - fprintf(stderr, "configuring plugin with name %s\n", sensor_info.name.c_str()); - return hardware_interface::return_type::OK; + return return_type::OK; } -hardware_interface::return_type +return_type TestSensorHardware::start() { - return hardware_interface::return_type::OK; + return return_type::OK; } -hardware_interface::return_type +return_type TestSensorHardware::stop() { - return hardware_interface::return_type::OK; + return return_type::OK; } -hardware_interface::hardware_interface_status +hardware_interface_status TestSensorHardware::get_status() const { - return hardware_interface::hardware_interface_status::UNKNOWN; + return hardware_interface_status::UNKNOWN; } -hardware_interface::return_type +return_type TestSensorHardware::read_sensors( - const std::vector> & sensors) const + const std::vector> & /* sensors */) const { - (void) sensors; - return hardware_interface::return_type::OK; + return return_type::OK; } #include "pluginlib/class_list_macros.hpp" // NOLINT diff --git a/controller_manager/test/test_hardware_resources/test_system_hardware.cpp b/controller_manager/test/test_hardware_resources/test_system_hardware.cpp index 99ce9452dd..e69efdd4e5 100644 --- a/controller_manager/test/test_hardware_resources/test_system_hardware.cpp +++ b/controller_manager/test/test_hardware_resources/test_system_hardware.cpp @@ -17,53 +17,52 @@ #include "./test_system_hardware.hpp" +using hardware_interface::hardware_interface_status; +using hardware_interface::return_type; + hardware_interface::return_type -TestSystemHardware::configure(const hardware_interface::HardwareInfo & system_info) +TestSystemHardware::configure(const hardware_interface::HardwareInfo & /* system_info */) { - fprintf(stderr, "configuring plugin with name %s\n", system_info.name.c_str()); - return hardware_interface::return_type::OK; + return return_type::OK; } -hardware_interface::return_type +return_type TestSystemHardware::start() { - return hardware_interface::return_type::OK; + return return_type::OK; } -hardware_interface::return_type +return_type TestSystemHardware::stop() { - return hardware_interface::return_type::OK; + return return_type::OK; } -hardware_interface::hardware_interface_status +hardware_interface_status TestSystemHardware::get_status() const { - return hardware_interface::hardware_interface_status::UNKNOWN; + return hardware_interface_status::UNKNOWN; } -hardware_interface::return_type +return_type TestSystemHardware::read_sensors( - std::vector> & sensors) const + std::vector> & /* sensors */) const { - (void) sensors; - return hardware_interface::return_type::OK; + return return_type::OK; } -hardware_interface::return_type +return_type TestSystemHardware::read_joints( - std::vector> & joints) const + std::vector> & /* joints */) const { - (void) joints; - return hardware_interface::return_type::OK; + return return_type::OK; } -hardware_interface::return_type +return_type TestSystemHardware::write_joints( - const std::vector> & joints) + const std::vector> & /* joints */) { - (void) joints; - return hardware_interface::return_type::OK; + return return_type::OK; } #include "pluginlib/class_list_macros.hpp" // NOLINT From 6d5f918d17271e7b0995e665b79e8361fbfbf1a6 Mon Sep 17 00:00:00 2001 From: Karsten Knese Date: Tue, 29 Sep 2020 15:35:27 -0700 Subject: [PATCH 08/18] introduce component interfaces Signed-off-by: Karsten Knese --- .../hardware_interface/components/joint.hpp | 3 +- .../components/joint_interface.hpp | 95 +++++++++++++++++++ .../hardware_interface/components/sensor.hpp | 3 +- .../components/sensor_interface.hpp | 71 ++++++++++++++ 4 files changed, 170 insertions(+), 2 deletions(-) create mode 100644 hardware_interface/include/hardware_interface/components/joint_interface.hpp create mode 100644 hardware_interface/include/hardware_interface/components/sensor_interface.hpp diff --git a/hardware_interface/include/hardware_interface/components/joint.hpp b/hardware_interface/include/hardware_interface/components/joint.hpp index be16bff8c3..6b76464fdb 100644 --- a/hardware_interface/include/hardware_interface/components/joint.hpp +++ b/hardware_interface/include/hardware_interface/components/joint.hpp @@ -19,6 +19,7 @@ #include #include "hardware_interface/components/component_info.hpp" +#include "hardware_interface/components/joint_interface.hpp" #include "hardware_interface/types/hardware_interface_return_values.hpp" #include "hardware_interface/visibility_control.h" @@ -32,7 +33,7 @@ namespace components * A joint is always 1-DoF and can have one or more interfaces (e.g., position, velocity, etc.) * A joint has to be able to receive command(s) and optionally can provide its state(s). */ -class Joint +class Joint : public JointInterface { public: HARDWARE_INTERFACE_PUBLIC diff --git a/hardware_interface/include/hardware_interface/components/joint_interface.hpp b/hardware_interface/include/hardware_interface/components/joint_interface.hpp new file mode 100644 index 0000000000..3d6b8d1f4d --- /dev/null +++ b/hardware_interface/include/hardware_interface/components/joint_interface.hpp @@ -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. + +#ifndef HARDWARE_INTERFACE__COMPONENTS__JOINT_INTERFACE_HPP_ +#define HARDWARE_INTERFACE__COMPONENTS__JOINT_INTERFACE_HPP_ + +#include +#include + +#include "hardware_interface/components/component_info.hpp" +#include "hardware_interface/types/hardware_interface_return_values.hpp" +#include "hardware_interface/visibility_control.h" + +namespace hardware_interface +{ +namespace components +{ + +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::vector get_command_interfaces() const = 0; + + HARDWARE_INTERFACE_PUBLIC + virtual + std::vector get_state_interfaces() const = 0; + + HARDWARE_INTERFACE_PUBLIC + virtual + return_type get_command( + std::vector & command, + const std::vector & interfaces) const = 0; + + HARDWARE_INTERFACE_PUBLIC + virtual + return_type get_command(std::vector & command) const = 0; + + HARDWARE_INTERFACE_PUBLIC + virtual + return_type set_command( + const std::vector & command, + const std::vector & interfaces) = 0; + + HARDWARE_INTERFACE_PUBLIC + virtual + return_type set_command(const std::vector & command) = 0; + + HARDWARE_INTERFACE_PUBLIC + virtual + return_type get_state( + std::vector & state, + const std::vector & interfaces) const = 0; + + HARDWARE_INTERFACE_PUBLIC + virtual + return_type get_state(std::vector & state) const = 0; + + HARDWARE_INTERFACE_PUBLIC + virtual + return_type set_state( + const std::vector & state, + const std::vector & interfaces) = 0; + + HARDWARE_INTERFACE_PUBLIC + virtual + return_type set_state(const std::vector & state) = 0; +}; + +} // namespace components +} // namespace hardware_interface +#endif // HARDWARE_INTERFACE__COMPONENTS__JOINT_INTERFACE_HPP_ diff --git a/hardware_interface/include/hardware_interface/components/sensor.hpp b/hardware_interface/include/hardware_interface/components/sensor.hpp index f41d5f6da6..0046fd505c 100644 --- a/hardware_interface/include/hardware_interface/components/sensor.hpp +++ b/hardware_interface/include/hardware_interface/components/sensor.hpp @@ -19,6 +19,7 @@ #include #include "hardware_interface/components/component_info.hpp" +#include "hardware_interface/components/sensor_interface.hpp" #include "hardware_interface/types/hardware_interface_return_values.hpp" #include "hardware_interface/visibility_control.h" @@ -32,7 +33,7 @@ namespace components * 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 +class Sensor : public SensorInterface { public: HARDWARE_INTERFACE_PUBLIC diff --git a/hardware_interface/include/hardware_interface/components/sensor_interface.hpp b/hardware_interface/include/hardware_interface/components/sensor_interface.hpp new file mode 100644 index 0000000000..2c04235f41 --- /dev/null +++ b/hardware_interface/include/hardware_interface/components/sensor_interface.hpp @@ -0,0 +1,71 @@ +// Copyright 2020 ros2_control Development Team +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef HARDWARE_INTERFACE__COMPONENTS__SENSOR_INTERFACE_HPP_ +#define HARDWARE_INTERFACE__COMPONENTS__SENSOR_INTERFACE_HPP_ + +#include +#include + +#include "hardware_interface/components/component_info.hpp" +#include "hardware_interface/types/hardware_interface_return_values.hpp" +#include "hardware_interface/visibility_control.h" + +namespace hardware_interface +{ +namespace components +{ + +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::vector get_state_interfaces() const = 0; + + HARDWARE_INTERFACE_PUBLIC + virtual + return_type get_state( + std::vector & state, + const std::vector & interfaces) const = 0; + + HARDWARE_INTERFACE_PUBLIC + virtual + return_type get_state(std::vector & state) const = 0; + + HARDWARE_INTERFACE_PUBLIC + virtual + return_type set_state( + const std::vector & state, + const std::vector & interfaces) = 0; + + HARDWARE_INTERFACE_PUBLIC + virtual + return_type set_state(const std::vector & state) = 0; +}; + +} // namespace components +} // namespace hardware_interface +#endif // HARDWARE_INTERFACE__COMPONENTS__SENSOR_INTERFACE_HPP_ From 76ca4ce8a8a80053fffe313d5fb4690660f94c53 Mon Sep 17 00:00:00 2001 From: Karsten Knese Date: Tue, 29 Sep 2020 18:23:15 -0700 Subject: [PATCH 09/18] wip make components pure interfaces Signed-off-by: Karsten Knese --- .../hardware_interface/components/joint.hpp | 133 +------ .../components/joint_interface.hpp | 122 +++++- hardware_interface/src/components/joint.cpp | 31 +- .../test/test_component_interfaces.cpp | 348 ++++++++++++++---- 4 files changed, 399 insertions(+), 235 deletions(-) diff --git a/hardware_interface/include/hardware_interface/components/joint.hpp b/hardware_interface/include/hardware_interface/components/joint.hpp index 6b76464fdb..d764c50d6e 100644 --- a/hardware_interface/include/hardware_interface/components/joint.hpp +++ b/hardware_interface/include/hardware_interface/components/joint.hpp @@ -28,178 +28,59 @@ namespace hardware_interface namespace components { -/** - * \brief Base Class for the "Joint" component used as a basic building block for a robot. - * A joint is always 1-DoF and can have one or more interfaces (e.g., position, velocity, etc.) - * A joint has to be able to receive command(s) and optionally can provide its state(s). - */ -class Joint : public JointInterface +class Joint final { public: HARDWARE_INTERFACE_PUBLIC Joint() = default; + explicit Joint(Joint && other) = default; + + explicit Joint(std::unique_ptr impl); + HARDWARE_INTERFACE_PUBLIC - virtual ~Joint() = default; - /** - * \brief Configure base joint class based on the description in the robot's URDF file. - * - * \param joint_info structure with data from URDF. - * \return return_type::OK if required data are provided and is successfully parsed, - * return_type::ERROR otherwise. - */ HARDWARE_INTERFACE_PUBLIC - virtual return_type configure(const ComponentInfo & joint_info); - /** - * \brief Provide the list of command interfaces configured for the joint. - * - * \return string list with command interfaces. - */ HARDWARE_INTERFACE_PUBLIC - virtual std::vector get_command_interfaces() const; - /** - * \brief Provide the list of state interfaces configured for the joint. - * - * \return string list with state interfaces. - */ HARDWARE_INTERFACE_PUBLIC - virtual std::vector get_state_interfaces() const; - /** - * \brief Get command list from the joint. This function is used in the write function of the - * actuator or system hardware. The parameters command and interfaces have the same order, - * and number of elements. Using the interfaces list, the hardware can choose which values to - * provide. - * - * \param command list of doubles with commands for the hardware. - * \param interfaces list of interfaces on which commands have to set. - * \return return_type::INTERFACE_VALUE_SIZE_NOT_EQUAL if command and interfaces arguments do not - * have the same length; return_type::INTERFACE_NOT_FOUND if one of provided interfaces is not - * defined for the joint; return return_type::INTERFACE_NOT_PROVIDED if the list of interfaces - * is empty; return_type::OK otherwise. - */ HARDWARE_INTERFACE_PUBLIC - virtual return_type get_command( std::vector & command, const std::vector & interfaces) const; - /** - * \brief Get complete command list for the joint. This function is used by the hardware to get - * complete command for it. The hardware valus have the same order as interfaces which - * can be recived by get_hardware_interfaces() function. Return value is used for API consistency. - * - * \param command list of doubles with commands for the hardware. - * \return return_type::OK always. - */ HARDWARE_INTERFACE_PUBLIC - virtual return_type get_command(std::vector & command) const; - /** - * \brief Set command list for the joint. This function is used by the controller to set the goal - * values for the hardware. The parameters command, and interfaces have the same order and number - * of elements. Using the interfaces list, the controller can choose which values to set. - * - * \param command list of doubles with commands for the hardware. - * \param interfaces list of interfaces on which commands have to be provided. - * \return return_type::INTERFACE_VALUE_SIZE_NOT_EQUAL if command and interfaces arguments do not - * have the same length; return_type::COMMAND_OUT_OF_LIMITS if one of the command values is out - * of limits; return_type::INTERFACE_NOT_FOUND if one of provided interfaces is not - * defined for the joint; return_type::OK otherwise. - * - * \todo The error handling in this function could lead to incosistant command or state variables - * for different interfaces. This should be changed in the future. - * (see: https://github.com/ros-controls/ros2_control/issues/129) - */ HARDWARE_INTERFACE_PUBLIC - virtual return_type set_command( const std::vector & command, const std::vector & interfaces); - /** - * \brief Get complete state list from the joint. This function is used by the hardware to get - * complete command for it. The hardware valus have the same order as interfaces which - * can be recived by get_hardware_interfaces() function. - * - * \param command list of doubles with commands for the hardware. - * \return return_type::INTERFACE_VALUE_SIZE_NOT_EQUAL is command size is not equal to number of - * joint's command interfaces; return_type::COMMAND_OUT_OF_LIMITS if one of the command values is out - * of limits; return_type::OK otherwise. - */ HARDWARE_INTERFACE_PUBLIC - virtual return_type set_command(const std::vector & command); - /** - * \brief Get state list from the joint. This function is used by the controller to get the - * actual state of the hardware. The parameters state, and interfaces have the same order and - * number of elements. Using the interfaces list, the controller can choose which values to get. - * - * \param state list of doubles with states of the hardware. - * \param interfaces list of interfaces on which states have to be provided. - * \return return_type::INTERFACE_VALUE_SIZE_NOT_EQUAL if state and interfaces arguments do not - * have the same length; return_type::INTERFACE_NOT_FOUND if one of provided interfaces is not - * defined for the joint; return return_type::INTERFACE_NOT_PROVIDED if the list of interfaces - * is empty; return_type::OK otherwise. - */ HARDWARE_INTERFACE_PUBLIC - virtual return_type get_state( std::vector & state, const std::vector & interfaces) const; - /** - * \brief Get complete state list from the joint. This function is used by the controller to get - * complete actual state of the hardware. The state values have the same order as interfaces which - * can be recived by get_state_interfaces() function. Return value is used for API consistency. - * - * \param state list of doubles with states of the hardware. - * \return return_type::OK always. - */ HARDWARE_INTERFACE_PUBLIC - virtual return_type get_state(std::vector & state) const; - /** - * \brief Set state list for the joint. This function is used by the hardware to set its actual - * state. The parameters state, and interfaces have the same order and number of elements. Using - * the interfaces list, the hardware can choose which values to set. - * - * \param state list of doubles with states of the hardware. - * \param interfaces list of interfaces on which states have to be provided. - * \return return_type::INTERFACE_VALUE_SIZE_NOT_EQUAL if state and interfaces arguments do not - * have the same length; return_type::INTERFACE_NOT_FOUND if one of provided interfaces is not - * defined for the joint; return_type::OK otherwise. - */ HARDWARE_INTERFACE_PUBLIC - virtual return_type set_state( const std::vector & state, const std::vector & interfaces); - /** - * \brief Set complete state list from the joint.This function is used by the hardware to set its - * complete actual state. The state values have the same order as interfaces which can be recived - * by get_state_interfaces() function. - * - * \param state list of doubles with states from the hardware. - * \return return_type::INTERFACE_VALUE_SIZE_NOT_EQUAL is command size is not equal to number of - * joint's state interfaces, return_type::OK otherwise. - */ HARDWARE_INTERFACE_PUBLIC - virtual return_type set_state(const std::vector & state); -protected: - ComponentInfo info_; - std::vector commands_; - std::vector states_; +private: + std::unique_ptr impl_; }; } // namespace components diff --git a/hardware_interface/include/hardware_interface/components/joint_interface.hpp b/hardware_interface/include/hardware_interface/components/joint_interface.hpp index 3d6b8d1f4d..5a9934874a 100644 --- a/hardware_interface/include/hardware_interface/components/joint_interface.hpp +++ b/hardware_interface/include/hardware_interface/components/joint_interface.hpp @@ -27,65 +27,161 @@ namespace hardware_interface namespace components { +/** + * \brief Interface for the "Joint" component used as a basic building block for a robot. + * A joint is always 1-DoF and can have one or more interfaces (e.g., position, velocity, etc.) + * A joint has to be able to receive command(s) and optionally can provide its state(s). + */ class JointInterface { public: - HARDWARE_INTERFACE_PUBLIC JointInterface() = default; - HARDWARE_INTERFACE_PUBLIC virtual ~JointInterface() = default; - HARDWARE_INTERFACE_PUBLIC + /** + * \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 return_type::ERROR otherwise. + */ virtual return_type configure(const ComponentInfo & joint_info) = 0; - HARDWARE_INTERFACE_PUBLIC + /** + * \brief Provide the list of command interfaces configured for the joint. + * + * \return string list with command interfaces. + */ virtual std::vector get_command_interfaces() const = 0; - HARDWARE_INTERFACE_PUBLIC + /** + * \brief Provide the list of state interfaces configured for the joint. + * + * \return string list with state interfaces. + */ virtual std::vector get_state_interfaces() const = 0; - HARDWARE_INTERFACE_PUBLIC + /** + * \brief Get command list from the joint. This function is used in the write function of the + * actuator or system hardware. The parameters command and interfaces have the same order, + * and number of elements. Using the interfaces list, the hardware can choose which values to + * provide. + * + * \param command list of doubles with commands for the hardware. + * \param interfaces list of interfaces on which commands have to set. + * \return return_type::INTERFACE_VALUE_SIZE_NOT_EQUAL if command and interfaces arguments do not + * have the same length; return_type::INTERFACE_NOT_FOUND if one of provided interfaces is not + * defined for the joint; return return_type::INTERFACE_NOT_PROVIDED if the list of interfaces + * is empty; return_type::OK otherwise. + */ virtual return_type get_command( std::vector & command, const std::vector & interfaces) const = 0; - HARDWARE_INTERFACE_PUBLIC + /** + * \brief Get complete command list for the joint. This function is used by the hardware to get + * complete command for it. The hardware valus have the same order as interfaces which + * can be recived by get_hardware_interfaces() function. Return value is used for API consistency. + * + * \param command list of doubles with commands for the hardware. + * \return return_type::OK always. + */ virtual return_type get_command(std::vector & command) const = 0; - HARDWARE_INTERFACE_PUBLIC + /** + * \brief Set command list for the joint. This function is used by the controller to set the goal + * values for the hardware. The parameters command, and interfaces have the same order and number + * of elements. Using the interfaces list, the controller can choose which values to set. + * + * \param command list of doubles with commands for the hardware. + * \param interfaces list of interfaces on which commands have to be provided. + * \return return_type::INTERFACE_VALUE_SIZE_NOT_EQUAL if command and interfaces arguments do not + * have the same length; return_type::COMMAND_OUT_OF_LIMITS if one of the command values is out + * of limits; return_type::INTERFACE_NOT_FOUND if one of provided interfaces is not + * defined for the joint; return_type::OK otherwise. + * + * \todo The error handling in this function could lead to incosistant command or state variables + * for different interfaces. This should be changed in the future. + * (see: https://github.com/ros-controls/ros2_control/issues/129) + */ virtual return_type set_command( const std::vector & command, const std::vector & interfaces) = 0; - HARDWARE_INTERFACE_PUBLIC + /** + * \brief Set 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. + */ virtual return_type set_command(const std::vector & command) = 0; - HARDWARE_INTERFACE_PUBLIC + /** + * \brief Get state list from the joint. This function is used by the controller to get the + * actual state of the hardware. The parameters state, and interfaces have the same order and + * number of elements. Using the interfaces list, the controller can choose which values to get. + * + * \param state list of doubles with states of the hardware. + * \param interfaces list of interfaces on which states have to be provided. + * \return return_type::INTERFACE_VALUE_SIZE_NOT_EQUAL if state and interfaces arguments do not + * have the same length; return_type::INTERFACE_NOT_FOUND if one of provided interfaces is not + * defined for the joint; return return_type::INTERFACE_NOT_PROVIDED if the list of interfaces + * is empty; return_type::OK otherwise. + */ virtual return_type get_state( std::vector & state, const std::vector & interfaces) const = 0; - HARDWARE_INTERFACE_PUBLIC + /** + * \brief Get complete state list from the joint. This function is used by the controller to get + * complete actual state of the hardware. The state values have the same order as interfaces which + * can be recived by get_state_interfaces() function. Return value is used for API consistency. + * + * \param state list of doubles with states of the hardware. + * \return return_type::OK always. + */ virtual return_type get_state(std::vector & state) const = 0; - HARDWARE_INTERFACE_PUBLIC + /** + * \brief Set state list for the joint. This function is used by the hardware to set its actual + * state. The parameters state, and interfaces have the same order and number of elements. Using + * the interfaces list, the hardware can choose which values to set. + * + * \param state list of doubles with states of the hardware. + * \param interfaces list of interfaces on which states have to be provided. + * \return return_type::INTERFACE_VALUE_SIZE_NOT_EQUAL if state and interfaces arguments do not + * have the same length; return_type::INTERFACE_NOT_FOUND if one of provided interfaces is not + * defined for the joint; return_type::OK otherwise. + */ virtual return_type set_state( const std::vector & state, const std::vector & interfaces) = 0; - HARDWARE_INTERFACE_PUBLIC + /** + * \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. + */ virtual return_type set_state(const std::vector & state) = 0; }; diff --git a/hardware_interface/src/components/joint.cpp b/hardware_interface/src/components/joint.cpp index 8aaa0c661d..59d1fd6deb 100644 --- a/hardware_interface/src/components/joint.cpp +++ b/hardware_interface/src/components/joint.cpp @@ -19,8 +19,6 @@ #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 @@ -28,68 +26,61 @@ namespace components return_type Joint::configure(const ComponentInfo & joint_info) { - info_ = joint_info; - if (info_.command_interfaces.size() > 0) { - commands_.resize(info_.command_interfaces.size()); - } - if (info_.state_interfaces.size() > 0) { - states_.resize(info_.state_interfaces.size()); - } - return return_type::OK; + return impl_->configure(joint_info); } std::vector Joint::get_command_interfaces() const { - return info_.command_interfaces; + return impl_->get_command_interfaces(); } std::vector Joint::get_state_interfaces() const { - return info_.state_interfaces; + return impl_->get_state_interfaces(); } return_type Joint::get_command( std::vector & command, const std::vector & interfaces) const { - return get_internal_values(command, interfaces, info_.command_interfaces, commands_); + return impl_->get_command(command, interfaces); } return_type Joint::get_command(std::vector & command) const { - return get_internal_values(command, commands_); + return impl_->get_command(command); } return_type Joint::set_command( const std::vector & command, const std::vector & interfaces) { - return set_internal_values(command, interfaces, info_.command_interfaces, commands_); + return impl_->set_command(command, interfaces); } return_type Joint::set_command(const std::vector & command) { - return set_internal_values(command, commands_); + return impl_->set_command(command); } return_type Joint::get_state( std::vector & state, const std::vector & interfaces) const { - return get_internal_values(state, interfaces, info_.state_interfaces, states_); + return impl_->get_state(state, interfaces); } return_type Joint::get_state(std::vector & state) const { - return get_internal_values(state, states_); + return impl_->get_state(state); } return_type Joint::set_state( const std::vector & state, const std::vector & interfaces) { - return set_internal_values(state, interfaces, info_.state_interfaces, states_); + return impl_->set_state(state, interfaces); } return_type Joint::set_state(const std::vector & state) { - return set_internal_values(state, states_); + return impl_->set_state(state); } } // namespace components diff --git a/hardware_interface/test/test_component_interfaces.cpp b/hardware_interface/test/test_component_interfaces.cpp index 0526be9bf0..47fea721ea 100644 --- a/hardware_interface/test/test_component_interfaces.cpp +++ b/hardware_interface/test/test_component_interfaces.cpp @@ -40,44 +40,126 @@ namespace hardware_interface namespace hardware_interfaces_components_test { -class DummyPositionJoint : public components::Joint +class DummyPositionJoint : public components::JointInterface { public: return_type configure(const components::ComponentInfo & joint_info) { - if (Joint::configure(joint_info) != return_type::OK) { - return return_type::ERROR; - } + 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(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 + { + return {HW_IF_POSITION}; + } + + std::vector get_state_interfaces() const + { + return {HW_IF_POSITION}; + } + + return_type get_command( + std::vector & command, + const std::vector & interfaces) const + { + if (command.size() != 1u) { return return_type::ERROR; } + if (interfaces.size() != 1u) { return return_type::ERROR; } + if (interfaces[0] != HW_IF_POSITION) { return return_type::ERROR; } + + command[0] = command_; + return return_type::OK; + } + + return_type get_command(std::vector & command) const + { + if (command.size() != 1u) { return return_type::ERROR; } + + command[0] = command_; + return return_type::OK; + } + + return_type set_command( + const std::vector & command, + const std::vector & interfaces) + { + if (command.size() != 1u) { return return_type::ERROR; } + if (interfaces.size() != 1u) { return return_type::ERROR; } + if (interfaces[0] != HW_IF_POSITION) { return return_type::ERROR; } + + command_ = command[0]; + return return_type::OK; + } + + return_type set_command(const std::vector & command) + { + if (command.size() != 1u) { return return_type::ERROR; } + + command_ = command[0]; + return return_type::OK; + } + + return_type get_state( + std::vector & state, + const std::vector & interfaces) const + { + if (state.size() != 1u) { return return_type::ERROR; } + if (interfaces.size() != 1u) { return return_type::ERROR; } + if (interfaces[0] != HW_IF_POSITION) { return return_type::ERROR; } + + state[0] = state_; + return return_type::OK; + } + + return_type get_state(std::vector & state) const + { + if (state.size() != 1u) { return return_type::ERROR; } + + state[0] = state_; + return return_type::OK; + } + + return_type set_state( + const std::vector & state, + const std::vector & interfaces) + { + if (state.size() != 1u) { return return_type::ERROR; } + if (interfaces.size() != 1u) { return return_type::ERROR; } + if (interfaces[0] != HW_IF_POSITION) { return return_type::ERROR; } + + state_ = state[0]; + return return_type::OK; + } + + return_type set_state(const std::vector & state) + { + if (state.size() != 1u) { return return_type::ERROR; } + + state_ = state[0]; + return return_type::OK; + } + private: + double command_ = 0.0; + double state_ = 0.0; + + components::ComponentInfo info_; double max_position_, min_position_; }; -class DummyMultiJoint : public components::Joint +class DummyMultiJoint : public components::JointInterface { public: return_type configure(const components::ComponentInfo & joint_info) { - if (Joint::configure(joint_info) != return_type::OK) { - return return_type::ERROR; - } + info_ = joint_info; if (info_.command_interfaces.size() < 2) { return return_type::ERROR; @@ -90,7 +172,119 @@ class DummyMultiJoint : public components::Joint return return_type::OK; } + std::vector get_command_interfaces() const + { + return {HW_IF_POSITION, HW_IF_VELOCITY}; + } + + std::vector get_state_interfaces() const + { + return {HW_IF_POSITION, HW_IF_VELOCITY}; + } + + return_type get_command( + std::vector & command, + const std::vector & interfaces) const + { + if (command.size() != 2u) { return return_type::ERROR; } + if (interfaces.size() != 2u) { return return_type::ERROR; } + if (interfaces[0] != HW_IF_POSITION && interfaces[1] != HW_IF_VELOCITY) { + return return_type::ERROR; + } + + command[0] = command_pos_; + command[1] = command_vel_; + return return_type::OK; + } + + return_type get_command(std::vector & command) const + { + if (command.size() != 2u) { return return_type::ERROR; } + + command[0] = command_pos_; + command[1] = command_vel_; + return return_type::OK; + } + + return_type set_command( + const std::vector & command, + const std::vector & interfaces) + { + if (command.size() != 2u) { return return_type::ERROR; } + if (interfaces.size() != 2u) { return return_type::ERROR; } + if (interfaces[0] != HW_IF_POSITION && interfaces[1] != HW_IF_VELOCITY) { + return return_type::ERROR; + } + + command_pos_ = command[0]; + command_vel_ = command[1]; + return return_type::OK; + } + + return_type set_command(const std::vector & command) + { + if (command.size() != 1u) { return return_type::ERROR; } + + command_pos_ = command[0]; + command_vel_ = command[1]; + return return_type::OK; + } + + return_type get_state( + std::vector & state, + const std::vector & interfaces) const + { + if (state.size() != 2u) { return return_type::ERROR; } + if (interfaces.size() != 2u) { return return_type::ERROR; } + if (interfaces[0] != HW_IF_POSITION && interfaces[1] != HW_IF_VELOCITY) { + return return_type::ERROR; + } + + state[0] = state_pos_; + state[1] = state_vel_; + return return_type::OK; + } + + return_type get_state(std::vector & state) const + { + if (state.size() != 2u) { return return_type::ERROR; } + + state[0] = state_pos_; + state[1] = state_vel_; + return return_type::OK; + } + + return_type set_state( + const std::vector & state, + const std::vector & interfaces) + { + if (state.size() != 2u) { return return_type::ERROR; } + if (interfaces.size() != 2u) { return return_type::ERROR; } + if (interfaces[0] != HW_IF_POSITION && interfaces[1] != HW_IF_VELOCITY) { + return return_type::ERROR; + } + + state_pos_ = state[0]; + state_vel_ = state[1]; + return return_type::OK; + } + + return_type set_state(const std::vector & state) + { + if (state.size() != 1u) { return return_type::ERROR; } + + state_pos_ = state[0]; + state_vel_ = state[1]; + return return_type::OK; + } + private: + double command_pos_ = 0.0; + double command_vel_ = 0.0; + double state_pos_ = 0.0; + double state_vel_ = 0.0; + + components::ComponentInfo info_; double max_position_, min_position_; double max_velocity_, min_velocity_; }; @@ -389,68 +583,69 @@ TEST_F(TestComponentInterfaces, joint_example_component_works) input.clear(); input.push_back(1.2); EXPECT_EQ(joint.set_command(input, interfaces), return_type::OK); - - std::vector output; - interfaces.clear(); - EXPECT_EQ(joint.get_command(output, interfaces), return_type::INTERFACE_NOT_PROVIDED); - interfaces.push_back(hardware_interface::HW_IF_POSITION); - EXPECT_EQ(joint.get_command(output, interfaces), return_type::OK); - ASSERT_THAT(output, SizeIs(1)); - EXPECT_EQ(output[0], 1.2); - interfaces.clear(); - interfaces.push_back(hardware_interface::HW_IF_VELOCITY); - EXPECT_EQ(joint.get_command(output, interfaces), return_type::INTERFACE_NOT_FOUND); - - input.clear(); - EXPECT_EQ(joint.set_command(input), return_type::INTERFACE_VALUE_SIZE_NOT_EQUAL); - input.push_back(2.1); - EXPECT_EQ(joint.set_command(input), return_type::OK); - - EXPECT_EQ(joint.get_command(output), return_type::OK); - ASSERT_THAT(output, SizeIs(1)); - EXPECT_EQ(output[0], 2.1); - - // State getters and setters - interfaces.clear(); - input.clear(); - input.push_back(2.1); - EXPECT_EQ(joint.set_state(input, interfaces), return_type::INTERFACE_NOT_PROVIDED); - interfaces.push_back(hardware_interface::HW_IF_VELOCITY); - EXPECT_EQ(joint.set_state(input, interfaces), return_type::INTERFACE_NOT_FOUND); - interfaces.push_back(hardware_interface::HW_IF_POSITION); - EXPECT_EQ(joint.set_state(input, interfaces), return_type::INTERFACE_VALUE_SIZE_NOT_EQUAL); - interfaces.clear(); - interfaces.push_back(hardware_interface::HW_IF_POSITION); - input.clear(); - input.push_back(1.2); - EXPECT_EQ(joint.set_state(input, interfaces), return_type::OK); - - output.clear(); - interfaces.clear(); - EXPECT_EQ(joint.get_command(output, interfaces), return_type::INTERFACE_NOT_PROVIDED); - interfaces.push_back(hardware_interface::HW_IF_POSITION); - EXPECT_EQ(joint.get_state(output, interfaces), return_type::OK); - ASSERT_THAT(output, SizeIs(1)); - EXPECT_EQ(output[0], 1.2); - interfaces.clear(); - interfaces.push_back(hardware_interface::HW_IF_VELOCITY); - EXPECT_EQ(joint.get_state(output, interfaces), return_type::INTERFACE_NOT_FOUND); - - input.clear(); - EXPECT_EQ(joint.set_state(input), return_type::INTERFACE_VALUE_SIZE_NOT_EQUAL); - input.push_back(2.1); - EXPECT_EQ(joint.set_state(input), return_type::OK); - - EXPECT_EQ(joint.get_state(output), return_type::OK); - ASSERT_THAT(output, SizeIs(1)); - EXPECT_EQ(output[0], 2.1); - - // Test DummyPositionJoint - joint_info.command_interfaces.push_back(hardware_interface::HW_IF_POSITION); - joint_info.command_interfaces.push_back(hardware_interface::HW_IF_VELOCITY); - EXPECT_EQ(joint.configure(joint_info), return_type::ERROR); +// +// std::vector output; +// interfaces.clear(); +// EXPECT_EQ(joint.get_command(output, interfaces), return_type::INTERFACE_NOT_PROVIDED); +// interfaces.push_back(hardware_interface::HW_IF_POSITION); +// EXPECT_EQ(joint.get_command(output, interfaces), return_type::OK); +// ASSERT_THAT(output, SizeIs(1)); +// EXPECT_EQ(output[0], 1.2); +// interfaces.clear(); +// interfaces.push_back(hardware_interface::HW_IF_VELOCITY); +// EXPECT_EQ(joint.get_command(output, interfaces), return_type::INTERFACE_NOT_FOUND); +// +// input.clear(); +// EXPECT_EQ(joint.set_command(input), return_type::INTERFACE_VALUE_SIZE_NOT_EQUAL); +// input.push_back(2.1); +// EXPECT_EQ(joint.set_command(input), return_type::OK); +// +// EXPECT_EQ(joint.get_command(output), return_type::OK); +// ASSERT_THAT(output, SizeIs(1)); +// EXPECT_EQ(output[0], 2.1); +// +// // State getters and setters +// interfaces.clear(); +// input.clear(); +// input.push_back(2.1); +// EXPECT_EQ(joint.set_state(input, interfaces), return_type::INTERFACE_NOT_PROVIDED); +// interfaces.push_back(hardware_interface::HW_IF_VELOCITY); +// EXPECT_EQ(joint.set_state(input, interfaces), return_type::INTERFACE_NOT_FOUND); +// interfaces.push_back(hardware_interface::HW_IF_POSITION); +// EXPECT_EQ(joint.set_state(input, interfaces), return_type::INTERFACE_VALUE_SIZE_NOT_EQUAL); +// interfaces.clear(); +// interfaces.push_back(hardware_interface::HW_IF_POSITION); +// input.clear(); +// input.push_back(1.2); +// EXPECT_EQ(joint.set_state(input, interfaces), return_type::OK); +// +// output.clear(); +// interfaces.clear(); +// EXPECT_EQ(joint.get_command(output, interfaces), return_type::INTERFACE_NOT_PROVIDED); +// interfaces.push_back(hardware_interface::HW_IF_POSITION); +// EXPECT_EQ(joint.get_state(output, interfaces), return_type::OK); +// ASSERT_THAT(output, SizeIs(1)); +// EXPECT_EQ(output[0], 1.2); +// interfaces.clear(); +// interfaces.push_back(hardware_interface::HW_IF_VELOCITY); +// EXPECT_EQ(joint.get_state(output, interfaces), return_type::INTERFACE_NOT_FOUND); +// +// input.clear(); +// EXPECT_EQ(joint.set_state(input), return_type::INTERFACE_VALUE_SIZE_NOT_EQUAL); +// input.push_back(2.1); +// EXPECT_EQ(joint.set_state(input), return_type::OK); +// +// EXPECT_EQ(joint.get_state(output), return_type::OK); +// ASSERT_THAT(output, SizeIs(1)); +// EXPECT_EQ(output[0], 2.1); +// +// // Test DummyPositionJoint +// joint_info.command_interfaces.push_back(hardware_interface::HW_IF_POSITION); +// joint_info.command_interfaces.push_back(hardware_interface::HW_IF_VELOCITY); +// EXPECT_EQ(joint.configure(joint_info), return_type::ERROR); } +/* TEST_F(TestComponentInterfaces, multi_joint_example_component_works) { DummyMultiJoint joint; @@ -718,3 +913,4 @@ TEST_F(TestComponentInterfaces, system_interface_with_hardware_works) EXPECT_EQ(system.stop(), return_type::OK); EXPECT_EQ(system.get_status(), status::STOPPED); } +*/ From 4b62a7c7fe86d8390ac5bd628c98365dd3ca0f97 Mon Sep 17 00:00:00 2001 From: Karsten Knese Date: Wed, 30 Sep 2020 13:23:03 -0700 Subject: [PATCH 10/18] introduce multi_interface_joint package Signed-off-by: Karsten Knese --- .../hardware_interface_return_values.hpp | 1 + .../test/test_component_interfaces.cpp | 26 +-- multi_interface_joint/CMakeLists.txt | 68 ++++++++ .../multi_interface_joint.hpp | 88 +++++++++++ .../visibility_control.h | 49 ++++++ multi_interface_joint/package.xml | 20 +++ .../src/multi_interface_joint.cpp | 132 ++++++++++++++++ .../test/test_multi_interface_joint.cpp | 148 ++++++++++++++++++ 8 files changed, 512 insertions(+), 20 deletions(-) create mode 100644 multi_interface_joint/CMakeLists.txt create mode 100644 multi_interface_joint/include/multi_interface_joint/multi_interface_joint.hpp create mode 100644 multi_interface_joint/include/multi_interface_joint/visibility_control.h create mode 100644 multi_interface_joint/package.xml create mode 100644 multi_interface_joint/src/multi_interface_joint.cpp create mode 100644 multi_interface_joint/test/test_multi_interface_joint.cpp 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..047cbb75f3 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 @@ -35,6 +35,7 @@ enum class return_type : std::uint8_t INTERFACE_NOT_FOUND = 30, INTERFACE_VALUE_SIZE_NOT_EQUAL = 31, INTERFACE_NOT_PROVIDED = 32, + INTERFACE_DUPLICATES = 33, COMMAND_OUT_OF_LIMITS = 40, }; diff --git a/hardware_interface/test/test_component_interfaces.cpp b/hardware_interface/test/test_component_interfaces.cpp index 47fea721ea..19045d280a 100644 --- a/hardware_interface/test/test_component_interfaces.cpp +++ b/hardware_interface/test/test_component_interfaces.cpp @@ -45,14 +45,8 @@ class DummyPositionJoint : public components::JointInterface public: return_type configure(const components::ComponentInfo & joint_info) { - info_ = joint_info; - - if (info_.command_interfaces.size() > 1 || info_.state_interfaces.size() > 1) { - return return_type::ERROR; - } - - max_position_ = stod(info_.parameters["max_position"]); - min_position_ = stod(info_.parameters["min_position"]); + max_position_ = stod(joint_info.parameters.at("max_position")); + min_position_ = stod(joint_info.parameters.at("min_position")); return return_type::OK; } @@ -150,7 +144,6 @@ class DummyPositionJoint : public components::JointInterface double command_ = 0.0; double state_ = 0.0; - components::ComponentInfo info_; double max_position_, min_position_; }; @@ -159,16 +152,10 @@ class DummyMultiJoint : public components::JointInterface public: return_type configure(const components::ComponentInfo & joint_info) { - info_ = joint_info; - - 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"]); + max_position_ = stod(joint_info.parameters.at("max_position")); + min_position_ = stod(joint_info.parameters.at("min_position")); + max_velocity_ = stod(joint_info.parameters.at("max_velocity")); + min_velocity_ = stod(joint_info.parameters.at("min_velocity")); return return_type::OK; } @@ -284,7 +271,6 @@ class DummyMultiJoint : public components::JointInterface double state_pos_ = 0.0; double state_vel_ = 0.0; - components::ComponentInfo info_; double max_position_, min_position_; double max_velocity_, min_velocity_; }; diff --git a/multi_interface_joint/CMakeLists.txt b/multi_interface_joint/CMakeLists.txt new file mode 100644 index 0000000000..7ba5c4f6a6 --- /dev/null +++ b/multi_interface_joint/CMakeLists.txt @@ -0,0 +1,68 @@ +cmake_minimum_required(VERSION 3.5) +project(multi_interface_joint) + +# 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() + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +find_package(ament_cmake_ros REQUIRED) +find_package(hardware_interface REQUIRED) + +add_library(multi_interface_joint src/multi_interface_joint.cpp) +target_include_directories(multi_interface_joint PUBLIC + $ + $) +ament_target_dependencies( + multi_interface_joint + "hardware_interface" +) + +# Causes the visibility macros to use dllexport rather than dllimport, +# which is appropriate when building the dll but not consuming it. +target_compile_definitions(multi_interface_joint PRIVATE "MULTI_INTERFACE_JOINT_BUILDING_LIBRARY") + +install( + DIRECTORY include/ + DESTINATION include +) +install( + TARGETS multi_interface_joint + EXPORT export_${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin +) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() + + ament_add_gmock(test_multi_interface_joint + test/test_multi_interface_joint.cpp + ) + target_link_libraries(test_multi_interface_joint multi_interface_joint) +endif() + +ament_export_include_directories( + include +) +ament_export_libraries( + multi_interface_joint +) +ament_export_targets( + export_${PROJECT_NAME} +) + +ament_package() diff --git a/multi_interface_joint/include/multi_interface_joint/multi_interface_joint.hpp b/multi_interface_joint/include/multi_interface_joint/multi_interface_joint.hpp new file mode 100644 index 0000000000..d8c10798da --- /dev/null +++ b/multi_interface_joint/include/multi_interface_joint/multi_interface_joint.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 MULTI_INTERFACE_JOINT__MULTI_INTERFACE_JOINT_HPP_ +#define MULTI_INTERFACE_JOINT__MULTI_INTERFACE_JOINT_HPP_ + +#include +#include + +#include "multi_interface_joint/visibility_control.h" + +#include "hardware_interface/components/joint_interface.hpp" + +namespace multi_interface_joint +{ + +class MultiInterfaceJoint : public hardware_interface::components::JointInterface +{ +public: + MultiInterfaceJoint() = default; + + virtual ~MultiInterfaceJoint() = default; + + MULTI_INTERFACE_JOINT_PUBLIC + hardware_interface::return_type configure( + const hardware_interface::components::ComponentInfo & joint_info) override; + + MULTI_INTERFACE_JOINT_PUBLIC + std::vector get_command_interfaces() const override; + + MULTI_INTERFACE_JOINT_PUBLIC + std::vector get_state_interfaces() const override; + + MULTI_INTERFACE_JOINT_PUBLIC + hardware_interface::return_type get_command( + std::vector & command, + const std::vector & interfaces) const override; + + MULTI_INTERFACE_JOINT_PUBLIC + hardware_interface::return_type get_command( + std::vector & command) const override; + + MULTI_INTERFACE_JOINT_PUBLIC + hardware_interface::return_type set_command( + const std::vector & command, + const std::vector & interfaces) override; + + MULTI_INTERFACE_JOINT_PUBLIC + hardware_interface::return_type set_command(const std::vector & command) override; + + MULTI_INTERFACE_JOINT_PUBLIC + hardware_interface::return_type get_state( + std::vector & state, + const std::vector & interfaces) const override; + + MULTI_INTERFACE_JOINT_PUBLIC + hardware_interface::return_type get_state(std::vector & state) const override; + + MULTI_INTERFACE_JOINT_PUBLIC + hardware_interface::return_type set_state( + const std::vector & state, + const std::vector & interfaces) override; + + MULTI_INTERFACE_JOINT_PUBLIC + hardware_interface::return_type set_state(const std::vector & state) override; + +protected: + std::vector command_interfaces_; + std::vector command_values_; + + std::vector state_interfaces_; + std::vector state_values_; +}; + +} // namespace multi_interface_joint + +#endif // MULTI_INTERFACE_JOINT__MULTI_INTERFACE_JOINT_HPP_ diff --git a/multi_interface_joint/include/multi_interface_joint/visibility_control.h b/multi_interface_joint/include/multi_interface_joint/visibility_control.h new file mode 100644 index 0000000000..22b1151d0b --- /dev/null +++ b/multi_interface_joint/include/multi_interface_joint/visibility_control.h @@ -0,0 +1,49 @@ +// 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 MULTI_INTERFACE_JOINT__VISIBILITY_CONTROL_H_ +#define MULTI_INTERFACE_JOINT__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 MULTI_INTERFACE_JOINT_EXPORT __attribute__ ((dllexport)) + #define MULTI_INTERFACE_JOINT_IMPORT __attribute__ ((dllimport)) + #else + #define MULTI_INTERFACE_JOINT_EXPORT __declspec(dllexport) + #define MULTI_INTERFACE_JOINT_IMPORT __declspec(dllimport) + #endif + #ifdef MULTI_INTERFACE_JOINT_BUILDING_LIBRARY + #define MULTI_INTERFACE_JOINT_PUBLIC MULTI_INTERFACE_JOINT_EXPORT + #else + #define MULTI_INTERFACE_JOINT_PUBLIC MULTI_INTERFACE_JOINT_IMPORT + #endif + #define MULTI_INTERFACE_JOINT_PUBLIC_TYPE MULTI_INTERFACE_JOINT_PUBLIC + #define MULTI_INTERFACE_JOINT_LOCAL +#else + #define MULTI_INTERFACE_JOINT_EXPORT __attribute__ ((visibility("default"))) + #define MULTI_INTERFACE_JOINT_IMPORT + #if __GNUC__ >= 4 + #define MULTI_INTERFACE_JOINT_PUBLIC __attribute__ ((visibility("default"))) + #define MULTI_INTERFACE_JOINT_LOCAL __attribute__ ((visibility("hidden"))) + #else + #define MULTI_INTERFACE_JOINT_PUBLIC + #define MULTI_INTERFACE_JOINT_LOCAL + #endif + #define MULTI_INTERFACE_JOINT_PUBLIC_TYPE +#endif + +#endif // MULTI_INTERFACE_JOINT__VISIBILITY_CONTROL_H_ diff --git a/multi_interface_joint/package.xml b/multi_interface_joint/package.xml new file mode 100644 index 0000000000..1be09da2c6 --- /dev/null +++ b/multi_interface_joint/package.xml @@ -0,0 +1,20 @@ + + + + multi_interface_joint + 0.0.0 + TODO: Package description + karsten + Apache license 2.0 + + ament_cmake_ros + + hardware_interface + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/multi_interface_joint/src/multi_interface_joint.cpp b/multi_interface_joint/src/multi_interface_joint.cpp new file mode 100644 index 0000000000..12946ea127 --- /dev/null +++ b/multi_interface_joint/src/multi_interface_joint.cpp @@ -0,0 +1,132 @@ +// 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 "multi_interface_joint/multi_interface_joint.hpp" + +#include +#include +#include + +namespace multi_interface_joint +{ + +hardware_interface::return_type MultiInterfaceJoint::configure( + const hardware_interface::components::ComponentInfo & joint_info) +{ + const size_t command_interfaces_size = joint_info.command_interfaces.size(); + const size_t state_interfaces_size = joint_info.state_interfaces.size(); + + // fail if no interfaces at all are specified + if (command_interfaces_size == state_interfaces_size && command_interfaces_size == 0u) { + return hardware_interface::return_type::INTERFACE_NOT_PROVIDED; + } + + auto has_duplicates = [](const std::vector interfaces) -> bool + { + std::set set(interfaces.begin(), interfaces.end()); + return set.size() == interfaces.size(); + }; + + // fail if command interfaces has duplicates + if (has_duplicates(joint_info.command_interfaces)) { + return hardware_interface::return_type::INTERFACE_DUPLICATES; + } + + // fail if state interfaces has duplicates + if (has_duplicates(joint_info.state_interfaces)) { + return hardware_interface::return_type::INTERFACE_DUPLICATES; + } + + command_interfaces_ = joint_info.command_interfaces; + command_values_.resize(command_interfaces_size); + + state_interfaces_ = joint_info.state_interfaces; + state_values_.resize(state_interfaces_size); + + return hardware_interface::return_type::OK; +} + +std::vector MultiInterfaceJoint::get_command_interfaces() const +{ + return command_interfaces_; +} + +std::vector MultiInterfaceJoint::get_state_interfaces() const +{ + return state_interfaces_; +} + +hardware_interface::return_type MultiInterfaceJoint::get_command( + std::vector & command, + const std::vector & interfaces) const +{ + (void) command; + (void) interfaces; + return hardware_interface::return_type::OK; +} + +hardware_interface::return_type MultiInterfaceJoint::get_command( + std::vector & command) const +{ + (void) command; + return hardware_interface::return_type::OK; +} + +hardware_interface::return_type MultiInterfaceJoint::set_command( + const std::vector & command, + const std::vector & interfaces) +{ + (void) command; + (void) interfaces; + return hardware_interface::return_type::OK; +} + +hardware_interface::return_type MultiInterfaceJoint::set_command( + const std::vector & command) +{ + (void) command; + return hardware_interface::return_type::OK; +} + +hardware_interface::return_type MultiInterfaceJoint::get_state( + std::vector & state, + const std::vector & interfaces) const +{ + (void) state; + (void) interfaces; + return hardware_interface::return_type::OK; +} + +hardware_interface::return_type MultiInterfaceJoint::get_state(std::vector & state) const +{ + (void) state; + return hardware_interface::return_type::OK; +} + +hardware_interface::return_type MultiInterfaceJoint::set_state( + const std::vector & state, + const std::vector & interfaces) +{ + (void) state; + (void) interfaces; + return hardware_interface::return_type::OK; +} + +hardware_interface::return_type MultiInterfaceJoint::set_state(const std::vector & state) +{ + (void) state; + return hardware_interface::return_type::OK; +} + +} // namespace multi_interface_joint diff --git a/multi_interface_joint/test/test_multi_interface_joint.cpp b/multi_interface_joint/test/test_multi_interface_joint.cpp new file mode 100644 index 0000000000..ce1e75b400 --- /dev/null +++ b/multi_interface_joint/test/test_multi_interface_joint.cpp @@ -0,0 +1,148 @@ +// 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 "gmock/gmock.h" + +#include "multi_interface_joint/multi_interface_joint.hpp" + +#include "hardware_interface/components/component_info.hpp" + +class TestMultiInterfaceJoint : public testing::Test +{ +}; + +TEST_F(TestMultiInterfaceJoint, empty_initialized) +{ + multi_interface_joint::MultiInterfaceJoint joint; + EXPECT_TRUE(joint.get_command_interfaces().empty()); + EXPECT_TRUE(joint.get_state_interfaces().empty()); +} + +TEST_F(TestMultiInterfaceJoint, wrong_initialized) +{ + { + multi_interface_joint::MultiInterfaceJoint joint; + hardware_interface::components::ComponentInfo joint_info; + EXPECT_EQ(hardware_interface::return_type::INTERFACE_NOT_PROVIDED, joint.configure(joint_info)); + } + + { + hardware_interface::components::ComponentInfo joint_info; + joint_info.command_interfaces.push_back("position"); + joint_info.command_interfaces.push_back("position"); + multi_interface_joint::MultiInterfaceJoint joint; + EXPECT_EQ(hardware_interface::return_type::INTERFACE_DUPLICATES, joint.configure(joint_info)); + } + + { + hardware_interface::components::ComponentInfo joint_info; + joint_info.state_interfaces.push_back("position"); + joint_info.state_interfaces.push_back("position"); + multi_interface_joint::MultiInterfaceJoint joint; + EXPECT_EQ(hardware_interface::return_type::INTERFACE_DUPLICATES, joint.configure(joint_info)); + } +} + +// joint_info.name = "DummyMultiJoint"; +// joint_info.parameters["max_position"] = "3.14"; +// joint_info.parameters["min_position"] = "-3.14"; +// joint_info.parameters["max_velocity"] = "1.14"; +// joint_info.parameters["min_velocity"] = "-1.14"; +// +// EXPECT_EQ(joint.configure(joint_info), return_type::ERROR); +// +// joint_info.command_interfaces.push_back(hardware_interface::HW_IF_POSITION); +// joint_info.command_interfaces.push_back(hardware_interface::HW_IF_VELOCITY); +// +// EXPECT_EQ(joint.configure(joint_info), return_type::OK); +// +// ASSERT_THAT(joint.get_command_interfaces(), SizeIs(2)); +// EXPECT_EQ(joint.get_command_interfaces()[0], hardware_interface::HW_IF_POSITION); +// ASSERT_THAT(joint.get_state_interfaces(), SizeIs(0)); +// +// joint_info.state_interfaces.push_back(hardware_interface::HW_IF_POSITION); +// joint_info.state_interfaces.push_back(hardware_interface::HW_IF_VELOCITY); +// EXPECT_EQ(joint.configure(joint_info), return_type::OK); +// ASSERT_THAT(joint.get_state_interfaces(), SizeIs(2)); +// EXPECT_EQ(joint.get_command_interfaces()[1], hardware_interface::HW_IF_VELOCITY); +// +// // Command getters and setters +// std::vector interfaces; +// std::vector input; +// input.push_back(2.1); +// EXPECT_EQ(joint.set_command(input, interfaces), return_type::INTERFACE_NOT_PROVIDED); +// interfaces.push_back(hardware_interface::HW_IF_EFFORT); +// EXPECT_EQ(joint.set_command(input, interfaces), return_type::INTERFACE_NOT_FOUND); +// interfaces.push_back(hardware_interface::HW_IF_VELOCITY); +// EXPECT_EQ(joint.set_command(input, interfaces), return_type::INTERFACE_VALUE_SIZE_NOT_EQUAL); +// interfaces.clear(); +// interfaces.push_back(hardware_interface::HW_IF_VELOCITY); +// input.clear(); +// input.push_back(1.02); +// EXPECT_EQ(joint.set_command(input, interfaces), return_type::OK); +// +// std::vector output; +// EXPECT_EQ(joint.get_command(output, interfaces), return_type::OK); +// ASSERT_THAT(output, SizeIs(1)); +// EXPECT_EQ(output[0], 1.02); +// interfaces.clear(); +// interfaces.push_back(hardware_interface::HW_IF_EFFORT); +// EXPECT_EQ(joint.get_command(output, interfaces), return_type::INTERFACE_NOT_FOUND); +// +// input.clear(); +// EXPECT_EQ(joint.set_command(input), return_type::INTERFACE_VALUE_SIZE_NOT_EQUAL); +// input.push_back(5.77); +// EXPECT_EQ(joint.set_command(input), return_type::INTERFACE_VALUE_SIZE_NOT_EQUAL); +// input.clear(); +// input.push_back(1.2); +// input.push_back(0.4); +// EXPECT_EQ(joint.set_command(input), return_type::OK); +// +// EXPECT_EQ(joint.get_command(output), return_type::OK); +// ASSERT_THAT(output, SizeIs(2)); +// EXPECT_EQ(output[1], 0.4); +// +// // State getters and setters +// interfaces.clear(); +// input.clear(); +// input.push_back(2.1); +// EXPECT_EQ(joint.set_state(input, interfaces), return_type::INTERFACE_NOT_PROVIDED); +// interfaces.push_back(hardware_interface::HW_IF_EFFORT); +// EXPECT_EQ(joint.set_state(input, interfaces), return_type::INTERFACE_NOT_FOUND); +// interfaces.push_back(hardware_interface::HW_IF_POSITION); +// EXPECT_EQ(joint.set_state(input, interfaces), return_type::INTERFACE_VALUE_SIZE_NOT_EQUAL); +// interfaces.clear(); +// interfaces.push_back(hardware_interface::HW_IF_POSITION); +// input.clear(); +// input.push_back(1.2); +// EXPECT_EQ(joint.set_state(input, interfaces), return_type::OK); +// +// output.clear(); +// EXPECT_EQ(joint.get_state(output, interfaces), return_type::OK); +// ASSERT_THAT(output, SizeIs(1)); +// EXPECT_EQ(output[0], 1.2); +// interfaces.clear(); +// interfaces.push_back(hardware_interface::HW_IF_EFFORT); +// EXPECT_EQ(joint.get_state(output, interfaces), return_type::INTERFACE_NOT_FOUND); +// +// input.clear(); +// EXPECT_EQ(joint.set_state(input), return_type::INTERFACE_VALUE_SIZE_NOT_EQUAL); +// input.push_back(2.1); +// input.push_back(1.02); +// EXPECT_EQ(joint.set_state(input), return_type::OK); +// +// EXPECT_EQ(joint.get_state(output), return_type::OK); +// ASSERT_THAT(output, SizeIs(2)); +// EXPECT_EQ(output[0], 2.1); +//} From ff46c23dcb6956e45c09da4b1a658e3eb4030ec5 Mon Sep 17 00:00:00 2001 From: Karsten Knese Date: Wed, 30 Sep 2020 15:01:39 -0700 Subject: [PATCH 11/18] transfer unit tests to new package Signed-off-by: Karsten Knese --- .../src/component_lists_management.hpp | 140 ++++++++++ .../src/multi_interface_joint.cpp | 32 +-- .../test/test_multi_interface_joint.cpp | 242 +++++++++++------- 3 files changed, 299 insertions(+), 115 deletions(-) create mode 100644 multi_interface_joint/src/component_lists_management.hpp diff --git a/multi_interface_joint/src/component_lists_management.hpp b/multi_interface_joint/src/component_lists_management.hpp new file mode 100644 index 0000000000..0865bdd15f --- /dev/null +++ b/multi_interface_joint/src/component_lists_management.hpp @@ -0,0 +1,140 @@ +// 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 COMPONENT_LISTS_MANAGEMENT_HPP_ +#define COMPONENT_LISTS_MANAGEMENT_HPP_ + +#include +#include +#include + +#include "hardware_interface/types/hardware_interface_return_values.hpp" + +namespace multi_interface_joint +{ + +using hardware_interface::return_type; + +/** + * \brief Get values for queried_interfaces from the int_values. int_values data structure matches + * int_interfaces vector. + * + * \param values values to return. + * \param queried_interfaces interfaces for which values are queried. + * \param int_interfaces full list of interfaces of a component. + * \param int_values internal values of a component. + * \return return_type::INTERFACE_VALUE_SIZE_NOT_EQUAL if values and queried_interfaces arguments + * do not have the same length; return_type::INTERFACE_NOT_FOUND if one of queried_interfaces is + * not defined in int_interfaces; return return_type::INTERFACE_NOT_PROVIDED if queried_interfaces + * list is is empty; return_type::OK otherwise. + */ +inline return_type get_internal_values( + std::vector & values, const std::vector & queried_interfaces, + const std::vector & int_interfaces, const std::vector & int_values) +{ + if (queried_interfaces.size() == 0) { + return return_type::INTERFACE_NOT_PROVIDED; + } + + for (const auto & interface : queried_interfaces) { + auto it = std::find( + int_interfaces.begin(), int_interfaces.end(), interface); + if (it != int_interfaces.end()) { + values.push_back(int_values[std::distance(int_interfaces.begin(), it)]); + } else { + values.clear(); + return return_type::INTERFACE_NOT_FOUND; + } + } + return return_type::OK; +} + +/** + * \brief Set all internal values to to other vector. Return value is used for API consistency. + * + * \param values output list of values. + * \param int_values internal values of the component. + * \return return_type::OK always. + */ +inline return_type get_internal_values( + std::vector & values, const std::vector & int_values) +{ + values.clear(); + for (const auto & int_value : int_values) { + values.push_back(int_value); + } + return return_type::OK; +} + +/** + * \brief set values for queried_interfaces to the int_values. int_values data structure matches + * int_interfaces vector. + * + * \param values values to set. + * \param queried_interfaces interfaces for which values are queried. + * \param int_interfaces full list of interfaces of a component. + * \param int_values internal values of a component. + * \return return return_type::INTERFACE_NOT_PROVIDED if + * queried_interfaces list is is empty; return_type::INTERFACE_VALUE_SIZE_NOT_EQUAL if values and + * queried_interfaces arguments do not have the same length; return_type::INTERFACE_NOT_FOUND if + * one of queried_interfaces is not defined in int_interfaces; return_type::OK otherwise. + * + * \todo The error handling in this function could lead to incosistant command or state variables + * for different interfaces. This should be changed in the future. + * (see: https://github.com/ros-controls/ros2_control/issues/129) + */ +inline return_type set_internal_values( + const std::vector & values, const std::vector & queried_interfaces, + const std::vector & int_interfaces, std::vector & int_values) +{ + if (queried_interfaces.size() == 0) { + return return_type::INTERFACE_NOT_PROVIDED; + } + if (values.size() != queried_interfaces.size()) { + return return_type::INTERFACE_VALUE_SIZE_NOT_EQUAL; + } + + for (auto q_it = queried_interfaces.begin(); q_it != queried_interfaces.end(); ++q_it) { + auto it = std::find(int_interfaces.begin(), int_interfaces.end(), *q_it); + if (it != int_interfaces.end()) { + int_values[std::distance(int_interfaces.begin(), it)] = + values[std::distance(queried_interfaces.begin(), q_it)]; + } else { + return return_type::INTERFACE_NOT_FOUND; + } + } + return return_type::OK; +} + +/** + * \brief set all values to compoenents internal values. + * + * \param values values to set. + * \param int_values internal values of a component. + * \return return_type::INTERFACE_VALUE_SIZE_NOT_EQUAL if the size of the arguments is not equal, + * return_type::OK otherwise. + */ +inline return_type set_internal_values( + const std::vector & values, std::vector & int_values) +{ + if (values.size() == int_values.size()) { + int_values = values; + } else { + return return_type::INTERFACE_VALUE_SIZE_NOT_EQUAL; + } + return return_type::OK; +} + +} // namespace multi_interface_joint +#endif // COMPONENT_LISTS_MANAGEMENT_HPP_ diff --git a/multi_interface_joint/src/multi_interface_joint.cpp b/multi_interface_joint/src/multi_interface_joint.cpp index 12946ea127..f838ea59b0 100644 --- a/multi_interface_joint/src/multi_interface_joint.cpp +++ b/multi_interface_joint/src/multi_interface_joint.cpp @@ -18,6 +18,8 @@ #include #include +#include "./component_lists_management.hpp" + namespace multi_interface_joint { @@ -35,7 +37,7 @@ hardware_interface::return_type MultiInterfaceJoint::configure( auto has_duplicates = [](const std::vector interfaces) -> bool { std::set set(interfaces.begin(), interfaces.end()); - return set.size() == interfaces.size(); + return set.size() != interfaces.size(); }; // fail if command interfaces has duplicates @@ -71,62 +73,50 @@ hardware_interface::return_type MultiInterfaceJoint::get_command( std::vector & command, const std::vector & interfaces) const { - (void) command; - (void) interfaces; - return hardware_interface::return_type::OK; + return get_internal_values(command, interfaces, command_interfaces_, command_values_); } hardware_interface::return_type MultiInterfaceJoint::get_command( std::vector & command) const { - (void) command; - return hardware_interface::return_type::OK; + return get_internal_values(command, command_values_); } hardware_interface::return_type MultiInterfaceJoint::set_command( const std::vector & command, const std::vector & interfaces) { - (void) command; - (void) interfaces; - return hardware_interface::return_type::OK; + return set_internal_values(command, interfaces, command_interfaces_, command_values_); } hardware_interface::return_type MultiInterfaceJoint::set_command( const std::vector & command) { - (void) command; - return hardware_interface::return_type::OK; + return set_internal_values(command, command_values_); } hardware_interface::return_type MultiInterfaceJoint::get_state( std::vector & state, const std::vector & interfaces) const { - (void) state; - (void) interfaces; - return hardware_interface::return_type::OK; + return get_internal_values(state, interfaces, state_interfaces_, state_values_); } hardware_interface::return_type MultiInterfaceJoint::get_state(std::vector & state) const { - (void) state; - return hardware_interface::return_type::OK; + return get_internal_values(state, state_values_); } hardware_interface::return_type MultiInterfaceJoint::set_state( const std::vector & state, const std::vector & interfaces) { - (void) state; - (void) interfaces; - return hardware_interface::return_type::OK; + return set_internal_values(state, interfaces, state_interfaces_, state_values_); } hardware_interface::return_type MultiInterfaceJoint::set_state(const std::vector & state) { - (void) state; - return hardware_interface::return_type::OK; + return set_internal_values(state, state_values_); } } // namespace multi_interface_joint diff --git a/multi_interface_joint/test/test_multi_interface_joint.cpp b/multi_interface_joint/test/test_multi_interface_joint.cpp index ce1e75b400..fced3b0042 100644 --- a/multi_interface_joint/test/test_multi_interface_joint.cpp +++ b/multi_interface_joint/test/test_multi_interface_joint.cpp @@ -12,13 +12,18 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "gmock/gmock.h" +#include +#include +#include #include "multi_interface_joint/multi_interface_joint.hpp" #include "hardware_interface/components/component_info.hpp" +#include "hardware_interface/types/hardware_interface_type_values.hpp" -class TestMultiInterfaceJoint : public testing::Test +using namespace ::testing; // NOLINT + +class TestMultiInterfaceJoint : public Test { }; @@ -54,95 +59,144 @@ TEST_F(TestMultiInterfaceJoint, wrong_initialized) } } -// joint_info.name = "DummyMultiJoint"; -// joint_info.parameters["max_position"] = "3.14"; -// joint_info.parameters["min_position"] = "-3.14"; -// joint_info.parameters["max_velocity"] = "1.14"; -// joint_info.parameters["min_velocity"] = "-1.14"; -// -// EXPECT_EQ(joint.configure(joint_info), return_type::ERROR); -// -// joint_info.command_interfaces.push_back(hardware_interface::HW_IF_POSITION); -// joint_info.command_interfaces.push_back(hardware_interface::HW_IF_VELOCITY); -// -// EXPECT_EQ(joint.configure(joint_info), return_type::OK); -// -// ASSERT_THAT(joint.get_command_interfaces(), SizeIs(2)); -// EXPECT_EQ(joint.get_command_interfaces()[0], hardware_interface::HW_IF_POSITION); -// ASSERT_THAT(joint.get_state_interfaces(), SizeIs(0)); -// -// joint_info.state_interfaces.push_back(hardware_interface::HW_IF_POSITION); -// joint_info.state_interfaces.push_back(hardware_interface::HW_IF_VELOCITY); -// EXPECT_EQ(joint.configure(joint_info), return_type::OK); -// ASSERT_THAT(joint.get_state_interfaces(), SizeIs(2)); -// EXPECT_EQ(joint.get_command_interfaces()[1], hardware_interface::HW_IF_VELOCITY); -// -// // Command getters and setters -// std::vector interfaces; -// std::vector input; -// input.push_back(2.1); -// EXPECT_EQ(joint.set_command(input, interfaces), return_type::INTERFACE_NOT_PROVIDED); -// interfaces.push_back(hardware_interface::HW_IF_EFFORT); -// EXPECT_EQ(joint.set_command(input, interfaces), return_type::INTERFACE_NOT_FOUND); -// interfaces.push_back(hardware_interface::HW_IF_VELOCITY); -// EXPECT_EQ(joint.set_command(input, interfaces), return_type::INTERFACE_VALUE_SIZE_NOT_EQUAL); -// interfaces.clear(); -// interfaces.push_back(hardware_interface::HW_IF_VELOCITY); -// input.clear(); -// input.push_back(1.02); -// EXPECT_EQ(joint.set_command(input, interfaces), return_type::OK); -// -// std::vector output; -// EXPECT_EQ(joint.get_command(output, interfaces), return_type::OK); -// ASSERT_THAT(output, SizeIs(1)); -// EXPECT_EQ(output[0], 1.02); -// interfaces.clear(); -// interfaces.push_back(hardware_interface::HW_IF_EFFORT); -// EXPECT_EQ(joint.get_command(output, interfaces), return_type::INTERFACE_NOT_FOUND); -// -// input.clear(); -// EXPECT_EQ(joint.set_command(input), return_type::INTERFACE_VALUE_SIZE_NOT_EQUAL); -// input.push_back(5.77); -// EXPECT_EQ(joint.set_command(input), return_type::INTERFACE_VALUE_SIZE_NOT_EQUAL); -// input.clear(); -// input.push_back(1.2); -// input.push_back(0.4); -// EXPECT_EQ(joint.set_command(input), return_type::OK); -// -// EXPECT_EQ(joint.get_command(output), return_type::OK); -// ASSERT_THAT(output, SizeIs(2)); -// EXPECT_EQ(output[1], 0.4); -// -// // State getters and setters -// interfaces.clear(); -// input.clear(); -// input.push_back(2.1); -// EXPECT_EQ(joint.set_state(input, interfaces), return_type::INTERFACE_NOT_PROVIDED); -// interfaces.push_back(hardware_interface::HW_IF_EFFORT); -// EXPECT_EQ(joint.set_state(input, interfaces), return_type::INTERFACE_NOT_FOUND); -// interfaces.push_back(hardware_interface::HW_IF_POSITION); -// EXPECT_EQ(joint.set_state(input, interfaces), return_type::INTERFACE_VALUE_SIZE_NOT_EQUAL); -// interfaces.clear(); -// interfaces.push_back(hardware_interface::HW_IF_POSITION); -// input.clear(); -// input.push_back(1.2); -// EXPECT_EQ(joint.set_state(input, interfaces), return_type::OK); -// -// output.clear(); -// EXPECT_EQ(joint.get_state(output, interfaces), return_type::OK); -// ASSERT_THAT(output, SizeIs(1)); -// EXPECT_EQ(output[0], 1.2); -// interfaces.clear(); -// interfaces.push_back(hardware_interface::HW_IF_EFFORT); -// EXPECT_EQ(joint.get_state(output, interfaces), return_type::INTERFACE_NOT_FOUND); -// -// input.clear(); -// EXPECT_EQ(joint.set_state(input), return_type::INTERFACE_VALUE_SIZE_NOT_EQUAL); -// input.push_back(2.1); -// input.push_back(1.02); -// EXPECT_EQ(joint.set_state(input), return_type::OK); -// -// EXPECT_EQ(joint.get_state(output), return_type::OK); -// ASSERT_THAT(output, SizeIs(2)); -// EXPECT_EQ(output[0], 2.1); -//} +TEST_F(TestMultiInterfaceJoint, correct_initialized) +{ + { + hardware_interface::components::ComponentInfo joint_info; + joint_info.command_interfaces.push_back("position"); + joint_info.command_interfaces.push_back("velocity"); + joint_info.state_interfaces.push_back("effort"); + multi_interface_joint::MultiInterfaceJoint joint; + EXPECT_EQ(hardware_interface::return_type::OK, joint.configure(joint_info)); + ASSERT_EQ(2u, joint.get_command_interfaces().size()); + ASSERT_EQ(1u, joint.get_state_interfaces().size()); + EXPECT_EQ("position", joint.get_command_interfaces()[0]); + EXPECT_EQ("velocity", joint.get_command_interfaces()[1]); + EXPECT_EQ("effort", joint.get_state_interfaces()[0]); + } +} + +TEST_F(TestMultiInterfaceJoint, getters_and_setters) +{ + multi_interface_joint::MultiInterfaceJoint joint; + + hardware_interface::components::ComponentInfo joint_info; + joint_info.command_interfaces.push_back(hardware_interface::HW_IF_POSITION); + joint_info.command_interfaces.push_back(hardware_interface::HW_IF_VELOCITY); + joint_info.state_interfaces.push_back(hardware_interface::HW_IF_POSITION); + joint_info.state_interfaces.push_back(hardware_interface::HW_IF_VELOCITY); + ASSERT_EQ( + hardware_interface::return_type::OK, joint.configure(joint_info)); + + // Command getters and setters + std::vector interfaces; + std::vector input; + input.push_back(2.1); + EXPECT_EQ( + hardware_interface::return_type::INTERFACE_NOT_PROVIDED, + joint.set_command(input, interfaces)); + + interfaces.push_back(hardware_interface::HW_IF_EFFORT); + EXPECT_EQ( + hardware_interface::return_type::INTERFACE_NOT_FOUND, + joint.set_command(input, interfaces)); + + interfaces.push_back(hardware_interface::HW_IF_VELOCITY); + EXPECT_EQ( + hardware_interface::return_type::INTERFACE_VALUE_SIZE_NOT_EQUAL, + joint.set_command(input, interfaces)); + + interfaces.clear(); + input.clear(); + interfaces.push_back(hardware_interface::HW_IF_VELOCITY); + input.push_back(1.02); + EXPECT_EQ( + hardware_interface::return_type::OK, + joint.set_command(input, interfaces)); + + std::vector output; + EXPECT_EQ( + hardware_interface::return_type::OK, + joint.get_command(output, interfaces)); + ASSERT_EQ(1u, output.size()); + EXPECT_EQ(1.02, output[0]); + + interfaces.clear(); + interfaces.push_back(hardware_interface::HW_IF_EFFORT); + EXPECT_EQ( + hardware_interface::return_type::INTERFACE_NOT_FOUND, + joint.get_command(output, interfaces)); + + input.clear(); + EXPECT_EQ( + hardware_interface::return_type::INTERFACE_VALUE_SIZE_NOT_EQUAL, + joint.set_command(input)); + input.push_back(5.77); + EXPECT_EQ( + hardware_interface::return_type::INTERFACE_VALUE_SIZE_NOT_EQUAL, + joint.set_command(input)); + + input.clear(); + input.push_back(1.2); + input.push_back(0.4); + EXPECT_EQ( + hardware_interface::return_type::OK, + joint.set_command(input)); + EXPECT_EQ( + hardware_interface::return_type::OK, + joint.get_command(output)); + ASSERT_EQ(2u, output.size()); + EXPECT_EQ(0.4, output[1]); + + // State getters and setters + interfaces.clear(); + input.clear(); + input.push_back(2.1); + EXPECT_EQ( + hardware_interface::return_type::INTERFACE_NOT_PROVIDED, + joint.set_state(input, interfaces)); + interfaces.push_back(hardware_interface::HW_IF_EFFORT); + EXPECT_EQ( + hardware_interface::return_type::INTERFACE_NOT_FOUND, + joint.set_state(input, interfaces)); + interfaces.push_back(hardware_interface::HW_IF_POSITION); + EXPECT_EQ( + hardware_interface::return_type::INTERFACE_VALUE_SIZE_NOT_EQUAL, + joint.set_state(input, interfaces)); + + interfaces.clear(); + interfaces.push_back(hardware_interface::HW_IF_POSITION); + input.clear(); + input.push_back(1.2); + EXPECT_EQ( + hardware_interface::return_type::OK, + joint.set_state(input, interfaces)); + + output.clear(); + EXPECT_EQ( + hardware_interface::return_type::OK, + joint.get_state(output, interfaces)); + ASSERT_THAT(1u, output.size()); + EXPECT_EQ(1.2, output[0]); + + interfaces.clear(); + interfaces.push_back(hardware_interface::HW_IF_EFFORT); + EXPECT_EQ( + hardware_interface::return_type::INTERFACE_NOT_FOUND, + joint.get_state(output, interfaces)); + + input.clear(); + EXPECT_EQ( + hardware_interface::return_type::INTERFACE_VALUE_SIZE_NOT_EQUAL, + joint.set_state(input)); + input.push_back(2.1); + input.push_back(1.02); + EXPECT_EQ( + hardware_interface::return_type::OK, + joint.set_state(input)); + EXPECT_EQ( + hardware_interface::return_type::OK, + joint.get_state(output)); + ASSERT_EQ(2u, output.size()); + EXPECT_EQ(2.1, output[0]); +} From 92732770c7ba0247c1d272f7eeb6de7d2a375278 Mon Sep 17 00:00:00 2001 From: Karsten Knese Date: Wed, 30 Sep 2020 18:25:17 -0700 Subject: [PATCH 12/18] use component interfaces Signed-off-by: Karsten Knese --- controller_manager/CMakeLists.txt | 4 +- controller_manager/src/resource_manager.cpp | 57 ++- .../test/test_hardware_resources.xml | 4 +- .../test_joint_component.cpp | 63 ++- .../test_joint_component.hpp | 51 ++- .../test_sensor_component.cpp | 33 +- .../test_sensor_component.hpp | 28 +- .../hardware_interface/components/joint.hpp | 3 +- .../hardware_interface/components/sensor.hpp | 69 +--- .../components/sensor_interface.hpp | 57 +++ hardware_interface/src/components/joint.cpp | 6 + hardware_interface/src/components/sensor.cpp | 24 +- .../test/test_component_interfaces.cpp | 379 ++---------------- multi_interface_sensor/CMakeLists.txt | 68 ++++ .../multi_interface_sensor.hpp | 65 +++ .../visibility_control.h | 49 +++ multi_interface_sensor/package.xml | 20 + .../src/component_lists_management.hpp | 140 +++++++ .../src/multi_interface_sensor.cpp | 82 ++++ .../test/test_multi_interface_sensor.cpp | 152 +++++++ 20 files changed, 900 insertions(+), 454 deletions(-) create mode 100644 multi_interface_sensor/CMakeLists.txt create mode 100644 multi_interface_sensor/include/multi_interface_sensor/multi_interface_sensor.hpp create mode 100644 multi_interface_sensor/include/multi_interface_sensor/visibility_control.h create mode 100644 multi_interface_sensor/package.xml create mode 100644 multi_interface_sensor/src/component_lists_management.hpp create mode 100644 multi_interface_sensor/src/multi_interface_sensor.cpp create mode 100644 multi_interface_sensor/test/test_multi_interface_sensor.cpp diff --git a/controller_manager/CMakeLists.txt b/controller_manager/CMakeLists.txt index 0eef758640..a09e598b6f 100644 --- a/controller_manager/CMakeLists.txt +++ b/controller_manager/CMakeLists.txt @@ -112,9 +112,11 @@ if(BUILD_TESTING) test/test_hardware_resources/test_sensor_component.cpp test/test_hardware_resources/test_sensor_hardware.cpp test/test_hardware_resources/test_system_hardware.cpp) + target_include_directories(test_hardware_resources PRIVATE include) ament_target_dependencies(test_hardware_resources hardware_interface - pluginlib) + pluginlib + ) install(TARGETS test_hardware_resources DESTINATION lib ) diff --git a/controller_manager/src/resource_manager.cpp b/controller_manager/src/resource_manager.cpp index a3efaed710..1ef7178540 100644 --- a/controller_manager/src/resource_manager.cpp +++ b/controller_manager/src/resource_manager.cpp @@ -19,6 +19,8 @@ #include "hardware_interface/actuator_hardware.hpp" #include "hardware_interface/components/component_info.hpp" +#include "hardware_interface/components/joint.hpp" +#include "hardware_interface/components/sensor.hpp" #include "hardware_interface/component_parser.hpp" #include "hardware_interface/hardware_info.hpp" #include "hardware_interface/sensor_hardware.hpp" @@ -36,9 +38,9 @@ class ResourceStorage static constexpr const char * pkg_name = "hardware_interface"; static constexpr const char * joint_component_interface_name = - "hardware_interface::components::Joint"; + "hardware_interface::components::JointInterface"; static constexpr const char * sensor_component_interface_name = - "hardware_interface::components::Sensor"; + "hardware_interface::components::SensorInterface"; static constexpr const char * actuator_interface_name = "hardware_interface::ActuatorHardwareInterface"; @@ -58,22 +60,39 @@ class ResourceStorage ~ResourceStorage() = default; + template + void initialize_component( + const hardware_interface::components::ComponentInfo & component_info, + pluginlib::ClassLoader & loader, + std::vector & container) + { + // hardware_class_type has to match class name in plugin xml description + // TODO(karsten1987) extract package from hardware_class_type + // e.g.: / + // TODO(dstoegl) why is it class_type here and "hardware_class_type" in hardware_info? + auto interface = std::unique_ptr( + loader.createUnmanagedInstance(component_info.class_type)); + ComponentT actuator(std::move(interface)); + container.emplace_back(std::move(actuator)); + container.back().configure(component_info); + } + void initialize_joint_component( const hardware_interface::components::ComponentInfo & component_info) { - joint_components_.emplace_back( - std::unique_ptr( - joint_component_loader_.createUnmanagedInstance(component_info.class_type))); - joint_components_.back()->configure(component_info); + initialize_component< + hardware_interface::components::Joint, + hardware_interface::components::JointInterface>( + component_info, joint_component_loader_, joint_components_); } void initialize_sensor_component( const hardware_interface::components::ComponentInfo & component_info) { - sensor_components_.emplace_back( - std::unique_ptr( - sensor_component_loader_.createUnmanagedInstance(component_info.class_type))); - sensor_components_.back()->configure(component_info); + initialize_component< + hardware_interface::components::Sensor, + hardware_interface::components::SensorInterface>( + component_info, sensor_component_loader_, sensor_components_); } template @@ -91,34 +110,36 @@ class ResourceStorage container.emplace_back(std::move(actuator)); container.back().configure(hardware_info); } - void initialize_actuator(const hardware_interface::HardwareInfo & hardware_info) { - initialize_hardware( hardware_info, actuator_loader_, actuators_); } void initialize_sensor(const hardware_interface::HardwareInfo & hardware_info) { - initialize_hardware( hardware_info, sensor_loader_, sensors_); } void initialize_system(const hardware_interface::HardwareInfo & hardware_info) { - initialize_hardware( hardware_info, system_loader_, systems_); } // components plugins - pluginlib::ClassLoader joint_component_loader_; - pluginlib::ClassLoader sensor_component_loader_; + pluginlib::ClassLoader joint_component_loader_; + pluginlib::ClassLoader sensor_component_loader_; - std::vector> joint_components_; - std::vector> sensor_components_; + std::vector joint_components_; + std::vector sensor_components_; // hardware plugins pluginlib::ClassLoader actuator_loader_; diff --git a/controller_manager/test/test_hardware_resources.xml b/controller_manager/test/test_hardware_resources.xml index d0494d45ec..fec0858c1e 100644 --- a/controller_manager/test/test_hardware_resources.xml +++ b/controller_manager/test/test_hardware_resources.xml @@ -18,13 +18,13 @@ - + Test Joint Component - + Test Sensor Component diff --git a/controller_manager/test/test_hardware_resources/test_joint_component.cpp b/controller_manager/test/test_hardware_resources/test_joint_component.cpp index 3ab5858eb0..743114c347 100644 --- a/controller_manager/test/test_hardware_resources/test_joint_component.cpp +++ b/controller_manager/test/test_hardware_resources/test_joint_component.cpp @@ -20,25 +20,74 @@ using hardware_interface::return_type; -return_type -TestJointComponent::configure( +return_type TestJointComponent::configure( const hardware_interface::components::ComponentInfo & /* joint_info */) { return return_type::OK; } -std::vector -TestJointComponent::get_command_interfaces() const +std::vector TestJointComponent::get_command_interfaces() const { return {"test_command_interface"}; } -std::vector -TestJointComponent::get_state_interfaces() const +std::vector TestJointComponent::get_state_interfaces() const { return {"test_state_interface"}; } +return_type TestJointComponent::get_command( + std::vector & /*command */, + const std::vector & /* interfaces */) const +{ + return return_type::OK; +} + +return_type TestJointComponent::get_command( + std::vector & /* command */) const +{ + return return_type::OK; +} + +return_type TestJointComponent::set_command( + const std::vector & /* command */, + const std::vector & /* interfaces */) +{ + return return_type::OK; +} + +return_type TestJointComponent::set_command( + const std::vector & /* command*/) +{ + return return_type::OK; +} + +return_type TestJointComponent::get_state( + std::vector & /* state */, + const std::vector & /* interfaces */) const +{ + return return_type::OK; +} + +return_type TestJointComponent::get_state( + std::vector & /* state */) const +{ + return return_type::OK; +} + +return_type TestJointComponent::set_state( + const std::vector & /* state */, + const std::vector & /* interfaces */) +{ + return return_type::OK; +} + +return_type TestJointComponent::set_state( + const std::vector & /* state */) +{ + return return_type::OK; +} + #include "pluginlib/class_list_macros.hpp" // NOLINT -PLUGINLIB_EXPORT_CLASS(TestJointComponent, hardware_interface::components::Joint) +PLUGINLIB_EXPORT_CLASS(TestJointComponent, hardware_interface::components::JointInterface) diff --git a/controller_manager/test/test_hardware_resources/test_joint_component.hpp b/controller_manager/test/test_hardware_resources/test_joint_component.hpp index b754cfa993..a8b20e9655 100644 --- a/controller_manager/test/test_hardware_resources/test_joint_component.hpp +++ b/controller_manager/test/test_hardware_resources/test_joint_component.hpp @@ -19,17 +19,58 @@ #include #include -#include "hardware_interface/components/joint.hpp" +#include "controller_manager/visibility_control.h" -class TestJointComponent : public hardware_interface::components::Joint +#include "hardware_interface/components/joint_interface.hpp" + +class TestJointComponent : public hardware_interface::components::JointInterface { public: - hardware_interface::return_type - configure(const hardware_interface::components::ComponentInfo & joint_info) override; + TestJointComponent() = default; + + virtual ~TestJointComponent() = default; + + CONTROLLER_MANAGER_PUBLIC + hardware_interface::return_type configure( + const hardware_interface::components::ComponentInfo & joint_info) override; + CONTROLLER_MANAGER_PUBLIC std::vector get_command_interfaces() const override; + CONTROLLER_MANAGER_PUBLIC std::vector get_state_interfaces() const override; -}; + CONTROLLER_MANAGER_PUBLIC + hardware_interface::return_type get_command( + std::vector & command, + const std::vector & interfaces) const override; + + CONTROLLER_MANAGER_PUBLIC + hardware_interface::return_type get_command( + std::vector & command) const override; + + CONTROLLER_MANAGER_PUBLIC + hardware_interface::return_type set_command( + const std::vector & command, + const std::vector & interfaces) override; + + CONTROLLER_MANAGER_PUBLIC + hardware_interface::return_type set_command(const std::vector & command) override; + + CONTROLLER_MANAGER_PUBLIC + hardware_interface::return_type get_state( + std::vector & state, + const std::vector & interfaces) const override; + + CONTROLLER_MANAGER_PUBLIC + hardware_interface::return_type get_state(std::vector & state) const override; + + CONTROLLER_MANAGER_PUBLIC + hardware_interface::return_type set_state( + const std::vector & state, + const std::vector & interfaces) override; + + CONTROLLER_MANAGER_PUBLIC + hardware_interface::return_type set_state(const std::vector & state) override; +}; #endif // TEST_HARDWARE_RESOURCES__TEST_JOINT_COMPONENT_HPP_ diff --git a/controller_manager/test/test_hardware_resources/test_sensor_component.cpp b/controller_manager/test/test_hardware_resources/test_sensor_component.cpp index 7ee7c6f0c6..08800f2c30 100644 --- a/controller_manager/test/test_hardware_resources/test_sensor_component.cpp +++ b/controller_manager/test/test_hardware_resources/test_sensor_component.cpp @@ -20,19 +20,42 @@ using hardware_interface::return_type; -return_type -TestSensorComponent::configure( +return_type TestSensorComponent::configure( const hardware_interface::components::ComponentInfo & /* sensor_info */) { return return_type::OK; } -std::vector -TestSensorComponent::get_state_interfaces() const +std::vector TestSensorComponent::get_state_interfaces() const { return {"test_state_interface"}; } +return_type TestSensorComponent::get_state( + std::vector & /* state */, + const std::vector & /* interfaces */) const +{ + return return_type::OK; +} + +return_type TestSensorComponent::get_state( + std::vector & /* state */) const +{ + return return_type::OK; +} + +return_type TestSensorComponent::set_state( + const std::vector & /* state */, + const std::vector & /* interfaces */) +{ + return return_type::OK; +} + +return_type TestSensorComponent::set_state( + const std::vector & /* state */) +{ + return return_type::OK; +} #include "pluginlib/class_list_macros.hpp" // NOLINT -PLUGINLIB_EXPORT_CLASS(TestSensorComponent, hardware_interface::components::Sensor) +PLUGINLIB_EXPORT_CLASS(TestSensorComponent, hardware_interface::components::SensorInterface) diff --git a/controller_manager/test/test_hardware_resources/test_sensor_component.hpp b/controller_manager/test/test_hardware_resources/test_sensor_component.hpp index ce7e8c1998..e607fa7f66 100644 --- a/controller_manager/test/test_hardware_resources/test_sensor_component.hpp +++ b/controller_manager/test/test_hardware_resources/test_sensor_component.hpp @@ -19,15 +19,39 @@ #include #include -#include "hardware_interface/components/sensor.hpp" +#include "controller_manager/visibility_control.h" -class TestSensorComponent : public hardware_interface::components::Sensor +#include "hardware_interface/components/sensor_interface.hpp" + +class TestSensorComponent : public hardware_interface::components::SensorInterface { public: + TestSensorComponent() = default; + + virtual ~TestSensorComponent() = default; + + CONTROLLER_MANAGER_PUBLIC hardware_interface::return_type configure(const hardware_interface::components::ComponentInfo & sensor_info) override; + CONTROLLER_MANAGER_PUBLIC std::vector get_state_interfaces() const override; + + CONTROLLER_MANAGER_PUBLIC + hardware_interface::return_type get_state( + std::vector & state, + const std::vector & interfaces) const override; + + CONTROLLER_MANAGER_PUBLIC + hardware_interface::return_type get_state(std::vector & state) const override; + + CONTROLLER_MANAGER_PUBLIC + hardware_interface::return_type set_state( + const std::vector & state, + const std::vector & interfaces) override; + + CONTROLLER_MANAGER_PUBLIC + hardware_interface::return_type set_state(const std::vector & state) override; }; #endif // TEST_HARDWARE_RESOURCES__TEST_SENSOR_COMPONENT_HPP_ diff --git a/hardware_interface/include/hardware_interface/components/joint.hpp b/hardware_interface/include/hardware_interface/components/joint.hpp index d764c50d6e..73b410e285 100644 --- a/hardware_interface/include/hardware_interface/components/joint.hpp +++ b/hardware_interface/include/hardware_interface/components/joint.hpp @@ -15,6 +15,7 @@ #ifndef HARDWARE_INTERFACE__COMPONENTS__JOINT_HPP_ #define HARDWARE_INTERFACE__COMPONENTS__JOINT_HPP_ +#include #include #include @@ -31,14 +32,12 @@ namespace components class Joint final { public: - HARDWARE_INTERFACE_PUBLIC Joint() = default; explicit Joint(Joint && other) = default; explicit Joint(std::unique_ptr impl); - HARDWARE_INTERFACE_PUBLIC ~Joint() = default; HARDWARE_INTERFACE_PUBLIC diff --git a/hardware_interface/include/hardware_interface/components/sensor.hpp b/hardware_interface/include/hardware_interface/components/sensor.hpp index 0046fd505c..56de7e8383 100644 --- a/hardware_interface/include/hardware_interface/components/sensor.hpp +++ b/hardware_interface/include/hardware_interface/components/sensor.hpp @@ -15,6 +15,7 @@ #ifndef HARDWARE_INTERFACE__COMPONENTS__SENSOR_HPP_ #define HARDWARE_INTERFACE__COMPONENTS__SENSOR_HPP_ +#include #include #include @@ -28,102 +29,48 @@ namespace hardware_interface namespace components { -/** - * \brief Base Class for "Sensor" component used as a basic build block for the devices which - * provide data. A sensor can have one or more interfaces (e.g., force, acceleration, etc.) to - * provide states for. The list of state interfaces defines this. - */ -class Sensor : public SensorInterface +class Sensor final { public: HARDWARE_INTERFACE_PUBLIC Sensor() = default; + explicit Sensor(Sensor && other) = default; + + explicit Sensor(std::unique_ptr impl); + HARDWARE_INTERFACE_PUBLIC virtual ~Sensor() = default; - /** - * \brief Configure base sensor class based on the description in the robot's URDF file. - * - * \param joint_info structure with data from URDF. - * \return return_type::OK if required data are provided and is successfully parsed, - * return_type::ERROR otherwise. - */ HARDWARE_INTERFACE_PUBLIC virtual return_type configure(const ComponentInfo & sensor_info); - /** - * \brief Provide the list of state interfaces configured for the sensor. - * - * \return string list with state interfaces. - */ HARDWARE_INTERFACE_PUBLIC virtual std::vector get_state_interfaces() const; - /** - * \brief Get state list from the sensor. This function is used by the controller to get the - * actual state of the hardware. The parameters state, and interfaces have the same order and - * number of elements. Using the interfaces list, the controller can choose which values to get. - * - * \param state list of doubles with states of the hardware. - * \param interfaces list of interfaces on which states have to be provided. - * \return return_type::INTERFACE_VALUE_SIZE_NOT_EQUAL if state and interfaces arguments do not - * have the same length; return_type::INTERFACE_NOT_FOUND if one of provided interfaces is not - * defined for the sensor; return return_type::INTERFACE_NOT_PROVIDED if the list of interfaces - * is empty; return_type::OK otherwise. - */ HARDWARE_INTERFACE_PUBLIC virtual return_type get_state( std::vector & state, const std::vector & interfaces) const; - /** - * \brief Get complete state list from the sensor. This function is used by the controller to get - * complete actual state of the hardware. The state values have the same order as interfaces which - * can be recived by get_state_interfaces() function. Return value is used for API consistency. - * - * \param state list of doubles with states of the hardware. - * \return return_type::OK always. - */ HARDWARE_INTERFACE_PUBLIC virtual return_type get_state(std::vector & state) const; - /** - * \brief Set state list for the sensor. This function is used by the hardware to set its actual - * state. The parameters state, and interfaces have the same order and number of elements. Using - * the interfaces list, the hardware can choose which values to set. - * - * \param state list of doubles with states of the hardware. - * \param interfaces list of interfaces on which states have to be provided. - * \return return_type::INTERFACE_VALUE_SIZE_NOT_EQUAL if state and interfaces arguments do not - * have the same length; return_type::INTERFACE_NOT_FOUND if one of provided interfaces is not - * defined for the sensor; return_type::OK otherwise. - */ HARDWARE_INTERFACE_PUBLIC virtual return_type set_state( const std::vector & state, const std::vector & interfaces); - /** - * \brief Set complete state list from the sensor.This function is used by the hardware to set its - * complete actual state. The state values have the same order as interfaces which can be recived - * by get_state_interfaces() function. - * - * \param state list of doubles with states from the hardware. - * \return return_type::INTERFACE_VALUE_SIZE_NOT_EQUAL is state size is not equal to number of - * sensor's state interfaces, return_type::OK otherwise. - */ HARDWARE_INTERFACE_PUBLIC virtual return_type set_state(const std::vector & state); -protected: - ComponentInfo info_; - std::vector states_; +private: + std::unique_ptr impl_; }; } // namespace components diff --git a/hardware_interface/include/hardware_interface/components/sensor_interface.hpp b/hardware_interface/include/hardware_interface/components/sensor_interface.hpp index 2c04235f41..d7c3e60a0d 100644 --- a/hardware_interface/include/hardware_interface/components/sensor_interface.hpp +++ b/hardware_interface/include/hardware_interface/components/sensor_interface.hpp @@ -27,6 +27,11 @@ namespace hardware_interface namespace components { +/** + * \brief Interface for the "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 SensorInterface { public: @@ -37,30 +42,82 @@ class SensorInterface virtual ~SensorInterface() = default; + /** + * \brief Configure base sensor class based on the description in the robot's URDF file. + * + * \param joint_info structure with data from URDF. + * \return return_type::OK if required data are provided and is successfully parsed, + * return_type::ERROR otherwise. + */ HARDWARE_INTERFACE_PUBLIC 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_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::INTERFACE_VALUE_SIZE_NOT_EQUAL if state and interfaces arguments do not + * have the same length; return_type::INTERFACE_NOT_FOUND if one of provided interfaces is not + * defined for the sensor; return return_type::INTERFACE_NOT_PROVIDED if the list of interfaces + * is empty; return_type::OK otherwise. + */ HARDWARE_INTERFACE_PUBLIC virtual return_type get_state( std::vector & state, const std::vector & interfaces) const = 0; + /** + * \brief Get complete state list from the sensor. This function is used by the controller to get + * complete actual state of the hardware. The state values have the same order as interfaces which + * can be recived by get_state_interfaces() function. Return value is used for API consistency. + * + * \param state list of doubles with states of the hardware. + * \return return_type::OK always. + */ HARDWARE_INTERFACE_PUBLIC virtual return_type get_state(std::vector & state) 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::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_PUBLIC virtual return_type set_state( const std::vector & state, const std::vector & interfaces) = 0; + /** + * \brief Set complete state list from the sensor.This function is used by the hardware to set its + * complete actual state. The state values have the same order as interfaces which can be recived + * by get_state_interfaces() function. + * + * \param state list of doubles with states from the hardware. + * \return return_type::INTERFACE_VALUE_SIZE_NOT_EQUAL is state size is not equal to number of + * sensor's state interfaces, return_type::OK otherwise. + */ HARDWARE_INTERFACE_PUBLIC virtual return_type set_state(const std::vector & state) = 0; diff --git a/hardware_interface/src/components/joint.cpp b/hardware_interface/src/components/joint.cpp index 59d1fd6deb..891b133d9f 100644 --- a/hardware_interface/src/components/joint.cpp +++ b/hardware_interface/src/components/joint.cpp @@ -12,7 +12,9 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include #include +#include #include #include "hardware_interface/components/joint.hpp" @@ -24,6 +26,10 @@ namespace hardware_interface namespace components { +Joint::Joint(std::unique_ptr impl) +: impl_(std::move(impl)) +{} + return_type Joint::configure(const ComponentInfo & joint_info) { return impl_->configure(joint_info); diff --git a/hardware_interface/src/components/sensor.cpp b/hardware_interface/src/components/sensor.cpp index 397bee648d..8d104c723a 100644 --- a/hardware_interface/src/components/sensor.cpp +++ b/hardware_interface/src/components/sensor.cpp @@ -12,7 +12,9 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include #include +#include #include #include "hardware_interface/components/sensor.hpp" @@ -26,40 +28,40 @@ namespace hardware_interface namespace components { -return_type Sensor::configure(const ComponentInfo & joint_info) +Sensor::Sensor(std::unique_ptr impl) +: impl_(std::move(impl)) +{} + +return_type Sensor::configure(const ComponentInfo & sensor_info) { - info_ = joint_info; - if (info_.state_interfaces.size() > 0) { - states_.resize(info_.state_interfaces.size()); - } - return return_type::OK; + return impl_->configure(sensor_info); } std::vector Sensor::get_state_interfaces() const { - return info_.state_interfaces; + return impl_->get_state_interfaces(); } return_type Sensor::get_state( std::vector & state, const std::vector & interfaces) const { - return get_internal_values(state, interfaces, info_.state_interfaces, states_); + return impl_->get_state(state, interfaces); } return_type Sensor::get_state(std::vector & state) const { - return get_internal_values(state, states_); + return impl_->get_state(state); } return_type Sensor::set_state( const std::vector & state, const std::vector & interfaces) { - return set_internal_values(state, interfaces, info_.state_interfaces, states_); + return impl_->set_state(state, interfaces); } return_type Sensor::set_state(const std::vector & state) { - return set_internal_values(state, states_); + return impl_->set_state(state); } } // namespace components diff --git a/hardware_interface/test/test_component_interfaces.cpp b/hardware_interface/test/test_component_interfaces.cpp index 19045d280a..ae5aa8b5ec 100644 --- a/hardware_interface/test/test_component_interfaces.cpp +++ b/hardware_interface/test/test_component_interfaces.cpp @@ -64,9 +64,9 @@ class DummyPositionJoint : public components::JointInterface std::vector & command, const std::vector & interfaces) const { - if (command.size() != 1u) { return return_type::ERROR; } - if (interfaces.size() != 1u) { return return_type::ERROR; } - if (interfaces[0] != HW_IF_POSITION) { return return_type::ERROR; } + if (command.size() != 1u) {return return_type::ERROR;} + if (interfaces.size() != 1u) {return return_type::ERROR;} + if (interfaces[0] != HW_IF_POSITION) {return return_type::ERROR;} command[0] = command_; return return_type::OK; @@ -74,7 +74,7 @@ class DummyPositionJoint : public components::JointInterface return_type get_command(std::vector & command) const { - if (command.size() != 1u) { return return_type::ERROR; } + if (command.size() != 1u) {return return_type::ERROR;} command[0] = command_; return return_type::OK; @@ -84,9 +84,9 @@ class DummyPositionJoint : public components::JointInterface const std::vector & command, const std::vector & interfaces) { - if (command.size() != 1u) { return return_type::ERROR; } - if (interfaces.size() != 1u) { return return_type::ERROR; } - if (interfaces[0] != HW_IF_POSITION) { return return_type::ERROR; } + if (command.size() != 1u) {return return_type::ERROR;} + if (interfaces.size() != 1u) {return return_type::ERROR;} + if (interfaces[0] != HW_IF_POSITION) {return return_type::ERROR;} command_ = command[0]; return return_type::OK; @@ -94,7 +94,7 @@ class DummyPositionJoint : public components::JointInterface return_type set_command(const std::vector & command) { - if (command.size() != 1u) { return return_type::ERROR; } + if (command.size() != 1u) {return return_type::ERROR;} command_ = command[0]; return return_type::OK; @@ -104,9 +104,9 @@ class DummyPositionJoint : public components::JointInterface std::vector & state, const std::vector & interfaces) const { - if (state.size() != 1u) { return return_type::ERROR; } - if (interfaces.size() != 1u) { return return_type::ERROR; } - if (interfaces[0] != HW_IF_POSITION) { return return_type::ERROR; } + if (state.size() != 1u) {return return_type::ERROR;} + if (interfaces.size() != 1u) {return return_type::ERROR;} + if (interfaces[0] != HW_IF_POSITION) {return return_type::ERROR;} state[0] = state_; return return_type::OK; @@ -114,7 +114,7 @@ class DummyPositionJoint : public components::JointInterface return_type get_state(std::vector & state) const { - if (state.size() != 1u) { return return_type::ERROR; } + if (state.size() != 1u) {return return_type::ERROR;} state[0] = state_; return return_type::OK; @@ -124,9 +124,9 @@ class DummyPositionJoint : public components::JointInterface const std::vector & state, const std::vector & interfaces) { - if (state.size() != 1u) { return return_type::ERROR; } - if (interfaces.size() != 1u) { return return_type::ERROR; } - if (interfaces[0] != HW_IF_POSITION) { return return_type::ERROR; } + if (state.size() != 1u) {return return_type::ERROR;} + if (interfaces.size() != 1u) {return return_type::ERROR;} + if (interfaces[0] != HW_IF_POSITION) {return return_type::ERROR;} state_ = state[0]; return return_type::OK; @@ -134,7 +134,7 @@ class DummyPositionJoint : public components::JointInterface return_type set_state(const std::vector & state) { - if (state.size() != 1u) { return return_type::ERROR; } + if (state.size() != 1u) {return return_type::ERROR;} state_ = state[0]; return return_type::OK; @@ -147,159 +147,12 @@ class DummyPositionJoint : public components::JointInterface double max_position_, min_position_; }; -class DummyMultiJoint : public components::JointInterface -{ -public: - return_type configure(const components::ComponentInfo & joint_info) - { - max_position_ = stod(joint_info.parameters.at("max_position")); - min_position_ = stod(joint_info.parameters.at("min_position")); - max_velocity_ = stod(joint_info.parameters.at("max_velocity")); - min_velocity_ = stod(joint_info.parameters.at("min_velocity")); - return return_type::OK; - } - - std::vector get_command_interfaces() const - { - return {HW_IF_POSITION, HW_IF_VELOCITY}; - } - - std::vector get_state_interfaces() const - { - return {HW_IF_POSITION, HW_IF_VELOCITY}; - } - - return_type get_command( - std::vector & command, - const std::vector & interfaces) const - { - if (command.size() != 2u) { return return_type::ERROR; } - if (interfaces.size() != 2u) { return return_type::ERROR; } - if (interfaces[0] != HW_IF_POSITION && interfaces[1] != HW_IF_VELOCITY) { - return return_type::ERROR; - } - - command[0] = command_pos_; - command[1] = command_vel_; - return return_type::OK; - } - - return_type get_command(std::vector & command) const - { - if (command.size() != 2u) { return return_type::ERROR; } - - command[0] = command_pos_; - command[1] = command_vel_; - return return_type::OK; - } - - return_type set_command( - const std::vector & command, - const std::vector & interfaces) - { - if (command.size() != 2u) { return return_type::ERROR; } - if (interfaces.size() != 2u) { return return_type::ERROR; } - if (interfaces[0] != HW_IF_POSITION && interfaces[1] != HW_IF_VELOCITY) { - return return_type::ERROR; - } - - command_pos_ = command[0]; - command_vel_ = command[1]; - return return_type::OK; - } - - return_type set_command(const std::vector & command) - { - if (command.size() != 1u) { return return_type::ERROR; } - - command_pos_ = command[0]; - command_vel_ = command[1]; - return return_type::OK; - } - - return_type get_state( - std::vector & state, - const std::vector & interfaces) const - { - if (state.size() != 2u) { return return_type::ERROR; } - if (interfaces.size() != 2u) { return return_type::ERROR; } - if (interfaces[0] != HW_IF_POSITION && interfaces[1] != HW_IF_VELOCITY) { - return return_type::ERROR; - } - - state[0] = state_pos_; - state[1] = state_vel_; - return return_type::OK; - } - - return_type get_state(std::vector & state) const - { - if (state.size() != 2u) { return return_type::ERROR; } - - state[0] = state_pos_; - state[1] = state_vel_; - return return_type::OK; - } - - return_type set_state( - const std::vector & state, - const std::vector & interfaces) - { - if (state.size() != 2u) { return return_type::ERROR; } - if (interfaces.size() != 2u) { return return_type::ERROR; } - if (interfaces[0] != HW_IF_POSITION && interfaces[1] != HW_IF_VELOCITY) { - return return_type::ERROR; - } - - state_pos_ = state[0]; - state_vel_ = state[1]; - return return_type::OK; - } - - return_type set_state(const std::vector & state) - { - if (state.size() != 1u) { return return_type::ERROR; } - - state_pos_ = state[0]; - state_vel_ = state[1]; - return return_type::OK; - } - -private: - double command_pos_ = 0.0; - double command_vel_ = 0.0; - double state_pos_ = 0.0; - double state_vel_ = 0.0; +} // namespace hardware_interfaces_components_test +} // namespace hardware_interface - double max_position_, min_position_; - double max_velocity_, min_velocity_; -}; - -class DummyForceTorqueSensor : public components::Sensor -{ -public: - return_type configure(const components::ComponentInfo & sensor_info) - { - if (Sensor::configure(sensor_info) != return_type::OK) { - return return_type::ERROR; - } - - if (info_.parameters["frame_id"] == "") { - return return_type::ERROR; - } - if (info_.state_interfaces.size() == 0) { - info_.state_interfaces.push_back("force_x"); - info_.state_interfaces.push_back("force_y"); - info_.state_interfaces.push_back("force_z"); - info_.state_interfaces.push_back("torque_x"); - info_.state_interfaces.push_back("torque_y"); - info_.state_interfaces.push_back("torque_z"); - } - states_ = {1.34, 5.67, 8.21, 5.63, 5.99, 4.32}; - return return_type::OK; - } -}; +// TODO(karsten1987): Transfer this test to the controller manager test resource plugin +/* class DummyActuatorHardware : public ActuatorHardwareInterface { return_type configure(const HardwareInfo & actuator_info) override @@ -505,9 +358,6 @@ class DummySystemHardware : public SystemHardwareInterface std::vector joints_hw_values_ = {-1.575, -0.7543}; }; -} // namespace hardware_interfaces_components_test -} // namespace hardware_interface - using hardware_interface::components::ComponentInfo; using hardware_interface::components::Joint; using hardware_interface::components::Sensor; @@ -521,7 +371,6 @@ using hardware_interface::SensorHardwareInterface; using hardware_interface::SystemHardware; using hardware_interface::SystemHardwareInterface; using hardware_interface::hardware_interfaces_components_test::DummyForceTorqueSensor; -using hardware_interface::hardware_interfaces_components_test::DummyMultiJoint; using hardware_interface::hardware_interfaces_components_test::DummyPositionJoint; using hardware_interface::hardware_interfaces_components_test::DummyActuatorHardware; @@ -569,138 +418,33 @@ TEST_F(TestComponentInterfaces, joint_example_component_works) input.clear(); input.push_back(1.2); EXPECT_EQ(joint.set_command(input, interfaces), return_type::OK); -// -// std::vector output; -// interfaces.clear(); -// EXPECT_EQ(joint.get_command(output, interfaces), return_type::INTERFACE_NOT_PROVIDED); -// interfaces.push_back(hardware_interface::HW_IF_POSITION); -// EXPECT_EQ(joint.get_command(output, interfaces), return_type::OK); -// ASSERT_THAT(output, SizeIs(1)); -// EXPECT_EQ(output[0], 1.2); -// interfaces.clear(); -// interfaces.push_back(hardware_interface::HW_IF_VELOCITY); -// EXPECT_EQ(joint.get_command(output, interfaces), return_type::INTERFACE_NOT_FOUND); -// -// input.clear(); -// EXPECT_EQ(joint.set_command(input), return_type::INTERFACE_VALUE_SIZE_NOT_EQUAL); -// input.push_back(2.1); -// EXPECT_EQ(joint.set_command(input), return_type::OK); -// -// EXPECT_EQ(joint.get_command(output), return_type::OK); -// ASSERT_THAT(output, SizeIs(1)); -// EXPECT_EQ(output[0], 2.1); -// -// // State getters and setters -// interfaces.clear(); -// input.clear(); -// input.push_back(2.1); -// EXPECT_EQ(joint.set_state(input, interfaces), return_type::INTERFACE_NOT_PROVIDED); -// interfaces.push_back(hardware_interface::HW_IF_VELOCITY); -// EXPECT_EQ(joint.set_state(input, interfaces), return_type::INTERFACE_NOT_FOUND); -// interfaces.push_back(hardware_interface::HW_IF_POSITION); -// EXPECT_EQ(joint.set_state(input, interfaces), return_type::INTERFACE_VALUE_SIZE_NOT_EQUAL); -// interfaces.clear(); -// interfaces.push_back(hardware_interface::HW_IF_POSITION); -// input.clear(); -// input.push_back(1.2); -// EXPECT_EQ(joint.set_state(input, interfaces), return_type::OK); -// -// output.clear(); -// interfaces.clear(); -// EXPECT_EQ(joint.get_command(output, interfaces), return_type::INTERFACE_NOT_PROVIDED); -// interfaces.push_back(hardware_interface::HW_IF_POSITION); -// EXPECT_EQ(joint.get_state(output, interfaces), return_type::OK); -// ASSERT_THAT(output, SizeIs(1)); -// EXPECT_EQ(output[0], 1.2); -// interfaces.clear(); -// interfaces.push_back(hardware_interface::HW_IF_VELOCITY); -// EXPECT_EQ(joint.get_state(output, interfaces), return_type::INTERFACE_NOT_FOUND); -// -// input.clear(); -// EXPECT_EQ(joint.set_state(input), return_type::INTERFACE_VALUE_SIZE_NOT_EQUAL); -// input.push_back(2.1); -// EXPECT_EQ(joint.set_state(input), return_type::OK); -// -// EXPECT_EQ(joint.get_state(output), return_type::OK); -// ASSERT_THAT(output, SizeIs(1)); -// EXPECT_EQ(output[0], 2.1); -// -// // Test DummyPositionJoint -// joint_info.command_interfaces.push_back(hardware_interface::HW_IF_POSITION); -// joint_info.command_interfaces.push_back(hardware_interface::HW_IF_VELOCITY); -// EXPECT_EQ(joint.configure(joint_info), return_type::ERROR); -} - -/* -TEST_F(TestComponentInterfaces, multi_joint_example_component_works) -{ - DummyMultiJoint joint; - - joint_info.name = "DummyMultiJoint"; - joint_info.parameters["max_position"] = "3.14"; - joint_info.parameters["min_position"] = "-3.14"; - joint_info.parameters["max_velocity"] = "1.14"; - joint_info.parameters["min_velocity"] = "-1.14"; - - EXPECT_EQ(joint.configure(joint_info), return_type::ERROR); - - joint_info.command_interfaces.push_back(hardware_interface::HW_IF_POSITION); - joint_info.command_interfaces.push_back(hardware_interface::HW_IF_VELOCITY); - - EXPECT_EQ(joint.configure(joint_info), return_type::OK); - - ASSERT_THAT(joint.get_command_interfaces(), SizeIs(2)); - EXPECT_EQ(joint.get_command_interfaces()[0], hardware_interface::HW_IF_POSITION); - ASSERT_THAT(joint.get_state_interfaces(), SizeIs(0)); - - joint_info.state_interfaces.push_back(hardware_interface::HW_IF_POSITION); - joint_info.state_interfaces.push_back(hardware_interface::HW_IF_VELOCITY); - EXPECT_EQ(joint.configure(joint_info), return_type::OK); - ASSERT_THAT(joint.get_state_interfaces(), SizeIs(2)); - EXPECT_EQ(joint.get_command_interfaces()[1], hardware_interface::HW_IF_VELOCITY); - - // Command getters and setters - std::vector interfaces; - std::vector input; - input.push_back(2.1); - EXPECT_EQ(joint.set_command(input, interfaces), return_type::INTERFACE_NOT_PROVIDED); - interfaces.push_back(hardware_interface::HW_IF_EFFORT); - EXPECT_EQ(joint.set_command(input, interfaces), return_type::INTERFACE_NOT_FOUND); - interfaces.push_back(hardware_interface::HW_IF_VELOCITY); - EXPECT_EQ(joint.set_command(input, interfaces), return_type::INTERFACE_VALUE_SIZE_NOT_EQUAL); - interfaces.clear(); - interfaces.push_back(hardware_interface::HW_IF_VELOCITY); - input.clear(); - input.push_back(1.02); - EXPECT_EQ(joint.set_command(input, interfaces), return_type::OK); std::vector output; + 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.02); + EXPECT_EQ(output[0], 1.2); interfaces.clear(); - interfaces.push_back(hardware_interface::HW_IF_EFFORT); + interfaces.push_back(hardware_interface::HW_IF_VELOCITY); EXPECT_EQ(joint.get_command(output, interfaces), return_type::INTERFACE_NOT_FOUND); input.clear(); EXPECT_EQ(joint.set_command(input), return_type::INTERFACE_VALUE_SIZE_NOT_EQUAL); - input.push_back(5.77); - EXPECT_EQ(joint.set_command(input), return_type::INTERFACE_VALUE_SIZE_NOT_EQUAL); - input.clear(); - input.push_back(1.2); - input.push_back(0.4); + input.push_back(2.1); EXPECT_EQ(joint.set_command(input), return_type::OK); EXPECT_EQ(joint.get_command(output), return_type::OK); - ASSERT_THAT(output, SizeIs(2)); - EXPECT_EQ(output[1], 0.4); + ASSERT_THAT(output, SizeIs(1)); + EXPECT_EQ(output[0], 2.1); // State getters and setters interfaces.clear(); input.clear(); input.push_back(2.1); EXPECT_EQ(joint.set_state(input, interfaces), return_type::INTERFACE_NOT_PROVIDED); - interfaces.push_back(hardware_interface::HW_IF_EFFORT); + 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); @@ -711,77 +455,32 @@ TEST_F(TestComponentInterfaces, multi_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); interfaces.clear(); - interfaces.push_back(hardware_interface::HW_IF_EFFORT); + interfaces.push_back(hardware_interface::HW_IF_VELOCITY); EXPECT_EQ(joint.get_state(output, interfaces), return_type::INTERFACE_NOT_FOUND); input.clear(); EXPECT_EQ(joint.set_state(input), return_type::INTERFACE_VALUE_SIZE_NOT_EQUAL); input.push_back(2.1); - input.push_back(1.02); EXPECT_EQ(joint.set_state(input), return_type::OK); EXPECT_EQ(joint.get_state(output), return_type::OK); - ASSERT_THAT(output, SizeIs(2)); - EXPECT_EQ(output[0], 2.1); -} - -TEST_F(TestComponentInterfaces, sensor_example_component_works) -{ - DummyForceTorqueSensor sensor; - - EXPECT_EQ(sensor.configure(sensor_info), return_type::OK); - ASSERT_THAT(sensor.get_state_interfaces(), SizeIs(6)); - EXPECT_EQ(sensor.get_state_interfaces()[0], "force_x"); - EXPECT_EQ(sensor.get_state_interfaces()[5], "torque_z"); - std::vector input = {5.23, 6.7, 2.5, 3.8, 8.9, 12.3}; - std::vector output; - std::vector interfaces; - EXPECT_EQ(sensor.get_state(output, interfaces), return_type::INTERFACE_NOT_PROVIDED); - interfaces.push_back("force_y"); - EXPECT_EQ(sensor.get_state(output, interfaces), return_type::OK); ASSERT_THAT(output, SizeIs(1)); - EXPECT_EQ(output[0], 5.67); - - // State getters and setters - interfaces.clear(); - EXPECT_EQ(sensor.set_state(input, interfaces), return_type::INTERFACE_NOT_PROVIDED); - interfaces.push_back(hardware_interface::HW_IF_VELOCITY); - interfaces.push_back(hardware_interface::HW_IF_VELOCITY); - interfaces.push_back(hardware_interface::HW_IF_VELOCITY); - EXPECT_EQ(sensor.set_state(input, interfaces), return_type::INTERFACE_VALUE_SIZE_NOT_EQUAL); - interfaces.push_back(hardware_interface::HW_IF_VELOCITY); - interfaces.push_back(hardware_interface::HW_IF_VELOCITY); - interfaces.push_back(hardware_interface::HW_IF_VELOCITY); - EXPECT_EQ(sensor.set_state(input, interfaces), return_type::INTERFACE_NOT_FOUND); - interfaces.clear(); - interfaces = sensor.get_state_interfaces(); - EXPECT_EQ(sensor.set_state(input, interfaces), return_type::OK); - - output.clear(); - EXPECT_EQ(sensor.get_state(output, interfaces), return_type::OK); - ASSERT_THAT(output, SizeIs(6)); - EXPECT_EQ(output[0], 5.23); - interfaces.clear(); - interfaces.push_back(hardware_interface::HW_IF_VELOCITY); - EXPECT_EQ(sensor.get_state(output, interfaces), return_type::INTERFACE_NOT_FOUND); - - input.clear(); - EXPECT_EQ(sensor.set_state(input), return_type::INTERFACE_VALUE_SIZE_NOT_EQUAL); - input = {5.23, 6.7, 2.5, 3.8, 8.9, 12.3}; - EXPECT_EQ(sensor.set_state(input), return_type::OK); - - EXPECT_EQ(sensor.get_state(output), return_type::OK); - ASSERT_THAT(output, SizeIs(6)); - EXPECT_EQ(output[5], 12.3); + EXPECT_EQ(output[0], 2.1); - sensor_info.parameters.clear(); - EXPECT_EQ(sensor.configure(sensor_info), return_type::ERROR); + // 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, actuator_hardware_interface_works) { ActuatorHardware actuator_hw(std::make_unique()); diff --git a/multi_interface_sensor/CMakeLists.txt b/multi_interface_sensor/CMakeLists.txt new file mode 100644 index 0000000000..8810e8c100 --- /dev/null +++ b/multi_interface_sensor/CMakeLists.txt @@ -0,0 +1,68 @@ +cmake_minimum_required(VERSION 3.5) +project(multi_interface_sensor) + +# 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() + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +find_package(ament_cmake_ros REQUIRED) +find_package(hardware_interface REQUIRED) + +add_library(multi_interface_sensor src/multi_interface_sensor.cpp) +target_include_directories(multi_interface_sensor PUBLIC + $ + $) +ament_target_dependencies( + multi_interface_sensor + "hardware_interface" +) + +# Causes the visibility macros to use dllexport rather than dllimport, +# which is appropriate when building the dll but not consuming it. +target_compile_definitions(multi_interface_sensor PRIVATE "MULTI_INTERFACE_SENSOR_BUILDING_LIBRARY") + +install( + DIRECTORY include/ + DESTINATION include +) +install( + TARGETS multi_interface_sensor + EXPORT export_${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin +) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() + + ament_add_gmock(test_multi_interface_sensor + test/test_multi_interface_sensor + ) + target_link_libraries(test_multi_interface_sensor multi_interface_sensor) +endif() + +ament_export_include_directories( + include +) +ament_export_libraries( + multi_interface_sensor +) +ament_export_targets( + export_${PROJECT_NAME} +) + +ament_package() diff --git a/multi_interface_sensor/include/multi_interface_sensor/multi_interface_sensor.hpp b/multi_interface_sensor/include/multi_interface_sensor/multi_interface_sensor.hpp new file mode 100644 index 0000000000..eaf6716b6c --- /dev/null +++ b/multi_interface_sensor/include/multi_interface_sensor/multi_interface_sensor.hpp @@ -0,0 +1,65 @@ +// 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 MULTI_INTERFACE_SENSOR__MULTI_INTERFACE_SENSOR_HPP_ +#define MULTI_INTERFACE_SENSOR__MULTI_INTERFACE_SENSOR_HPP_ + +#include +#include + +#include "multi_interface_sensor/visibility_control.h" + +#include "hardware_interface/components/sensor_interface.hpp" + +namespace multi_interface_sensor +{ + +class MultiInterfaceSensor : public hardware_interface::components::SensorInterface +{ +public: + MultiInterfaceSensor() = default; + + virtual ~MultiInterfaceSensor() = default; + + MULTI_INTERFACE_SENSOR_PUBLIC + hardware_interface::return_type configure( + const hardware_interface::components::ComponentInfo & sensor_info) override; + + MULTI_INTERFACE_SENSOR_PUBLIC + std::vector get_state_interfaces() const override; + + MULTI_INTERFACE_SENSOR_PUBLIC + hardware_interface::return_type get_state( + std::vector & state, + const std::vector & interfaces) const override; + + MULTI_INTERFACE_SENSOR_PUBLIC + hardware_interface::return_type get_state(std::vector & state) const override; + + MULTI_INTERFACE_SENSOR_PUBLIC + hardware_interface::return_type set_state( + const std::vector & state, + const std::vector & interfaces) override; + + MULTI_INTERFACE_SENSOR_PUBLIC + hardware_interface::return_type set_state(const std::vector & state) override; + +protected: + std::vector state_interfaces_; + std::vector state_values_; +}; + +} // namespace multi_interface_sensor + +#endif // MULTI_INTERFACE_SENSOR__MULTI_INTERFACE_SENSOR_HPP_ diff --git a/multi_interface_sensor/include/multi_interface_sensor/visibility_control.h b/multi_interface_sensor/include/multi_interface_sensor/visibility_control.h new file mode 100644 index 0000000000..43e489299a --- /dev/null +++ b/multi_interface_sensor/include/multi_interface_sensor/visibility_control.h @@ -0,0 +1,49 @@ +// 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 MULTI_INTERFACE_SENSOR__VISIBILITY_CONTROL_H_ +#define MULTI_INTERFACE_SENSOR__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 MULTI_INTERFACE_SENSOR_EXPORT __attribute__ ((dllexport)) + #define MULTI_INTERFACE_SENSOR_IMPORT __attribute__ ((dllimport)) + #else + #define MULTI_INTERFACE_SENSOR_EXPORT __declspec(dllexport) + #define MULTI_INTERFACE_SENSOR_IMPORT __declspec(dllimport) + #endif + #ifdef MULTI_INTERFACE_SENSOR_BUILDING_LIBRARY + #define MULTI_INTERFACE_SENSOR_PUBLIC MULTI_INTERFACE_SENSOR_EXPORT + #else + #define MULTI_INTERFACE_SENSOR_PUBLIC MULTI_INTERFACE_SENSOR_IMPORT + #endif + #define MULTI_INTERFACE_SENSOR_PUBLIC_TYPE MULTI_INTERFACE_SENSOR_PUBLIC + #define MULTI_INTERFACE_SENSOR_LOCAL +#else + #define MULTI_INTERFACE_SENSOR_EXPORT __attribute__ ((visibility("default"))) + #define MULTI_INTERFACE_SENSOR_IMPORT + #if __GNUC__ >= 4 + #define MULTI_INTERFACE_SENSOR_PUBLIC __attribute__ ((visibility("default"))) + #define MULTI_INTERFACE_SENSOR_LOCAL __attribute__ ((visibility("hidden"))) + #else + #define MULTI_INTERFACE_SENSOR_PUBLIC + #define MULTI_INTERFACE_SENSOR_LOCAL + #endif + #define MULTI_INTERFACE_SENSOR_PUBLIC_TYPE +#endif + +#endif // MULTI_INTERFACE_SENSOR__VISIBILITY_CONTROL_H_ diff --git a/multi_interface_sensor/package.xml b/multi_interface_sensor/package.xml new file mode 100644 index 0000000000..2c18cc3c93 --- /dev/null +++ b/multi_interface_sensor/package.xml @@ -0,0 +1,20 @@ + + + + multi_interface_sensor + 0.0.0 + TODO: Package description + karsten + Apache license 2.0 + + ament_cmake_ros + + hardware_interface + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/multi_interface_sensor/src/component_lists_management.hpp b/multi_interface_sensor/src/component_lists_management.hpp new file mode 100644 index 0000000000..b73c8c6b28 --- /dev/null +++ b/multi_interface_sensor/src/component_lists_management.hpp @@ -0,0 +1,140 @@ +// 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 COMPONENT_LISTS_MANAGEMENT_HPP_ +#define COMPONENT_LISTS_MANAGEMENT_HPP_ + +#include +#include +#include + +#include "hardware_interface/types/hardware_interface_return_values.hpp" + +namespace multi_interface_sensor +{ + +using hardware_interface::return_type; + +/** + * \brief Get values for queried_interfaces from the int_values. int_values data structure matches + * int_interfaces vector. + * + * \param values values to return. + * \param queried_interfaces interfaces for which values are queried. + * \param int_interfaces full list of interfaces of a component. + * \param int_values internal values of a component. + * \return return_type::INTERFACE_VALUE_SIZE_NOT_EQUAL if values and queried_interfaces arguments + * do not have the same length; return_type::INTERFACE_NOT_FOUND if one of queried_interfaces is + * not defined in int_interfaces; return return_type::INTERFACE_NOT_PROVIDED if queried_interfaces + * list is is empty; return_type::OK otherwise. + */ +inline return_type get_internal_values( + std::vector & values, const std::vector & queried_interfaces, + const std::vector & int_interfaces, const std::vector & int_values) +{ + if (queried_interfaces.size() == 0) { + return return_type::INTERFACE_NOT_PROVIDED; + } + + for (const auto & interface : queried_interfaces) { + auto it = std::find( + int_interfaces.begin(), int_interfaces.end(), interface); + if (it != int_interfaces.end()) { + values.push_back(int_values[std::distance(int_interfaces.begin(), it)]); + } else { + values.clear(); + return return_type::INTERFACE_NOT_FOUND; + } + } + return return_type::OK; +} + +/** + * \brief Set all internal values to to other vector. Return value is used for API consistency. + * + * \param values output list of values. + * \param int_values internal values of the component. + * \return return_type::OK always. + */ +inline return_type get_internal_values( + std::vector & values, const std::vector & int_values) +{ + values.clear(); + for (const auto & int_value : int_values) { + values.push_back(int_value); + } + return return_type::OK; +} + +/** + * \brief set values for queried_interfaces to the int_values. int_values data structure matches + * int_interfaces vector. + * + * \param values values to set. + * \param queried_interfaces interfaces for which values are queried. + * \param int_interfaces full list of interfaces of a component. + * \param int_values internal values of a component. + * \return return return_type::INTERFACE_NOT_PROVIDED if + * queried_interfaces list is is empty; return_type::INTERFACE_VALUE_SIZE_NOT_EQUAL if values and + * queried_interfaces arguments do not have the same length; return_type::INTERFACE_NOT_FOUND if + * one of queried_interfaces is not defined in int_interfaces; return_type::OK otherwise. + * + * \todo The error handling in this function could lead to incosistant command or state variables + * for different interfaces. This should be changed in the future. + * (see: https://github.com/ros-controls/ros2_control/issues/129) + */ +inline return_type set_internal_values( + const std::vector & values, const std::vector & queried_interfaces, + const std::vector & int_interfaces, std::vector & int_values) +{ + if (queried_interfaces.size() == 0) { + return return_type::INTERFACE_NOT_PROVIDED; + } + if (values.size() != queried_interfaces.size()) { + return return_type::INTERFACE_VALUE_SIZE_NOT_EQUAL; + } + + for (auto q_it = queried_interfaces.begin(); q_it != queried_interfaces.end(); ++q_it) { + auto it = std::find(int_interfaces.begin(), int_interfaces.end(), *q_it); + if (it != int_interfaces.end()) { + int_values[std::distance(int_interfaces.begin(), it)] = + values[std::distance(queried_interfaces.begin(), q_it)]; + } else { + return return_type::INTERFACE_NOT_FOUND; + } + } + return return_type::OK; +} + +/** + * \brief set all values to compoenents internal values. + * + * \param values values to set. + * \param int_values internal values of a component. + * \return return_type::INTERFACE_VALUE_SIZE_NOT_EQUAL if the size of the arguments is not equal, + * return_type::OK otherwise. + */ +inline return_type set_internal_values( + const std::vector & values, std::vector & int_values) +{ + if (values.size() == int_values.size()) { + int_values = values; + } else { + return return_type::INTERFACE_VALUE_SIZE_NOT_EQUAL; + } + return return_type::OK; +} + +} // namespace multi_interface_sensor +#endif // COMPONENT_LISTS_MANAGEMENT_HPP_ diff --git a/multi_interface_sensor/src/multi_interface_sensor.cpp b/multi_interface_sensor/src/multi_interface_sensor.cpp new file mode 100644 index 0000000000..591679f9d6 --- /dev/null +++ b/multi_interface_sensor/src/multi_interface_sensor.cpp @@ -0,0 +1,82 @@ +// 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 "multi_interface_sensor/multi_interface_sensor.hpp" + +#include +#include +#include + +#include "./component_lists_management.hpp" + +namespace multi_interface_sensor +{ + +hardware_interface::return_type MultiInterfaceSensor::configure( + const hardware_interface::components::ComponentInfo & sensor_info) +{ + const size_t state_interfaces_size = sensor_info.state_interfaces.size(); + + // fail if no interfaces at all are specified + if (state_interfaces_size == 0u) { + return hardware_interface::return_type::INTERFACE_NOT_PROVIDED; + } + + auto has_duplicates = [](const std::vector interfaces) -> bool + { + std::set set(interfaces.begin(), interfaces.end()); + return set.size() != interfaces.size(); + }; + + // fail if state interfaces has duplicates + if (has_duplicates(sensor_info.state_interfaces)) { + return hardware_interface::return_type::INTERFACE_DUPLICATES; + } + + state_interfaces_ = sensor_info.state_interfaces; + state_values_.resize(state_interfaces_size); + + return hardware_interface::return_type::OK; +} + +std::vector MultiInterfaceSensor::get_state_interfaces() const +{ + return state_interfaces_; +} + +hardware_interface::return_type MultiInterfaceSensor::get_state( + std::vector & state, + const std::vector & interfaces) const +{ + return get_internal_values(state, interfaces, state_interfaces_, state_values_); +} + +hardware_interface::return_type MultiInterfaceSensor::get_state(std::vector & state) const +{ + return get_internal_values(state, state_values_); +} + +hardware_interface::return_type MultiInterfaceSensor::set_state( + const std::vector & state, + const std::vector & interfaces) +{ + return set_internal_values(state, interfaces, state_interfaces_, state_values_); +} + +hardware_interface::return_type MultiInterfaceSensor::set_state(const std::vector & state) +{ + return set_internal_values(state, state_values_); +} + +} // namespace multi_interface_sensor diff --git a/multi_interface_sensor/test/test_multi_interface_sensor.cpp b/multi_interface_sensor/test/test_multi_interface_sensor.cpp new file mode 100644 index 0000000000..7c9d19aba4 --- /dev/null +++ b/multi_interface_sensor/test/test_multi_interface_sensor.cpp @@ -0,0 +1,152 @@ +// 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 "multi_interface_sensor/multi_interface_sensor.hpp" + +#include "hardware_interface/components/component_info.hpp" +#include "hardware_interface/types/hardware_interface_type_values.hpp" + +using namespace ::testing; // NOLINT + +class TestMultiInterfaceSensor : public Test +{ +}; + +TEST_F(TestMultiInterfaceSensor, empty_initialized) +{ + multi_interface_sensor::MultiInterfaceSensor sensor; + EXPECT_TRUE(sensor.get_state_interfaces().empty()); +} + +TEST_F(TestMultiInterfaceSensor, wrong_initialized) +{ + { + multi_interface_sensor::MultiInterfaceSensor sensor; + hardware_interface::components::ComponentInfo sensor_info; + EXPECT_EQ( + hardware_interface::return_type::INTERFACE_NOT_PROVIDED, sensor.configure(sensor_info)); + } + + { + hardware_interface::components::ComponentInfo sensor_info; + sensor_info.state_interfaces.push_back("position"); + sensor_info.state_interfaces.push_back("position"); + multi_interface_sensor::MultiInterfaceSensor sensor; + EXPECT_EQ(hardware_interface::return_type::INTERFACE_DUPLICATES, sensor.configure(sensor_info)); + } +} + +TEST_F(TestMultiInterfaceSensor, correct_initialized) +{ + { + hardware_interface::components::ComponentInfo sensor_info; + sensor_info.state_interfaces.push_back("effort"); + multi_interface_sensor::MultiInterfaceSensor sensor; + EXPECT_EQ(hardware_interface::return_type::OK, sensor.configure(sensor_info)); + ASSERT_EQ(1u, sensor.get_state_interfaces().size()); + EXPECT_EQ("effort", sensor.get_state_interfaces()[0]); + } +} + +TEST_F(TestMultiInterfaceSensor, getters_and_setters) +{ + multi_interface_sensor::MultiInterfaceSensor sensor; + hardware_interface::components::ComponentInfo sensor_info; + sensor_info.state_interfaces.push_back("force_x"); + sensor_info.state_interfaces.push_back("force_y"); + sensor_info.state_interfaces.push_back("force_z"); + sensor_info.state_interfaces.push_back("torque_x"); + sensor_info.state_interfaces.push_back("torque_y"); + sensor_info.state_interfaces.push_back("torque_z"); + ASSERT_EQ( + hardware_interface::return_type::OK, sensor.configure(sensor_info)); + // default initialize values + ASSERT_EQ( + hardware_interface::return_type::OK, + sensor.set_state({1.34, 5.67, 8.21, 5.63, 5.99, 4.32})); + + EXPECT_EQ( + hardware_interface::return_type::OK, + sensor.configure(sensor_info)); + ASSERT_EQ(6u, sensor.get_state_interfaces().size()); + EXPECT_EQ(sensor.get_state_interfaces()[0], "force_x"); + EXPECT_EQ(sensor.get_state_interfaces()[5], "torque_z"); + std::vector input = {5.23, 6.7, 2.5, 3.8, 8.9, 12.3}; + std::vector output; + std::vector interfaces; + EXPECT_EQ( + hardware_interface::return_type::INTERFACE_NOT_PROVIDED, + sensor.get_state(output, interfaces)); + interfaces.push_back("force_y"); + EXPECT_EQ( + hardware_interface::return_type::OK, + sensor.get_state(output, interfaces)); + ASSERT_EQ(1u, output.size()); + EXPECT_EQ(5.67, output[0]); + + // State getters and setters + interfaces.clear(); + EXPECT_EQ( + hardware_interface::return_type::INTERFACE_NOT_PROVIDED, + sensor.set_state(input, interfaces)); + 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( + hardware_interface::return_type::INTERFACE_VALUE_SIZE_NOT_EQUAL, + sensor.set_state(input, interfaces)); + 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( + hardware_interface::return_type::INTERFACE_NOT_FOUND, + sensor.set_state(input, interfaces)); + interfaces.clear(); + interfaces = sensor.get_state_interfaces(); + EXPECT_EQ( + hardware_interface::return_type::OK, + sensor.set_state(input, interfaces)); + + output.clear(); + EXPECT_EQ( + hardware_interface::return_type::OK, + sensor.get_state(output, interfaces)); + ASSERT_EQ(6u, output.size()); + EXPECT_EQ(5.23, output[0]); + + interfaces.clear(); + interfaces.push_back(hardware_interface::HW_IF_VELOCITY); + EXPECT_EQ( + hardware_interface::return_type::INTERFACE_NOT_FOUND, + sensor.get_state(output, interfaces)); + + input.clear(); + EXPECT_EQ( + hardware_interface::return_type::INTERFACE_VALUE_SIZE_NOT_EQUAL, + sensor.set_state(input)); + input = {5.23, 6.7, 2.5, 3.8, 8.9, 12.3}; + EXPECT_EQ( + hardware_interface::return_type::OK, + sensor.set_state(input)); + + EXPECT_EQ( + hardware_interface::return_type::OK, + sensor.get_state(output)); + ASSERT_EQ(6u, output.size()); + EXPECT_EQ(12.3, output[5]); +} From 4c3ff4178817c7230517c3be5e3647ac7f8e4072 Mon Sep 17 00:00:00 2001 From: Karsten Knese Date: Wed, 7 Oct 2020 13:43:56 -0700 Subject: [PATCH 13/18] update doxygen Signed-off-by: Karsten Knese --- .../include/hardware_interface/components/sensor_interface.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/hardware_interface/include/hardware_interface/components/sensor_interface.hpp b/hardware_interface/include/hardware_interface/components/sensor_interface.hpp index d7c3e60a0d..f082a6be21 100644 --- a/hardware_interface/include/hardware_interface/components/sensor_interface.hpp +++ b/hardware_interface/include/hardware_interface/components/sensor_interface.hpp @@ -45,7 +45,7 @@ class SensorInterface /** * \brief Configure base sensor class based on the description in the robot's URDF file. * - * \param joint_info structure with data from URDF. + * \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. */ From 9c7e1b684c0fd14f01f1c54776d7a70e904fa78f Mon Sep 17 00:00:00 2001 From: Karsten Knese Date: Thu, 8 Oct 2020 16:20:32 -0700 Subject: [PATCH 14/18] safe resource loaning Signed-off-by: Karsten Knese --- controller_manager/CMakeLists.txt | 5 +- controller_manager/src/resource_manager.cpp | 154 +++++++++++++++--- controller_manager/src/resource_manager.hpp | 91 ++++++++++- .../test/test_resource_manager.cpp | 44 ++++- .../hardware_interface/components/joint.hpp | 13 +- .../hardware_interface/components/sensor.hpp | 22 ++- hardware_interface/src/components/joint.cpp | 10 +- hardware_interface/src/components/sensor.cpp | 10 +- ros2_control/package.xml | 2 + 9 files changed, 298 insertions(+), 53 deletions(-) diff --git a/controller_manager/CMakeLists.txt b/controller_manager/CMakeLists.txt index a09e598b6f..bf1e82499f 100644 --- a/controller_manager/CMakeLists.txt +++ b/controller_manager/CMakeLists.txt @@ -123,12 +123,15 @@ if(BUILD_TESTING) pluginlib_export_plugin_description_file( hardware_interface test/test_hardware_resources.xml) - ament_add_gtest( + ament_add_gmock( test_resource_manager test/test_resource_manager.cpp + ENV ASAN_OPTIONS=detect_container_overflow=0 ) target_include_directories(test_resource_manager PRIVATE include src) target_link_libraries(test_resource_manager controller_manager) + set_target_properties(test_resource_manager PROPERTIES COMPILE_FLAGS "-fsanitize=undefined -fsanitize=address") + set_target_properties(test_resource_manager PROPERTIES LINK_FLAGS "-fsanitize=undefined -fsanitize=address") endif() ament_export_libraries( diff --git a/controller_manager/src/resource_manager.cpp b/controller_manager/src/resource_manager.cpp index 1ef7178540..ac9792a9b2 100644 --- a/controller_manager/src/resource_manager.cpp +++ b/controller_manager/src/resource_manager.cpp @@ -15,6 +15,7 @@ #include #include #include +#include #include #include "hardware_interface/actuator_hardware.hpp" @@ -33,20 +34,22 @@ namespace controller_manager { +using ComponentDeleter = hardware_interface::components::Deleter; + class ResourceStorage { - static constexpr const char * pkg_name = "hardware_interface"; + static constexpr auto pkg_name = "hardware_interface"; - static constexpr const char * joint_component_interface_name = + static constexpr auto joint_component_interface_name = "hardware_interface::components::JointInterface"; - static constexpr const char * sensor_component_interface_name = + static constexpr auto sensor_component_interface_name = "hardware_interface::components::SensorInterface"; - static constexpr const char * actuator_interface_name = + static constexpr auto actuator_interface_name = "hardware_interface::ActuatorHardwareInterface"; - static constexpr const char * sensor_interface_name = + static constexpr auto sensor_interface_name = "hardware_interface::SensorHardwareInterface"; - static constexpr const char * system_interface_name = + static constexpr auto system_interface_name = "hardware_interface::SystemHardwareInterface"; public: @@ -64,52 +67,53 @@ class ResourceStorage void initialize_component( const hardware_interface::components::ComponentInfo & component_info, pluginlib::ClassLoader & loader, - std::vector & container) + std::unordered_map & container, + ComponentDeleter deleter) { // hardware_class_type has to match class name in plugin xml description // TODO(karsten1987) extract package from hardware_class_type // e.g.: / // TODO(dstoegl) why is it class_type here and "hardware_class_type" in hardware_info? - auto interface = std::unique_ptr( - loader.createUnmanagedInstance(component_info.class_type)); - ComponentT actuator(std::move(interface)); - container.emplace_back(std::move(actuator)); - container.back().configure(component_info); + auto interface = loader.createSharedInstance(component_info.class_type); + container.emplace(std::make_pair(component_info.name, ComponentT(interface, deleter))); + container.at(component_info.name).configure(component_info); } void initialize_joint_component( - const hardware_interface::components::ComponentInfo & component_info) + const hardware_interface::components::ComponentInfo & component_info, + ComponentDeleter deleter) { initialize_component< hardware_interface::components::Joint, hardware_interface::components::JointInterface>( - component_info, joint_component_loader_, joint_components_); + component_info, joint_component_loader_, joint_components_, deleter); } void initialize_sensor_component( - const hardware_interface::components::ComponentInfo & component_info) + const hardware_interface::components::ComponentInfo & component_info, + ComponentDeleter deleter) { initialize_component< hardware_interface::components::Sensor, hardware_interface::components::SensorInterface>( - component_info, sensor_component_loader_, sensor_components_); + component_info, sensor_component_loader_, sensor_components_, deleter); } template void initialize_hardware( const hardware_interface::HardwareInfo & hardware_info, pluginlib::ClassLoader & loader, - std::vector & container) + std::unordered_map & container) { // hardware_class_type has to match class name in plugin xml description // TODO(karsten1987) extract package from hardware_class_type // e.g.: / auto interface = std::unique_ptr( loader.createUnmanagedInstance(hardware_info.hardware_class_type)); - HardwareT actuator(std::move(interface)); - container.emplace_back(std::move(actuator)); - container.back().configure(hardware_info); + container.emplace(std::make_pair(hardware_info.name, HardwareT(std::move(interface)))); + container.at(hardware_info.name).configure(hardware_info); } + void initialize_actuator(const hardware_interface::HardwareInfo & hardware_info) { initialize_hardware< @@ -138,17 +142,17 @@ class ResourceStorage pluginlib::ClassLoader joint_component_loader_; pluginlib::ClassLoader sensor_component_loader_; - std::vector joint_components_; - std::vector sensor_components_; + std::unordered_map joint_components_; + std::unordered_map sensor_components_; // hardware plugins pluginlib::ClassLoader actuator_loader_; pluginlib::ClassLoader sensor_loader_; pluginlib::ClassLoader system_loader_; - std::vector actuators_; - std::vector sensors_; - std::vector systems_; + std::unordered_map actuators_; + std::unordered_map sensors_; + std::unordered_map systems_; }; ResourceManager::ResourceManager() @@ -178,37 +182,133 @@ ResourceManager::ResourceManager(const std::string & urdf) } for (const auto & joint : hardware.joints) { - resource_storage_->initialize_joint_component(joint); + claimed_resource_map_.insert({joint.name, false}); + auto deleter = std::bind(&ResourceManager::release_component, this, joint.name); + resource_storage_->initialize_joint_component(joint, deleter); + // TODO(karsten1987) Verify that parser warns when sensor and joint + // component are called the same. } for (const auto & sensor : hardware.sensors) { - resource_storage_->initialize_sensor_component(sensor); + claimed_resource_map_.insert({sensor.name, false}); + auto deleter = std::bind(&ResourceManager::release_component, this, sensor.name); + resource_storage_->initialize_sensor_component(sensor, deleter); } } } +void ResourceManager::release_component(const std::string & component_id) +{ + std::lock_guard lg(resource_lock_); + claimed_resource_map_[component_id] = false; +} + size_t ResourceManager::joint_components_size() const { return resource_storage_->joint_components_.size(); } +std::vector ResourceManager::joint_components_name() const +{ + std::vector ret; + ret.reserve(resource_storage_->joint_components_.size()); + for (auto const & component : resource_storage_->joint_components_) { + ret.emplace_back(std::get<0>(component)); + } + + return ret; +} + size_t ResourceManager::sensor_components_size() const { return resource_storage_->sensor_components_.size(); } +std::vector ResourceManager::sensor_components_name() const +{ + std::vector ret; + ret.reserve(resource_storage_->sensor_components_.size()); + for (const auto & component : resource_storage_->sensor_components_) { + ret.emplace_back(std::get<0>(component)); + } + + return ret; +} + size_t ResourceManager::actuator_interfaces_size() const { return resource_storage_->actuators_.size(); } +std::vector ResourceManager::actuator_interfaces_name() const +{ + std::vector ret; + ret.reserve(resource_storage_->actuators_.size()); + for (const auto & hardware : resource_storage_->actuators_) { + ret.emplace_back(std::get<0>(hardware)); + } + + return ret; +} + size_t ResourceManager::sensor_interfaces_size() const { return resource_storage_->sensors_.size(); } +std::vector ResourceManager::sensor_interfaces_name() const +{ + std::vector ret; + ret.reserve(resource_storage_->sensors_.size()); + for (const auto & hardware : resource_storage_->sensors_) { + ret.emplace_back(std::get<0>(hardware)); + } + + return ret; +} + size_t ResourceManager::system_interfaces_size() const { return resource_storage_->systems_.size(); } + +std::vector ResourceManager::system_interfaces_name() const +{ + std::vector ret; + ret.reserve(resource_storage_->systems_.size()); + for (const auto & hardware : resource_storage_->systems_) { + ret.emplace_back(std::get<0>(hardware)); + } + + return ret; +} + +hardware_interface::components::Sensor +ResourceManager::claim_sensor(const std::string & sensor_id) +{ + std::lock_guard lg(resource_lock_); + if (!sensor_exists(sensor_id)) { + throw std::runtime_error(std::string("sensor id") + sensor_id + " doesn't exist"); + } + if (sensor_is_claimed(sensor_id)) { + throw std::runtime_error(std::string("sensor id") + sensor_id + " is already claimed"); + } + + claimed_resource_map_[sensor_id] = true; + return resource_storage_->sensor_components_[sensor_id]; +} + +bool ResourceManager::sensor_exists(const std::string & sensor_id) const +{ + return resource_storage_->sensor_components_.find(sensor_id) != + resource_storage_->sensor_components_.end(); +} + +bool ResourceManager::sensor_is_claimed(const std::string & sensor_id) const +{ + std::lock_guard lg(resource_lock_); + if (!sensor_exists(sensor_id)) {return false;} + + return claimed_resource_map_.at(sensor_id); +} } // namespace controller_manager diff --git a/controller_manager/src/resource_manager.hpp b/controller_manager/src/resource_manager.hpp index 3646317619..643c62e6e6 100644 --- a/controller_manager/src/resource_manager.hpp +++ b/controller_manager/src/resource_manager.hpp @@ -17,6 +17,10 @@ #include #include +#include +#include + +#include "hardware_interface/components/sensor.hpp" namespace controller_manager { @@ -30,24 +34,105 @@ class ResourceManager explicit ResourceManager(const std::string & urdf); - ResourceManager(const ResourceManager &) = delete; + ResourceManager(const ResourceManager & other) = delete; ~ResourceManager(); + /// Return the number of joint components + /** + * \return number of joints + */ size_t joint_components_size() const; + /// Return the names of joint components + /** + * \return vector of joint names + */ + std::vector joint_components_name() const; + + /// Return the number of sensor components + /** + * \return number of sensors + */ size_t sensor_components_size() const; + /// Return the names of sensor components + /** + * \return vector of sensor names + */ + std::vector sensor_components_name() const; + + /// Return the number of hardware actuators + /** + * \return number of actuators + */ size_t actuator_interfaces_size() const; + /// Return the names of hardware actuators + /** + * \return vector of actuator names + */ + std::vector actuator_interfaces_name() const; + + /// Return the number of hardware sensors + /** + * \return number of sensors + */ size_t sensor_interfaces_size() const; + /// Return the names of hardware sensors + /** + * \return vector of sensor names + */ + std::vector sensor_interfaces_name() const; + + /// Return the number of hardware systems + /** + * \return number of sysytems + */ size_t system_interfaces_size() const; - // loan_joint(const std::string & name); - // loan_sensor(const std::string & name); + /// Return the names of hardware systems + /** + * \return vector of system names + */ + std::vector system_interfaces_name() const; + + /// Claim a sensor component based on its id + /** + * The resource is claimed as long as being in scope. + * Once the resource is going out of scope, the destructor + * returns and thus frees the resource to claimed by others. + * + * \param sensor_id String identifier which sensor to claim + * \return Sensor claimed sensor component + */ + hardware_interface::components::Sensor claim_sensor(const std::string & sensor_id); + + /// Verify if a sensor exist with a given id + /** + * Before claiming a sensor, it might be worth to check if the sensor exists. + * \param sensor_id String identifier which sensor to look for + * \return bool True if sensor exists, else false + */ + bool sensor_exists(const std::string & sensor_id) const; + + /// Verify if a sensor is claimed + /** + * Sensors can only uniquely be claimed. + * + * \param sensor_id String identifier which sensor to look for + * \return bool True if component is claimed, else false + */ + bool sensor_is_claimed(const std::string & sensor_id) const; private: + void release_component(const std::string & component_id); + + // TODO(karsten1987): Optimize this with std::vector, maps are bad + std::unordered_map claimed_resource_map_; + + mutable std::recursive_mutex resource_lock_; std::unique_ptr resource_storage_; }; diff --git a/controller_manager/test/test_resource_manager.cpp b/controller_manager/test/test_resource_manager.cpp index 93d76ce0ac..53364f8cc7 100644 --- a/controller_manager/test/test_resource_manager.cpp +++ b/controller_manager/test/test_resource_manager.cpp @@ -12,13 +12,15 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include +#include #include #include #include "resource_manager.hpp" -class TestResourceManager : public ::testing::Test +using namespace ::testing; // NOLINT + +class TestResourceManager : public Test { public: static void SetUpTestCase() @@ -137,6 +139,11 @@ class TestResourceManager : public ::testing::Test TEST_F(TestResourceManager, initialization_empty) { controller_manager::ResourceManager rm; + + EXPECT_EQ(0u, rm.joint_components_size()); + EXPECT_EQ(0u, rm.sensor_components_size()); + EXPECT_FALSE(rm.sensor_exists("sensor1")); + EXPECT_EQ(0u, rm.actuator_interfaces_size()); EXPECT_EQ(0u, rm.sensor_interfaces_size()); EXPECT_EQ(0u, rm.system_interfaces_size()); @@ -147,9 +154,42 @@ TEST_F(TestResourceManager, initialization_with_urdf) { controller_manager::ResourceManager rm(urdf); EXPECT_EQ(3u, rm.joint_components_size()); + auto joint_component_names = rm.joint_components_name(); + for (const auto & joint_name : {"joint1", "joint2", "joint3"}) { + EXPECT_NE( + joint_component_names.end(), + std::find(joint_component_names.begin(), joint_component_names.end(), joint_name)); + } + EXPECT_EQ(1u, rm.sensor_components_size()); + EXPECT_EQ("sensor1", rm.sensor_components_name()[0]); + EXPECT_TRUE(rm.sensor_exists("sensor1")) << "sensor1 does not exist"; + EXPECT_FALSE(rm.sensor_exists("non-existing-sensor")); EXPECT_EQ(1u, rm.actuator_interfaces_size()); + EXPECT_EQ("TestActuatorHardware", rm.actuator_interfaces_name()[0]); EXPECT_EQ(1u, rm.sensor_interfaces_size()); + EXPECT_EQ("TestSensorHardware", rm.sensor_interfaces_name()[0]); EXPECT_EQ(1u, rm.system_interfaces_size()); + EXPECT_EQ("TestSystemHardware", rm.system_interfaces_name()[0]); +} + +TEST_F(TestResourceManager, resource_claiming) { + auto urdf = urdf_head_ + test_hardware_resource_system_ + urdf_tail_; + controller_manager::ResourceManager rm(urdf); + + EXPECT_EQ(1u, rm.sensor_components_size()); + EXPECT_FALSE(rm.sensor_is_claimed("sensor1")); + + { + auto sensor1 = rm.claim_sensor("sensor1"); + EXPECT_TRUE(rm.sensor_is_claimed("sensor1")); + try { + auto sensor1_again = rm.claim_sensor("sensor1"); + FAIL(); + } catch (const std::runtime_error &) { + SUCCEED(); + } + } + EXPECT_FALSE(rm.sensor_is_claimed("sensor1")); } diff --git a/hardware_interface/include/hardware_interface/components/joint.hpp b/hardware_interface/include/hardware_interface/components/joint.hpp index 73b410e285..215ce8ebf9 100644 --- a/hardware_interface/include/hardware_interface/components/joint.hpp +++ b/hardware_interface/include/hardware_interface/components/joint.hpp @@ -29,16 +29,20 @@ namespace hardware_interface namespace components { +using Deleter = std::function; + class Joint final { public: Joint() = default; - explicit Joint(Joint && other) = default; + Joint(Joint && other) = default; - explicit Joint(std::unique_ptr impl); + HARDWARE_INTERFACE_PUBLIC + Joint(std::shared_ptr impl, Deleter deleter); - ~Joint() = default; + HARDWARE_INTERFACE_PUBLIC + ~Joint(); HARDWARE_INTERFACE_PUBLIC return_type configure(const ComponentInfo & joint_info); @@ -79,7 +83,8 @@ class Joint final return_type set_state(const std::vector & state); private: - std::unique_ptr impl_; + std::shared_ptr impl_; + Deleter deleter_; }; } // namespace components diff --git a/hardware_interface/include/hardware_interface/components/sensor.hpp b/hardware_interface/include/hardware_interface/components/sensor.hpp index 56de7e8383..98df21f2de 100644 --- a/hardware_interface/include/hardware_interface/components/sensor.hpp +++ b/hardware_interface/include/hardware_interface/components/sensor.hpp @@ -29,48 +29,46 @@ namespace hardware_interface namespace components { +using Deleter = std::function; + class Sensor final { public: - HARDWARE_INTERFACE_PUBLIC Sensor() = default; - explicit Sensor(Sensor && other) = default; + HARDWARE_INTERFACE_PUBLIC + Sensor(std::shared_ptr impl, Deleter deleter); + + Sensor(const Sensor & other) = default; - explicit Sensor(std::unique_ptr impl); + Sensor(Sensor && other) = default; HARDWARE_INTERFACE_PUBLIC - virtual - ~Sensor() = default; + ~Sensor(); HARDWARE_INTERFACE_PUBLIC - virtual return_type configure(const ComponentInfo & sensor_info); HARDWARE_INTERFACE_PUBLIC - virtual std::vector get_state_interfaces() const; HARDWARE_INTERFACE_PUBLIC - virtual return_type get_state( std::vector & state, const std::vector & interfaces) const; HARDWARE_INTERFACE_PUBLIC - virtual return_type get_state(std::vector & state) const; HARDWARE_INTERFACE_PUBLIC - virtual return_type set_state( const std::vector & state, const std::vector & interfaces); HARDWARE_INTERFACE_PUBLIC - virtual return_type set_state(const std::vector & state); private: - std::unique_ptr impl_; + std::shared_ptr impl_; + Deleter deleter_; }; } // namespace components diff --git a/hardware_interface/src/components/joint.cpp b/hardware_interface/src/components/joint.cpp index 891b133d9f..82035c8efc 100644 --- a/hardware_interface/src/components/joint.cpp +++ b/hardware_interface/src/components/joint.cpp @@ -26,10 +26,16 @@ namespace hardware_interface namespace components { -Joint::Joint(std::unique_ptr impl) -: impl_(std::move(impl)) +Joint::Joint(std::shared_ptr impl, Deleter deleter) +: impl_(std::move(impl)), + deleter_(deleter) {} +Joint::~Joint() +{ + if (deleter_) {deleter_();} +} + return_type Joint::configure(const ComponentInfo & joint_info) { return impl_->configure(joint_info); diff --git a/hardware_interface/src/components/sensor.cpp b/hardware_interface/src/components/sensor.cpp index 8d104c723a..f69720ad9b 100644 --- a/hardware_interface/src/components/sensor.cpp +++ b/hardware_interface/src/components/sensor.cpp @@ -28,10 +28,16 @@ namespace hardware_interface namespace components { -Sensor::Sensor(std::unique_ptr impl) -: impl_(std::move(impl)) +Sensor::Sensor(std::shared_ptr impl, Deleter deleter) +: impl_(std::move(impl)), + deleter_(deleter) {} +Sensor::~Sensor() +{ + if (deleter_) {deleter_();} +} + return_type Sensor::configure(const ComponentInfo & sensor_info) { return impl_->configure(sensor_info); diff --git a/ros2_control/package.xml b/ros2_control/package.xml index e4673a0bbe..48e2d2e520 100644 --- a/ros2_control/package.xml +++ b/ros2_control/package.xml @@ -14,6 +14,8 @@ controller_interface controller_manager hardware_interface + multi_interface_joint + multi_interface_sensor transmission_interface test_robot_hardware From 9b056138f40ca22696987363aeb1a111ac02af24 Mon Sep 17 00:00:00 2001 From: Karsten Knese Date: Thu, 8 Oct 2020 19:30:58 -0700 Subject: [PATCH 15/18] fix gcc Signed-off-by: Karsten Knese --- controller_manager/src/resource_manager.hpp | 1 + .../include/hardware_interface/components/joint.hpp | 1 + .../include/hardware_interface/components/sensor.hpp | 1 + 3 files changed, 3 insertions(+) diff --git a/controller_manager/src/resource_manager.hpp b/controller_manager/src/resource_manager.hpp index 643c62e6e6..221568b120 100644 --- a/controller_manager/src/resource_manager.hpp +++ b/controller_manager/src/resource_manager.hpp @@ -16,6 +16,7 @@ #define RESOURCE_MANAGER_HPP_ #include +#include #include #include #include diff --git a/hardware_interface/include/hardware_interface/components/joint.hpp b/hardware_interface/include/hardware_interface/components/joint.hpp index 215ce8ebf9..e7627b1d55 100644 --- a/hardware_interface/include/hardware_interface/components/joint.hpp +++ b/hardware_interface/include/hardware_interface/components/joint.hpp @@ -15,6 +15,7 @@ #ifndef HARDWARE_INTERFACE__COMPONENTS__JOINT_HPP_ #define HARDWARE_INTERFACE__COMPONENTS__JOINT_HPP_ +#include #include #include #include diff --git a/hardware_interface/include/hardware_interface/components/sensor.hpp b/hardware_interface/include/hardware_interface/components/sensor.hpp index 98df21f2de..4b42da9e47 100644 --- a/hardware_interface/include/hardware_interface/components/sensor.hpp +++ b/hardware_interface/include/hardware_interface/components/sensor.hpp @@ -15,6 +15,7 @@ #ifndef HARDWARE_INTERFACE__COMPONENTS__SENSOR_HPP_ #define HARDWARE_INTERFACE__COMPONENTS__SENSOR_HPP_ +#include #include #include #include From 09ce264e284f66d6ffad6152fdb03e5f2a9b2324 Mon Sep 17 00:00:00 2001 From: Karsten Knese Date: Fri, 9 Oct 2020 12:11:48 -0700 Subject: [PATCH 16/18] claim joint components Signed-off-by: Karsten Knese --- controller_manager/src/resource_manager.cpp | 128 ++++++++++++------ controller_manager/src/resource_manager.hpp | 109 +++++++++------ .../test/test_resource_manager.cpp | 50 ++++--- .../hardware_interface/components/joint.hpp | 2 + 4 files changed, 189 insertions(+), 100 deletions(-) diff --git a/controller_manager/src/resource_manager.cpp b/controller_manager/src/resource_manager.cpp index ac9792a9b2..0c711a40de 100644 --- a/controller_manager/src/resource_manager.cpp +++ b/controller_manager/src/resource_manager.cpp @@ -182,7 +182,7 @@ ResourceManager::ResourceManager(const std::string & urdf) } for (const auto & joint : hardware.joints) { - claimed_resource_map_.insert({joint.name, false}); + claimed_component_map_.insert({joint.name, false}); auto deleter = std::bind(&ResourceManager::release_component, this, joint.name); resource_storage_->initialize_joint_component(joint, deleter); // TODO(karsten1987) Verify that parser warns when sensor and joint @@ -190,7 +190,7 @@ ResourceManager::ResourceManager(const std::string & urdf) } for (const auto & sensor : hardware.sensors) { - claimed_resource_map_.insert({sensor.name, false}); + claimed_component_map_.insert({sensor.name, false}); auto deleter = std::bind(&ResourceManager::release_component, this, sensor.name); resource_storage_->initialize_sensor_component(sensor, deleter); } @@ -200,15 +200,48 @@ ResourceManager::ResourceManager(const std::string & urdf) void ResourceManager::release_component(const std::string & component_id) { std::lock_guard lg(resource_lock_); - claimed_resource_map_[component_id] = false; + claimed_component_map_[component_id] = false; } -size_t ResourceManager::joint_components_size() const +template +inline auto component_exists(const std::string & component_name, const ContainerT & container) +{ + return container.find(component_name) != container.end(); +} + +template +inline auto component_is_claimed( + const std::string & component_name, + const ContainerT & container, + const std::unordered_map & claimed_component_map) +{ + if (!component_exists(component_name, container)) {return false;} + return claimed_component_map.at(component_name); +} + +template +inline auto claim_component( + const std::string & component_name, + const ContainerT & container, + std::unordered_map & claimed_component_map) +{ + if (!component_exists(component_name, container)) { + throw std::runtime_error(std::string("component ") + component_name + " doesn't exist"); + } + if (component_is_claimed(component_name, container, claimed_component_map)) { + throw std::runtime_error(std::string("component ") + component_name + " is already claimed"); + } + + claimed_component_map.at(component_name) = true; + return container.at(component_name); +} + +size_t ResourceManager::joint_size() const { return resource_storage_->joint_components_.size(); } -std::vector ResourceManager::joint_components_name() const +std::vector ResourceManager::joint_names() const { std::vector ret; ret.reserve(resource_storage_->joint_components_.size()); @@ -219,12 +252,32 @@ std::vector ResourceManager::joint_components_name() const return ret; } -size_t ResourceManager::sensor_components_size() const +bool ResourceManager::joint_exists(const std::string & joint_name) const +{ + return component_exists(joint_name, resource_storage_->joint_components_); +} + +bool ResourceManager::joint_is_claimed(const std::string & joint_name) const +{ + std::lock_guard lg(resource_lock_); + return component_is_claimed( + joint_name, resource_storage_->joint_components_, claimed_component_map_); +} + +hardware_interface::components::Joint +ResourceManager::claim_joint(const std::string & joint_name) +{ + std::lock_guard lg(resource_lock_); + return claim_component( + joint_name, resource_storage_->joint_components_, claimed_component_map_); +} + +size_t ResourceManager::sensor_size() const { return resource_storage_->sensor_components_.size(); } -std::vector ResourceManager::sensor_components_name() const +std::vector ResourceManager::sensor_names() const { std::vector ret; ret.reserve(resource_storage_->sensor_components_.size()); @@ -235,12 +288,32 @@ std::vector ResourceManager::sensor_components_name() const return ret; } -size_t ResourceManager::actuator_interfaces_size() const +bool ResourceManager::sensor_exists(const std::string & sensor_name) const +{ + return component_exists(sensor_name, resource_storage_->sensor_components_); +} + +bool ResourceManager::sensor_is_claimed(const std::string & sensor_name) const +{ + std::lock_guard lg(resource_lock_); + return component_is_claimed( + sensor_name, resource_storage_->sensor_components_, claimed_component_map_); +} + +hardware_interface::components::Sensor +ResourceManager::claim_sensor(const std::string & sensor_name) +{ + std::lock_guard lg(resource_lock_); + return claim_component( + sensor_name, resource_storage_->sensor_components_, claimed_component_map_); +} + +size_t ResourceManager::actuator_interface_size() const { return resource_storage_->actuators_.size(); } -std::vector ResourceManager::actuator_interfaces_name() const +std::vector ResourceManager::actuator_interface_names() const { std::vector ret; ret.reserve(resource_storage_->actuators_.size()); @@ -251,12 +324,12 @@ std::vector ResourceManager::actuator_interfaces_name() const return ret; } -size_t ResourceManager::sensor_interfaces_size() const +size_t ResourceManager::sensor_interface_size() const { return resource_storage_->sensors_.size(); } -std::vector ResourceManager::sensor_interfaces_name() const +std::vector ResourceManager::sensor_interface_names() const { std::vector ret; ret.reserve(resource_storage_->sensors_.size()); @@ -267,12 +340,12 @@ std::vector ResourceManager::sensor_interfaces_name() const return ret; } -size_t ResourceManager::system_interfaces_size() const +size_t ResourceManager::system_interface_size() const { return resource_storage_->systems_.size(); } -std::vector ResourceManager::system_interfaces_name() const +std::vector ResourceManager::system_interface_names() const { std::vector ret; ret.reserve(resource_storage_->systems_.size()); @@ -282,33 +355,4 @@ std::vector ResourceManager::system_interfaces_name() const return ret; } - -hardware_interface::components::Sensor -ResourceManager::claim_sensor(const std::string & sensor_id) -{ - std::lock_guard lg(resource_lock_); - if (!sensor_exists(sensor_id)) { - throw std::runtime_error(std::string("sensor id") + sensor_id + " doesn't exist"); - } - if (sensor_is_claimed(sensor_id)) { - throw std::runtime_error(std::string("sensor id") + sensor_id + " is already claimed"); - } - - claimed_resource_map_[sensor_id] = true; - return resource_storage_->sensor_components_[sensor_id]; -} - -bool ResourceManager::sensor_exists(const std::string & sensor_id) const -{ - return resource_storage_->sensor_components_.find(sensor_id) != - resource_storage_->sensor_components_.end(); -} - -bool ResourceManager::sensor_is_claimed(const std::string & sensor_id) const -{ - std::lock_guard lg(resource_lock_); - if (!sensor_exists(sensor_id)) {return false;} - - return claimed_resource_map_.at(sensor_id); -} } // namespace controller_manager diff --git a/controller_manager/src/resource_manager.hpp b/controller_manager/src/resource_manager.hpp index 221568b120..dc085b09bb 100644 --- a/controller_manager/src/resource_manager.hpp +++ b/controller_manager/src/resource_manager.hpp @@ -21,6 +21,7 @@ #include #include +#include "hardware_interface/components/joint.hpp" #include "hardware_interface/components/sensor.hpp" namespace controller_manager @@ -43,95 +44,123 @@ class ResourceManager /** * \return number of joints */ - size_t joint_components_size() const; + size_t joint_size() const; /// Return the names of joint components /** * \return vector of joint names */ - std::vector joint_components_name() const; + std::vector joint_names() const; + + /// Verify if a joint exist with a given name + /** + * Before claiming a joint, it might be worth to check if the joint exists. + * \param joint_name String identifier which joint to look for + * \return bool True if joint exists, else false + */ + bool joint_exists(const std::string & joint_name) const; + + /// Verify if a joint is claimed + /** + * Joints can only uniquely be claimed. + * + * \param joint_name String identifier which joint to look for + * \return bool True if component is claimed, else false + */ + bool joint_is_claimed(const std::string & joint_name) const; + + /// Claim a joint component based on its name + /** + * The resource is claimed as long as being in scope. + * Once the resource is going out of scope, the destructor + * returns and thus frees the resource to claimed by others. + * + * \param joint_name String identifier which joint to claim + * \return joint claimed joint component + */ + hardware_interface::components::Joint claim_joint(const std::string & joint_name); /// Return the number of sensor components /** * \return number of sensors */ - size_t sensor_components_size() const; + size_t sensor_size() const; /// Return the names of sensor components /** * \return vector of sensor names */ - std::vector sensor_components_name() const; + std::vector sensor_names() const; + + /// Verify if a sensor exist with a given name + /** + * Before claiming a sensor, it might be worth to check if the sensor exists. + * \param sensor_name String identifier which sensor to look for + * \return bool True if sensor exists, else false + */ + bool sensor_exists(const std::string & sensor_name) const; + + /// Verify if a sensor is claimed + /** + * Sensors can only uniquely be claimed. + * + * \param sensor_name String identifier which sensor to look for + * \return bool True if component is claimed, else false + */ + bool sensor_is_claimed(const std::string & sensor_name) const; + + /// Claim a sensor component based on its name + /** + * The resource is claimed as long as being in scope. + * Once the resource is going out of scope, the destructor + * returns and thus frees the resource to claimed by others. + * + * \param sensor_name String identifier which sensor to claim + * \return Sensor claimed sensor component + */ + hardware_interface::components::Sensor claim_sensor(const std::string & sensor_name); /// Return the number of hardware actuators /** * \return number of actuators */ - size_t actuator_interfaces_size() const; + size_t actuator_interface_size() const; /// Return the names of hardware actuators /** * \return vector of actuator names */ - std::vector actuator_interfaces_name() const; + std::vector actuator_interface_names() const; /// Return the number of hardware sensors /** * \return number of sensors */ - size_t sensor_interfaces_size() const; + size_t sensor_interface_size() const; /// Return the names of hardware sensors /** * \return vector of sensor names */ - std::vector sensor_interfaces_name() const; + std::vector sensor_interface_names() const; /// Return the number of hardware systems /** - * \return number of sysytems + * \return number of sytems */ - size_t system_interfaces_size() const; + size_t system_interface_size() const; /// Return the names of hardware systems /** * \return vector of system names */ - std::vector system_interfaces_name() const; - - /// Claim a sensor component based on its id - /** - * The resource is claimed as long as being in scope. - * Once the resource is going out of scope, the destructor - * returns and thus frees the resource to claimed by others. - * - * \param sensor_id String identifier which sensor to claim - * \return Sensor claimed sensor component - */ - hardware_interface::components::Sensor claim_sensor(const std::string & sensor_id); - - /// Verify if a sensor exist with a given id - /** - * Before claiming a sensor, it might be worth to check if the sensor exists. - * \param sensor_id String identifier which sensor to look for - * \return bool True if sensor exists, else false - */ - bool sensor_exists(const std::string & sensor_id) const; - - /// Verify if a sensor is claimed - /** - * Sensors can only uniquely be claimed. - * - * \param sensor_id String identifier which sensor to look for - * \return bool True if component is claimed, else false - */ - bool sensor_is_claimed(const std::string & sensor_id) const; + std::vector system_interface_names() const; private: void release_component(const std::string & component_id); // TODO(karsten1987): Optimize this with std::vector, maps are bad - std::unordered_map claimed_resource_map_; + std::unordered_map claimed_component_map_; mutable std::recursive_mutex resource_lock_; std::unique_ptr resource_storage_; diff --git a/controller_manager/test/test_resource_manager.cpp b/controller_manager/test/test_resource_manager.cpp index 53364f8cc7..479954a5f6 100644 --- a/controller_manager/test/test_resource_manager.cpp +++ b/controller_manager/test/test_resource_manager.cpp @@ -140,45 +140,45 @@ class TestResourceManager : public Test TEST_F(TestResourceManager, initialization_empty) { controller_manager::ResourceManager rm; - EXPECT_EQ(0u, rm.joint_components_size()); - EXPECT_EQ(0u, rm.sensor_components_size()); + EXPECT_EQ(0u, rm.joint_size()); + EXPECT_EQ(0u, rm.sensor_size()); EXPECT_FALSE(rm.sensor_exists("sensor1")); - EXPECT_EQ(0u, rm.actuator_interfaces_size()); - EXPECT_EQ(0u, rm.sensor_interfaces_size()); - EXPECT_EQ(0u, rm.system_interfaces_size()); + EXPECT_EQ(0u, rm.actuator_interface_size()); + EXPECT_EQ(0u, rm.sensor_interface_size()); + EXPECT_EQ(0u, rm.system_interface_size()); } TEST_F(TestResourceManager, initialization_with_urdf) { auto urdf = urdf_head_ + test_hardware_resource_system_ + urdf_tail_; controller_manager::ResourceManager rm(urdf); - EXPECT_EQ(3u, rm.joint_components_size()); - auto joint_component_names = rm.joint_components_name(); + EXPECT_EQ(3u, rm.joint_size()); + auto joint_names = rm.joint_names(); for (const auto & joint_name : {"joint1", "joint2", "joint3"}) { EXPECT_NE( - joint_component_names.end(), - std::find(joint_component_names.begin(), joint_component_names.end(), joint_name)); + joint_names.end(), + std::find(joint_names.begin(), joint_names.end(), joint_name)); } - EXPECT_EQ(1u, rm.sensor_components_size()); - EXPECT_EQ("sensor1", rm.sensor_components_name()[0]); + EXPECT_EQ(1u, rm.sensor_size()); + EXPECT_EQ("sensor1", rm.sensor_names()[0]); EXPECT_TRUE(rm.sensor_exists("sensor1")) << "sensor1 does not exist"; EXPECT_FALSE(rm.sensor_exists("non-existing-sensor")); - EXPECT_EQ(1u, rm.actuator_interfaces_size()); - EXPECT_EQ("TestActuatorHardware", rm.actuator_interfaces_name()[0]); - EXPECT_EQ(1u, rm.sensor_interfaces_size()); - EXPECT_EQ("TestSensorHardware", rm.sensor_interfaces_name()[0]); - EXPECT_EQ(1u, rm.system_interfaces_size()); - EXPECT_EQ("TestSystemHardware", rm.system_interfaces_name()[0]); + EXPECT_EQ(1u, rm.actuator_interface_size()); + EXPECT_EQ("TestActuatorHardware", rm.actuator_interface_names()[0]); + EXPECT_EQ(1u, rm.sensor_interface_size()); + EXPECT_EQ("TestSensorHardware", rm.sensor_interface_names()[0]); + EXPECT_EQ(1u, rm.system_interface_size()); + EXPECT_EQ("TestSystemHardware", rm.system_interface_names()[0]); } TEST_F(TestResourceManager, resource_claiming) { auto urdf = urdf_head_ + test_hardware_resource_system_ + urdf_tail_; controller_manager::ResourceManager rm(urdf); - EXPECT_EQ(1u, rm.sensor_components_size()); + EXPECT_EQ(1u, rm.sensor_size()); EXPECT_FALSE(rm.sensor_is_claimed("sensor1")); { @@ -192,4 +192,18 @@ TEST_F(TestResourceManager, resource_claiming) { } } EXPECT_FALSE(rm.sensor_is_claimed("sensor1")); + + for (const auto & joint_name : {"joint1", "joint1", "joint1", "joint2", "joint3"}) { + { + auto joint = rm.claim_joint(joint_name); + EXPECT_TRUE(rm.joint_is_claimed(joint_name)); + try { + auto joint1_again = rm.claim_joint(joint_name); + FAIL(); + } catch (const std::runtime_error &) { + SUCCEED(); + } + } + EXPECT_FALSE(rm.joint_is_claimed(joint_name)); + } } diff --git a/hardware_interface/include/hardware_interface/components/joint.hpp b/hardware_interface/include/hardware_interface/components/joint.hpp index e7627b1d55..26f7e84cd1 100644 --- a/hardware_interface/include/hardware_interface/components/joint.hpp +++ b/hardware_interface/include/hardware_interface/components/joint.hpp @@ -37,6 +37,8 @@ class Joint final public: Joint() = default; + Joint(const Joint & other) = default; + Joint(Joint && other) = default; HARDWARE_INTERFACE_PUBLIC From 34e5317d6c92d1575c13f7c587b5a4dfa463c981 Mon Sep 17 00:00:00 2001 From: Karsten Knese Date: Fri, 9 Oct 2020 14:58:44 -0700 Subject: [PATCH 17/18] allow for interface api access Signed-off-by: Karsten Knese --- controller_manager/CMakeLists.txt | 2 +- .../test_joint_component.hpp | 3 ++ .../test_sensor_component.hpp | 3 ++ .../test/test_resource_manager.cpp | 33 +++++++++++++++++ .../hardware_interface/components/joint.hpp | 35 +++++++++++++++---- .../hardware_interface/components/sensor.hpp | 35 +++++++++++++++---- 6 files changed, 98 insertions(+), 13 deletions(-) diff --git a/controller_manager/CMakeLists.txt b/controller_manager/CMakeLists.txt index bf1e82499f..8091361554 100644 --- a/controller_manager/CMakeLists.txt +++ b/controller_manager/CMakeLists.txt @@ -129,7 +129,7 @@ if(BUILD_TESTING) ENV ASAN_OPTIONS=detect_container_overflow=0 ) target_include_directories(test_resource_manager PRIVATE include src) - target_link_libraries(test_resource_manager controller_manager) + target_link_libraries(test_resource_manager controller_manager test_hardware_resources) set_target_properties(test_resource_manager PROPERTIES COMPILE_FLAGS "-fsanitize=undefined -fsanitize=address") set_target_properties(test_resource_manager PROPERTIES LINK_FLAGS "-fsanitize=undefined -fsanitize=address") endif() diff --git a/controller_manager/test/test_hardware_resources/test_joint_component.hpp b/controller_manager/test/test_hardware_resources/test_joint_component.hpp index a8b20e9655..07c738a635 100644 --- a/controller_manager/test/test_hardware_resources/test_joint_component.hpp +++ b/controller_manager/test/test_hardware_resources/test_joint_component.hpp @@ -72,5 +72,8 @@ class TestJointComponent : public hardware_interface::components::JointInterface CONTROLLER_MANAGER_PUBLIC hardware_interface::return_type set_state(const std::vector & state) override; + + // Some additional API + bool return_true() const {return true;} }; #endif // TEST_HARDWARE_RESOURCES__TEST_JOINT_COMPONENT_HPP_ diff --git a/controller_manager/test/test_hardware_resources/test_sensor_component.hpp b/controller_manager/test/test_hardware_resources/test_sensor_component.hpp index e607fa7f66..fa78d25df2 100644 --- a/controller_manager/test/test_hardware_resources/test_sensor_component.hpp +++ b/controller_manager/test/test_hardware_resources/test_sensor_component.hpp @@ -52,6 +52,9 @@ class TestSensorComponent : public hardware_interface::components::SensorInterfa CONTROLLER_MANAGER_PUBLIC hardware_interface::return_type set_state(const std::vector & state) override; + + // Some additional API + bool return_true() const {return true;} }; #endif // TEST_HARDWARE_RESOURCES__TEST_SENSOR_COMPONENT_HPP_ diff --git a/controller_manager/test/test_resource_manager.cpp b/controller_manager/test/test_resource_manager.cpp index 479954a5f6..8d9fb6324f 100644 --- a/controller_manager/test/test_resource_manager.cpp +++ b/controller_manager/test/test_resource_manager.cpp @@ -18,6 +18,9 @@ #include "resource_manager.hpp" +#include "test_hardware_resources/test_joint_component.hpp" +#include "test_hardware_resources/test_sensor_component.hpp" + using namespace ::testing; // NOLINT class TestResourceManager : public Test @@ -207,3 +210,33 @@ TEST_F(TestResourceManager, resource_claiming) { EXPECT_FALSE(rm.joint_is_claimed(joint_name)); } } + +TEST_F(TestResourceManager, interface_access) { + auto urdf = urdf_head_ + test_hardware_resource_system_ + urdf_tail_; + controller_manager::ResourceManager rm(urdf); + + EXPECT_EQ(3u, rm.joint_size()); + EXPECT_EQ(1u, rm.sensor_size()); + + { + auto joint1 = rm.claim_joint("joint1"); + { + auto test_joint = joint1.as(); + EXPECT_TRUE(test_joint.return_true()); + EXPECT_TRUE(rm.joint_is_claimed("joint1")); + } + EXPECT_TRUE(rm.joint_is_claimed("joint1")); + } + EXPECT_FALSE(rm.joint_is_claimed("joint1")); + + { + auto sensor1 = rm.claim_sensor("sensor1"); + { + auto test_sensor = sensor1.as(); + EXPECT_TRUE(test_sensor.return_true()); + EXPECT_TRUE(rm.sensor_is_claimed("sensor1")); + } + EXPECT_TRUE(rm.sensor_is_claimed("sensor1")); + } + EXPECT_FALSE(rm.sensor_is_claimed("sensor1")); +} diff --git a/hardware_interface/include/hardware_interface/components/joint.hpp b/hardware_interface/include/hardware_interface/components/joint.hpp index 26f7e84cd1..79f45000c6 100644 --- a/hardware_interface/include/hardware_interface/components/joint.hpp +++ b/hardware_interface/include/hardware_interface/components/joint.hpp @@ -32,21 +32,44 @@ namespace components using Deleter = std::function; +/// Joint wrapper class +/** + * The class represents a wrapper around an implementation of the JointInterface. + * This allows to represent various implementations under the same common API. + * Joint serves as a handle which can get claimed and loaned to each individual controller + * to obtain exclusive ownership over the component. + */ class Joint final { public: - Joint() = default; - - Joint(const Joint & other) = default; - - Joint(Joint && other) = default; - + /// Constructor for a joint component + /** + * The joint is constructed with a concrete JointInterface implementation. + * The second parameter serves as a callback upon destruction, which is used + * to cleanup the component or signal its destruction to other entities such + * as the resource manager within the controller manager. + * + * \param impl Shared pointer to a precise implementation of the joint component. + * \param deleter Callback function to be called upon destruction. + */ HARDWARE_INTERFACE_PUBLIC Joint(std::shared_ptr impl, Deleter deleter); HARDWARE_INTERFACE_PUBLIC ~Joint(); + /// Access the underlying implementation + /** + * The function returns a reference to the implementation. + * This allows for a full-access API for optimal use. + * + * \note: The given template parameter has to be directly convertible. + * A wrongly specified template argument might lead to UB. + * \return Reference to the casted interface. + */ + template + auto & as() {return *std::static_pointer_cast(impl_);} + HARDWARE_INTERFACE_PUBLIC return_type configure(const ComponentInfo & joint_info); diff --git a/hardware_interface/include/hardware_interface/components/sensor.hpp b/hardware_interface/include/hardware_interface/components/sensor.hpp index 4b42da9e47..de30a64195 100644 --- a/hardware_interface/include/hardware_interface/components/sensor.hpp +++ b/hardware_interface/include/hardware_interface/components/sensor.hpp @@ -32,21 +32,44 @@ namespace components using Deleter = std::function; +/// Sensor wrapper class +/** + * The class represents a wrapper around an implementation of the SensorInterface. + * This allows to represent various implementations under the same common API. + * Sensor serves as a handle which can get claimed and loaned to each individual controller + * to obtain exclusive ownership over the component. + */ class Sensor final { public: - Sensor() = default; - + /// Constructor for a sensor component + /** + * The sensor is constructed with a precise SensorInterface implementation. + * The second parameter serves as a callback upon destruction, which is used + * to cleanup the component or signal its destruction to other entities such + * as the resource manager within the controller manager. + * + * \param impl Shared pointer to a precise implementation of the sensor component. + * \param deleter Callback function to be called upon destruction. + */ HARDWARE_INTERFACE_PUBLIC Sensor(std::shared_ptr impl, Deleter deleter); - Sensor(const Sensor & other) = default; - - Sensor(Sensor && other) = default; - HARDWARE_INTERFACE_PUBLIC ~Sensor(); + /// Access the underlying implementation + /** + * The function returns a reference to the implementation. + * This allows for a full-access API for optimal use. + * + * \note: The given template parameter has to be directly convertible. + * A wrongly specified template argument might lead to UB. + * \return Reference to the casted interface. + */ + template + auto & as() {return *std::static_pointer_cast(impl_);} + HARDWARE_INTERFACE_PUBLIC return_type configure(const ComponentInfo & sensor_info); From 0a33718c7096a00f9093e75dedd70dc3eb31c432 Mon Sep 17 00:00:00 2001 From: Karsten Knese Date: Mon, 12 Oct 2020 15:33:34 -0700 Subject: [PATCH 18/18] hardware_interface_status -> status Signed-off-by: Karsten Knese --- .../test/test_hardware_resources/test_actuator_hardware.cpp | 6 +++--- .../test/test_hardware_resources/test_actuator_hardware.hpp | 2 +- .../test/test_hardware_resources/test_sensor_hardware.cpp | 6 +++--- .../test/test_hardware_resources/test_sensor_hardware.hpp | 2 +- .../test/test_hardware_resources/test_system_hardware.cpp | 6 +++--- .../test/test_hardware_resources/test_system_hardware.hpp | 2 +- 6 files changed, 12 insertions(+), 12 deletions(-) diff --git a/controller_manager/test/test_hardware_resources/test_actuator_hardware.cpp b/controller_manager/test/test_hardware_resources/test_actuator_hardware.cpp index 68c6a30481..e952880e4d 100644 --- a/controller_manager/test/test_hardware_resources/test_actuator_hardware.cpp +++ b/controller_manager/test/test_hardware_resources/test_actuator_hardware.cpp @@ -16,7 +16,7 @@ #include "./test_actuator_hardware.hpp" -using hardware_interface::hardware_interface_status; +using hardware_interface::status; using hardware_interface::return_type; return_type @@ -37,10 +37,10 @@ TestActuatorHardware::stop() return return_type::OK; } -hardware_interface_status +status TestActuatorHardware::get_status() const { - return hardware_interface_status::UNKNOWN; + return status::UNKNOWN; } return_type diff --git a/controller_manager/test/test_hardware_resources/test_actuator_hardware.hpp b/controller_manager/test/test_hardware_resources/test_actuator_hardware.hpp index daaaded4fc..ede3e72c72 100644 --- a/controller_manager/test/test_hardware_resources/test_actuator_hardware.hpp +++ b/controller_manager/test/test_hardware_resources/test_actuator_hardware.hpp @@ -31,7 +31,7 @@ class TestActuatorHardware : public hardware_interface::ActuatorHardwareInterfac hardware_interface::return_type stop() override; - hardware_interface::hardware_interface_status + hardware_interface::status get_status() const override; hardware_interface::return_type diff --git a/controller_manager/test/test_hardware_resources/test_sensor_hardware.cpp b/controller_manager/test/test_hardware_resources/test_sensor_hardware.cpp index b418c5b896..1b03abee0a 100644 --- a/controller_manager/test/test_hardware_resources/test_sensor_hardware.cpp +++ b/controller_manager/test/test_hardware_resources/test_sensor_hardware.cpp @@ -17,7 +17,7 @@ #include "./test_sensor_hardware.hpp" -using hardware_interface::hardware_interface_status; +using hardware_interface::status; using hardware_interface::return_type; return_type @@ -38,10 +38,10 @@ TestSensorHardware::stop() return return_type::OK; } -hardware_interface_status +status TestSensorHardware::get_status() const { - return hardware_interface_status::UNKNOWN; + return status::UNKNOWN; } return_type diff --git a/controller_manager/test/test_hardware_resources/test_sensor_hardware.hpp b/controller_manager/test/test_hardware_resources/test_sensor_hardware.hpp index 23b82b7f2f..3937e9d654 100644 --- a/controller_manager/test/test_hardware_resources/test_sensor_hardware.hpp +++ b/controller_manager/test/test_hardware_resources/test_sensor_hardware.hpp @@ -32,7 +32,7 @@ class TestSensorHardware : public hardware_interface::SensorHardwareInterface hardware_interface::return_type stop() override; - hardware_interface::hardware_interface_status + hardware_interface::status get_status() const override; hardware_interface::return_type diff --git a/controller_manager/test/test_hardware_resources/test_system_hardware.cpp b/controller_manager/test/test_hardware_resources/test_system_hardware.cpp index e69efdd4e5..43613408a6 100644 --- a/controller_manager/test/test_hardware_resources/test_system_hardware.cpp +++ b/controller_manager/test/test_hardware_resources/test_system_hardware.cpp @@ -17,7 +17,7 @@ #include "./test_system_hardware.hpp" -using hardware_interface::hardware_interface_status; +using hardware_interface::status; using hardware_interface::return_type; hardware_interface::return_type @@ -38,10 +38,10 @@ TestSystemHardware::stop() return return_type::OK; } -hardware_interface_status +status TestSystemHardware::get_status() const { - return hardware_interface_status::UNKNOWN; + return status::UNKNOWN; } return_type diff --git a/controller_manager/test/test_hardware_resources/test_system_hardware.hpp b/controller_manager/test/test_hardware_resources/test_system_hardware.hpp index c2d62bd8e6..1fcbcc8967 100644 --- a/controller_manager/test/test_hardware_resources/test_system_hardware.hpp +++ b/controller_manager/test/test_hardware_resources/test_system_hardware.hpp @@ -32,7 +32,7 @@ class TestSystemHardware : public hardware_interface::SystemHardwareInterface hardware_interface::return_type stop() override; - hardware_interface::hardware_interface_status + hardware_interface::status get_status() const override; hardware_interface::return_type