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/controller_manager/src/resource_manager.hpp b/controller_manager/src/resource_manager.hpp deleted file mode 100644 index 0feb9c4278..0000000000 --- a/controller_manager/src/resource_manager.hpp +++ /dev/null @@ -1,101 +0,0 @@ -// 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 -#include - -namespace controller_manager -{ - -class ResourceStorage; - -class ResourceManager -{ -public: - /// 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); - - ResourceManager(const ResourceManager &) = delete; - - ~ResourceManager(); - - /// 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_interface_keys() const; - - /// Checks whether a interface is registered under the given key. - /** - * \return true if interface exist, false otherwise. - */ - bool state_interface_exists(const std::string & key) const; - - /// 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_interface_keys() const; - - /// Checks whether a interface is registered under the given key. - /** - * \return true if interface exist, false otherwise. - */ - bool command_interface_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: - std::unique_ptr resource_storage_; -}; - -} // namespace controller_manager -#endif // RESOURCE_MANAGER_HPP_ 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/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; diff --git a/hardware_interface/include/hardware_interface/loaned_command_interface.hpp b/hardware_interface/include/hardware_interface/loaned_command_interface.hpp new file mode 100644 index 0000000000..d5a23c8fc2 --- /dev/null +++ b/hardware_interface/include/hardware_interface/loaned_command_interface.hpp @@ -0,0 +1,69 @@ +// 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 HARDWARE_INTERFACE__LOANED_COMMAND_INTERFACE_HPP_ +#define HARDWARE_INTERFACE__LOANED_COMMAND_INTERFACE_HPP_ + +#include +#include + +#include "hardware_interface/handle.hpp" + +namespace hardware_interface +{ + +class LoanedCommandInterface +{ +public: + using Deleter = std::function; + + explicit LoanedCommandInterface(CommandInterface & command_interface) + : LoanedCommandInterface(command_interface, nullptr) + {} + + LoanedCommandInterface( + 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); + } + + double get_value() const + { + return command_interface_.get_value(); + } + +protected: + CommandInterface & command_interface_; + Deleter deleter_; +}; + +} // namespace hardware_interface +#endif // HARDWARE_INTERFACE__LOANED_COMMAND_INTERFACE_HPP_ diff --git a/hardware_interface/include/hardware_interface/loaned_state_interface.hpp b/hardware_interface/include/hardware_interface/loaned_state_interface.hpp new file mode 100644 index 0000000000..ca3df8189c --- /dev/null +++ b/hardware_interface/include/hardware_interface/loaned_state_interface.hpp @@ -0,0 +1,64 @@ +// 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 HARDWARE_INTERFACE__LOANED_STATE_INTERFACE_HPP_ +#define HARDWARE_INTERFACE__LOANED_STATE_INTERFACE_HPP_ + +#include +#include + +#include "hardware_interface/handle.hpp" + +namespace hardware_interface +{ + +class LoanedStateInterface +{ +public: + using Deleter = std::function; + + explicit LoanedStateInterface(StateInterface & state_interface) + : LoanedStateInterface(state_interface, nullptr) + {} + + LoanedStateInterface( + 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() const + { + return state_interface_.get_value(); + } + +protected: + StateInterface & state_interface_; + Deleter deleter_; +}; + +} // namespace hardware_interface +#endif // HARDWARE_INTERFACE__LOANED_STATE_INTERFACE_HPP_ diff --git a/hardware_interface/include/hardware_interface/resource_manager.hpp b/hardware_interface/include/hardware_interface/resource_manager.hpp new file mode 100644 index 0000000000..ad3486cbb0 --- /dev/null +++ b/hardware_interface/include/hardware_interface/resource_manager.hpp @@ -0,0 +1,187 @@ +// 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 HARDWARE_INTERFACE__RESOURCE_MANAGER_HPP_ +#define HARDWARE_INTERFACE__RESOURCE_MANAGER_HPP_ + +#include +#include +#include +#include +#include + +#include "hardware_interface/loaned_command_interface.hpp" +#include "hardware_interface/loaned_state_interface.hpp" + +namespace hardware_interface +{ +namespace components +{ +class ActuatorInterface; +class SensorInterface; +class SystemInterface; +} // namespace components + +class ResourceStorage; + +class ResourceManager +{ +public: + /// 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); + + ResourceManager(const ResourceManager &) = delete; + + ~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. + * + * \return vector of strings, containing all registered keys. + */ + std::vector state_interface_keys() const; + + /// 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. + * + * \return vector of strings, containing all registered keys. + */ + std::vector command_interface_keys() const; + + /// Checks whether a command interface is registered under the given key. + /** + * \return true if interface exist, false otherwise. + */ + bool command_interface_exists(const std::string & key) const; + + /// Return the number of loaded actuator components. + /** + * \return number of actuator components. + */ + 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); + + std::unordered_map claimed_command_interface_map_; + + mutable std::recursive_mutex resource_lock_; + std::unique_ptr resource_storage_; +}; + +} // 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 61% rename from controller_manager/src/resource_manager.cpp rename to hardware_interface/src/resource_manager.cpp index 127be230a9..200b6bd39c 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) { @@ -79,58 +79,63 @@ 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 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()); + 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) + void initialize_system( + 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()); - import_command_interfaces(systems_.back()); + import_command_interfaces(systems_.back(), claimed_command_interface_map); } // 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; @@ -146,13 +151,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_); } } @@ -204,6 +209,22 @@ ResourceManager::ResourceManager(const std::string & urdf, bool validate_interfa } } +void ResourceManager::release_command_interface(const std::string & key) +{ + std::lock_guard lg(resource_lock_); + 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"); + } + + return LoanedStateInterface(resource_storage_->state_interface_map_.at(key)); +} + std::vector ResourceManager::state_interface_keys() const { std::vector keys; @@ -219,6 +240,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"); + } + + 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"); + } + + 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,18 +284,41 @@ bool ResourceManager::command_interface_exists(const std::string & key) const resource_storage_->command_interface_map_.end(); } -size_t ResourceManager::actuator_interfaces_size() const +void ResourceManager::import_component(std::unique_ptr actuator) +{ + resource_storage_->actuators_.emplace_back( + 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(); } -size_t ResourceManager::sensor_interfaces_size() const +void ResourceManager::import_component(std::unique_ptr sensor) +{ + resource_storage_->sensors_.emplace_back(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(); } -size_t ResourceManager::system_interfaces_size() const +void ResourceManager::import_component(std::unique_ptr 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_); +} + +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 56% rename from controller_manager/test/test_resource_manager.cpp rename to hardware_interface/test/test_resource_manager.cpp index 2410c3223d..7c2de79d1e 100644 --- a/controller_manager/test/test_resource_manager.cpp +++ b/hardware_interface/test/test_resource_manager.cpp @@ -17,8 +17,10 @@ #include #include #include +#include -#include "resource_manager.hpp" +#include "hardware_interface/components/actuator_interface.hpp" +#include "hardware_interface/resource_manager.hpp" class TestResourceManager : public ::testing::Test { @@ -179,22 +181,22 @@ 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_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()); @@ -214,10 +216,149 @@ 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; 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 + hardware_interface::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_; + hardware_interface::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)); + { + EXPECT_ANY_THROW(rm.claim_command_interface(key)); + } + } + 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"}) + { + { + auto interface = rm.claim_command_interface(key); + EXPECT_TRUE(rm.command_interface_is_claimed(key)); + { + EXPECT_ANY_THROW(rm.claim_command_interface(key)); + } + } + 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); + { + EXPECT_NO_THROW(rm.claim_state_interface(key)); + } + } + } +} + +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 + hardware_interface::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")); +}