From 5c0f0a732e7d1c7a1316cc6ab11c16a27ba93bbb Mon Sep 17 00:00:00 2001 From: Karsten Knese Date: Wed, 21 Oct 2020 23:56:49 -0700 Subject: [PATCH 01/18] introducing handles Signed-off-by: Karsten Knese --- .../include/hardware_interface/handle.hpp | 2 +- .../hardware_interface/sensor_handle.hpp | 31 +++++++++++++++++++ hardware_interface/test/test_joint_handle.cpp | 17 ++++++++++ 3 files changed, 49 insertions(+), 1 deletion(-) create mode 100644 hardware_interface/include/hardware_interface/sensor_handle.hpp diff --git a/hardware_interface/include/hardware_interface/handle.hpp b/hardware_interface/include/hardware_interface/handle.hpp index 12a6cecffc..e021d08fba 100644 --- a/hardware_interface/include/hardware_interface/handle.hpp +++ b/hardware_interface/include/hardware_interface/handle.hpp @@ -32,7 +32,7 @@ class ReadOnlyHandle const std::string & name, const std::string & interface_name, double * value_ptr = nullptr) - : name_(name), interface_name_(interface_name), value_ptr_(value_ptr) + : name_(std::move(name)), interface_name_(std::move(interface_name)), value_ptr_(value_ptr) { } diff --git a/hardware_interface/include/hardware_interface/sensor_handle.hpp b/hardware_interface/include/hardware_interface/sensor_handle.hpp new file mode 100644 index 0000000000..593fea16d5 --- /dev/null +++ b/hardware_interface/include/hardware_interface/sensor_handle.hpp @@ -0,0 +1,31 @@ +// Copyright 2020 ros2_control development team +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef HARDWARE_INTERFACE__SENSOR_HANDLE_HPP_ +#define HARDWARE_INTERFACE__SENSOR_HANDLE_HPP_ + +#include "hardware_interface/handle.hpp" + +namespace hardware_interface +{ +/** A handle used to get a value on a given sensor interface. */ +class SensorHandle : public ReadOnlyHandle +{ +public: + using ReadOnlyHandle::ReadOnlyHandle; +}; + +} // namespace hardware_interface + +#endif // HARDWARE_INTERFACE__SENSOR_HANDLE_HPP_ diff --git a/hardware_interface/test/test_joint_handle.cpp b/hardware_interface/test/test_joint_handle.cpp index 6400a6a4be..7de4942fa9 100644 --- a/hardware_interface/test/test_joint_handle.cpp +++ b/hardware_interface/test/test_joint_handle.cpp @@ -73,3 +73,20 @@ TEST(TestJointHandle, with_value_ptr_initializes_new_handle_correctly) EXPECT_ANY_THROW(handle.get_value()); EXPECT_DOUBLE_EQ(new_handle.get_value(), value); } + +TEST(TestJointHandle, joint_command_handle) +{ + double value = 1.337; + JointCommandHandle handle{JOINT_NAME, FOO_INTERFACE, &value}; + EXPECT_DOUBLE_EQ(handle.get_value(), value); + EXPECT_NO_THROW(handle.set_value(0.0)); + EXPECT_DOUBLE_EQ(handle.get_value(), 0.0); +} + +TEST(TestJointHandle, joint_state_handle) +{ + double value = 1.337; + JointStateHandle handle{JOINT_NAME, FOO_INTERFACE, &value}; + EXPECT_DOUBLE_EQ(handle.get_value(), value); + // handle.set_value(5); compiler error, no set_value function +} From 2549bebd7be27566cdd444ebca8d032de44a1052 Mon Sep 17 00:00:00 2001 From: Karsten Knese Date: Thu, 22 Oct 2020 14:22:20 -0700 Subject: [PATCH 02/18] component interfaces & tests Signed-off-by: Karsten Knese --- .../components/actuator_interface.hpp | 1 + .../hardware_interface/sensor_handle.hpp | 31 ------------------- .../test/test_component_interfaces.cpp | 1 + hardware_interface/test/test_joint_handle.cpp | 17 ---------- 4 files changed, 2 insertions(+), 48 deletions(-) delete mode 100644 hardware_interface/include/hardware_interface/sensor_handle.hpp diff --git a/hardware_interface/include/hardware_interface/components/actuator_interface.hpp b/hardware_interface/include/hardware_interface/components/actuator_interface.hpp index 4e1cc3e6d9..0a2fb2b8c4 100644 --- a/hardware_interface/include/hardware_interface/components/actuator_interface.hpp +++ b/hardware_interface/include/hardware_interface/components/actuator_interface.hpp @@ -20,6 +20,7 @@ #include "hardware_interface/handle.hpp" #include "hardware_interface/hardware_info.hpp" +#include "hardware_interface/joint_handle.hpp" #include "hardware_interface/types/hardware_interface_return_values.hpp" #include "hardware_interface/types/hardware_interface_status_values.hpp" diff --git a/hardware_interface/include/hardware_interface/sensor_handle.hpp b/hardware_interface/include/hardware_interface/sensor_handle.hpp deleted file mode 100644 index 593fea16d5..0000000000 --- a/hardware_interface/include/hardware_interface/sensor_handle.hpp +++ /dev/null @@ -1,31 +0,0 @@ -// Copyright 2020 ros2_control development team -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef HARDWARE_INTERFACE__SENSOR_HANDLE_HPP_ -#define HARDWARE_INTERFACE__SENSOR_HANDLE_HPP_ - -#include "hardware_interface/handle.hpp" - -namespace hardware_interface -{ -/** A handle used to get a value on a given sensor interface. */ -class SensorHandle : public ReadOnlyHandle -{ -public: - using ReadOnlyHandle::ReadOnlyHandle; -}; - -} // namespace hardware_interface - -#endif // HARDWARE_INTERFACE__SENSOR_HANDLE_HPP_ diff --git a/hardware_interface/test/test_component_interfaces.cpp b/hardware_interface/test/test_component_interfaces.cpp index 72b41b2060..f568e1d9ed 100644 --- a/hardware_interface/test/test_component_interfaces.cpp +++ b/hardware_interface/test/test_component_interfaces.cpp @@ -325,3 +325,4 @@ TEST(TestComponentInterfaces, dummy_system) ASSERT_EQ(hardware_interface::return_type::OK, system_hw.write()); } } + diff --git a/hardware_interface/test/test_joint_handle.cpp b/hardware_interface/test/test_joint_handle.cpp index 7de4942fa9..6400a6a4be 100644 --- a/hardware_interface/test/test_joint_handle.cpp +++ b/hardware_interface/test/test_joint_handle.cpp @@ -73,20 +73,3 @@ TEST(TestJointHandle, with_value_ptr_initializes_new_handle_correctly) EXPECT_ANY_THROW(handle.get_value()); EXPECT_DOUBLE_EQ(new_handle.get_value(), value); } - -TEST(TestJointHandle, joint_command_handle) -{ - double value = 1.337; - JointCommandHandle handle{JOINT_NAME, FOO_INTERFACE, &value}; - EXPECT_DOUBLE_EQ(handle.get_value(), value); - EXPECT_NO_THROW(handle.set_value(0.0)); - EXPECT_DOUBLE_EQ(handle.get_value(), 0.0); -} - -TEST(TestJointHandle, joint_state_handle) -{ - double value = 1.337; - JointStateHandle handle{JOINT_NAME, FOO_INTERFACE, &value}; - EXPECT_DOUBLE_EQ(handle.get_value(), value); - // handle.set_value(5); compiler error, no set_value function -} From a0979c06c7a17c5a6cc6b353cc47fdcc5fb26ca8 Mon Sep 17 00:00:00 2001 From: Karsten Knese Date: Thu, 22 Oct 2020 14:26:54 -0700 Subject: [PATCH 03/18] linters Signed-off-by: Karsten Knese --- .../include/hardware_interface/components/actuator_interface.hpp | 1 - hardware_interface/test/test_component_interfaces.cpp | 1 - 2 files changed, 2 deletions(-) diff --git a/hardware_interface/include/hardware_interface/components/actuator_interface.hpp b/hardware_interface/include/hardware_interface/components/actuator_interface.hpp index 0a2fb2b8c4..4e1cc3e6d9 100644 --- a/hardware_interface/include/hardware_interface/components/actuator_interface.hpp +++ b/hardware_interface/include/hardware_interface/components/actuator_interface.hpp @@ -20,7 +20,6 @@ #include "hardware_interface/handle.hpp" #include "hardware_interface/hardware_info.hpp" -#include "hardware_interface/joint_handle.hpp" #include "hardware_interface/types/hardware_interface_return_values.hpp" #include "hardware_interface/types/hardware_interface_status_values.hpp" diff --git a/hardware_interface/test/test_component_interfaces.cpp b/hardware_interface/test/test_component_interfaces.cpp index f568e1d9ed..72b41b2060 100644 --- a/hardware_interface/test/test_component_interfaces.cpp +++ b/hardware_interface/test/test_component_interfaces.cpp @@ -325,4 +325,3 @@ TEST(TestComponentInterfaces, dummy_system) ASSERT_EQ(hardware_interface::return_type::OK, system_hw.write()); } } - From 223f211f64791a139a34bd189060c0b5afb59969 Mon Sep 17 00:00:00 2001 From: Karsten Knese Date: Thu, 22 Oct 2020 21:46:29 -0700 Subject: [PATCH 04/18] import resource manager Signed-off-by: Karsten Knese --- controller_manager/CMakeLists.txt | 26 ++- controller_manager/src/resource_manager.cpp | 145 +++++++++++++++++ controller_manager/src/resource_manager.hpp | 51 ++++++ .../test/test_components/test_actuator.cpp | 77 +++++++++ .../test/test_components/test_components.xml | 20 +++ .../test/test_components/test_sensor.cpp | 64 ++++++++ .../test/test_components/test_system.cpp | 77 +++++++++ .../{ => test_controller}/test_controller.xml | 0 .../test/test_resource_manager.cpp | 152 ++++++++++++++++++ .../components/actuator.hpp | 2 + .../hardware_interface/components/sensor.hpp | 2 + .../components/sensor_interface.hpp | 1 + .../components/system_interface.hpp | 1 + 13 files changed, 615 insertions(+), 3 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_components/test_actuator.cpp create mode 100644 controller_manager/test/test_components/test_components.xml create mode 100644 controller_manager/test/test_components/test_sensor.cpp create mode 100644 controller_manager/test/test_components/test_system.cpp rename controller_manager/test/{ => test_controller}/test_controller.xml (100%) create mode 100644 controller_manager/test/test_resource_manager.cpp diff --git a/controller_manager/CMakeLists.txt b/controller_manager/CMakeLists.txt index 0a93d78d94..c0171c8cd9 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,11 @@ 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") + pluginlib_export_plugin_description_file( + controller_interface test/test_controller/test_controller.xml) + install(TARGETS test_controller + DESTINATION lib + ) ament_add_gmock( test_controller_manager @@ -101,11 +107,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_components SHARED + test/test_components/test_actuator.cpp + test/test_components/test_sensor.cpp + test/test_components/test_system.cpp) + ament_target_dependencies(test_components + hardware_interface + pluginlib) + install(TARGETS test_components DESTINATION lib ) + pluginlib_export_plugin_description_file( + hardware_interface test/test_components/test_components.xml) + + ament_add_gmock( + 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..993c028b6d --- /dev/null +++ b/controller_manager/src/resource_manager.cpp @@ -0,0 +1,145 @@ +// 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/actuator.hpp" +#include "hardware_interface/components/actuator_interface.hpp" +#include "hardware_interface/components/sensor.hpp" +#include "hardware_interface/components/sensor_interface.hpp" +#include "hardware_interface/components/system.hpp" +#include "hardware_interface/components/system_interface.hpp" +#include "hardware_interface/component_parser.hpp" +#include "hardware_interface/hardware_info.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::components::ActuatorInterface"; + static constexpr const char * sensor_interface_name = + "hardware_interface::components::SensorInterface"; + static constexpr const char * system_interface_name = + "hardware_interface::components::SystemInterface"; + +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 + // 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); + } + + 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_); + } + + // hardware plugins + 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..b014cc7f31 --- /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; + + ~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_components/test_actuator.cpp b/controller_manager/test/test_components/test_actuator.cpp new file mode 100644 index 0000000000..31014b2345 --- /dev/null +++ b/controller_manager/test/test_components/test_actuator.cpp @@ -0,0 +1,77 @@ +// Copyright 2020 ros2_control Development Team +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include + +#include "hardware_interface/components/actuator_interface.hpp" + +using hardware_interface::status; +using hardware_interface::return_type; +using hardware_interface::StateHandle; +using hardware_interface::CommandHandle; + +class TestActuator : public hardware_interface::components::ActuatorInterface +{ + return_type configure(const hardware_interface::HardwareInfo & actuator_info) override + { + actuator_info_ = actuator_info; + return return_type::OK; + } + + std::vector export_state_handles() override + { + std::vector state_handles; + + return state_handles; + } + + std::vector export_command_handles() override + { + std::vector command_handles; + + return command_handles; + } + + return_type start() override + { + return return_type::OK; + } + + return_type stop() override + { + return return_type::OK; + } + + status get_status() const override + { + return status::UNKNOWN; + } + + return_type read() override + { + return return_type::OK; + } + + return_type write() override + { + return return_type::OK; + } + +private: + hardware_interface::HardwareInfo actuator_info_; +}; + +#include "pluginlib/class_list_macros.hpp" // NOLINT +PLUGINLIB_EXPORT_CLASS(TestActuator, hardware_interface::components::ActuatorInterface) diff --git a/controller_manager/test/test_components/test_components.xml b/controller_manager/test/test_components/test_components.xml new file mode 100644 index 0000000000..7dd4feea40 --- /dev/null +++ b/controller_manager/test/test_components/test_components.xml @@ -0,0 +1,20 @@ + + + + + Test Actuator + + + + + + Test Sensor + + + + + + Test System + + + diff --git a/controller_manager/test/test_components/test_sensor.cpp b/controller_manager/test/test_components/test_sensor.cpp new file mode 100644 index 0000000000..7fc40a530a --- /dev/null +++ b/controller_manager/test/test_components/test_sensor.cpp @@ -0,0 +1,64 @@ +// Copyright 2020 ros2_control Development Team +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include + +#include "hardware_interface/components/sensor_interface.hpp" + +using hardware_interface::status; +using hardware_interface::return_type; +using hardware_interface::StateHandle; + +class TestSensor : public hardware_interface::components::SensorInterface +{ + return_type configure(const hardware_interface::HardwareInfo & sensor_info) override + { + sensor_info_ = sensor_info; + return return_type::OK; + } + + std::vector export_state_handles() override + { + std::vector state_handles; + + return state_handles; + } + + return_type start() override + { + return return_type::OK; + } + + return_type stop() override + { + return return_type::OK; + } + + status get_status() const override + { + return status::UNKNOWN; + } + + return_type read() override + { + return return_type::OK; + } + +private: + hardware_interface::HardwareInfo sensor_info_; +}; + +#include "pluginlib/class_list_macros.hpp" // NOLINT +PLUGINLIB_EXPORT_CLASS(TestSensor, hardware_interface::components::SensorInterface) diff --git a/controller_manager/test/test_components/test_system.cpp b/controller_manager/test/test_components/test_system.cpp new file mode 100644 index 0000000000..6249644862 --- /dev/null +++ b/controller_manager/test/test_components/test_system.cpp @@ -0,0 +1,77 @@ +// Copyright 2020 ros2_control Development Team +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include + +#include "hardware_interface/components/system_interface.hpp" + +using hardware_interface::status; +using hardware_interface::return_type; +using hardware_interface::StateHandle; +using hardware_interface::CommandHandle; + +class TestSystem : public hardware_interface::components::SystemInterface +{ + return_type configure(const hardware_interface::HardwareInfo & system_info) override + { + system_info_ = system_info; + return return_type::OK; + } + + std::vector export_state_handles() override + { + std::vector state_handles; + + return state_handles; + } + + std::vector export_command_handles() override + { + std::vector command_handles; + + return command_handles; + } + + return_type start() override + { + return return_type::OK; + } + + return_type stop() override + { + return return_type::OK; + } + + status get_status() const override + { + return status::UNKNOWN; + } + + return_type read() override + { + return return_type::OK; + } + + return_type write() override + { + return return_type::OK; + } + +private: + hardware_interface::HardwareInfo system_info_; +}; + +#include "pluginlib/class_list_macros.hpp" // NOLINT +PLUGINLIB_EXPORT_CLASS(TestSystem, hardware_interface::components::SystemInterface) diff --git a/controller_manager/test/test_controller.xml b/controller_manager/test/test_controller/test_controller.xml similarity index 100% rename from controller_manager/test/test_controller.xml rename to controller_manager/test/test_controller/test_controller.xml diff --git a/controller_manager/test/test_resource_manager.cpp b/controller_manager/test/test_resource_manager.cpp new file mode 100644 index 0000000000..640993b551 --- /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"( + +)"; + + test_hardware_resource_system_ = + R"( + + + test_actuator + 2 + 2 + + + test_joint_component + -1 + 1 + + + + + test_sensor + 2 + 2 + + + test_sensor + -1 + 1 + + + + + test_system + 2 + 2 + + + test_joint_component + -1 + 1 + + + test_joint_component + -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/components/actuator.hpp b/hardware_interface/include/hardware_interface/components/actuator.hpp index cc75430b4e..720d7e93d1 100644 --- a/hardware_interface/include/hardware_interface/components/actuator.hpp +++ b/hardware_interface/include/hardware_interface/components/actuator.hpp @@ -39,6 +39,8 @@ class Actuator final HARDWARE_INTERFACE_PUBLIC explicit Actuator(std::unique_ptr impl); + Actuator(Actuator && other) = default; + ~Actuator() = 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 04b00f0288..18762b28cc 100644 --- a/hardware_interface/include/hardware_interface/components/sensor.hpp +++ b/hardware_interface/include/hardware_interface/components/sensor.hpp @@ -41,6 +41,8 @@ class Sensor final HARDWARE_INTERFACE_PUBLIC explicit Sensor(std::unique_ptr impl); + Sensor(Sensor && other) = default; + ~Sensor() = default; 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 index 894cd1bd1d..399040004d 100644 --- a/hardware_interface/include/hardware_interface/components/sensor_interface.hpp +++ b/hardware_interface/include/hardware_interface/components/sensor_interface.hpp @@ -18,6 +18,7 @@ #include #include +#include "hardware_interface/handle.hpp" #include "hardware_interface/hardware_info.hpp" #include "hardware_interface/types/hardware_interface_return_values.hpp" #include "hardware_interface/types/hardware_interface_status_values.hpp" diff --git a/hardware_interface/include/hardware_interface/components/system_interface.hpp b/hardware_interface/include/hardware_interface/components/system_interface.hpp index dda1281e76..dc93c0d564 100644 --- a/hardware_interface/include/hardware_interface/components/system_interface.hpp +++ b/hardware_interface/include/hardware_interface/components/system_interface.hpp @@ -18,6 +18,7 @@ #include #include +#include "hardware_interface/handle.hpp" #include "hardware_interface/hardware_info.hpp" #include "hardware_interface/types/hardware_interface_return_values.hpp" #include "hardware_interface/types/hardware_interface_status_values.hpp" From 059f96a82dc362a1ef7eae917c6d4ec84cc6f6a1 Mon Sep 17 00:00:00 2001 From: Karsten Knese Date: Thu, 22 Oct 2020 21:50:51 -0700 Subject: [PATCH 05/18] correct year Signed-off-by: Karsten Knese --- controller_manager/test/test_resource_manager.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/controller_manager/test/test_resource_manager.cpp b/controller_manager/test/test_resource_manager.cpp index 640993b551..12579b43dc 100644 --- a/controller_manager/test/test_resource_manager.cpp +++ b/controller_manager/test/test_resource_manager.cpp @@ -1,4 +1,4 @@ -// Copyright 2017 Open Source Robotics Foundation, Inc. +// 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. From 891e3c62bc5ed35548b6d9c9b0f871685a05c542 Mon Sep 17 00:00:00 2001 From: Karsten Knese Date: Fri, 23 Oct 2020 00:02:16 -0700 Subject: [PATCH 06/18] import handles from loaded components Signed-off-by: Karsten Knese --- controller_manager/src/resource_manager.cpp | 53 ++++++++++++++++++- controller_manager/src/resource_manager.hpp | 8 +-- .../test/test_components/test_actuator.cpp | 9 ++++ .../test/test_resource_manager.cpp | 17 ++++++ 4 files changed, 83 insertions(+), 4 deletions(-) diff --git a/controller_manager/src/resource_manager.cpp b/controller_manager/src/resource_manager.cpp index 993c028b6d..f90fab6abd 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/components/actuator.hpp" @@ -66,7 +67,28 @@ class ResourceStorage loader.createUnmanagedInstance(hardware_info.hardware_class_type)); HardwareT actuator(std::move(interface)); container.emplace_back(std::move(actuator)); - container.back().configure(hardware_info); + } + + template + void import_state_handles(HardwareT & hardware) + { + auto handles = hardware.export_state_handles(); + for (auto i = 0u; i < handles.size(); ++i) { + auto key = handles[i].get_name() + "/" + handles[i].get_interface_name(); + state_handle_map_.emplace( + std::make_pair(key, std::move(handles[i]))); + } + } + + template + void import_command_handles(HardwareT & hardware) + { + auto handles = hardware.export_command_handles(); + for (auto i = 0u; i < handles.size(); ++i) { + auto key = handles[i].get_name() + "/" + handles[i].get_interface_name(); + command_handle_map_.emplace( + std::make_pair(key, std::move(handles[i]))); + } } void initialize_actuator(const hardware_interface::HardwareInfo & hardware_info) @@ -74,6 +96,9 @@ class ResourceStorage initialize_hardware( hardware_info, actuator_loader_, actuators_); + actuators_.back().configure(hardware_info); + import_state_handles(actuators_.back()); + import_command_handles(actuators_.back()); } void initialize_sensor(const hardware_interface::HardwareInfo & hardware_info) @@ -81,6 +106,8 @@ class ResourceStorage initialize_hardware( hardware_info, sensor_loader_, sensors_); + sensors_.back().configure(hardware_info); + import_state_handles(sensors_.back()); } void initialize_system(const hardware_interface::HardwareInfo & hardware_info) @@ -88,6 +115,9 @@ class ResourceStorage initialize_hardware( hardware_info, system_loader_, systems_); + systems_.back().configure(hardware_info); + import_state_handles(systems_.back()); + import_command_handles(systems_.back()); } // hardware plugins @@ -98,6 +128,9 @@ class ResourceStorage std::vector actuators_; std::vector sensors_; std::vector systems_; + + std::unordered_map state_handle_map_; + std::unordered_map command_handle_map_; }; ResourceManager::ResourceManager() @@ -128,6 +161,24 @@ ResourceManager::ResourceManager(const std::string & urdf) } } +std::vector ResourceManager::state_handle_keys() const +{ + std::vector keys; + for (const auto & item : resource_storage_->state_handle_map_) { + keys.push_back(std::get<0>(item)); + } + return keys; +} + +std::vector ResourceManager::command_handle_keys() const +{ + std::vector keys; + for (const auto & item : resource_storage_->command_handle_map_) { + keys.push_back(std::get<0>(item)); + } + return keys; +} + size_t ResourceManager::actuator_interfaces_size() const { return resource_storage_->actuators_.size(); diff --git a/controller_manager/src/resource_manager.hpp b/controller_manager/src/resource_manager.hpp index b014cc7f31..676ece76dc 100644 --- a/controller_manager/src/resource_manager.hpp +++ b/controller_manager/src/resource_manager.hpp @@ -17,6 +17,7 @@ #include #include +#include namespace controller_manager { @@ -34,15 +35,16 @@ class ResourceManager ~ResourceManager(); + std::vector state_handle_keys() const; + + std::vector command_handle_keys() const; + 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_; }; diff --git a/controller_manager/test/test_components/test_actuator.cpp b/controller_manager/test/test_components/test_actuator.cpp index 31014b2345..bc8d7c1f3a 100644 --- a/controller_manager/test/test_components/test_actuator.cpp +++ b/controller_manager/test/test_components/test_actuator.cpp @@ -33,6 +33,10 @@ class TestActuator : public hardware_interface::components::ActuatorInterface std::vector export_state_handles() override { std::vector state_handles; + state_handles.emplace_back( + hardware_interface::StateHandle("joint1", "position", &position_state_)); + state_handles.emplace_back( + hardware_interface::StateHandle("joint1", "velocity", &velocity_state_)); return state_handles; } @@ -40,6 +44,8 @@ class TestActuator : public hardware_interface::components::ActuatorInterface std::vector export_command_handles() override { std::vector command_handles; + command_handles.emplace_back( + hardware_interface::CommandHandle("joint1", "velocity", &velocity_command_)); return command_handles; } @@ -70,6 +76,9 @@ class TestActuator : public hardware_interface::components::ActuatorInterface } private: + double position_state_ = 0.0; + double velocity_state_ = 0.0; + double velocity_command_ = 0.0; hardware_interface::HardwareInfo actuator_info_; }; diff --git a/controller_manager/test/test_resource_manager.cpp b/controller_manager/test/test_resource_manager.cpp index 12579b43dc..e99f02ce8b 100644 --- a/controller_manager/test/test_resource_manager.cpp +++ b/controller_manager/test/test_resource_manager.cpp @@ -13,6 +13,8 @@ // limitations under the License. #include + +#include #include #include @@ -149,4 +151,19 @@ TEST_F(TestResourceManager, initialization_with_urdf) { EXPECT_EQ(1u, rm.actuator_interfaces_size()); EXPECT_EQ(1u, rm.sensor_interfaces_size()); EXPECT_EQ(1u, rm.system_interfaces_size()); + + auto state_handle_keys = rm.state_handle_keys(); + // extracting a list from an unordered_map doesn't yield deterministic results + // sort the list to make comparison clear. + std::sort(state_handle_keys.begin(), state_handle_keys.end()); + ASSERT_EQ(2u, state_handle_keys.size()); + EXPECT_EQ("joint1/position", state_handle_keys[0]); + EXPECT_EQ("joint1/velocity", state_handle_keys[1]); + + auto command_handle_keys = rm.command_handle_keys(); + // extracting a list from an unordered_map doesn't yield deterministic results + // sort the list to make comparison clear. + std::sort(command_handle_keys.begin(), command_handle_keys.end()); + ASSERT_EQ(1u, command_handle_keys.size()); + EXPECT_EQ("joint1/velocity", command_handle_keys[0]); } From 68dadd78f8d9bba306f886cb527eb90787d9e93c Mon Sep 17 00:00:00 2001 From: Karsten Knese Date: Fri, 23 Oct 2020 12:16:04 -0700 Subject: [PATCH 07/18] wip / debug Signed-off-by: Karsten Knese --- .../test/test_components/test_actuator.cpp | 6 ++++++ .../test/test_resource_manager.cpp | 20 ++++++------------- 2 files changed, 12 insertions(+), 14 deletions(-) diff --git a/controller_manager/test/test_components/test_actuator.cpp b/controller_manager/test/test_components/test_actuator.cpp index bc8d7c1f3a..7c20f5d014 100644 --- a/controller_manager/test/test_components/test_actuator.cpp +++ b/controller_manager/test/test_components/test_actuator.cpp @@ -27,6 +27,12 @@ class TestActuator : public hardware_interface::components::ActuatorInterface return_type configure(const hardware_interface::HardwareInfo & actuator_info) override { actuator_info_ = actuator_info; + for (const auto & joint : actuator_info_.joints) { + fprintf(stderr, "joint info: %s\n", joint.name.c_str()); + for (const auto & command_interface : joint.command_interfaces) { + fprintf(stderr, "\t%s\n", command_interface.name.c_str()); + } + } return return_type::OK; } diff --git a/controller_manager/test/test_resource_manager.cpp b/controller_manager/test/test_resource_manager.cpp index e99f02ce8b..e1156dc7df 100644 --- a/controller_manager/test/test_resource_manager.cpp +++ b/controller_manager/test/test_resource_manager.cpp @@ -91,13 +91,9 @@ class TestResourceManager : public ::testing::Test test_actuator - 2 - 2 - test_joint_component - -1 - 1 + @@ -107,9 +103,7 @@ class TestResourceManager : public ::testing::Test 2 - test_sensor - -1 - 1 + @@ -119,14 +113,12 @@ class TestResourceManager : public ::testing::Test 2 - test_joint_component - -1 - 1 + + - test_joint_component - -1 - 1 + + )"; From b87640c0ddbc79d38162a684b8cf6d37950c19f0 Mon Sep 17 00:00:00 2001 From: Karsten Knese Date: Fri, 23 Oct 2020 17:59:16 -0700 Subject: [PATCH 08/18] parse components Signed-off-by: Karsten Knese --- controller_manager/src/resource_manager.cpp | 16 ++++++++- controller_manager/src/resource_manager.hpp | 36 +++++++++++++++++++ .../test/test_components/test_actuator.cpp | 28 ++++++++++----- .../test/test_components/test_sensor.cpp | 8 +++++ .../test/test_components/test_system.cpp | 13 +++++++ .../test/test_resource_manager.cpp | 29 +++++++-------- 6 files changed, 106 insertions(+), 24 deletions(-) diff --git a/controller_manager/src/resource_manager.cpp b/controller_manager/src/resource_manager.cpp index f90fab6abd..73f597784f 100644 --- a/controller_manager/src/resource_manager.cpp +++ b/controller_manager/src/resource_manager.cpp @@ -96,7 +96,9 @@ class ResourceStorage initialize_hardware( hardware_info, actuator_loader_, actuators_); - actuators_.back().configure(hardware_info); + if (hardware_interface::return_type::OK != actuators_.back().configure(hardware_info)) { + throw std::runtime_error(std::string("failed to configure ") + hardware_info.name); + } import_state_handles(actuators_.back()); import_command_handles(actuators_.back()); } @@ -170,6 +172,12 @@ std::vector ResourceManager::state_handle_keys() const return keys; } +bool ResourceManager::state_handle_exists(const std::string & key) const +{ + return resource_storage_->state_handle_map_.find(key) != + resource_storage_->state_handle_map_.end(); +} + std::vector ResourceManager::command_handle_keys() const { std::vector keys; @@ -179,6 +187,12 @@ std::vector ResourceManager::command_handle_keys() const return keys; } +bool ResourceManager::command_handle_exists(const std::string & key) const +{ + return resource_storage_->command_handle_map_.find(key) != + resource_storage_->command_handle_map_.end(); +} + size_t ResourceManager::actuator_interfaces_size() const { return resource_storage_->actuators_.size(); diff --git a/controller_manager/src/resource_manager.hpp b/controller_manager/src/resource_manager.hpp index 676ece76dc..ad5e93a4db 100644 --- a/controller_manager/src/resource_manager.hpp +++ b/controller_manager/src/resource_manager.hpp @@ -35,14 +35,50 @@ class ResourceManager ~ResourceManager(); + /// Returns all registered state handles keys. + /** + * The keys are collected from each loaded hardware component. + * + * \return vector of strings, containing all registered keys. + */ std::vector state_handle_keys() const; + /// Checks whether a handle is registered under the given key. + /** + * \return true if handle exist, false otherwise. + */ + bool state_handle_exists(const std::string & key) const; + + /// Returns all registered command handles keys. + /** + * The keys are collected from each loaded hardware component. + * + * \return vector of strings, containing all registered keys. + */ std::vector command_handle_keys() const; + /// Checks whether a handle is registered under the given key. + /** + * \return true if handle exist, false otherwise. + */ + bool command_handle_exists(const std::string & key) const; + + /// Return the number of loaded actuator components. + /** + * \return number of actuator components. + */ size_t actuator_interfaces_size() const; + /// Return the number of loaded sensor components. + /** + * \return number of sensor components. + */ size_t sensor_interfaces_size() const; + /// Return the number of loaded system components. + /** + * \return number of system components. + */ size_t system_interfaces_size() const; private: diff --git a/controller_manager/test/test_components/test_actuator.cpp b/controller_manager/test/test_components/test_actuator.cpp index 7c20f5d014..74a1e13a81 100644 --- a/controller_manager/test/test_components/test_actuator.cpp +++ b/controller_manager/test/test_components/test_actuator.cpp @@ -27,12 +27,13 @@ class TestActuator : public hardware_interface::components::ActuatorInterface return_type configure(const hardware_interface::HardwareInfo & actuator_info) override { actuator_info_ = actuator_info; - for (const auto & joint : actuator_info_.joints) { - fprintf(stderr, "joint info: %s\n", joint.name.c_str()); - for (const auto & command_interface : joint.command_interfaces) { - fprintf(stderr, "\t%s\n", command_interface.name.c_str()); - } - } + // can only control one joint + if (actuator_info_.joints.size() != 1) {return return_type::ERROR;} + // can only control in position + if (actuator_info_.joints[0].command_interfaces.size() != 1) {return return_type::ERROR;} + // can only give feedback state for position and velocity + if (actuator_info_.joints[0].state_interfaces.size() != 2) {return return_type::ERROR;} + return return_type::OK; } @@ -40,9 +41,15 @@ class TestActuator : public hardware_interface::components::ActuatorInterface { std::vector state_handles; state_handles.emplace_back( - hardware_interface::StateHandle("joint1", "position", &position_state_)); + hardware_interface::StateHandle( + actuator_info_.joints[0].name, + actuator_info_.joints[0].state_interfaces[0].name, + &position_state_)); state_handles.emplace_back( - hardware_interface::StateHandle("joint1", "velocity", &velocity_state_)); + hardware_interface::StateHandle( + actuator_info_.joints[0].name, + actuator_info_.joints[0].state_interfaces[1].name, + &velocity_state_)); return state_handles; } @@ -51,7 +58,10 @@ class TestActuator : public hardware_interface::components::ActuatorInterface { std::vector command_handles; command_handles.emplace_back( - hardware_interface::CommandHandle("joint1", "velocity", &velocity_command_)); + hardware_interface::CommandHandle( + actuator_info_.joints[0].name, + actuator_info_.joints[0].command_interfaces[0].name, + &velocity_command_)); return command_handles; } diff --git a/controller_manager/test/test_components/test_sensor.cpp b/controller_manager/test/test_components/test_sensor.cpp index 7fc40a530a..377f1e46ed 100644 --- a/controller_manager/test/test_components/test_sensor.cpp +++ b/controller_manager/test/test_components/test_sensor.cpp @@ -26,12 +26,19 @@ class TestSensor : public hardware_interface::components::SensorInterface return_type configure(const hardware_interface::HardwareInfo & sensor_info) override { sensor_info_ = sensor_info; + // can only give feedback state for velocity + if (sensor_info_.sensors[0].state_interfaces.size() != 1) {return return_type::ERROR;} return return_type::OK; } std::vector export_state_handles() override { std::vector state_handles; + state_handles.emplace_back( + hardware_interface::StateHandle( + sensor_info_.sensors[0].name, + sensor_info_.sensors[0].state_interfaces[0].name, + &velocity_state_)); return state_handles; } @@ -57,6 +64,7 @@ class TestSensor : public hardware_interface::components::SensorInterface } private: + double velocity_state_ = 0.0; hardware_interface::HardwareInfo sensor_info_; }; diff --git a/controller_manager/test/test_components/test_system.cpp b/controller_manager/test/test_components/test_system.cpp index 6249644862..a2825042b0 100644 --- a/controller_manager/test/test_components/test_system.cpp +++ b/controller_manager/test/test_components/test_system.cpp @@ -12,6 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include #include #include @@ -33,6 +34,11 @@ class TestSystem : public hardware_interface::components::SystemInterface std::vector export_state_handles() override { std::vector state_handles; + for (auto i = 0u; i < system_info_.joints.size(); ++i) { + state_handles.emplace_back( + hardware_interface::StateHandle( + system_info_.joints[i].name, "position", &position_state_[i])); + } return state_handles; } @@ -40,6 +46,11 @@ class TestSystem : public hardware_interface::components::SystemInterface std::vector export_command_handles() override { std::vector command_handles; + for (auto i = 0u; i < system_info_.joints.size(); ++i) { + command_handles.emplace_back( + hardware_interface::CommandHandle( + system_info_.joints[i].name, "velocity", &velocity_command_[i])); + } return command_handles; } @@ -70,6 +81,8 @@ class TestSystem : public hardware_interface::components::SystemInterface } private: + std::array velocity_command_ = {0.0, 0.0}; + std::array position_state_ = {0.0, 0.0}; hardware_interface::HardwareInfo system_info_; }; diff --git a/controller_manager/test/test_resource_manager.cpp b/controller_manager/test/test_resource_manager.cpp index e1156dc7df..183f0bb6e2 100644 --- a/controller_manager/test/test_resource_manager.cpp +++ b/controller_manager/test/test_resource_manager.cpp @@ -90,15 +90,17 @@ class TestResourceManager : public ::testing::Test R"( - test_actuator + test_actuator + + - test_sensor + test_sensor 2 2 @@ -108,7 +110,7 @@ class TestResourceManager : public ::testing::Test - test_system + test_system 2 2 @@ -145,17 +147,16 @@ TEST_F(TestResourceManager, initialization_with_urdf) { EXPECT_EQ(1u, rm.system_interfaces_size()); auto state_handle_keys = rm.state_handle_keys(); - // extracting a list from an unordered_map doesn't yield deterministic results - // sort the list to make comparison clear. - std::sort(state_handle_keys.begin(), state_handle_keys.end()); - ASSERT_EQ(2u, state_handle_keys.size()); - EXPECT_EQ("joint1/position", state_handle_keys[0]); - EXPECT_EQ("joint1/velocity", state_handle_keys[1]); + ASSERT_EQ(5u, state_handle_keys.size()); + EXPECT_TRUE(rm.state_handle_exists("joint1/position")); + EXPECT_TRUE(rm.state_handle_exists("joint1/velocity")); + EXPECT_TRUE(rm.state_handle_exists("sensor1/velocity")); + EXPECT_TRUE(rm.state_handle_exists("joint2/position")); + EXPECT_TRUE(rm.state_handle_exists("joint3/position")); auto command_handle_keys = rm.command_handle_keys(); - // extracting a list from an unordered_map doesn't yield deterministic results - // sort the list to make comparison clear. - std::sort(command_handle_keys.begin(), command_handle_keys.end()); - ASSERT_EQ(1u, command_handle_keys.size()); - EXPECT_EQ("joint1/velocity", command_handle_keys[0]); + ASSERT_EQ(3u, command_handle_keys.size()); + EXPECT_TRUE(rm.command_handle_exists("joint1/position")); + EXPECT_TRUE(rm.command_handle_exists("joint2/velocity")); + EXPECT_TRUE(rm.command_handle_exists("joint3/velocity")); } From 37fc3d84ebe027a62fd0c8f90597ad4148fb0984 Mon Sep 17 00:00:00 2001 From: Karsten Knese Date: Mon, 2 Nov 2020 12:44:19 -0800 Subject: [PATCH 09/18] changes after rebase Signed-off-by: Karsten Knese --- controller_manager/src/resource_manager.cpp | 58 +++++++++---------- controller_manager/src/resource_manager.hpp | 20 +++---- .../test/test_components/test_actuator.cpp | 28 ++++----- .../test/test_components/test_sensor.cpp | 12 ++-- .../test/test_components/test_system.cpp | 24 ++++---- .../test/test_resource_manager.cpp | 24 ++++---- .../hardware_interface/components/system.hpp | 2 + .../include/hardware_interface/handle.hpp | 2 +- 8 files changed, 86 insertions(+), 84 deletions(-) diff --git a/controller_manager/src/resource_manager.cpp b/controller_manager/src/resource_manager.cpp index 73f597784f..ed856087cb 100644 --- a/controller_manager/src/resource_manager.cpp +++ b/controller_manager/src/resource_manager.cpp @@ -70,24 +70,24 @@ class ResourceStorage } template - void import_state_handles(HardwareT & hardware) + void import_state_interfaces(HardwareT & hardware) { - auto handles = hardware.export_state_handles(); - for (auto i = 0u; i < handles.size(); ++i) { - auto key = handles[i].get_name() + "/" + handles[i].get_interface_name(); - state_handle_map_.emplace( - std::make_pair(key, std::move(handles[i]))); + auto interfaces = hardware.export_state_interfaces(); + for (auto i = 0u; i < interfaces.size(); ++i) { + auto key = interfaces[i].get_name() + "/" + interfaces[i].get_interface_name(); + state_interface_map_.emplace( + std::make_pair(key, std::move(interfaces[i]))); } } template - void import_command_handles(HardwareT & hardware) + void import_command_interfaces(HardwareT & hardware) { - auto handles = hardware.export_command_handles(); - for (auto i = 0u; i < handles.size(); ++i) { - auto key = handles[i].get_name() + "/" + handles[i].get_interface_name(); - command_handle_map_.emplace( - std::make_pair(key, std::move(handles[i]))); + auto interfaces = hardware.export_command_interfaces(); + for (auto i = 0u; i < interfaces.size(); ++i) { + auto key = interfaces[i].get_name() + "/" + interfaces[i].get_interface_name(); + command_interface_map_.emplace( + std::make_pair(key, std::move(interfaces[i]))); } } @@ -99,8 +99,8 @@ class ResourceStorage if (hardware_interface::return_type::OK != actuators_.back().configure(hardware_info)) { throw std::runtime_error(std::string("failed to configure ") + hardware_info.name); } - import_state_handles(actuators_.back()); - import_command_handles(actuators_.back()); + import_state_interfaces(actuators_.back()); + import_command_interfaces(actuators_.back()); } void initialize_sensor(const hardware_interface::HardwareInfo & hardware_info) @@ -109,7 +109,7 @@ class ResourceStorage hardware_interface::components::SensorInterface>( hardware_info, sensor_loader_, sensors_); sensors_.back().configure(hardware_info); - import_state_handles(sensors_.back()); + import_state_interfaces(sensors_.back()); } void initialize_system(const hardware_interface::HardwareInfo & hardware_info) @@ -118,8 +118,8 @@ class ResourceStorage hardware_interface::components::SystemInterface>( hardware_info, system_loader_, systems_); systems_.back().configure(hardware_info); - import_state_handles(systems_.back()); - import_command_handles(systems_.back()); + import_state_interfaces(systems_.back()); + import_command_interfaces(systems_.back()); } // hardware plugins @@ -131,8 +131,8 @@ class ResourceStorage std::vector sensors_; std::vector systems_; - std::unordered_map state_handle_map_; - std::unordered_map command_handle_map_; + std::unordered_map state_interface_map_; + std::unordered_map command_interface_map_; }; ResourceManager::ResourceManager() @@ -163,34 +163,34 @@ ResourceManager::ResourceManager(const std::string & urdf) } } -std::vector ResourceManager::state_handle_keys() const +std::vector ResourceManager::state_interface_keys() const { std::vector keys; - for (const auto & item : resource_storage_->state_handle_map_) { + for (const auto & item : resource_storage_->state_interface_map_) { keys.push_back(std::get<0>(item)); } return keys; } -bool ResourceManager::state_handle_exists(const std::string & key) const +bool ResourceManager::state_interface_exists(const std::string & key) const { - return resource_storage_->state_handle_map_.find(key) != - resource_storage_->state_handle_map_.end(); + return resource_storage_->state_interface_map_.find(key) != + resource_storage_->state_interface_map_.end(); } -std::vector ResourceManager::command_handle_keys() const +std::vector ResourceManager::command_interface_keys() const { std::vector keys; - for (const auto & item : resource_storage_->command_handle_map_) { + for (const auto & item : resource_storage_->command_interface_map_) { keys.push_back(std::get<0>(item)); } return keys; } -bool ResourceManager::command_handle_exists(const std::string & key) const +bool ResourceManager::command_interface_exists(const std::string & key) const { - return resource_storage_->command_handle_map_.find(key) != - resource_storage_->command_handle_map_.end(); + return resource_storage_->command_interface_map_.find(key) != + resource_storage_->command_interface_map_.end(); } size_t ResourceManager::actuator_interfaces_size() const diff --git a/controller_manager/src/resource_manager.hpp b/controller_manager/src/resource_manager.hpp index ad5e93a4db..17b53f05f1 100644 --- a/controller_manager/src/resource_manager.hpp +++ b/controller_manager/src/resource_manager.hpp @@ -35,33 +35,33 @@ class ResourceManager ~ResourceManager(); - /// Returns all registered state handles keys. + /// Returns all registered state interfaces keys. /** * The keys are collected from each loaded hardware component. * * \return vector of strings, containing all registered keys. */ - std::vector state_handle_keys() const; + std::vector state_interface_keys() const; - /// Checks whether a handle is registered under the given key. + /// Checks whether a interface is registered under the given key. /** - * \return true if handle exist, false otherwise. + * \return true if interface exist, false otherwise. */ - bool state_handle_exists(const std::string & key) const; + bool state_interface_exists(const std::string & key) const; - /// Returns all registered command handles keys. + /// Returns all registered command interfaces keys. /** * The keys are collected from each loaded hardware component. * * \return vector of strings, containing all registered keys. */ - std::vector command_handle_keys() const; + std::vector command_interface_keys() const; - /// Checks whether a handle is registered under the given key. + /// Checks whether a interface is registered under the given key. /** - * \return true if handle exist, false otherwise. + * \return true if interface exist, false otherwise. */ - bool command_handle_exists(const std::string & key) const; + bool command_interface_exists(const std::string & key) const; /// Return the number of loaded actuator components. /** diff --git a/controller_manager/test/test_components/test_actuator.cpp b/controller_manager/test/test_components/test_actuator.cpp index 74a1e13a81..8da2a37a46 100644 --- a/controller_manager/test/test_components/test_actuator.cpp +++ b/controller_manager/test/test_components/test_actuator.cpp @@ -19,8 +19,8 @@ using hardware_interface::status; using hardware_interface::return_type; -using hardware_interface::StateHandle; -using hardware_interface::CommandHandle; +using hardware_interface::StateInterface; +using hardware_interface::CommandInterface; class TestActuator : public hardware_interface::components::ActuatorInterface { @@ -37,33 +37,33 @@ class TestActuator : public hardware_interface::components::ActuatorInterface return return_type::OK; } - std::vector export_state_handles() override + std::vector export_state_interfaces() override { - std::vector state_handles; - state_handles.emplace_back( - hardware_interface::StateHandle( + std::vector state_interfaces; + state_interfaces.emplace_back( + hardware_interface::StateInterface( actuator_info_.joints[0].name, actuator_info_.joints[0].state_interfaces[0].name, &position_state_)); - state_handles.emplace_back( - hardware_interface::StateHandle( + state_interfaces.emplace_back( + hardware_interface::StateInterface( actuator_info_.joints[0].name, actuator_info_.joints[0].state_interfaces[1].name, &velocity_state_)); - return state_handles; + return state_interfaces; } - std::vector export_command_handles() override + std::vector export_command_interfaces() override { - std::vector command_handles; - command_handles.emplace_back( - hardware_interface::CommandHandle( + std::vector command_interfaces; + command_interfaces.emplace_back( + hardware_interface::CommandInterface( actuator_info_.joints[0].name, actuator_info_.joints[0].command_interfaces[0].name, &velocity_command_)); - return command_handles; + return command_interfaces; } return_type start() override diff --git a/controller_manager/test/test_components/test_sensor.cpp b/controller_manager/test/test_components/test_sensor.cpp index 377f1e46ed..868878341f 100644 --- a/controller_manager/test/test_components/test_sensor.cpp +++ b/controller_manager/test/test_components/test_sensor.cpp @@ -19,7 +19,7 @@ using hardware_interface::status; using hardware_interface::return_type; -using hardware_interface::StateHandle; +using hardware_interface::StateInterface; class TestSensor : public hardware_interface::components::SensorInterface { @@ -31,16 +31,16 @@ class TestSensor : public hardware_interface::components::SensorInterface return return_type::OK; } - std::vector export_state_handles() override + std::vector export_state_interfaces() override { - std::vector state_handles; - state_handles.emplace_back( - hardware_interface::StateHandle( + std::vector state_interfaces; + state_interfaces.emplace_back( + hardware_interface::StateInterface( sensor_info_.sensors[0].name, sensor_info_.sensors[0].state_interfaces[0].name, &velocity_state_)); - return state_handles; + return state_interfaces; } return_type start() override diff --git a/controller_manager/test/test_components/test_system.cpp b/controller_manager/test/test_components/test_system.cpp index a2825042b0..e19ebc522f 100644 --- a/controller_manager/test/test_components/test_system.cpp +++ b/controller_manager/test/test_components/test_system.cpp @@ -20,8 +20,8 @@ using hardware_interface::status; using hardware_interface::return_type; -using hardware_interface::StateHandle; -using hardware_interface::CommandHandle; +using hardware_interface::StateInterface; +using hardware_interface::CommandInterface; class TestSystem : public hardware_interface::components::SystemInterface { @@ -31,28 +31,28 @@ class TestSystem : public hardware_interface::components::SystemInterface return return_type::OK; } - std::vector export_state_handles() override + std::vector export_state_interfaces() override { - std::vector state_handles; + std::vector state_interfaces; for (auto i = 0u; i < system_info_.joints.size(); ++i) { - state_handles.emplace_back( - hardware_interface::StateHandle( + state_interfaces.emplace_back( + hardware_interface::StateInterface( system_info_.joints[i].name, "position", &position_state_[i])); } - return state_handles; + return state_interfaces; } - std::vector export_command_handles() override + std::vector export_command_interfaces() override { - std::vector command_handles; + std::vector command_interfaces; for (auto i = 0u; i < system_info_.joints.size(); ++i) { - command_handles.emplace_back( - hardware_interface::CommandHandle( + command_interfaces.emplace_back( + hardware_interface::CommandInterface( system_info_.joints[i].name, "velocity", &velocity_command_[i])); } - return command_handles; + return command_interfaces; } return_type start() override diff --git a/controller_manager/test/test_resource_manager.cpp b/controller_manager/test/test_resource_manager.cpp index 183f0bb6e2..ba646c0ea3 100644 --- a/controller_manager/test/test_resource_manager.cpp +++ b/controller_manager/test/test_resource_manager.cpp @@ -146,17 +146,17 @@ TEST_F(TestResourceManager, initialization_with_urdf) { EXPECT_EQ(1u, rm.sensor_interfaces_size()); EXPECT_EQ(1u, rm.system_interfaces_size()); - auto state_handle_keys = rm.state_handle_keys(); - ASSERT_EQ(5u, state_handle_keys.size()); - EXPECT_TRUE(rm.state_handle_exists("joint1/position")); - EXPECT_TRUE(rm.state_handle_exists("joint1/velocity")); - EXPECT_TRUE(rm.state_handle_exists("sensor1/velocity")); - EXPECT_TRUE(rm.state_handle_exists("joint2/position")); - EXPECT_TRUE(rm.state_handle_exists("joint3/position")); + auto state_interface_keys = rm.state_interface_keys(); + ASSERT_EQ(5u, state_interface_keys.size()); + EXPECT_TRUE(rm.state_interface_exists("joint1/position")); + EXPECT_TRUE(rm.state_interface_exists("joint1/velocity")); + EXPECT_TRUE(rm.state_interface_exists("sensor1/velocity")); + EXPECT_TRUE(rm.state_interface_exists("joint2/position")); + EXPECT_TRUE(rm.state_interface_exists("joint3/position")); - auto command_handle_keys = rm.command_handle_keys(); - ASSERT_EQ(3u, command_handle_keys.size()); - EXPECT_TRUE(rm.command_handle_exists("joint1/position")); - EXPECT_TRUE(rm.command_handle_exists("joint2/velocity")); - EXPECT_TRUE(rm.command_handle_exists("joint3/velocity")); + auto command_interface_keys = rm.command_interface_keys(); + ASSERT_EQ(3u, command_interface_keys.size()); + EXPECT_TRUE(rm.command_interface_exists("joint1/position")); + EXPECT_TRUE(rm.command_interface_exists("joint2/velocity")); + EXPECT_TRUE(rm.command_interface_exists("joint3/velocity")); } diff --git a/hardware_interface/include/hardware_interface/components/system.hpp b/hardware_interface/include/hardware_interface/components/system.hpp index 1aa214a5e4..d11474db56 100644 --- a/hardware_interface/include/hardware_interface/components/system.hpp +++ b/hardware_interface/include/hardware_interface/components/system.hpp @@ -39,6 +39,8 @@ class System final HARDWARE_INTERFACE_PUBLIC explicit System(std::unique_ptr impl); + System(System && other) = default; + ~System() = default; HARDWARE_INTERFACE_PUBLIC diff --git a/hardware_interface/include/hardware_interface/handle.hpp b/hardware_interface/include/hardware_interface/handle.hpp index e021d08fba..12a6cecffc 100644 --- a/hardware_interface/include/hardware_interface/handle.hpp +++ b/hardware_interface/include/hardware_interface/handle.hpp @@ -32,7 +32,7 @@ class ReadOnlyHandle const std::string & name, const std::string & interface_name, double * value_ptr = nullptr) - : name_(std::move(name)), interface_name_(std::move(interface_name)), value_ptr_(value_ptr) + : name_(name), interface_name_(interface_name), value_ptr_(value_ptr) { } From f2e9432e29626ff12a1a89c1dc0785c7627731f0 Mon Sep 17 00:00:00 2001 From: Karsten Knese Date: Mon, 2 Nov 2020 13:19:04 -0800 Subject: [PATCH 10/18] component parser as shared library Signed-off-by: Karsten Knese --- hardware_interface/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/hardware_interface/CMakeLists.txt b/hardware_interface/CMakeLists.txt index 09c96d9eea..a188f0ad0c 100644 --- a/hardware_interface/CMakeLists.txt +++ b/hardware_interface/CMakeLists.txt @@ -42,7 +42,7 @@ ament_target_dependencies( target_compile_definitions(hardware_interface PRIVATE "HARDWARE_INTERFACE_BUILDING_DLL") add_library( - component_parser + component_parser SHARED src/component_parser.cpp ) target_include_directories( From 184fc243dedb2aea8f8c23628e823afee5d2fc93 Mon Sep 17 00:00:00 2001 From: Karsten Knese Date: Tue, 3 Nov 2020 12:46:15 -0800 Subject: [PATCH 11/18] validate urdf configuratin Signed-off-by: Karsten Knese --- controller_manager/src/resource_manager.cpp | 51 +++++++++++- controller_manager/src/resource_manager.hpp | 3 +- .../test/test_components/test_actuator.cpp | 22 ++++-- .../test/test_components/test_system.cpp | 8 ++ .../test/test_resource_manager.cpp | 68 +++++++++++++++- .../hardware_interface/component_info.hpp | 77 ------------------- .../hardware_interface/hardware_info.hpp | 52 ++++++++++++- hardware_interface/src/component_parser.cpp | 1 - 8 files changed, 190 insertions(+), 92 deletions(-) delete mode 100644 hardware_interface/include/hardware_interface/component_info.hpp diff --git a/controller_manager/src/resource_manager.cpp b/controller_manager/src/resource_manager.cpp index ed856087cb..7398ac1c90 100644 --- a/controller_manager/src/resource_manager.cpp +++ b/controller_manager/src/resource_manager.cpp @@ -52,8 +52,6 @@ class ResourceStorage system_loader_(pkg_name, system_interface_name) {} - ~ResourceStorage() = default; - template void initialize_hardware( const hardware_interface::HardwareInfo & hardware_info, @@ -141,7 +139,7 @@ ResourceManager::ResourceManager() ResourceManager::~ResourceManager() = default; -ResourceManager::ResourceManager(const std::string & urdf) +ResourceManager::ResourceManager(const std::string & urdf, bool validate_interfaces) : resource_storage_(std::make_unique()) { const std::string system_type = "system"; @@ -161,6 +159,53 @@ ResourceManager::ResourceManager(const std::string & urdf) resource_storage_->initialize_system(hardware); } } + + auto validate_storage = [this, &hardware_info]() -> void + { + std::vector missing_state_keys = {}; + std::vector missing_command_keys = {}; + + for (const auto & hardware : hardware_info) { + for (const auto & joint : hardware.joints) { + for (const auto & state_interface : joint.state_interfaces) { + if (!state_interface_exists(joint.name + "/" + state_interface.name)) { + missing_state_keys.emplace_back(joint.name + "/" + state_interface.name); + } + } + for (const auto & command_interface : joint.command_interfaces) { + if (!command_interface_exists(joint.name + "/" + command_interface.name)) { + missing_state_keys.emplace_back(joint.name + "/" + command_interface.name); + } + } + } + for (const auto & sensor : hardware.sensors) { + for (const auto & state_interface : sensor.state_interfaces) { + if (!state_interface_exists(sensor.name + "/" + state_interface.name)) { + missing_state_keys.emplace_back(sensor.name + "/" + state_interface.name); + } + } + } + } + + if (!missing_state_keys.empty() || !missing_command_keys.empty()) { + std::string err_msg = "wrong state or command interface configuration.\n"; + err_msg += "missing state interfaces:\n"; + for (const auto & missing_key : missing_state_keys) { + err_msg += missing_key + "\t"; + } + err_msg += "\nmissing command interfaces:\n"; + for (const auto & missing_key : missing_command_keys) { + err_msg += missing_key + "\t"; + } + + throw std::runtime_error(err_msg); + } + }; + + // throw on missing state and command interfaces, not specified keys are being ignored + if (validate_interfaces) { + validate_storage(); + } } std::vector ResourceManager::state_interface_keys() const diff --git a/controller_manager/src/resource_manager.hpp b/controller_manager/src/resource_manager.hpp index 17b53f05f1..df1881e7a9 100644 --- a/controller_manager/src/resource_manager.hpp +++ b/controller_manager/src/resource_manager.hpp @@ -29,7 +29,8 @@ class ResourceManager public: ResourceManager(); - explicit ResourceManager(const std::string & urdf); + explicit ResourceManager( + const std::string & urdf, bool validate_interfaces = true); ResourceManager(const ResourceManager &) = delete; diff --git a/controller_manager/test/test_components/test_actuator.cpp b/controller_manager/test/test_components/test_actuator.cpp index 8da2a37a46..6c1c6daba9 100644 --- a/controller_manager/test/test_components/test_actuator.cpp +++ b/controller_manager/test/test_components/test_actuator.cpp @@ -27,12 +27,17 @@ class TestActuator : public hardware_interface::components::ActuatorInterface return_type configure(const hardware_interface::HardwareInfo & actuator_info) override { actuator_info_ = actuator_info; - // can only control one joint - if (actuator_info_.joints.size() != 1) {return return_type::ERROR;} - // can only control in position - if (actuator_info_.joints[0].command_interfaces.size() != 1) {return return_type::ERROR;} - // can only give feedback state for position and velocity - if (actuator_info_.joints[0].state_interfaces.size() != 2) {return return_type::ERROR;} + + /* + * a hardware can optional prove for incorrect info here. + * + * // can only control one joint + * if (actuator_info_.joints.size() != 1) {return return_type::ERROR;} + * // can only control in position + * if (actuator_info_.joints[0].command_interfaces.size() != 1) {return return_type::ERROR;} + * // can only give feedback state for position and velocity + * if (actuator_info_.joints[0].state_interfaces.size() != 2) {return return_type::ERROR;} + */ return return_type::OK; } @@ -50,6 +55,11 @@ class TestActuator : public hardware_interface::components::ActuatorInterface actuator_info_.joints[0].name, actuator_info_.joints[0].state_interfaces[1].name, &velocity_state_)); + state_interfaces.emplace_back( + hardware_interface::StateInterface( + actuator_info_.joints[0].name, + "some_unlisted_interface", + nullptr)); return state_interfaces; } diff --git a/controller_manager/test/test_components/test_system.cpp b/controller_manager/test/test_components/test_system.cpp index e19ebc522f..0074d86bd9 100644 --- a/controller_manager/test/test_components/test_system.cpp +++ b/controller_manager/test/test_components/test_system.cpp @@ -38,6 +38,12 @@ class TestSystem : public hardware_interface::components::SystemInterface state_interfaces.emplace_back( hardware_interface::StateInterface( system_info_.joints[i].name, "position", &position_state_[i])); + state_interfaces.emplace_back( + hardware_interface::StateInterface( + system_info_.joints[i].name, "velocity", &velocity_state_[i])); + state_interfaces.emplace_back( + hardware_interface::StateInterface( + system_info_.joints[i].name, "acceleration", &acceleration_state_[i])); } return state_interfaces; @@ -83,6 +89,8 @@ class TestSystem : public hardware_interface::components::SystemInterface private: std::array velocity_command_ = {0.0, 0.0}; std::array position_state_ = {0.0, 0.0}; + std::array velocity_state_ = {0.0, 0.0}; + std::array acceleration_state_ = {0.0, 0.0}; hardware_interface::HardwareInfo system_info_; }; diff --git a/controller_manager/test/test_resource_manager.cpp b/controller_manager/test/test_resource_manager.cpp index ba646c0ea3..22eb1787a5 100644 --- a/controller_manager/test/test_resource_manager.cpp +++ b/controller_manager/test/test_resource_manager.cpp @@ -124,10 +124,57 @@ class TestResourceManager : public ::testing::Test )"; + + test_hardware_resource_system_missing_keys_ = + R"( + + + test_actuator + + + + + + + + + + + + test_sensor + 2 + 2 + + + + + + + + + test_system + 2 + 2 + + + + + + + + + + + + + + +)"; } std::string urdf_head_; std::string test_hardware_resource_system_; + std::string test_hardware_resource_system_missing_keys_; std::string urdf_tail_; }; @@ -140,14 +187,20 @@ 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); + ASSERT_NO_THROW(controller_manager::ResourceManager rm(urdf)); +} + +TEST_F(TestResourceManager, initialization_with_urdf_manual_validation) { + auto urdf = urdf_head_ + test_hardware_resource_system_ + urdf_tail_; + // we validate the results manually + controller_manager::ResourceManager rm(urdf, false); EXPECT_EQ(1u, rm.actuator_interfaces_size()); EXPECT_EQ(1u, rm.sensor_interfaces_size()); EXPECT_EQ(1u, rm.system_interfaces_size()); auto state_interface_keys = rm.state_interface_keys(); - ASSERT_EQ(5u, state_interface_keys.size()); + ASSERT_EQ(10u, state_interface_keys.size()); EXPECT_TRUE(rm.state_interface_exists("joint1/position")); EXPECT_TRUE(rm.state_interface_exists("joint1/velocity")); EXPECT_TRUE(rm.state_interface_exists("sensor1/velocity")); @@ -160,3 +213,14 @@ TEST_F(TestResourceManager, initialization_with_urdf) { EXPECT_TRUE(rm.command_interface_exists("joint2/velocity")); EXPECT_TRUE(rm.command_interface_exists("joint3/velocity")); } + +TEST_F(TestResourceManager, initialization_with_wrong_urdf) { + auto urdf = urdf_head_ + test_hardware_resource_system_missing_keys_ + urdf_tail_; + try { + controller_manager::ResourceManager rm(urdf); + FAIL(); + } catch (const std::exception & e) { + std::cout << e.what() << std::endl; + SUCCEED() << e.what(); + } +} diff --git a/hardware_interface/include/hardware_interface/component_info.hpp b/hardware_interface/include/hardware_interface/component_info.hpp deleted file mode 100644 index 258ada906a..0000000000 --- a/hardware_interface/include/hardware_interface/component_info.hpp +++ /dev/null @@ -1,77 +0,0 @@ -// Copyright 2020 ros2_control Development Team -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef HARDWARE_INTERFACE__COMPONENT_INFO_HPP_ -#define HARDWARE_INTERFACE__COMPONENT_INFO_HPP_ - -#include -#include -#include - -namespace hardware_interface -{ - -/** - * \brief This structure stores information about components defined for a specific hardware - * in robot's URDF. - */ -struct InterfaceInfo -{ - /** - * \brief name of the command interfaces that can be set, e.g. "position", "velocity", etc. - * Used by joints. - */ - std::string name; - /** - * \brief (optional) minimal allowed values of the interface. - */ - std::string min; - /** - * \brief (optional) maximal allowed values of the interface. - */ - std::string max; -}; - -/** - * \brief This structure stores information about components defined for a specific hardware - * in robot's URDF. - */ -struct ComponentInfo -{ - /** - * \brief name of the component. - */ - std::string name; - /** - * \brief type of the component: sensor or joint. - */ - std::string type; - /** - * \brief name of the command interfaces that can be set, e.g. "position", "velocity", etc. - * Used by joints. - */ - std::vector command_interfaces; - /** - * \brief name of the state interfaces that can be read, e.g. "position", "velocity", etc. - * Used by Joints and Sensors. - */ - std::vector state_interfaces; - /** - * \brief (optional) key-value pairs of component parameters, e.g. min/max values or serial number. - */ - std::unordered_map parameters; -}; - -} // namespace hardware_interface -#endif // HARDWARE_INTERFACE__COMPONENT_INFO_HPP_ diff --git a/hardware_interface/include/hardware_interface/hardware_info.hpp b/hardware_interface/include/hardware_interface/hardware_info.hpp index b858388600..349b88f9f3 100644 --- a/hardware_interface/include/hardware_interface/hardware_info.hpp +++ b/hardware_interface/include/hardware_interface/hardware_info.hpp @@ -20,11 +20,59 @@ #include #include -#include "hardware_interface/component_info.hpp" - namespace hardware_interface { +/** + * \brief This structure stores information about components defined for a specific hardware + * in robot's URDF. + */ +struct InterfaceInfo +{ + /** + * \brief name of the command interfaces that can be set, e.g. "position", "velocity", etc. + * Used by joints. + */ + std::string name; + /** + * \brief (optional) minimal allowed values of the interface. + */ + std::string min; + /** + * \brief (optional) maximal allowed values of the interface. + */ + std::string max; +}; + +/** + * \brief This structure stores information about components defined for a specific hardware + * in robot's URDF. + */ +struct ComponentInfo +{ + /** + * \brief name of the component. + */ + std::string name; + /** + * \brief type of the component: sensor or joint. + */ + std::string type; + /** + * \brief name of the command interfaces that can be set, e.g. "position", "velocity", etc. + * Used by joints. + */ + std::vector command_interfaces; + /** + * \brief name of the state interfaces that can be read, e.g. "position", "velocity", etc. + * Used by Joints and Sensors. + */ + std::vector state_interfaces; + /** + * \brief (optional) key-value pairs of component parameters, e.g. min/max values or serial number. + */ + std::unordered_map parameters; +}; /** * \brief This structure stores information about hardware defined in a robot's URDF. */ diff --git a/hardware_interface/src/component_parser.cpp b/hardware_interface/src/component_parser.cpp index e0d7d3679e..3e87936361 100644 --- a/hardware_interface/src/component_parser.cpp +++ b/hardware_interface/src/component_parser.cpp @@ -18,7 +18,6 @@ #include #include -#include "hardware_interface/component_info.hpp" #include "hardware_interface/hardware_info.hpp" #include "hardware_interface/component_parser.hpp" From 9f632e3e7851327d501f99e01e2c23e48c074452 Mon Sep 17 00:00:00 2001 From: Karsten Knese Date: Tue, 3 Nov 2020 12:51:44 -0800 Subject: [PATCH 12/18] documentation Signed-off-by: Karsten Knese --- controller_manager/src/resource_manager.hpp | 13 +++++++++++++ 1 file changed, 13 insertions(+) diff --git a/controller_manager/src/resource_manager.hpp b/controller_manager/src/resource_manager.hpp index df1881e7a9..190bc08602 100644 --- a/controller_manager/src/resource_manager.hpp +++ b/controller_manager/src/resource_manager.hpp @@ -29,6 +29,19 @@ class ResourceManager public: ResourceManager(); + /// Constructor for the Resource Manager. + /** + * The implementation loads the specified urdf and initializes the + * hardware components listed within as well as populate their respective + * state and command interfaces. + * + * If the interfaces ought to be validated, the constructor throws an exception + * in case the URDF lists interfaces which are not available. + * + * \param urdf string containing the URDF. + * \param validate_interfaces boolean argument indicating whether the exported + * interfaces ought to be validated. Defaults to true. + */ explicit ResourceManager( const std::string & urdf, bool validate_interfaces = true); From 56b11f4293045522ae4a3e66949f7f87b3451675 Mon Sep 17 00:00:00 2001 From: Karsten Knese Date: Tue, 3 Nov 2020 12:55:21 -0800 Subject: [PATCH 13/18] remove default constructor Signed-off-by: Karsten Knese --- controller_manager/src/resource_manager.cpp | 4 ---- controller_manager/src/resource_manager.hpp | 2 -- controller_manager/test/test_resource_manager.cpp | 5 +---- 3 files changed, 1 insertion(+), 10 deletions(-) diff --git a/controller_manager/src/resource_manager.cpp b/controller_manager/src/resource_manager.cpp index 7398ac1c90..127be230a9 100644 --- a/controller_manager/src/resource_manager.cpp +++ b/controller_manager/src/resource_manager.cpp @@ -133,10 +133,6 @@ class ResourceStorage std::unordered_map command_interface_map_; }; -ResourceManager::ResourceManager() -: resource_storage_(std::make_unique()) -{} - ResourceManager::~ResourceManager() = default; ResourceManager::ResourceManager(const std::string & urdf, bool validate_interfaces) diff --git a/controller_manager/src/resource_manager.hpp b/controller_manager/src/resource_manager.hpp index 190bc08602..0feb9c4278 100644 --- a/controller_manager/src/resource_manager.hpp +++ b/controller_manager/src/resource_manager.hpp @@ -27,8 +27,6 @@ class ResourceStorage; class ResourceManager { public: - ResourceManager(); - /// Constructor for the Resource Manager. /** * The implementation loads the specified urdf and initializes the diff --git a/controller_manager/test/test_resource_manager.cpp b/controller_manager/test/test_resource_manager.cpp index 22eb1787a5..2410c3223d 100644 --- a/controller_manager/test/test_resource_manager.cpp +++ b/controller_manager/test/test_resource_manager.cpp @@ -179,10 +179,7 @@ class TestResourceManager : public ::testing::Test }; 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()); + ASSERT_ANY_THROW(controller_manager::ResourceManager rm("")); } TEST_F(TestResourceManager, initialization_with_urdf) { From f6f682a684e90c20af0be1f730eaf2eb98babbf8 Mon Sep 17 00:00:00 2001 From: Karsten Knese Date: Tue, 3 Nov 2020 19:29:12 -0800 Subject: [PATCH 14/18] resource loaning Signed-off-by: Karsten Knese --- .../loaned_command_interface.hpp | 57 ++++++++++++++++++ controller_manager/src/resource_manager.cpp | 54 ++++++++++++++++- controller_manager/src/resource_manager.hpp | 41 +++++++++++-- .../test/test_resource_manager.cpp | 60 ++++++++++++++++++- .../include/hardware_interface/handle.hpp | 8 ++- 5 files changed, 208 insertions(+), 12 deletions(-) create mode 100644 controller_manager/include/controller_manager/loaned_command_interface.hpp diff --git a/controller_manager/include/controller_manager/loaned_command_interface.hpp b/controller_manager/include/controller_manager/loaned_command_interface.hpp new file mode 100644 index 0000000000..7e6b051aed --- /dev/null +++ b/controller_manager/include/controller_manager/loaned_command_interface.hpp @@ -0,0 +1,57 @@ +// 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 CONTROLLER_MANAGER__LOANED_COMMAND_INTERFACE_HPP_ +#define CONTROLLER_MANAGER__LOANED_COMMAND_INTERFACE_HPP_ + +#include "hardware_interface/handle.hpp" + +namespace controller_manager +{ + +class LoanedCommandInterface +{ +public: + using Deleter = std::function; + + LoanedCommandInterface( + hardware_interface::CommandInterface & command_interface, + Deleter && deleter) + : command_interface_(command_interface), + deleter_(std::forward(deleter)) + {} + + LoanedCommandInterface(const LoanedCommandInterface & other) = delete; + + LoanedCommandInterface(LoanedCommandInterface && other) = default; + + virtual ~LoanedCommandInterface() + { + if (deleter_) { + deleter_(); + } + } + + void set_value(double val) + { + command_interface_.set_value(val); + } + +private: + hardware_interface::CommandInterface & command_interface_; + Deleter deleter_; +}; + +} // namespace controller_manager +#endif // CONTROLLER_MANAGER__LOANED_COMMAND_INTERFACE_HPP_ diff --git a/controller_manager/src/resource_manager.cpp b/controller_manager/src/resource_manager.cpp index 127be230a9..1047245cfa 100644 --- a/controller_manager/src/resource_manager.cpp +++ b/controller_manager/src/resource_manager.cpp @@ -12,6 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include #include #include #include @@ -202,6 +203,24 @@ ResourceManager::ResourceManager(const std::string & urdf, bool validate_interfa if (validate_interfaces) { validate_storage(); } + + claimed_command_interface_map_.reserve(resource_storage_->command_interface_map_.size()); + for (auto & command_interface_it : resource_storage_->command_interface_map_) { + const auto & interface_name = std::get<0>(command_interface_it); + claimed_command_interface_map_[interface_name] = false; + //// set deleter to command handle + //hardware_interface::CommandInterface::Deleter d = std::bind( + // &ResourceManager::release_command_interface, this, interface_name); + //std::get<1>(command_interface_it).set_deleter(std::move(d)); + } +} + +// make this a lambda? +void ResourceManager::release_command_interface(const std::string & key) +{ + std::lock_guard lg(resource_lock_); + std::cout << "releasing " << key << std::endl; + claimed_command_interface_map_[key] = false; } std::vector ResourceManager::state_interface_keys() const @@ -219,6 +238,35 @@ bool ResourceManager::state_interface_exists(const std::string & key) const resource_storage_->state_interface_map_.end(); } +bool ResourceManager::command_interface_is_claimed(const std::string & key) const +{ + if (!command_interface_exists(key)) { + return false; + } + + std::lock_guard lg(resource_lock_); + return claimed_command_interface_map_.at(key); +} + +LoanedCommandInterface ResourceManager::claim_command_interface(const std::string & key) +{ + if (!command_interface_exists(key)) { + throw std::runtime_error( + std::string("command interface with key") + key + " does not exist"); + } + + if (command_interface_is_claimed(key)) { + throw std::runtime_error( + std::string("command interface with key") + key + " is already claimed"); + } + + std::lock_guard lg(resource_lock_); + claimed_command_interface_map_[key] = true; + return LoanedCommandInterface( + resource_storage_->command_interface_map_.at(key), + std::bind(&ResourceManager::release_command_interface, this, key)); +} + std::vector ResourceManager::command_interface_keys() const { std::vector keys; @@ -234,17 +282,17 @@ bool ResourceManager::command_interface_exists(const std::string & key) const resource_storage_->command_interface_map_.end(); } -size_t ResourceManager::actuator_interfaces_size() const +size_t ResourceManager::actuator_components_size() const { return resource_storage_->actuators_.size(); } -size_t ResourceManager::sensor_interfaces_size() const +size_t ResourceManager::sensor_components_size() const { return resource_storage_->sensors_.size(); } -size_t ResourceManager::system_interfaces_size() const +size_t ResourceManager::system_components_size() const { return resource_storage_->systems_.size(); } diff --git a/controller_manager/src/resource_manager.hpp b/controller_manager/src/resource_manager.hpp index 0feb9c4278..d28c082a2f 100644 --- a/controller_manager/src/resource_manager.hpp +++ b/controller_manager/src/resource_manager.hpp @@ -17,8 +17,11 @@ #include #include +#include #include +#include "controller_manager/loaned_command_interface.hpp" + namespace controller_manager { @@ -55,12 +58,34 @@ class ResourceManager */ std::vector state_interface_keys() const; - /// Checks whether a interface is registered under the given key. + /// Checks whether a state interface is registered under the given key. /** * \return true if interface exist, false otherwise. */ bool state_interface_exists(const std::string & key) const; + /// Checks whether a command interface is already claimed. + /** + * Any command interface can only be claimed by a single instance. + * \note the equivalent function does not exist for state interfaces. + * These are solely read-only and can thus be used by multiple instances. + * + * \param key string identifying the interface to check. + * \return true if interface is already claimed, false if available. + */ + bool command_interface_is_claimed(const std::string & key) const; + + /// Claim a command interface given its key. + /** + * 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 key String identifier which command interface to claim + * \return command interface + */ + LoanedCommandInterface claim_command_interface(const std::string & key); + /// Returns all registered command interfaces keys. /** * The keys are collected from each loaded hardware component. @@ -69,7 +94,7 @@ class ResourceManager */ std::vector command_interface_keys() const; - /// Checks whether a interface is registered under the given key. + /// Checks whether a command interface is registered under the given key. /** * \return true if interface exist, false otherwise. */ @@ -79,21 +104,27 @@ class ResourceManager /** * \return number of actuator components. */ - size_t actuator_interfaces_size() const; + size_t actuator_components_size() const; /// Return the number of loaded sensor components. /** * \return number of sensor components. */ - size_t sensor_interfaces_size() const; + size_t sensor_components_size() const; /// Return the number of loaded system components. /** * \return number of system components. */ - size_t system_interfaces_size() const; + size_t system_components_size() const; private: + void release_command_interface(const std::string & key); + + // TODO(karsten1987): Optimize this with std::vector, maps are bad + std::unordered_map claimed_command_interface_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 2410c3223d..81e3ee2903 100644 --- a/controller_manager/test/test_resource_manager.cpp +++ b/controller_manager/test/test_resource_manager.cpp @@ -192,9 +192,9 @@ TEST_F(TestResourceManager, initialization_with_urdf_manual_validation) { // we validate the results manually controller_manager::ResourceManager rm(urdf, false); - EXPECT_EQ(1u, rm.actuator_interfaces_size()); - EXPECT_EQ(1u, rm.sensor_interfaces_size()); - EXPECT_EQ(1u, rm.system_interfaces_size()); + EXPECT_EQ(1u, rm.actuator_components_size()); + EXPECT_EQ(1u, rm.sensor_components_size()); + EXPECT_EQ(1u, rm.system_components_size()); auto state_interface_keys = rm.state_interface_keys(); ASSERT_EQ(10u, state_interface_keys.size()); @@ -221,3 +221,57 @@ TEST_F(TestResourceManager, initialization_with_wrong_urdf) { SUCCEED() << e.what(); } } + +TEST_F(TestResourceManager, initialization_with_urdf_unclaimed) { + auto urdf = urdf_head_ + test_hardware_resource_system_ + urdf_tail_; + // we validate the results manually + controller_manager::ResourceManager rm(urdf); + + auto command_interface_keys = rm.command_interface_keys(); + for (const auto & key : command_interface_keys) { + EXPECT_FALSE(rm.command_interface_is_claimed(key)); + } + // state interfaces don't have to be locked, hence any arbitrary key + // should return false. + auto state_interface_keys = rm.state_interface_keys(); + for (const auto & key : state_interface_keys) { + EXPECT_FALSE(rm.command_interface_is_claimed(key)); + } +} + +TEST_F(TestResourceManager, resource_claiming) { + auto urdf = urdf_head_ + test_hardware_resource_system_ + urdf_tail_; + controller_manager::ResourceManager rm(urdf); + + const auto key = "joint1/position"; + EXPECT_FALSE(rm.command_interface_is_claimed(key)); + + { + auto position_command_interface = rm.claim_command_interface(key); + EXPECT_TRUE(rm.command_interface_is_claimed(key)); + try { + auto sensor1_again = rm.claim_command_interface(key); + FAIL(); + } catch (const std::runtime_error &) { + SUCCEED(); + } + } + EXPECT_FALSE(rm.command_interface_is_claimed(key)); + + for (const auto & key : + {"joint1/position", "joint1/position", "joint1/position", "joint2/velocity", + "joint3/velocity"}) + { + { + auto interface = rm.claim_command_interface(key); + EXPECT_TRUE(rm.command_interface_is_claimed(key)); + try { + auto interface_again = rm.claim_command_interface(key); + FAIL(); + } catch (const std::runtime_error &) { + SUCCEED(); + } + } + EXPECT_FALSE(rm.command_interface_is_claimed(key)); + } +} diff --git a/hardware_interface/include/hardware_interface/handle.hpp b/hardware_interface/include/hardware_interface/handle.hpp index 12a6cecffc..7677bf37ab 100644 --- a/hardware_interface/include/hardware_interface/handle.hpp +++ b/hardware_interface/include/hardware_interface/handle.hpp @@ -139,7 +139,7 @@ class ReadWriteHandle : public ReadOnlyHandle class StateInterface : public ReadOnlyHandle { public: - StateInterface(const StateInterface & other) = delete; + StateInterface(const StateInterface & other) = default; StateInterface(StateInterface && other) = default; @@ -149,6 +149,12 @@ class StateInterface : public ReadOnlyHandle class CommandInterface : public ReadWriteHandle { public: + /// CommandInterface copy constructor is actively deleted + /** + * Command interfaces are having a unique ownership and thus + * can't be copied in order to avoid simultaneous writes to + * the same resource. + */ CommandInterface(const CommandInterface & other) = delete; CommandInterface(CommandInterface && other) = default; From 3deba8db0b119768d18335ccfcb0590cdd531f50 Mon Sep 17 00:00:00 2001 From: Karsten Knese Date: Wed, 4 Nov 2020 12:35:55 -0800 Subject: [PATCH 15/18] loan state interface Signed-off-by: Karsten Knese --- .../loaned_command_interface.hpp | 11 ++++ .../loaned_state_interface.hpp | 63 +++++++++++++++++++ controller_manager/src/resource_manager.cpp | 17 ++--- controller_manager/src/resource_manager.hpp | 12 ++++ .../test/test_resource_manager.cpp | 17 +++++ 5 files changed, 113 insertions(+), 7 deletions(-) create mode 100644 controller_manager/include/controller_manager/loaned_state_interface.hpp diff --git a/controller_manager/include/controller_manager/loaned_command_interface.hpp b/controller_manager/include/controller_manager/loaned_command_interface.hpp index 7e6b051aed..47455a96b3 100644 --- a/controller_manager/include/controller_manager/loaned_command_interface.hpp +++ b/controller_manager/include/controller_manager/loaned_command_interface.hpp @@ -15,6 +15,8 @@ #ifndef CONTROLLER_MANAGER__LOANED_COMMAND_INTERFACE_HPP_ #define CONTROLLER_MANAGER__LOANED_COMMAND_INTERFACE_HPP_ +#include + #include "hardware_interface/handle.hpp" namespace controller_manager @@ -25,6 +27,10 @@ class LoanedCommandInterface public: using Deleter = std::function; + explicit LoanedCommandInterface(hardware_interface::CommandInterface & command_interface) + : LoanedCommandInterface(command_interface, nullptr) + {} + LoanedCommandInterface( hardware_interface::CommandInterface & command_interface, Deleter && deleter) @@ -48,6 +54,11 @@ class LoanedCommandInterface command_interface_.set_value(val); } + double get_value() + { + return command_interface_.get_value(); + } + private: hardware_interface::CommandInterface & command_interface_; Deleter deleter_; diff --git a/controller_manager/include/controller_manager/loaned_state_interface.hpp b/controller_manager/include/controller_manager/loaned_state_interface.hpp new file mode 100644 index 0000000000..97bf5b6de9 --- /dev/null +++ b/controller_manager/include/controller_manager/loaned_state_interface.hpp @@ -0,0 +1,63 @@ +// 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 CONTROLLER_MANAGER__LOANED_STATE_INTERFACE_HPP_ +#define CONTROLLER_MANAGER__LOANED_STATE_INTERFACE_HPP_ + +#include + +#include "hardware_interface/handle.hpp" + +namespace controller_manager +{ + +class LoanedStateInterface +{ +public: + using Deleter = std::function; + + explicit LoanedStateInterface(hardware_interface::StateInterface & state_interface) + : LoanedStateInterface(state_interface, nullptr) + {} + + LoanedStateInterface( + hardware_interface::StateInterface & state_interface, + Deleter && deleter) + : state_interface_(state_interface), + deleter_(std::forward(deleter)) + {} + + LoanedStateInterface(const LoanedStateInterface & other) = delete; + + LoanedStateInterface(LoanedStateInterface && other) = default; + + virtual ~LoanedStateInterface() + { + if (deleter_) { + deleter_(); + } + } + + double get_value() + { + return state_interface_.get_value(); + } + +private: + hardware_interface::StateInterface & state_interface_; + Deleter deleter_; +}; + +} // namespace controller_manager +#endif // CONTROLLER_MANAGER__LOANED_STATE_INTERFACE_HPP_ diff --git a/controller_manager/src/resource_manager.cpp b/controller_manager/src/resource_manager.cpp index 1047245cfa..f4c3f091cb 100644 --- a/controller_manager/src/resource_manager.cpp +++ b/controller_manager/src/resource_manager.cpp @@ -12,7 +12,6 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include #include #include #include @@ -208,21 +207,25 @@ ResourceManager::ResourceManager(const std::string & urdf, bool validate_interfa for (auto & command_interface_it : resource_storage_->command_interface_map_) { const auto & interface_name = std::get<0>(command_interface_it); claimed_command_interface_map_[interface_name] = false; - //// set deleter to command handle - //hardware_interface::CommandInterface::Deleter d = std::bind( - // &ResourceManager::release_command_interface, this, interface_name); - //std::get<1>(command_interface_it).set_deleter(std::move(d)); } } -// make this a lambda? void ResourceManager::release_command_interface(const std::string & key) { std::lock_guard lg(resource_lock_); - std::cout << "releasing " << key << std::endl; claimed_command_interface_map_[key] = false; } +LoanedStateInterface ResourceManager::claim_state_interface(const std::string & key) +{ + if (!state_interface_exists(key)) { + throw std::runtime_error( + std::string("state interface with key") + key + " does not exist"); + } + + std::lock_guard lg(resource_lock_); + return LoanedStateInterface(resource_storage_->state_interface_map_.at(key)); +} std::vector ResourceManager::state_interface_keys() const { std::vector keys; diff --git a/controller_manager/src/resource_manager.hpp b/controller_manager/src/resource_manager.hpp index d28c082a2f..c0685612f7 100644 --- a/controller_manager/src/resource_manager.hpp +++ b/controller_manager/src/resource_manager.hpp @@ -21,6 +21,7 @@ #include #include "controller_manager/loaned_command_interface.hpp" +#include "controller_manager/loaned_state_interface.hpp" namespace controller_manager { @@ -50,6 +51,17 @@ class ResourceManager ~ResourceManager(); + /// Claim a state interface given its key. + /** + * The resource is claimed as long as being in scope. + * Once the resource is going out of scope, the destructor + * returns. + * + * \param key String identifier which state interface to claim + * \return state interface + */ + LoanedStateInterface claim_state_interface(const std::string & key); + /// Returns all registered state interfaces keys. /** * The keys are collected from each loaded hardware component. diff --git a/controller_manager/test/test_resource_manager.cpp b/controller_manager/test/test_resource_manager.cpp index 81e3ee2903..bfe1819a38 100644 --- a/controller_manager/test/test_resource_manager.cpp +++ b/controller_manager/test/test_resource_manager.cpp @@ -258,6 +258,7 @@ TEST_F(TestResourceManager, resource_claiming) { } EXPECT_FALSE(rm.command_interface_is_claimed(key)); + // command interfaces can only be claimed once for (const auto & key : {"joint1/position", "joint1/position", "joint1/position", "joint2/velocity", "joint3/velocity"}) @@ -274,4 +275,20 @@ TEST_F(TestResourceManager, resource_claiming) { } EXPECT_FALSE(rm.command_interface_is_claimed(key)); } + + // state interfaces can be claimed multiple times + for (const auto & key : + {"joint1/position", "joint1/velocity", "sensor1/velocity", "joint2/position", + "joint3/position"}) + { + { + auto interface = rm.claim_state_interface(key); + try { + auto interface_again = rm.claim_state_interface(key); + SUCCEED(); + } catch (const std::runtime_error &) { + FAIL(); + } + } + } } From 7ac53d593bf4e0583efe49831aab881da478393f Mon Sep 17 00:00:00 2001 From: Karsten Knese Date: Wed, 4 Nov 2020 17:17:18 -0800 Subject: [PATCH 16/18] import externally declared components Signed-off-by: Karsten Knese --- controller_manager/src/resource_manager.cpp | 58 +++++++++++--- controller_manager/src/resource_manager.hpp | 47 +++++++++++ .../test/test_resource_manager.cpp | 80 +++++++++++++++++++ 3 files changed, 172 insertions(+), 13 deletions(-) diff --git a/controller_manager/src/resource_manager.cpp b/controller_manager/src/resource_manager.cpp index f4c3f091cb..49798a617f 100644 --- a/controller_manager/src/resource_manager.cpp +++ b/controller_manager/src/resource_manager.cpp @@ -79,17 +79,23 @@ class ResourceStorage } template - void import_command_interfaces(HardwareT & hardware) + void import_command_interfaces( + HardwareT & hardware, + std::unordered_map & claimed_command_interface_map) { auto interfaces = hardware.export_command_interfaces(); for (auto i = 0u; i < interfaces.size(); ++i) { auto key = interfaces[i].get_name() + "/" + interfaces[i].get_interface_name(); command_interface_map_.emplace( std::make_pair(key, std::move(interfaces[i]))); + claimed_command_interface_map.emplace( + std::make_pair(key, false)); } } - void initialize_actuator(const hardware_interface::HardwareInfo & hardware_info) + void initialize_actuator( + const hardware_interface::HardwareInfo & hardware_info, + std::unordered_map & claimed_command_interface_map) { initialize_hardware( @@ -98,7 +104,7 @@ class ResourceStorage throw std::runtime_error(std::string("failed to configure ") + hardware_info.name); } import_state_interfaces(actuators_.back()); - import_command_interfaces(actuators_.back()); + import_command_interfaces(actuators_.back(), claimed_command_interface_map); } void initialize_sensor(const hardware_interface::HardwareInfo & hardware_info) @@ -110,14 +116,16 @@ class ResourceStorage import_state_interfaces(sensors_.back()); } - void initialize_system(const hardware_interface::HardwareInfo & hardware_info) + void initialize_system( + const hardware_interface::HardwareInfo & hardware_info, + std::unordered_map & claimed_command_interface_map) { initialize_hardware( hardware_info, system_loader_, systems_); systems_.back().configure(hardware_info); import_state_interfaces(systems_.back()); - import_command_interfaces(systems_.back()); + import_command_interfaces(systems_.back(), claimed_command_interface_map); } // hardware plugins @@ -146,13 +154,13 @@ ResourceManager::ResourceManager(const std::string & urdf, bool validate_interfa for (const auto & hardware : hardware_info) { if (hardware.type == actuator_type) { - resource_storage_->initialize_actuator(hardware); + resource_storage_->initialize_actuator(hardware, claimed_command_interface_map_); } if (hardware.type == sensor_type) { resource_storage_->initialize_sensor(hardware); } if (hardware.type == system_type) { - resource_storage_->initialize_system(hardware); + resource_storage_->initialize_system(hardware, claimed_command_interface_map_); } } @@ -202,12 +210,6 @@ ResourceManager::ResourceManager(const std::string & urdf, bool validate_interfa if (validate_interfaces) { validate_storage(); } - - claimed_command_interface_map_.reserve(resource_storage_->command_interface_map_.size()); - for (auto & command_interface_it : resource_storage_->command_interface_map_) { - const auto & interface_name = std::get<0>(command_interface_it); - claimed_command_interface_map_[interface_name] = false; - } } void ResourceManager::release_command_interface(const std::string & key) @@ -285,16 +287,46 @@ bool ResourceManager::command_interface_exists(const std::string & key) const resource_storage_->command_interface_map_.end(); } +void ResourceManager::import_component( + std::unique_ptr actuator) +{ + resource_storage_->actuators_.emplace_back( + hardware_interface::components::Actuator( + std::move( + actuator))); + resource_storage_->import_state_interfaces(resource_storage_->actuators_.back()); + resource_storage_->import_command_interfaces( + resource_storage_->actuators_.back(), claimed_command_interface_map_); +} + size_t ResourceManager::actuator_components_size() const { return resource_storage_->actuators_.size(); } +void ResourceManager::import_component( + std::unique_ptr sensor) +{ + resource_storage_->sensors_.emplace_back( + hardware_interface::components::Sensor(std::move(sensor))); + resource_storage_->import_state_interfaces(resource_storage_->sensors_.back()); +} + size_t ResourceManager::sensor_components_size() const { return resource_storage_->sensors_.size(); } +void ResourceManager::import_component( + std::unique_ptr system) +{ + resource_storage_->systems_.emplace_back( + hardware_interface::components::System(std::move(system))); + resource_storage_->import_state_interfaces(resource_storage_->systems_.back()); + resource_storage_->import_command_interfaces( + resource_storage_->systems_.back(), claimed_command_interface_map_); +} + size_t ResourceManager::system_components_size() const { return resource_storage_->systems_.size(); diff --git a/controller_manager/src/resource_manager.hpp b/controller_manager/src/resource_manager.hpp index c0685612f7..b420765298 100644 --- a/controller_manager/src/resource_manager.hpp +++ b/controller_manager/src/resource_manager.hpp @@ -23,6 +23,16 @@ #include "controller_manager/loaned_command_interface.hpp" #include "controller_manager/loaned_state_interface.hpp" +namespace hardware_interface +{ +namespace components +{ +class ActuatorInterface; +class SensorInterface; +class SystemInterface; +} // namespace components +} // namespace hardware_interface + namespace controller_manager { @@ -118,18 +128,55 @@ class ResourceManager */ size_t actuator_components_size() const; + /// Import a hardware component which is not listed in the URDF + /** + * Components which are initialized outside a URDF can be added post initialization. + * + * \note this might invalidate existing state and command interfaces and should thus + * not be called when a controller is running. + * \note given that no hardware_info is available, the component has to be configured + * externally and prior to the call to import. + * \param actuator pointer to the actuator interface. + */ + void import_component( + std::unique_ptr actuator); + /// Return the number of loaded sensor components. /** * \return number of sensor components. */ size_t sensor_components_size() const; + /// Import a hardware component which is not listed in the URDF + /** + * Components which are initialized outside a URDF can be added post initialization. + * + * \note this might invalidate existing state and command interfaces and should thus + * not be called when a controller is running. + * \note given that no hardware_info is available, the component has to be configured + * externally and prior to the call to import. + * \param sensor pointer to the sensor interface. + */ + void import_component(std::unique_ptr sensor); + /// Return the number of loaded system components. /** * \return number of system components. */ size_t system_components_size() const; + /// Import a hardware component which is not listed in the URDF + /** + * Components which are initialized outside a URDF can be added post initialization. + * + * \note this might invalidate existing state and command interfaces and should thus + * not be called when a controller is running. + * \note given that no hardware_info is available, the component has to be configured + * externally and prior to the call to import. + * \param system pointer to the system interface. + */ + void import_component(std::unique_ptr system); + private: void release_command_interface(const std::string & key); diff --git a/controller_manager/test/test_resource_manager.cpp b/controller_manager/test/test_resource_manager.cpp index bfe1819a38..df8b34e9fb 100644 --- a/controller_manager/test/test_resource_manager.cpp +++ b/controller_manager/test/test_resource_manager.cpp @@ -17,6 +17,9 @@ #include #include #include +#include + +#include "hardware_interface/components/actuator_interface.hpp" #include "resource_manager.hpp" @@ -292,3 +295,80 @@ TEST_F(TestResourceManager, resource_claiming) { } } } + +class ExternalComponent : public hardware_interface::components::ActuatorInterface +{ + hardware_interface::return_type configure(const hardware_interface::HardwareInfo &) override + { + return hardware_interface::return_type::OK; + } + + std::vector export_state_interfaces() override + { + std::vector state_interfaces; + state_interfaces.emplace_back( + hardware_interface::StateInterface( + "external_joint", "external_state_interface", nullptr)); + + return state_interfaces; + } + + std::vector export_command_interfaces() override + { + std::vector command_interfaces; + command_interfaces.emplace_back( + hardware_interface::CommandInterface( + "external_joint", "external_command_interface", nullptr)); + + return command_interfaces; + } + + hardware_interface::return_type start() override + { + return hardware_interface::return_type::OK; + } + + hardware_interface::return_type stop() override + { + return hardware_interface::return_type::OK; + } + + hardware_interface::status get_status() const override + { + return hardware_interface::status::UNKNOWN; + } + + hardware_interface::return_type read() override + { + return hardware_interface::return_type::OK; + } + + hardware_interface::return_type write() override + { + return hardware_interface::return_type::OK; + } +}; + +TEST_F(TestResourceManager, post_initialization_add_components) { + auto urdf = urdf_head_ + test_hardware_resource_system_ + urdf_tail_; + // we validate the results manually + controller_manager::ResourceManager rm(urdf, false); + + EXPECT_EQ(1u, rm.actuator_components_size()); + EXPECT_EQ(1u, rm.sensor_components_size()); + EXPECT_EQ(1u, rm.system_components_size()); + + ASSERT_EQ(10u, rm.state_interface_keys().size()); + ASSERT_EQ(3u, rm.command_interface_keys().size()); + + rm.import_component(std::make_unique()); + EXPECT_EQ(2u, rm.actuator_components_size()); + + ASSERT_EQ(11u, rm.state_interface_keys().size()); + EXPECT_TRUE(rm.state_interface_exists("external_joint/external_state_interface")); + ASSERT_EQ(4u, rm.command_interface_keys().size()); + EXPECT_TRUE(rm.command_interface_exists("external_joint/external_command_interface")); + + EXPECT_NO_THROW(rm.claim_state_interface("external_joint/external_state_interface")); + EXPECT_NO_THROW(rm.claim_command_interface("external_joint/external_command_interface")); +} From 76a8e5cb06a0f58d63183b609f279a34f33f2eec Mon Sep 17 00:00:00 2001 From: Karsten Knese Date: Mon, 9 Nov 2020 14:56:00 -0800 Subject: [PATCH 17/18] move resource manager to hardware interface (#226) * move resource manager to hardware interface Signed-off-by: Karsten Knese * include functional for std::function Co-authored-by: Bence Magyar --- controller_manager/CMakeLists.txt | 21 ------- hardware_interface/CMakeLists.txt | 51 ++++++++------- .../loaned_command_interface.hpp | 17 ++--- .../loaned_state_interface.hpp | 17 ++--- .../hardware_interface}/resource_manager.hpp | 23 +++---- hardware_interface/package.xml | 1 + .../src/resource_manager.cpp | 62 ++++++++----------- .../test/test_components/test_actuator.cpp | 0 .../test/test_components/test_components.xml | 0 .../test/test_components/test_sensor.cpp | 0 .../test/test_components/test_system.cpp | 0 .../test/test_resource_manager.cpp | 17 +++-- 12 files changed, 90 insertions(+), 119 deletions(-) rename {controller_manager/include/controller_manager => hardware_interface/include/hardware_interface}/loaned_command_interface.hpp (75%) rename {controller_manager/include/controller_manager => hardware_interface/include/hardware_interface}/loaned_state_interface.hpp (74%) rename {controller_manager/src => hardware_interface/include/hardware_interface}/resource_manager.hpp (91%) rename {controller_manager => hardware_interface}/src/resource_manager.cpp (82%) rename {controller_manager => hardware_interface}/test/test_components/test_actuator.cpp (100%) rename {controller_manager => hardware_interface}/test/test_components/test_components.xml (100%) rename {controller_manager => hardware_interface}/test/test_components/test_sensor.cpp (100%) rename {controller_manager => hardware_interface}/test/test_components/test_system.cpp (100%) rename {controller_manager => hardware_interface}/test/test_resource_manager.cpp (96%) diff --git a/controller_manager/CMakeLists.txt b/controller_manager/CMakeLists.txt index c0171c8cd9..4dd37237d0 100644 --- a/controller_manager/CMakeLists.txt +++ b/controller_manager/CMakeLists.txt @@ -21,7 +21,6 @@ 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 @@ -106,26 +105,6 @@ if(BUILD_TESTING) test_controller_manager_srvs test_robot_hardware ) - - add_library(test_components SHARED - test/test_components/test_actuator.cpp - test/test_components/test_sensor.cpp - test/test_components/test_system.cpp) - ament_target_dependencies(test_components - hardware_interface - pluginlib) - install(TARGETS test_components - DESTINATION lib - ) - pluginlib_export_plugin_description_file( - hardware_interface test/test_components/test_components.xml) - - ament_add_gmock( - 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/hardware_interface/CMakeLists.txt b/hardware_interface/CMakeLists.txt index a188f0ad0c..3c460859e5 100644 --- a/hardware_interface/CMakeLists.txt +++ b/hardware_interface/CMakeLists.txt @@ -12,6 +12,7 @@ endif() find_package(ament_cmake REQUIRED) find_package(control_msgs REQUIRED) +find_package(pluginlib REQUIRED) find_package(rcpputils REQUIRED) find_package(rcutils REQUIRED) find_package(tinyxml2_vendor REQUIRED) @@ -23,7 +24,9 @@ add_library( src/components/actuator.cpp src/components/sensor.cpp src/components/system.cpp + src/component_parser.cpp src/operation_mode_handle.cpp + src/resource_manager.cpp src/robot_hardware.cpp ) target_include_directories( @@ -34,27 +37,15 @@ target_include_directories( ament_target_dependencies( hardware_interface control_msgs + pluginlib rcutils rcpputils ) # Causes the visibility macros to use dllexport rather than dllimport, # which is appropriate when building the dll but not consuming it. target_compile_definitions(hardware_interface PRIVATE "HARDWARE_INTERFACE_BUILDING_DLL") - -add_library( - component_parser SHARED - 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") +# prevent pluginlib from using boost +target_compile_definitions(hardware_interface PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS") install( DIRECTORY include/ @@ -63,7 +54,6 @@ install( install( TARGETS - component_parser hardware_interface RUNTIME DESTINATION bin ARCHIVE DESTINATION lib @@ -80,26 +70,21 @@ if(BUILD_TESTING) ament_target_dependencies(test_macros rcpputils) ament_add_gmock(test_robot_hardware_interfaces test/test_robot_hardware_interface.cpp) - target_include_directories(test_robot_hardware_interfaces PRIVATE include) target_link_libraries(test_robot_hardware_interfaces hardware_interface) ament_add_gmock(test_register_actuators test/test_register_actuators.cpp) - target_include_directories(test_register_actuators PRIVATE include) target_link_libraries(test_register_actuators hardware_interface) ament_target_dependencies(test_register_actuators rcpputils) ament_add_gmock(test_register_joints test/test_register_joints.cpp) - target_include_directories(test_register_joints PRIVATE include) target_link_libraries(test_register_joints hardware_interface) ament_target_dependencies(test_register_joints rcpputils) ament_add_gmock(test_actuator_handle test/test_actuator_handle.cpp) - target_include_directories(test_actuator_handle PRIVATE include) target_link_libraries(test_actuator_handle hardware_interface) ament_target_dependencies(test_actuator_handle rcpputils) ament_add_gmock(test_joint_handle test/test_joint_handle.cpp) - target_include_directories(test_joint_handle PRIVATE include) target_link_libraries(test_joint_handle hardware_interface) ament_target_dependencies(test_joint_handle rcpputils) @@ -107,19 +92,37 @@ if(BUILD_TESTING) target_link_libraries(test_component_interfaces 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 hardware_interface) + + add_library(test_components SHARED + test/test_components/test_actuator.cpp + test/test_components/test_sensor.cpp + test/test_components/test_system.cpp) + target_link_libraries(test_components hardware_interface) + ament_target_dependencies(test_components + pluginlib) + install(TARGETS test_components + DESTINATION lib + ) + pluginlib_export_plugin_description_file( + hardware_interface test/test_components/test_components.xml) + + ament_add_gmock( + test_resource_manager + test/test_resource_manager.cpp + ) + target_link_libraries(test_resource_manager hardware_interface) endif() ament_export_include_directories( include ) ament_export_libraries( - component_parser hardware_interface ) ament_export_dependencies( control_msgs + pluginlib rcpputils tinyxml2_vendor TinyXML2 diff --git a/controller_manager/include/controller_manager/loaned_command_interface.hpp b/hardware_interface/include/hardware_interface/loaned_command_interface.hpp similarity index 75% rename from controller_manager/include/controller_manager/loaned_command_interface.hpp rename to hardware_interface/include/hardware_interface/loaned_command_interface.hpp index 47455a96b3..953869323a 100644 --- a/controller_manager/include/controller_manager/loaned_command_interface.hpp +++ b/hardware_interface/include/hardware_interface/loaned_command_interface.hpp @@ -12,14 +12,15 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef CONTROLLER_MANAGER__LOANED_COMMAND_INTERFACE_HPP_ -#define CONTROLLER_MANAGER__LOANED_COMMAND_INTERFACE_HPP_ +#ifndef HARDWARE_INTERFACE__LOANED_COMMAND_INTERFACE_HPP_ +#define HARDWARE_INTERFACE__LOANED_COMMAND_INTERFACE_HPP_ +#include #include #include "hardware_interface/handle.hpp" -namespace controller_manager +namespace hardware_interface { class LoanedCommandInterface @@ -27,12 +28,12 @@ class LoanedCommandInterface public: using Deleter = std::function; - explicit LoanedCommandInterface(hardware_interface::CommandInterface & command_interface) + explicit LoanedCommandInterface(CommandInterface & command_interface) : LoanedCommandInterface(command_interface, nullptr) {} LoanedCommandInterface( - hardware_interface::CommandInterface & command_interface, + CommandInterface & command_interface, Deleter && deleter) : command_interface_(command_interface), deleter_(std::forward(deleter)) @@ -60,9 +61,9 @@ class LoanedCommandInterface } private: - hardware_interface::CommandInterface & command_interface_; + CommandInterface & command_interface_; Deleter deleter_; }; -} // namespace controller_manager -#endif // CONTROLLER_MANAGER__LOANED_COMMAND_INTERFACE_HPP_ +} // namespace hardware_interface +#endif // HARDWARE_INTERFACE__LOANED_COMMAND_INTERFACE_HPP_ diff --git a/controller_manager/include/controller_manager/loaned_state_interface.hpp b/hardware_interface/include/hardware_interface/loaned_state_interface.hpp similarity index 74% rename from controller_manager/include/controller_manager/loaned_state_interface.hpp rename to hardware_interface/include/hardware_interface/loaned_state_interface.hpp index 97bf5b6de9..ab6c3cf06c 100644 --- a/controller_manager/include/controller_manager/loaned_state_interface.hpp +++ b/hardware_interface/include/hardware_interface/loaned_state_interface.hpp @@ -12,14 +12,15 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef CONTROLLER_MANAGER__LOANED_STATE_INTERFACE_HPP_ -#define CONTROLLER_MANAGER__LOANED_STATE_INTERFACE_HPP_ +#ifndef HARDWARE_INTERFACE__LOANED_STATE_INTERFACE_HPP_ +#define HARDWARE_INTERFACE__LOANED_STATE_INTERFACE_HPP_ +#include #include #include "hardware_interface/handle.hpp" -namespace controller_manager +namespace hardware_interface { class LoanedStateInterface @@ -27,12 +28,12 @@ class LoanedStateInterface public: using Deleter = std::function; - explicit LoanedStateInterface(hardware_interface::StateInterface & state_interface) + explicit LoanedStateInterface(StateInterface & state_interface) : LoanedStateInterface(state_interface, nullptr) {} LoanedStateInterface( - hardware_interface::StateInterface & state_interface, + StateInterface & state_interface, Deleter && deleter) : state_interface_(state_interface), deleter_(std::forward(deleter)) @@ -55,9 +56,9 @@ class LoanedStateInterface } private: - hardware_interface::StateInterface & state_interface_; + StateInterface & state_interface_; Deleter deleter_; }; -} // namespace controller_manager -#endif // CONTROLLER_MANAGER__LOANED_STATE_INTERFACE_HPP_ +} // namespace hardware_interface +#endif // HARDWARE_INTERFACE__LOANED_STATE_INTERFACE_HPP_ diff --git a/controller_manager/src/resource_manager.hpp b/hardware_interface/include/hardware_interface/resource_manager.hpp similarity index 91% rename from controller_manager/src/resource_manager.hpp rename to hardware_interface/include/hardware_interface/resource_manager.hpp index b420765298..077de472a6 100644 --- a/controller_manager/src/resource_manager.hpp +++ b/hardware_interface/include/hardware_interface/resource_manager.hpp @@ -12,16 +12,17 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef RESOURCE_MANAGER_HPP_ -#define RESOURCE_MANAGER_HPP_ +#ifndef HARDWARE_INTERFACE__RESOURCE_MANAGER_HPP_ +#define HARDWARE_INTERFACE__RESOURCE_MANAGER_HPP_ #include +#include #include #include #include -#include "controller_manager/loaned_command_interface.hpp" -#include "controller_manager/loaned_state_interface.hpp" +#include "hardware_interface/loaned_command_interface.hpp" +#include "hardware_interface/loaned_state_interface.hpp" namespace hardware_interface { @@ -31,10 +32,6 @@ class ActuatorInterface; class SensorInterface; class SystemInterface; } // namespace components -} // namespace hardware_interface - -namespace controller_manager -{ class ResourceStorage; @@ -139,7 +136,7 @@ class ResourceManager * \param actuator pointer to the actuator interface. */ void import_component( - std::unique_ptr actuator); + std::unique_ptr actuator); /// Return the number of loaded sensor components. /** @@ -157,7 +154,7 @@ class ResourceManager * externally and prior to the call to import. * \param sensor pointer to the sensor interface. */ - void import_component(std::unique_ptr sensor); + void import_component(std::unique_ptr sensor); /// Return the number of loaded system components. /** @@ -175,7 +172,7 @@ class ResourceManager * externally and prior to the call to import. * \param system pointer to the system interface. */ - void import_component(std::unique_ptr system); + void import_component(std::unique_ptr system); private: void release_command_interface(const std::string & key); @@ -187,5 +184,5 @@ class ResourceManager std::unique_ptr resource_storage_; }; -} // namespace controller_manager -#endif // RESOURCE_MANAGER_HPP_ +} // namespace hardware_interface +#endif // HARDWARE_INTERFACE__RESOURCE_MANAGER_HPP_ diff --git a/hardware_interface/package.xml b/hardware_interface/package.xml index 8866f1c95b..3d23a196f6 100644 --- a/hardware_interface/package.xml +++ b/hardware_interface/package.xml @@ -10,6 +10,7 @@ ament_cmake control_msgs + pluginlib rcpputils tinyxml2_vendor diff --git a/controller_manager/src/resource_manager.cpp b/hardware_interface/src/resource_manager.cpp similarity index 82% rename from controller_manager/src/resource_manager.cpp rename to hardware_interface/src/resource_manager.cpp index 55d4aa1ef5..c47a7ab55c 100644 --- a/controller_manager/src/resource_manager.cpp +++ b/hardware_interface/src/resource_manager.cpp @@ -12,6 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include "hardware_interface/resource_manager.hpp" + #include #include #include @@ -29,9 +31,7 @@ #include "pluginlib/class_loader.hpp" -#include "./resource_manager.hpp" - -namespace controller_manager +namespace hardware_interface { class ResourceStorage @@ -54,7 +54,7 @@ class ResourceStorage template void initialize_hardware( - const hardware_interface::HardwareInfo & hardware_info, + const HardwareInfo & hardware_info, pluginlib::ClassLoader & loader, std::vector & container) { @@ -94,34 +94,31 @@ class ResourceStorage } void initialize_actuator( - const hardware_interface::HardwareInfo & hardware_info, + const HardwareInfo & hardware_info, std::unordered_map & claimed_command_interface_map) { - initialize_hardware( + initialize_hardware( hardware_info, actuator_loader_, actuators_); - if (hardware_interface::return_type::OK != actuators_.back().configure(hardware_info)) { + if (return_type::OK != actuators_.back().configure(hardware_info)) { throw std::runtime_error(std::string("failed to configure ") + hardware_info.name); } import_state_interfaces(actuators_.back()); import_command_interfaces(actuators_.back(), claimed_command_interface_map); } - void initialize_sensor(const hardware_interface::HardwareInfo & hardware_info) + void initialize_sensor(const HardwareInfo & hardware_info) { - initialize_hardware( + initialize_hardware( hardware_info, sensor_loader_, sensors_); sensors_.back().configure(hardware_info); import_state_interfaces(sensors_.back()); } void initialize_system( - const hardware_interface::HardwareInfo & hardware_info, + const HardwareInfo & hardware_info, std::unordered_map & claimed_command_interface_map) { - initialize_hardware( + initialize_hardware( hardware_info, system_loader_, systems_); systems_.back().configure(hardware_info); import_state_interfaces(systems_.back()); @@ -129,16 +126,16 @@ class ResourceStorage } // hardware plugins - pluginlib::ClassLoader actuator_loader_; - pluginlib::ClassLoader sensor_loader_; - pluginlib::ClassLoader system_loader_; + pluginlib::ClassLoader actuator_loader_; + pluginlib::ClassLoader sensor_loader_; + pluginlib::ClassLoader system_loader_; - std::vector actuators_; - std::vector sensors_; - std::vector systems_; + std::vector actuators_; + std::vector sensors_; + std::vector systems_; - std::unordered_map state_interface_map_; - std::unordered_map command_interface_map_; + std::unordered_map state_interface_map_; + std::unordered_map command_interface_map_; }; ResourceManager::~ResourceManager() = default; @@ -288,13 +285,10 @@ bool ResourceManager::command_interface_exists(const std::string & key) const resource_storage_->command_interface_map_.end(); } -void ResourceManager::import_component( - std::unique_ptr actuator) +void ResourceManager::import_component(std::unique_ptr actuator) { resource_storage_->actuators_.emplace_back( - hardware_interface::components::Actuator( - std::move( - actuator))); + components::Actuator(std::move(actuator))); resource_storage_->import_state_interfaces(resource_storage_->actuators_.back()); resource_storage_->import_command_interfaces( resource_storage_->actuators_.back(), claimed_command_interface_map_); @@ -305,11 +299,9 @@ size_t ResourceManager::actuator_components_size() const return resource_storage_->actuators_.size(); } -void ResourceManager::import_component( - std::unique_ptr sensor) +void ResourceManager::import_component(std::unique_ptr sensor) { - resource_storage_->sensors_.emplace_back( - hardware_interface::components::Sensor(std::move(sensor))); + resource_storage_->sensors_.emplace_back(components::Sensor(std::move(sensor))); resource_storage_->import_state_interfaces(resource_storage_->sensors_.back()); } @@ -318,11 +310,9 @@ size_t ResourceManager::sensor_components_size() const return resource_storage_->sensors_.size(); } -void ResourceManager::import_component( - std::unique_ptr system) +void ResourceManager::import_component(std::unique_ptr system) { - resource_storage_->systems_.emplace_back( - hardware_interface::components::System(std::move(system))); + resource_storage_->systems_.emplace_back(components::System(std::move(system))); resource_storage_->import_state_interfaces(resource_storage_->systems_.back()); resource_storage_->import_command_interfaces( resource_storage_->systems_.back(), claimed_command_interface_map_); @@ -332,4 +322,4 @@ size_t ResourceManager::system_components_size() const { return resource_storage_->systems_.size(); } -} // namespace controller_manager +} // namespace hardware_interface diff --git a/controller_manager/test/test_components/test_actuator.cpp b/hardware_interface/test/test_components/test_actuator.cpp similarity index 100% rename from controller_manager/test/test_components/test_actuator.cpp rename to hardware_interface/test/test_components/test_actuator.cpp diff --git a/controller_manager/test/test_components/test_components.xml b/hardware_interface/test/test_components/test_components.xml similarity index 100% rename from controller_manager/test/test_components/test_components.xml rename to hardware_interface/test/test_components/test_components.xml diff --git a/controller_manager/test/test_components/test_sensor.cpp b/hardware_interface/test/test_components/test_sensor.cpp similarity index 100% rename from controller_manager/test/test_components/test_sensor.cpp rename to hardware_interface/test/test_components/test_sensor.cpp diff --git a/controller_manager/test/test_components/test_system.cpp b/hardware_interface/test/test_components/test_system.cpp similarity index 100% rename from controller_manager/test/test_components/test_system.cpp rename to hardware_interface/test/test_components/test_system.cpp diff --git a/controller_manager/test/test_resource_manager.cpp b/hardware_interface/test/test_resource_manager.cpp similarity index 96% rename from controller_manager/test/test_resource_manager.cpp rename to hardware_interface/test/test_resource_manager.cpp index df8b34e9fb..8a7abfbdad 100644 --- a/controller_manager/test/test_resource_manager.cpp +++ b/hardware_interface/test/test_resource_manager.cpp @@ -20,8 +20,7 @@ #include #include "hardware_interface/components/actuator_interface.hpp" - -#include "resource_manager.hpp" +#include "hardware_interface/resource_manager.hpp" class TestResourceManager : public ::testing::Test { @@ -182,18 +181,18 @@ class TestResourceManager : public ::testing::Test }; TEST_F(TestResourceManager, initialization_empty) { - ASSERT_ANY_THROW(controller_manager::ResourceManager rm("")); + ASSERT_ANY_THROW(hardware_interface::ResourceManager rm("")); } TEST_F(TestResourceManager, initialization_with_urdf) { auto urdf = urdf_head_ + test_hardware_resource_system_ + urdf_tail_; - ASSERT_NO_THROW(controller_manager::ResourceManager rm(urdf)); + ASSERT_NO_THROW(hardware_interface::ResourceManager rm(urdf)); } TEST_F(TestResourceManager, initialization_with_urdf_manual_validation) { auto urdf = urdf_head_ + test_hardware_resource_system_ + urdf_tail_; // we validate the results manually - controller_manager::ResourceManager rm(urdf, false); + hardware_interface::ResourceManager rm(urdf, false); EXPECT_EQ(1u, rm.actuator_components_size()); EXPECT_EQ(1u, rm.sensor_components_size()); @@ -217,7 +216,7 @@ TEST_F(TestResourceManager, initialization_with_urdf_manual_validation) { TEST_F(TestResourceManager, initialization_with_wrong_urdf) { auto urdf = urdf_head_ + test_hardware_resource_system_missing_keys_ + urdf_tail_; try { - controller_manager::ResourceManager rm(urdf); + hardware_interface::ResourceManager rm(urdf); FAIL(); } catch (const std::exception & e) { std::cout << e.what() << std::endl; @@ -228,7 +227,7 @@ TEST_F(TestResourceManager, initialization_with_wrong_urdf) { TEST_F(TestResourceManager, initialization_with_urdf_unclaimed) { auto urdf = urdf_head_ + test_hardware_resource_system_ + urdf_tail_; // we validate the results manually - controller_manager::ResourceManager rm(urdf); + hardware_interface::ResourceManager rm(urdf); auto command_interface_keys = rm.command_interface_keys(); for (const auto & key : command_interface_keys) { @@ -244,7 +243,7 @@ TEST_F(TestResourceManager, initialization_with_urdf_unclaimed) { TEST_F(TestResourceManager, resource_claiming) { auto urdf = urdf_head_ + test_hardware_resource_system_ + urdf_tail_; - controller_manager::ResourceManager rm(urdf); + hardware_interface::ResourceManager rm(urdf); const auto key = "joint1/position"; EXPECT_FALSE(rm.command_interface_is_claimed(key)); @@ -352,7 +351,7 @@ class ExternalComponent : public hardware_interface::components::ActuatorInterfa TEST_F(TestResourceManager, post_initialization_add_components) { auto urdf = urdf_head_ + test_hardware_resource_system_ + urdf_tail_; // we validate the results manually - controller_manager::ResourceManager rm(urdf, false); + hardware_interface::ResourceManager rm(urdf, false); EXPECT_EQ(1u, rm.actuator_components_size()); EXPECT_EQ(1u, rm.sensor_components_size()); From 39e87e6df029eeafc9ecec34f867092489dbd0c7 Mon Sep 17 00:00:00 2001 From: Karsten Knese Date: Mon, 9 Nov 2020 15:13:55 -0800 Subject: [PATCH 18/18] address review comments Signed-off-by: Karsten Knese --- .../loaned_command_interface.hpp | 4 ++-- .../loaned_state_interface.hpp | 4 ++-- .../hardware_interface/resource_manager.hpp | 1 - hardware_interface/src/resource_manager.cpp | 3 +-- .../test/test_resource_manager.cpp | 21 ++++++------------- 5 files changed, 11 insertions(+), 22 deletions(-) diff --git a/hardware_interface/include/hardware_interface/loaned_command_interface.hpp b/hardware_interface/include/hardware_interface/loaned_command_interface.hpp index 953869323a..d5a23c8fc2 100644 --- a/hardware_interface/include/hardware_interface/loaned_command_interface.hpp +++ b/hardware_interface/include/hardware_interface/loaned_command_interface.hpp @@ -55,12 +55,12 @@ class LoanedCommandInterface command_interface_.set_value(val); } - double get_value() + double get_value() const { return command_interface_.get_value(); } -private: +protected: CommandInterface & command_interface_; Deleter deleter_; }; diff --git a/hardware_interface/include/hardware_interface/loaned_state_interface.hpp b/hardware_interface/include/hardware_interface/loaned_state_interface.hpp index ab6c3cf06c..ca3df8189c 100644 --- a/hardware_interface/include/hardware_interface/loaned_state_interface.hpp +++ b/hardware_interface/include/hardware_interface/loaned_state_interface.hpp @@ -50,12 +50,12 @@ class LoanedStateInterface } } - double get_value() + double get_value() const { return state_interface_.get_value(); } -private: +protected: StateInterface & state_interface_; Deleter deleter_; }; diff --git a/hardware_interface/include/hardware_interface/resource_manager.hpp b/hardware_interface/include/hardware_interface/resource_manager.hpp index 077de472a6..ad3486cbb0 100644 --- a/hardware_interface/include/hardware_interface/resource_manager.hpp +++ b/hardware_interface/include/hardware_interface/resource_manager.hpp @@ -177,7 +177,6 @@ class ResourceManager private: void release_command_interface(const std::string & key); - // TODO(karsten1987): Optimize this with std::vector, maps are bad std::unordered_map claimed_command_interface_map_; mutable std::recursive_mutex resource_lock_; diff --git a/hardware_interface/src/resource_manager.cpp b/hardware_interface/src/resource_manager.cpp index c47a7ab55c..200b6bd39c 100644 --- a/hardware_interface/src/resource_manager.cpp +++ b/hardware_interface/src/resource_manager.cpp @@ -222,7 +222,6 @@ LoanedStateInterface ResourceManager::claim_state_interface(const std::string & std::string("state interface with key") + key + " does not exist"); } - std::lock_guard lg(resource_lock_); return LoanedStateInterface(resource_storage_->state_interface_map_.at(key)); } @@ -258,12 +257,12 @@ LoanedCommandInterface ResourceManager::claim_command_interface(const std::strin std::string("command interface with key") + key + " does not exist"); } + std::lock_guard lg(resource_lock_); if (command_interface_is_claimed(key)) { throw std::runtime_error( std::string("command interface with key") + key + " is already claimed"); } - std::lock_guard lg(resource_lock_); claimed_command_interface_map_[key] = true; return LoanedCommandInterface( resource_storage_->command_interface_map_.at(key), diff --git a/hardware_interface/test/test_resource_manager.cpp b/hardware_interface/test/test_resource_manager.cpp index 8a7abfbdad..7c2de79d1e 100644 --- a/hardware_interface/test/test_resource_manager.cpp +++ b/hardware_interface/test/test_resource_manager.cpp @@ -251,11 +251,8 @@ TEST_F(TestResourceManager, resource_claiming) { { auto position_command_interface = rm.claim_command_interface(key); EXPECT_TRUE(rm.command_interface_is_claimed(key)); - try { - auto sensor1_again = rm.claim_command_interface(key); - FAIL(); - } catch (const std::runtime_error &) { - SUCCEED(); + { + EXPECT_ANY_THROW(rm.claim_command_interface(key)); } } EXPECT_FALSE(rm.command_interface_is_claimed(key)); @@ -268,11 +265,8 @@ TEST_F(TestResourceManager, resource_claiming) { { auto interface = rm.claim_command_interface(key); EXPECT_TRUE(rm.command_interface_is_claimed(key)); - try { - auto interface_again = rm.claim_command_interface(key); - FAIL(); - } catch (const std::runtime_error &) { - SUCCEED(); + { + EXPECT_ANY_THROW(rm.claim_command_interface(key)); } } EXPECT_FALSE(rm.command_interface_is_claimed(key)); @@ -285,11 +279,8 @@ TEST_F(TestResourceManager, resource_claiming) { { { auto interface = rm.claim_state_interface(key); - try { - auto interface_again = rm.claim_state_interface(key); - SUCCEED(); - } catch (const std::runtime_error &) { - FAIL(); + { + EXPECT_NO_THROW(rm.claim_state_interface(key)); } } }