diff --git a/controller_manager/CMakeLists.txt b/controller_manager/CMakeLists.txt index 0a93d78d94..8091361554 100644 --- a/controller_manager/CMakeLists.txt +++ b/controller_manager/CMakeLists.txt @@ -21,6 +21,7 @@ find_package(rclcpp REQUIRED) add_library(controller_manager SHARED src/controller_manager.cpp + src/resource_manager.cpp ) target_include_directories(controller_manager PRIVATE include) ament_target_dependencies(controller_manager @@ -65,6 +66,10 @@ if(BUILD_TESTING) target_include_directories(test_controller PRIVATE include) target_link_libraries(test_controller controller_manager) target_compile_definitions(test_controller PRIVATE "CONTROLLER_MANAGER_BUILDING_DLL") + install(TARGETS test_controller + DESTINATION lib + ) + pluginlib_export_plugin_description_file(controller_interface test/test_controller.xml) ament_add_gmock( test_controller_manager @@ -101,11 +106,32 @@ if(BUILD_TESTING) test_robot_hardware ) - pluginlib_export_plugin_description_file(controller_interface test/test_controller.xml) - - install(TARGETS test_controller + add_library(test_hardware_resources SHARED + test/test_hardware_resources/test_actuator_hardware.cpp + test/test_hardware_resources/test_joint_component.cpp + test/test_hardware_resources/test_sensor_component.cpp + test/test_hardware_resources/test_sensor_hardware.cpp + test/test_hardware_resources/test_system_hardware.cpp) + target_include_directories(test_hardware_resources PRIVATE include) + ament_target_dependencies(test_hardware_resources + hardware_interface + pluginlib + ) + install(TARGETS test_hardware_resources DESTINATION lib ) + pluginlib_export_plugin_description_file( + hardware_interface test/test_hardware_resources.xml) + + ament_add_gmock( + test_resource_manager + test/test_resource_manager.cpp + ENV ASAN_OPTIONS=detect_container_overflow=0 + ) + target_include_directories(test_resource_manager PRIVATE include src) + target_link_libraries(test_resource_manager controller_manager test_hardware_resources) + set_target_properties(test_resource_manager PROPERTIES COMPILE_FLAGS "-fsanitize=undefined -fsanitize=address") + set_target_properties(test_resource_manager PROPERTIES LINK_FLAGS "-fsanitize=undefined -fsanitize=address") endif() 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..0c711a40de --- /dev/null +++ b/controller_manager/src/resource_manager.cpp @@ -0,0 +1,358 @@ +// 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 + +#include "hardware_interface/actuator_hardware.hpp" +#include "hardware_interface/components/component_info.hpp" +#include "hardware_interface/components/joint.hpp" +#include "hardware_interface/components/sensor.hpp" +#include "hardware_interface/component_parser.hpp" +#include "hardware_interface/hardware_info.hpp" +#include "hardware_interface/sensor_hardware.hpp" +#include "hardware_interface/system_hardware.hpp" + +#include "pluginlib/class_loader.hpp" + +#include "./resource_manager.hpp" + +namespace controller_manager +{ + +using ComponentDeleter = hardware_interface::components::Deleter; + +class ResourceStorage +{ + static constexpr auto pkg_name = "hardware_interface"; + + static constexpr auto joint_component_interface_name = + "hardware_interface::components::JointInterface"; + static constexpr auto sensor_component_interface_name = + "hardware_interface::components::SensorInterface"; + + static constexpr auto actuator_interface_name = + "hardware_interface::ActuatorHardwareInterface"; + static constexpr auto sensor_interface_name = + "hardware_interface::SensorHardwareInterface"; + static constexpr auto system_interface_name = + "hardware_interface::SystemHardwareInterface"; + +public: + ResourceStorage() + : joint_component_loader_(pkg_name, joint_component_interface_name), + sensor_component_loader_(pkg_name, sensor_component_interface_name), + actuator_loader_(pkg_name, actuator_interface_name), + sensor_loader_(pkg_name, sensor_interface_name), + system_loader_(pkg_name, system_interface_name) + {} + + ~ResourceStorage() = default; + + template + void initialize_component( + const hardware_interface::components::ComponentInfo & component_info, + pluginlib::ClassLoader & loader, + std::unordered_map & container, + ComponentDeleter deleter) + { + // hardware_class_type has to match class name in plugin xml description + // TODO(karsten1987) extract package from hardware_class_type + // e.g.: / + // TODO(dstoegl) why is it class_type here and "hardware_class_type" in hardware_info? + auto interface = loader.createSharedInstance(component_info.class_type); + container.emplace(std::make_pair(component_info.name, ComponentT(interface, deleter))); + container.at(component_info.name).configure(component_info); + } + + void initialize_joint_component( + const hardware_interface::components::ComponentInfo & component_info, + ComponentDeleter deleter) + { + initialize_component< + hardware_interface::components::Joint, + hardware_interface::components::JointInterface>( + component_info, joint_component_loader_, joint_components_, deleter); + } + + void initialize_sensor_component( + const hardware_interface::components::ComponentInfo & component_info, + ComponentDeleter deleter) + { + initialize_component< + hardware_interface::components::Sensor, + hardware_interface::components::SensorInterface>( + component_info, sensor_component_loader_, sensor_components_, deleter); + } + + template + void initialize_hardware( + const hardware_interface::HardwareInfo & hardware_info, + pluginlib::ClassLoader & loader, + std::unordered_map & container) + { + // hardware_class_type has to match class name in plugin xml description + // TODO(karsten1987) extract package from hardware_class_type + // e.g.: / + auto interface = std::unique_ptr( + loader.createUnmanagedInstance(hardware_info.hardware_class_type)); + container.emplace(std::make_pair(hardware_info.name, HardwareT(std::move(interface)))); + container.at(hardware_info.name).configure(hardware_info); + } + + void initialize_actuator(const hardware_interface::HardwareInfo & hardware_info) + { + initialize_hardware< + hardware_interface::ActuatorHardware, + hardware_interface::ActuatorHardwareInterface>( + hardware_info, actuator_loader_, actuators_); + } + + void initialize_sensor(const hardware_interface::HardwareInfo & hardware_info) + { + initialize_hardware< + hardware_interface::SensorHardware, + hardware_interface::SensorHardwareInterface>( + hardware_info, sensor_loader_, sensors_); + } + + void initialize_system(const hardware_interface::HardwareInfo & hardware_info) + { + initialize_hardware< + hardware_interface::SystemHardware, + hardware_interface::SystemHardwareInterface>( + hardware_info, system_loader_, systems_); + } + + // components plugins + pluginlib::ClassLoader joint_component_loader_; + pluginlib::ClassLoader sensor_component_loader_; + + std::unordered_map joint_components_; + std::unordered_map sensor_components_; + + // hardware plugins + pluginlib::ClassLoader actuator_loader_; + pluginlib::ClassLoader sensor_loader_; + pluginlib::ClassLoader system_loader_; + + std::unordered_map actuators_; + std::unordered_map sensors_; + std::unordered_map 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); + } + + for (const auto & joint : hardware.joints) { + claimed_component_map_.insert({joint.name, false}); + auto deleter = std::bind(&ResourceManager::release_component, this, joint.name); + resource_storage_->initialize_joint_component(joint, deleter); + // TODO(karsten1987) Verify that parser warns when sensor and joint + // component are called the same. + } + + for (const auto & sensor : hardware.sensors) { + claimed_component_map_.insert({sensor.name, false}); + auto deleter = std::bind(&ResourceManager::release_component, this, sensor.name); + resource_storage_->initialize_sensor_component(sensor, deleter); + } + } +} + +void ResourceManager::release_component(const std::string & component_id) +{ + std::lock_guard lg(resource_lock_); + claimed_component_map_[component_id] = false; +} + +template +inline auto component_exists(const std::string & component_name, const ContainerT & container) +{ + return container.find(component_name) != container.end(); +} + +template +inline auto component_is_claimed( + const std::string & component_name, + const ContainerT & container, + const std::unordered_map & claimed_component_map) +{ + if (!component_exists(component_name, container)) {return false;} + return claimed_component_map.at(component_name); +} + +template +inline auto claim_component( + const std::string & component_name, + const ContainerT & container, + std::unordered_map & claimed_component_map) +{ + if (!component_exists(component_name, container)) { + throw std::runtime_error(std::string("component ") + component_name + " doesn't exist"); + } + if (component_is_claimed(component_name, container, claimed_component_map)) { + throw std::runtime_error(std::string("component ") + component_name + " is already claimed"); + } + + claimed_component_map.at(component_name) = true; + return container.at(component_name); +} + +size_t ResourceManager::joint_size() const +{ + return resource_storage_->joint_components_.size(); +} + +std::vector ResourceManager::joint_names() const +{ + std::vector ret; + ret.reserve(resource_storage_->joint_components_.size()); + for (auto const & component : resource_storage_->joint_components_) { + ret.emplace_back(std::get<0>(component)); + } + + return ret; +} + +bool ResourceManager::joint_exists(const std::string & joint_name) const +{ + return component_exists(joint_name, resource_storage_->joint_components_); +} + +bool ResourceManager::joint_is_claimed(const std::string & joint_name) const +{ + std::lock_guard lg(resource_lock_); + return component_is_claimed( + joint_name, resource_storage_->joint_components_, claimed_component_map_); +} + +hardware_interface::components::Joint +ResourceManager::claim_joint(const std::string & joint_name) +{ + std::lock_guard lg(resource_lock_); + return claim_component( + joint_name, resource_storage_->joint_components_, claimed_component_map_); +} + +size_t ResourceManager::sensor_size() const +{ + return resource_storage_->sensor_components_.size(); +} + +std::vector ResourceManager::sensor_names() const +{ + std::vector ret; + ret.reserve(resource_storage_->sensor_components_.size()); + for (const auto & component : resource_storage_->sensor_components_) { + ret.emplace_back(std::get<0>(component)); + } + + return ret; +} + +bool ResourceManager::sensor_exists(const std::string & sensor_name) const +{ + return component_exists(sensor_name, resource_storage_->sensor_components_); +} + +bool ResourceManager::sensor_is_claimed(const std::string & sensor_name) const +{ + std::lock_guard lg(resource_lock_); + return component_is_claimed( + sensor_name, resource_storage_->sensor_components_, claimed_component_map_); +} + +hardware_interface::components::Sensor +ResourceManager::claim_sensor(const std::string & sensor_name) +{ + std::lock_guard lg(resource_lock_); + return claim_component( + sensor_name, resource_storage_->sensor_components_, claimed_component_map_); +} + +size_t ResourceManager::actuator_interface_size() const +{ + return resource_storage_->actuators_.size(); +} + +std::vector ResourceManager::actuator_interface_names() const +{ + std::vector ret; + ret.reserve(resource_storage_->actuators_.size()); + for (const auto & hardware : resource_storage_->actuators_) { + ret.emplace_back(std::get<0>(hardware)); + } + + return ret; +} + +size_t ResourceManager::sensor_interface_size() const +{ + return resource_storage_->sensors_.size(); +} + +std::vector ResourceManager::sensor_interface_names() const +{ + std::vector ret; + ret.reserve(resource_storage_->sensors_.size()); + for (const auto & hardware : resource_storage_->sensors_) { + ret.emplace_back(std::get<0>(hardware)); + } + + return ret; +} + +size_t ResourceManager::system_interface_size() const +{ + return resource_storage_->systems_.size(); +} + +std::vector ResourceManager::system_interface_names() const +{ + std::vector ret; + ret.reserve(resource_storage_->systems_.size()); + for (const auto & hardware : resource_storage_->systems_) { + ret.emplace_back(std::get<0>(hardware)); + } + + return ret; +} +} // 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..dc085b09bb --- /dev/null +++ b/controller_manager/src/resource_manager.hpp @@ -0,0 +1,170 @@ +// 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 +#include +#include + +#include "hardware_interface/components/joint.hpp" +#include "hardware_interface/components/sensor.hpp" + +namespace controller_manager +{ + +class ResourceStorage; + +class ResourceManager +{ +public: + ResourceManager(); + + explicit ResourceManager(const std::string & urdf); + + ResourceManager(const ResourceManager & other) = delete; + + ~ResourceManager(); + + /// Return the number of joint components + /** + * \return number of joints + */ + size_t joint_size() const; + + /// Return the names of joint components + /** + * \return vector of joint names + */ + std::vector joint_names() const; + + /// Verify if a joint exist with a given name + /** + * Before claiming a joint, it might be worth to check if the joint exists. + * \param joint_name String identifier which joint to look for + * \return bool True if joint exists, else false + */ + bool joint_exists(const std::string & joint_name) const; + + /// Verify if a joint is claimed + /** + * Joints can only uniquely be claimed. + * + * \param joint_name String identifier which joint to look for + * \return bool True if component is claimed, else false + */ + bool joint_is_claimed(const std::string & joint_name) const; + + /// Claim a joint component based on its name + /** + * The resource is claimed as long as being in scope. + * Once the resource is going out of scope, the destructor + * returns and thus frees the resource to claimed by others. + * + * \param joint_name String identifier which joint to claim + * \return joint claimed joint component + */ + hardware_interface::components::Joint claim_joint(const std::string & joint_name); + + /// Return the number of sensor components + /** + * \return number of sensors + */ + size_t sensor_size() const; + + /// Return the names of sensor components + /** + * \return vector of sensor names + */ + std::vector sensor_names() const; + + /// Verify if a sensor exist with a given name + /** + * Before claiming a sensor, it might be worth to check if the sensor exists. + * \param sensor_name String identifier which sensor to look for + * \return bool True if sensor exists, else false + */ + bool sensor_exists(const std::string & sensor_name) const; + + /// Verify if a sensor is claimed + /** + * Sensors can only uniquely be claimed. + * + * \param sensor_name String identifier which sensor to look for + * \return bool True if component is claimed, else false + */ + bool sensor_is_claimed(const std::string & sensor_name) const; + + /// Claim a sensor component based on its name + /** + * The resource is claimed as long as being in scope. + * Once the resource is going out of scope, the destructor + * returns and thus frees the resource to claimed by others. + * + * \param sensor_name String identifier which sensor to claim + * \return Sensor claimed sensor component + */ + hardware_interface::components::Sensor claim_sensor(const std::string & sensor_name); + + /// Return the number of hardware actuators + /** + * \return number of actuators + */ + size_t actuator_interface_size() const; + + /// Return the names of hardware actuators + /** + * \return vector of actuator names + */ + std::vector actuator_interface_names() const; + + /// Return the number of hardware sensors + /** + * \return number of sensors + */ + size_t sensor_interface_size() const; + + /// Return the names of hardware sensors + /** + * \return vector of sensor names + */ + std::vector sensor_interface_names() const; + + /// Return the number of hardware systems + /** + * \return number of sytems + */ + size_t system_interface_size() const; + + /// Return the names of hardware systems + /** + * \return vector of system names + */ + std::vector system_interface_names() const; + +private: + void release_component(const std::string & component_id); + + // TODO(karsten1987): Optimize this with std::vector, maps are bad + std::unordered_map claimed_component_map_; + + mutable std::recursive_mutex resource_lock_; + std::unique_ptr resource_storage_; +}; + +} // namespace controller_manager +#endif // RESOURCE_MANAGER_HPP_ diff --git a/controller_manager/test/test_hardware_resources.xml b/controller_manager/test/test_hardware_resources.xml new file mode 100644 index 0000000000..fec0858c1e --- /dev/null +++ b/controller_manager/test/test_hardware_resources.xml @@ -0,0 +1,33 @@ + + + + + Test Hardware Actuator + + + + + + Test Hardware Sensor + + + + + + Test Hardware System + + + + + + Test Joint Component + + + + + + Test Sensor Component + + + + diff --git a/controller_manager/test/test_hardware_resources/test_actuator_hardware.cpp b/controller_manager/test/test_hardware_resources/test_actuator_hardware.cpp new file mode 100644 index 0000000000..e952880e4d --- /dev/null +++ b/controller_manager/test/test_hardware_resources/test_actuator_hardware.cpp @@ -0,0 +1,62 @@ +// Copyright 2020 ros2_control Development Team +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include "./test_actuator_hardware.hpp" + +using hardware_interface::status; +using hardware_interface::return_type; + +return_type +TestActuatorHardware::configure(const hardware_interface::HardwareInfo & /* actuator_info */) +{ + return return_type::OK; +} + +return_type +TestActuatorHardware::start() +{ + return return_type::OK; +} + +return_type +TestActuatorHardware::stop() +{ + return return_type::OK; +} + +status +TestActuatorHardware::get_status() const +{ + return status::UNKNOWN; +} + +return_type +TestActuatorHardware::read_joint( + std::shared_ptr/* joint */) const +{ + return return_type::OK; +} + +return_type +TestActuatorHardware::write_joint( + const std::shared_ptr/* joint */) +{ + return return_type::OK; +} + +#include "pluginlib/class_list_macros.hpp" // NOLINT + +PLUGINLIB_EXPORT_CLASS(TestActuatorHardware, hardware_interface::ActuatorHardwareInterface) diff --git a/controller_manager/test/test_hardware_resources/test_actuator_hardware.hpp b/controller_manager/test/test_hardware_resources/test_actuator_hardware.hpp new file mode 100644 index 0000000000..ede3e72c72 --- /dev/null +++ b/controller_manager/test/test_hardware_resources/test_actuator_hardware.hpp @@ -0,0 +1,44 @@ +// Copyright 2020 ros2_control Development Team +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef TEST_HARDWARE_RESOURCES__TEST_ACTUATOR_HARDWARE_HPP_ +#define TEST_HARDWARE_RESOURCES__TEST_ACTUATOR_HARDWARE_HPP_ + +#include + +#include "hardware_interface/actuator_hardware_interface.hpp" + +class TestActuatorHardware : public hardware_interface::ActuatorHardwareInterface +{ +public: + hardware_interface::return_type + configure(const hardware_interface::HardwareInfo & actuator_info) override; + + hardware_interface::return_type + start() override; + + hardware_interface::return_type + stop() override; + + hardware_interface::status + get_status() const override; + + hardware_interface::return_type + read_joint(std::shared_ptr joint) const override; + + hardware_interface::return_type + write_joint(const std::shared_ptr joint) override; +}; + +#endif // TEST_HARDWARE_RESOURCES__TEST_ACTUATOR_HARDWARE_HPP_ diff --git a/controller_manager/test/test_hardware_resources/test_joint_component.cpp b/controller_manager/test/test_hardware_resources/test_joint_component.cpp new file mode 100644 index 0000000000..743114c347 --- /dev/null +++ b/controller_manager/test/test_hardware_resources/test_joint_component.cpp @@ -0,0 +1,93 @@ +// Copyright 2020 ros2_control Development Team +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include + +#include "./test_joint_component.hpp" + +using hardware_interface::return_type; + +return_type TestJointComponent::configure( + const hardware_interface::components::ComponentInfo & /* joint_info */) +{ + return return_type::OK; +} + +std::vector TestJointComponent::get_command_interfaces() const +{ + return {"test_command_interface"}; +} + +std::vector TestJointComponent::get_state_interfaces() const +{ + return {"test_state_interface"}; +} + +return_type TestJointComponent::get_command( + std::vector & /*command */, + const std::vector & /* interfaces */) const +{ + return return_type::OK; +} + +return_type TestJointComponent::get_command( + std::vector & /* command */) const +{ + return return_type::OK; +} + +return_type TestJointComponent::set_command( + const std::vector & /* command */, + const std::vector & /* interfaces */) +{ + return return_type::OK; +} + +return_type TestJointComponent::set_command( + const std::vector & /* command*/) +{ + return return_type::OK; +} + +return_type TestJointComponent::get_state( + std::vector & /* state */, + const std::vector & /* interfaces */) const +{ + return return_type::OK; +} + +return_type TestJointComponent::get_state( + std::vector & /* state */) const +{ + return return_type::OK; +} + +return_type TestJointComponent::set_state( + const std::vector & /* state */, + const std::vector & /* interfaces */) +{ + return return_type::OK; +} + +return_type TestJointComponent::set_state( + const std::vector & /* state */) +{ + return return_type::OK; +} + +#include "pluginlib/class_list_macros.hpp" // NOLINT + +PLUGINLIB_EXPORT_CLASS(TestJointComponent, hardware_interface::components::JointInterface) diff --git a/controller_manager/test/test_hardware_resources/test_joint_component.hpp b/controller_manager/test/test_hardware_resources/test_joint_component.hpp new file mode 100644 index 0000000000..07c738a635 --- /dev/null +++ b/controller_manager/test/test_hardware_resources/test_joint_component.hpp @@ -0,0 +1,79 @@ +// Copyright 2020 ros2_control Development Team +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef TEST_HARDWARE_RESOURCES__TEST_JOINT_COMPONENT_HPP_ +#define TEST_HARDWARE_RESOURCES__TEST_JOINT_COMPONENT_HPP_ + +#include +#include +#include + +#include "controller_manager/visibility_control.h" + +#include "hardware_interface/components/joint_interface.hpp" + +class TestJointComponent : public hardware_interface::components::JointInterface +{ +public: + TestJointComponent() = default; + + virtual ~TestJointComponent() = default; + + CONTROLLER_MANAGER_PUBLIC + hardware_interface::return_type configure( + const hardware_interface::components::ComponentInfo & joint_info) override; + + CONTROLLER_MANAGER_PUBLIC + std::vector get_command_interfaces() const override; + + CONTROLLER_MANAGER_PUBLIC + std::vector get_state_interfaces() const override; + + CONTROLLER_MANAGER_PUBLIC + hardware_interface::return_type get_command( + std::vector & command, + const std::vector & interfaces) const override; + + CONTROLLER_MANAGER_PUBLIC + hardware_interface::return_type get_command( + std::vector & command) const override; + + CONTROLLER_MANAGER_PUBLIC + hardware_interface::return_type set_command( + const std::vector & command, + const std::vector & interfaces) override; + + CONTROLLER_MANAGER_PUBLIC + hardware_interface::return_type set_command(const std::vector & command) override; + + CONTROLLER_MANAGER_PUBLIC + hardware_interface::return_type get_state( + std::vector & state, + const std::vector & interfaces) const override; + + CONTROLLER_MANAGER_PUBLIC + hardware_interface::return_type get_state(std::vector & state) const override; + + CONTROLLER_MANAGER_PUBLIC + hardware_interface::return_type set_state( + const std::vector & state, + const std::vector & interfaces) override; + + CONTROLLER_MANAGER_PUBLIC + hardware_interface::return_type set_state(const std::vector & state) override; + + // Some additional API + bool return_true() const {return true;} +}; +#endif // TEST_HARDWARE_RESOURCES__TEST_JOINT_COMPONENT_HPP_ diff --git a/controller_manager/test/test_hardware_resources/test_sensor_component.cpp b/controller_manager/test/test_hardware_resources/test_sensor_component.cpp new file mode 100644 index 0000000000..08800f2c30 --- /dev/null +++ b/controller_manager/test/test_hardware_resources/test_sensor_component.cpp @@ -0,0 +1,61 @@ +// Copyright 2020 ros2_control Development Team +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include + +#include "./test_sensor_component.hpp" + +using hardware_interface::return_type; + +return_type TestSensorComponent::configure( + const hardware_interface::components::ComponentInfo & /* sensor_info */) +{ + return return_type::OK; +} + +std::vector TestSensorComponent::get_state_interfaces() const +{ + return {"test_state_interface"}; +} + +return_type TestSensorComponent::get_state( + std::vector & /* state */, + const std::vector & /* interfaces */) const +{ + return return_type::OK; +} + +return_type TestSensorComponent::get_state( + std::vector & /* state */) const +{ + return return_type::OK; +} + +return_type TestSensorComponent::set_state( + const std::vector & /* state */, + const std::vector & /* interfaces */) +{ + return return_type::OK; +} + +return_type TestSensorComponent::set_state( + const std::vector & /* state */) +{ + return return_type::OK; +} +#include "pluginlib/class_list_macros.hpp" // NOLINT + +PLUGINLIB_EXPORT_CLASS(TestSensorComponent, hardware_interface::components::SensorInterface) diff --git a/controller_manager/test/test_hardware_resources/test_sensor_component.hpp b/controller_manager/test/test_hardware_resources/test_sensor_component.hpp new file mode 100644 index 0000000000..fa78d25df2 --- /dev/null +++ b/controller_manager/test/test_hardware_resources/test_sensor_component.hpp @@ -0,0 +1,60 @@ +// Copyright 2020 ros2_control Development Team +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef TEST_HARDWARE_RESOURCES__TEST_SENSOR_COMPONENT_HPP_ +#define TEST_HARDWARE_RESOURCES__TEST_SENSOR_COMPONENT_HPP_ + +#include +#include +#include + +#include "controller_manager/visibility_control.h" + +#include "hardware_interface/components/sensor_interface.hpp" + +class TestSensorComponent : public hardware_interface::components::SensorInterface +{ +public: + TestSensorComponent() = default; + + virtual ~TestSensorComponent() = default; + + CONTROLLER_MANAGER_PUBLIC + hardware_interface::return_type + configure(const hardware_interface::components::ComponentInfo & sensor_info) override; + + CONTROLLER_MANAGER_PUBLIC + std::vector get_state_interfaces() const override; + + CONTROLLER_MANAGER_PUBLIC + hardware_interface::return_type get_state( + std::vector & state, + const std::vector & interfaces) const override; + + CONTROLLER_MANAGER_PUBLIC + hardware_interface::return_type get_state(std::vector & state) const override; + + CONTROLLER_MANAGER_PUBLIC + hardware_interface::return_type set_state( + const std::vector & state, + const std::vector & interfaces) override; + + CONTROLLER_MANAGER_PUBLIC + hardware_interface::return_type set_state(const std::vector & state) override; + + // Some additional API + bool return_true() const {return true;} +}; + +#endif // TEST_HARDWARE_RESOURCES__TEST_SENSOR_COMPONENT_HPP_ diff --git a/controller_manager/test/test_hardware_resources/test_sensor_hardware.cpp b/controller_manager/test/test_hardware_resources/test_sensor_hardware.cpp new file mode 100644 index 0000000000..1b03abee0a --- /dev/null +++ b/controller_manager/test/test_hardware_resources/test_sensor_hardware.cpp @@ -0,0 +1,56 @@ +// Copyright 2020 ros2_control Development Team +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include + +#include "./test_sensor_hardware.hpp" + +using hardware_interface::status; +using hardware_interface::return_type; + +return_type +TestSensorHardware::configure(const hardware_interface::HardwareInfo & /* sensor_info */) +{ + return return_type::OK; +} + +return_type +TestSensorHardware::start() +{ + return return_type::OK; +} + +return_type +TestSensorHardware::stop() +{ + return return_type::OK; +} + +status +TestSensorHardware::get_status() const +{ + return status::UNKNOWN; +} + +return_type +TestSensorHardware::read_sensors( + const std::vector> & /* sensors */) const +{ + return return_type::OK; +} + +#include "pluginlib/class_list_macros.hpp" // NOLINT + +PLUGINLIB_EXPORT_CLASS(TestSensorHardware, hardware_interface::SensorHardwareInterface) diff --git a/controller_manager/test/test_hardware_resources/test_sensor_hardware.hpp b/controller_manager/test/test_hardware_resources/test_sensor_hardware.hpp new file mode 100644 index 0000000000..3937e9d654 --- /dev/null +++ b/controller_manager/test/test_hardware_resources/test_sensor_hardware.hpp @@ -0,0 +1,43 @@ +// Copyright 2020 ros2_control Development Team +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef TEST_HARDWARE_RESOURCES__TEST_SENSOR_HARDWARE_HPP_ +#define TEST_HARDWARE_RESOURCES__TEST_SENSOR_HARDWARE_HPP_ + +#include +#include + +#include "hardware_interface/sensor_hardware_interface.hpp" + +class TestSensorHardware : public hardware_interface::SensorHardwareInterface +{ +public: + hardware_interface::return_type + configure(const hardware_interface::HardwareInfo & sensor_info) override; + + hardware_interface::return_type + start() override; + + hardware_interface::return_type + stop() override; + + hardware_interface::status + get_status() const override; + + hardware_interface::return_type + read_sensors(const std::vector> & sensors) + const override; +}; + +#endif // TEST_HARDWARE_RESOURCES__TEST_SENSOR_HARDWARE_HPP_ diff --git a/controller_manager/test/test_hardware_resources/test_system_hardware.cpp b/controller_manager/test/test_hardware_resources/test_system_hardware.cpp new file mode 100644 index 0000000000..43613408a6 --- /dev/null +++ b/controller_manager/test/test_hardware_resources/test_system_hardware.cpp @@ -0,0 +1,70 @@ +// Copyright 2020 ros2_control Development Team +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include + +#include "./test_system_hardware.hpp" + +using hardware_interface::status; +using hardware_interface::return_type; + +hardware_interface::return_type +TestSystemHardware::configure(const hardware_interface::HardwareInfo & /* system_info */) +{ + return return_type::OK; +} + +return_type +TestSystemHardware::start() +{ + return return_type::OK; +} + +return_type +TestSystemHardware::stop() +{ + return return_type::OK; +} + +status +TestSystemHardware::get_status() const +{ + return status::UNKNOWN; +} + +return_type +TestSystemHardware::read_sensors( + std::vector> & /* sensors */) const +{ + return return_type::OK; +} + +return_type +TestSystemHardware::read_joints( + std::vector> & /* joints */) const +{ + return return_type::OK; +} + +return_type +TestSystemHardware::write_joints( + const std::vector> & /* joints */) +{ + return return_type::OK; +} + +#include "pluginlib/class_list_macros.hpp" // NOLINT + +PLUGINLIB_EXPORT_CLASS(TestSystemHardware, hardware_interface::SystemHardwareInterface) diff --git a/controller_manager/test/test_hardware_resources/test_system_hardware.hpp b/controller_manager/test/test_hardware_resources/test_system_hardware.hpp new file mode 100644 index 0000000000..1fcbcc8967 --- /dev/null +++ b/controller_manager/test/test_hardware_resources/test_system_hardware.hpp @@ -0,0 +1,51 @@ +// Copyright 2020 ros2_control Development Team +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef TEST_HARDWARE_RESOURCES__TEST_SYSTEM_HARDWARE_HPP_ +#define TEST_HARDWARE_RESOURCES__TEST_SYSTEM_HARDWARE_HPP_ + +#include +#include + +#include "hardware_interface/system_hardware_interface.hpp" + +class TestSystemHardware : public hardware_interface::SystemHardwareInterface +{ +public: + hardware_interface::return_type + configure(const hardware_interface::HardwareInfo & system_info) override; + + hardware_interface::return_type + start() override; + + hardware_interface::return_type + stop() override; + + hardware_interface::status + get_status() const override; + + hardware_interface::return_type + read_sensors( + std::vector> & sensors) const override; + + hardware_interface::return_type + read_joints( + std::vector> & joints) const override; + + hardware_interface::return_type + write_joints( + const std::vector> & joints) override; +}; + +#endif // TEST_HARDWARE_RESOURCES__TEST_SYSTEM_HARDWARE_HPP_ diff --git a/controller_manager/test/test_resource_manager.cpp b/controller_manager/test/test_resource_manager.cpp new file mode 100644 index 0000000000..8d9fb6324f --- /dev/null +++ b/controller_manager/test/test_resource_manager.cpp @@ -0,0 +1,242 @@ +// 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" + +#include "test_hardware_resources/test_joint_component.hpp" +#include "test_hardware_resources/test_sensor_component.hpp" + +using namespace ::testing; // NOLINT + +class TestResourceManager : public Test +{ +public: + static void SetUpTestCase() + { + } + + void SetUp() + { + urdf_head_ = + R"( + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +)"; + + urdf_tail_ = + R"( + +)"; + + test_hardware_resource_system_ = + R"( + + + test_actuator_hardware + 2 + 2 + + + test_joint_component + -1 + 1 + + + + + test_sensor_hardware + 2 + 2 + + + test_sensor_component + -1 + 1 + + + + + test_system_hardware + 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.joint_size()); + EXPECT_EQ(0u, rm.sensor_size()); + EXPECT_FALSE(rm.sensor_exists("sensor1")); + + EXPECT_EQ(0u, rm.actuator_interface_size()); + EXPECT_EQ(0u, rm.sensor_interface_size()); + EXPECT_EQ(0u, rm.system_interface_size()); +} + +TEST_F(TestResourceManager, initialization_with_urdf) { + auto urdf = urdf_head_ + test_hardware_resource_system_ + urdf_tail_; + controller_manager::ResourceManager rm(urdf); + + EXPECT_EQ(3u, rm.joint_size()); + auto joint_names = rm.joint_names(); + for (const auto & joint_name : {"joint1", "joint2", "joint3"}) { + EXPECT_NE( + joint_names.end(), + std::find(joint_names.begin(), joint_names.end(), joint_name)); + } + + EXPECT_EQ(1u, rm.sensor_size()); + EXPECT_EQ("sensor1", rm.sensor_names()[0]); + EXPECT_TRUE(rm.sensor_exists("sensor1")) << "sensor1 does not exist"; + EXPECT_FALSE(rm.sensor_exists("non-existing-sensor")); + + EXPECT_EQ(1u, rm.actuator_interface_size()); + EXPECT_EQ("TestActuatorHardware", rm.actuator_interface_names()[0]); + EXPECT_EQ(1u, rm.sensor_interface_size()); + EXPECT_EQ("TestSensorHardware", rm.sensor_interface_names()[0]); + EXPECT_EQ(1u, rm.system_interface_size()); + EXPECT_EQ("TestSystemHardware", rm.system_interface_names()[0]); +} + +TEST_F(TestResourceManager, resource_claiming) { + auto urdf = urdf_head_ + test_hardware_resource_system_ + urdf_tail_; + controller_manager::ResourceManager rm(urdf); + + EXPECT_EQ(1u, rm.sensor_size()); + EXPECT_FALSE(rm.sensor_is_claimed("sensor1")); + + { + auto sensor1 = rm.claim_sensor("sensor1"); + EXPECT_TRUE(rm.sensor_is_claimed("sensor1")); + try { + auto sensor1_again = rm.claim_sensor("sensor1"); + FAIL(); + } catch (const std::runtime_error &) { + SUCCEED(); + } + } + EXPECT_FALSE(rm.sensor_is_claimed("sensor1")); + + for (const auto & joint_name : {"joint1", "joint1", "joint1", "joint2", "joint3"}) { + { + auto joint = rm.claim_joint(joint_name); + EXPECT_TRUE(rm.joint_is_claimed(joint_name)); + try { + auto joint1_again = rm.claim_joint(joint_name); + FAIL(); + } catch (const std::runtime_error &) { + SUCCEED(); + } + } + EXPECT_FALSE(rm.joint_is_claimed(joint_name)); + } +} + +TEST_F(TestResourceManager, interface_access) { + auto urdf = urdf_head_ + test_hardware_resource_system_ + urdf_tail_; + controller_manager::ResourceManager rm(urdf); + + EXPECT_EQ(3u, rm.joint_size()); + EXPECT_EQ(1u, rm.sensor_size()); + + { + auto joint1 = rm.claim_joint("joint1"); + { + auto test_joint = joint1.as(); + EXPECT_TRUE(test_joint.return_true()); + EXPECT_TRUE(rm.joint_is_claimed("joint1")); + } + EXPECT_TRUE(rm.joint_is_claimed("joint1")); + } + EXPECT_FALSE(rm.joint_is_claimed("joint1")); + + { + auto sensor1 = rm.claim_sensor("sensor1"); + { + auto test_sensor = sensor1.as(); + EXPECT_TRUE(test_sensor.return_true()); + EXPECT_TRUE(rm.sensor_is_claimed("sensor1")); + } + EXPECT_TRUE(rm.sensor_is_claimed("sensor1")); + } + EXPECT_FALSE(rm.sensor_is_claimed("sensor1")); +} diff --git a/hardware_interface/CMakeLists.txt b/hardware_interface/CMakeLists.txt index 4e9f289919..d2467982be 100644 --- a/hardware_interface/CMakeLists.txt +++ b/hardware_interface/CMakeLists.txt @@ -41,32 +41,22 @@ ament_target_dependencies( # which is appropriate when building the dll but not consuming it. target_compile_definitions(hardware_interface PRIVATE "HARDWARE_INTERFACE_BUILDING_DLL") -add_library( - component_parser - src/component_parser.cpp -) -target_include_directories( - component_parser - PUBLIC - include -) -ament_target_dependencies( - component_parser - TinyXML2 -) -target_compile_definitions(component_parser PRIVATE "HARDWARE_INTERFACE_BUILDING_DLL") - add_library( components SHARED src/components/joint.cpp src/components/sensor.cpp + src/component_parser.cpp ) target_include_directories( components PUBLIC include ) +ament_target_dependencies( + components + TinyXML2 +) # Causes the visibility macros to use dllexport rather than dllimport, # which is appropriate when building the dll but not consuming it. target_compile_definitions(components PRIVATE "HARDWARE_INTERFACE_BUILDING_DLL") @@ -78,7 +68,6 @@ install( install( TARGETS - component_parser components hardware_interface RUNTIME DESTINATION bin @@ -124,19 +113,13 @@ if(BUILD_TESTING) target_link_libraries(test_component_interfaces components hardware_interface) ament_add_gmock(test_component_parser test/test_component_parser.cpp) - target_link_libraries(test_component_parser component_parser) - ament_target_dependencies(test_component_parser TinyXML2) + target_link_libraries(test_component_parser components) endif() -ament_export_dependencies( - rclcpp - rcpputils -) ament_export_include_directories( include ) ament_export_libraries( - component_parser components hardware_interface ) diff --git a/hardware_interface/include/hardware_interface/actuator_hardware.hpp b/hardware_interface/include/hardware_interface/actuator_hardware.hpp index ecca54042d..da8db8d1b5 100644 --- a/hardware_interface/include/hardware_interface/actuator_hardware.hpp +++ b/hardware_interface/include/hardware_interface/actuator_hardware.hpp @@ -17,6 +17,7 @@ #include +#include "hardware_interface/actuator_hardware_interface.hpp" #include "hardware_interface/hardware_info.hpp" #include "hardware_interface/types/hardware_interface_return_values.hpp" #include "hardware_interface/types/hardware_interface_status_values.hpp" @@ -29,13 +30,14 @@ namespace components { class Joint; } // namespace components -class ActuatorHardwareInterface; class ActuatorHardware final { public: ActuatorHardware() = default; + explicit ActuatorHardware(ActuatorHardware && other) = default; + explicit ActuatorHardware(std::unique_ptr impl); ~ActuatorHardware() = default; diff --git a/hardware_interface/include/hardware_interface/components/joint.hpp b/hardware_interface/include/hardware_interface/components/joint.hpp index 0101afa6f0..79f45000c6 100644 --- a/hardware_interface/include/hardware_interface/components/joint.hpp +++ b/hardware_interface/include/hardware_interface/components/joint.hpp @@ -15,10 +15,13 @@ #ifndef HARDWARE_INTERFACE__COMPONENTS__JOINT_HPP_ #define HARDWARE_INTERFACE__COMPONENTS__JOINT_HPP_ +#include +#include #include #include #include "hardware_interface/components/component_info.hpp" +#include "hardware_interface/components/joint_interface.hpp" #include "hardware_interface/types/hardware_interface_return_values.hpp" #include "hardware_interface/visibility_control.h" @@ -27,178 +30,87 @@ namespace hardware_interface namespace components { +using Deleter = std::function; + +/// Joint wrapper class /** - * \brief Base Class for the "Joint" component used as a basic building block for a robot. - * A joint is always 1-DoF and can have one or more interfaces (e.g., position, velocity, etc.) - * A joint has to be able to receive command(s) and optionally can provide its state(s). + * The class represents a wrapper around an implementation of the JointInterface. + * This allows to represent various implementations under the same common API. + * Joint serves as a handle which can get claimed and loaned to each individual controller + * to obtain exclusive ownership over the component. */ -class Joint +class Joint final { public: + /// Constructor for a joint component + /** + * The joint is constructed with a concrete JointInterface implementation. + * The second parameter serves as a callback upon destruction, which is used + * to cleanup the component or signal its destruction to other entities such + * as the resource manager within the controller manager. + * + * \param impl Shared pointer to a precise implementation of the joint component. + * \param deleter Callback function to be called upon destruction. + */ HARDWARE_INTERFACE_PUBLIC - Joint() = default; + Joint(std::shared_ptr impl, Deleter deleter); HARDWARE_INTERFACE_PUBLIC - virtual - ~Joint() = default; + ~Joint(); + /// Access the underlying implementation /** - * \brief Configure base joint class based on the description in the robot's URDF file. + * The function returns a reference to the implementation. + * This allows for a full-access API for optimal use. * - * \param joint_info structure with data from URDF. - * \return return_type::OK if required data are provided and is successfully parsed, - * return_type::ERROR otherwise. + * \note: The given template parameter has to be directly convertible. + * A wrongly specified template argument might lead to UB. + * \return Reference to the casted interface. */ + template + auto & as() {return *std::static_pointer_cast(impl_);} + HARDWARE_INTERFACE_PUBLIC - virtual return_type configure(const ComponentInfo & joint_info); - /** - * \brief Provide the list of command interfaces configured for the joint. - * - * \return string list with command interfaces. - */ HARDWARE_INTERFACE_PUBLIC - virtual std::vector get_command_interfaces() const; - /** - * \brief Provide the list of state interfaces configured for the joint. - * - * \return string list with state interfaces. - */ HARDWARE_INTERFACE_PUBLIC - virtual std::vector get_state_interfaces() const; - /** - * \brief Get command list from the joint. This function is used in the write function of the - * actuator or system hardware. The parameters command and interfaces have the same order, - * and number of elements. Using the interfaces list, the hardware can choose which values to - * provide. - * - * \param command list of doubles with commands for the hardware. - * \param interfaces list of interfaces on which commands have to set. - * \return return_type::INTERFACE_VALUE_SIZE_NOT_EQUAL if command and interfaces arguments do not - * have the same length; return_type::INTERFACE_NOT_FOUND if one of provided interfaces is not - * defined for the joint; return return_type::INTERFACE_NOT_PROVIDED if the list of interfaces - * is empty; return_type::OK otherwise. - */ - HARDWARE_INTERFACE_EXPORT - virtual + HARDWARE_INTERFACE_PUBLIC return_type get_command( std::vector & command, const std::vector & interfaces) const; - /** - * \brief Get complete command list for the joint. This function is used by the hardware to get - * complete command for it. The hardware valus have the same order as interfaces which - * can be recived by get_hardware_interfaces() function. Return value is used for API consistency. - * - * \param command list of doubles with commands for the hardware. - * \return return_type::OK always. - */ - HARDWARE_INTERFACE_EXPORT - virtual + HARDWARE_INTERFACE_PUBLIC return_type get_command(std::vector & command) const; - /** - * \brief Set command list for the joint. This function is used by the controller to set the goal - * values for the hardware. The parameters command, and interfaces have the same order and number - * of elements. Using the interfaces list, the controller can choose which values to set. - * - * \param command list of doubles with commands for the hardware. - * \param interfaces list of interfaces on which commands have to be provided. - * \return return_type::INTERFACE_VALUE_SIZE_NOT_EQUAL if command and interfaces arguments do not - * have the same length; return_type::COMMAND_OUT_OF_LIMITS if one of the command values is out - * of limits; return_type::INTERFACE_NOT_FOUND if one of provided interfaces is not - * defined for the joint; return_type::OK otherwise. - * - * \todo The error handling in this function could lead to incosistant command or state variables - * for different interfaces. This should be changed in the future. - * (see: https://github.com/ros-controls/ros2_control/issues/129) - */ - HARDWARE_INTERFACE_EXPORT - virtual + HARDWARE_INTERFACE_PUBLIC return_type set_command( const std::vector & command, const std::vector & interfaces); - /** - * \brief Get complete state list from the joint. This function is used by the hardware to get - * complete command for it. The hardware valus have the same order as interfaces which - * can be recived by get_hardware_interfaces() function. - * - * \param command list of doubles with commands for the hardware. - * \return return_type::INTERFACE_VALUE_SIZE_NOT_EQUAL is command size is not equal to number of - * joint's command interfaces; return_type::COMMAND_OUT_OF_LIMITS if one of the command values is out - * of limits; return_type::OK otherwise. - */ - HARDWARE_INTERFACE_EXPORT - virtual + HARDWARE_INTERFACE_PUBLIC return_type set_command(const std::vector & command); - /** - * \brief Get state list from the joint. This function is used by the controller to get the - * actual state of the hardware. The parameters state, and interfaces have the same order and - * number of elements. Using the interfaces list, the controller can choose which values to get. - * - * \param state list of doubles with states of the hardware. - * \param interfaces list of interfaces on which states have to be provided. - * \return return_type::INTERFACE_VALUE_SIZE_NOT_EQUAL if state and interfaces arguments do not - * have the same length; return_type::INTERFACE_NOT_FOUND if one of provided interfaces is not - * defined for the joint; return return_type::INTERFACE_NOT_PROVIDED if the list of interfaces - * is empty; return_type::OK otherwise. - */ - HARDWARE_INTERFACE_EXPORT - virtual + HARDWARE_INTERFACE_PUBLIC return_type get_state( std::vector & state, const std::vector & interfaces) const; - /** - * \brief Get complete state list from the joint. This function is used by the controller to get - * complete actual state of the hardware. The state values have the same order as interfaces which - * can be recived by get_state_interfaces() function. Return value is used for API consistency. - * - * \param state list of doubles with states of the hardware. - * \return return_type::OK always. - */ - HARDWARE_INTERFACE_EXPORT - virtual + HARDWARE_INTERFACE_PUBLIC return_type get_state(std::vector & state) const; - /** - * \brief Set state list for the joint. This function is used by the hardware to set its actual - * state. The parameters state, and interfaces have the same order and number of elements. Using - * the interfaces list, the hardware can choose which values to set. - * - * \param state list of doubles with states of the hardware. - * \param interfaces list of interfaces on which states have to be provided. - * \return return_type::INTERFACE_VALUE_SIZE_NOT_EQUAL if state and interfaces arguments do not - * have the same length; return_type::INTERFACE_NOT_FOUND if one of provided interfaces is not - * defined for the joint; return_type::OK otherwise. - */ - HARDWARE_INTERFACE_EXPORT - virtual + HARDWARE_INTERFACE_PUBLIC return_type set_state( const std::vector & state, const std::vector & interfaces); - /** - * \brief Set complete state list from the joint.This function is used by the hardware to set its - * complete actual state. The state values have the same order as interfaces which can be recived - * by get_state_interfaces() function. - * - * \param state list of doubles with states from the hardware. - * \return return_type::INTERFACE_VALUE_SIZE_NOT_EQUAL is command size is not equal to number of - * joint's state interfaces, return_type::OK otherwise. - */ - HARDWARE_INTERFACE_EXPORT - virtual + HARDWARE_INTERFACE_PUBLIC return_type set_state(const std::vector & state); -protected: - ComponentInfo info_; - std::vector commands_; - std::vector states_; +private: + std::shared_ptr impl_; + Deleter deleter_; }; } // namespace components diff --git a/hardware_interface/include/hardware_interface/components/joint_interface.hpp b/hardware_interface/include/hardware_interface/components/joint_interface.hpp new file mode 100644 index 0000000000..5a9934874a --- /dev/null +++ b/hardware_interface/include/hardware_interface/components/joint_interface.hpp @@ -0,0 +1,191 @@ +// Copyright 2020 ros2_control Development Team +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef HARDWARE_INTERFACE__COMPONENTS__JOINT_INTERFACE_HPP_ +#define HARDWARE_INTERFACE__COMPONENTS__JOINT_INTERFACE_HPP_ + +#include +#include + +#include "hardware_interface/components/component_info.hpp" +#include "hardware_interface/types/hardware_interface_return_values.hpp" +#include "hardware_interface/visibility_control.h" + +namespace hardware_interface +{ +namespace components +{ + +/** + * \brief Interface for the "Joint" component used as a basic building block for a robot. + * A joint is always 1-DoF and can have one or more interfaces (e.g., position, velocity, etc.) + * A joint has to be able to receive command(s) and optionally can provide its state(s). + */ +class JointInterface +{ +public: + JointInterface() = default; + + virtual + ~JointInterface() = default; + + /** + * \brief Configure joint based on the description in the robot's URDF file. + * + * \param joint_info structure with data from URDF. + * \return return_type::OK if required data are provided and is successfully parsed, + * \return return_type::ERROR otherwise. + */ + virtual + return_type configure(const ComponentInfo & joint_info) = 0; + + /** + * \brief Provide the list of command interfaces configured for the joint. + * + * \return string list with command interfaces. + */ + virtual + std::vector get_command_interfaces() const = 0; + + /** + * \brief Provide the list of state interfaces configured for the joint. + * + * \return string list with state interfaces. + */ + virtual + std::vector get_state_interfaces() const = 0; + + /** + * \brief Get command list from the joint. This function is used in the write function of the + * actuator or system hardware. The parameters command and interfaces have the same order, + * and number of elements. Using the interfaces list, the hardware can choose which values to + * provide. + * + * \param command list of doubles with commands for the hardware. + * \param interfaces list of interfaces on which commands have to set. + * \return return_type::INTERFACE_VALUE_SIZE_NOT_EQUAL if command and interfaces arguments do not + * have the same length; return_type::INTERFACE_NOT_FOUND if one of provided interfaces is not + * defined for the joint; return return_type::INTERFACE_NOT_PROVIDED if the list of interfaces + * is empty; return_type::OK otherwise. + */ + virtual + return_type get_command( + std::vector & command, + const std::vector & interfaces) const = 0; + + /** + * \brief Get complete command list for the joint. This function is used by the hardware to get + * complete command for it. The hardware valus have the same order as interfaces which + * can be recived by get_hardware_interfaces() function. Return value is used for API consistency. + * + * \param command list of doubles with commands for the hardware. + * \return return_type::OK always. + */ + virtual + return_type get_command(std::vector & command) const = 0; + + /** + * \brief Set command list for the joint. This function is used by the controller to set the goal + * values for the hardware. The parameters command, and interfaces have the same order and number + * of elements. Using the interfaces list, the controller can choose which values to set. + * + * \param command list of doubles with commands for the hardware. + * \param interfaces list of interfaces on which commands have to be provided. + * \return return_type::INTERFACE_VALUE_SIZE_NOT_EQUAL if command and interfaces arguments do not + * have the same length; return_type::COMMAND_OUT_OF_LIMITS if one of the command values is out + * of limits; return_type::INTERFACE_NOT_FOUND if one of provided interfaces is not + * defined for the joint; return_type::OK otherwise. + * + * \todo The error handling in this function could lead to incosistant command or state variables + * for different interfaces. This should be changed in the future. + * (see: https://github.com/ros-controls/ros2_control/issues/129) + */ + virtual + return_type set_command( + const std::vector & command, + const std::vector & interfaces) = 0; + + /** + * \brief Set complete state list from the joint. This function is used by the hardware to get + * complete command for it. The hardware valus have the same order as interfaces which + * can be recived by get_hardware_interfaces() function. + * + * \param command list of doubles with commands for the hardware. + * \return return_type::INTERFACE_VALUE_SIZE_NOT_EQUAL is command size is not equal to number of + * joint's command interfaces; return_type::COMMAND_OUT_OF_LIMITS if one of the command values is out + * of limits; return_type::OK otherwise. + */ + virtual + return_type set_command(const std::vector & command) = 0; + + /** + * \brief Get state list from the joint. This function is used by the controller to get the + * actual state of the hardware. The parameters state, and interfaces have the same order and + * number of elements. Using the interfaces list, the controller can choose which values to get. + * + * \param state list of doubles with states of the hardware. + * \param interfaces list of interfaces on which states have to be provided. + * \return return_type::INTERFACE_VALUE_SIZE_NOT_EQUAL if state and interfaces arguments do not + * have the same length; return_type::INTERFACE_NOT_FOUND if one of provided interfaces is not + * defined for the joint; return return_type::INTERFACE_NOT_PROVIDED if the list of interfaces + * is empty; return_type::OK otherwise. + */ + virtual + return_type get_state( + std::vector & state, + const std::vector & interfaces) const = 0; + + /** + * \brief Get complete state list from the joint. This function is used by the controller to get + * complete actual state of the hardware. The state values have the same order as interfaces which + * can be recived by get_state_interfaces() function. Return value is used for API consistency. + * + * \param state list of doubles with states of the hardware. + * \return return_type::OK always. + */ + virtual + return_type get_state(std::vector & state) const = 0; + + /** + * \brief Set state list for the joint. This function is used by the hardware to set its actual + * state. The parameters state, and interfaces have the same order and number of elements. Using + * the interfaces list, the hardware can choose which values to set. + * + * \param state list of doubles with states of the hardware. + * \param interfaces list of interfaces on which states have to be provided. + * \return return_type::INTERFACE_VALUE_SIZE_NOT_EQUAL if state and interfaces arguments do not + * have the same length; return_type::INTERFACE_NOT_FOUND if one of provided interfaces is not + * defined for the joint; return_type::OK otherwise. + */ + virtual + return_type set_state( + const std::vector & state, + const std::vector & interfaces) = 0; + + /** + * \brief Set complete state list from the joint.This function is used by the hardware to set its + * complete actual state. The state values have the same order as interfaces which can be recived + * by get_state_interfaces() function. + * + * \param state list of doubles with states from the hardware. + * \return return_type::INTERFACE_VALUE_SIZE_NOT_EQUAL is command size is not equal to number of + * joint's state interfaces, return_type::OK otherwise. + */ + virtual + return_type set_state(const std::vector & state) = 0; +}; + +} // namespace components +} // namespace hardware_interface +#endif // HARDWARE_INTERFACE__COMPONENTS__JOINT_INTERFACE_HPP_ diff --git a/hardware_interface/include/hardware_interface/components/sensor.hpp b/hardware_interface/include/hardware_interface/components/sensor.hpp index b44fb12718..de30a64195 100644 --- a/hardware_interface/include/hardware_interface/components/sensor.hpp +++ b/hardware_interface/include/hardware_interface/components/sensor.hpp @@ -15,10 +15,13 @@ #ifndef HARDWARE_INTERFACE__COMPONENTS__SENSOR_HPP_ #define HARDWARE_INTERFACE__COMPONENTS__SENSOR_HPP_ +#include +#include #include #include #include "hardware_interface/components/component_info.hpp" +#include "hardware_interface/components/sensor_interface.hpp" #include "hardware_interface/types/hardware_interface_return_values.hpp" #include "hardware_interface/visibility_control.h" @@ -27,102 +30,69 @@ namespace hardware_interface namespace components { +using Deleter = std::function; + +/// Sensor wrapper class /** - * \brief Base Class for "Sensor" component used as a basic build block for the devices which - * provide data. A sensor can have one or more interfaces (e.g., force, acceleration, etc.) to - * provide states for. The list of state interfaces defines this. + * The class represents a wrapper around an implementation of the SensorInterface. + * This allows to represent various implementations under the same common API. + * Sensor serves as a handle which can get claimed and loaned to each individual controller + * to obtain exclusive ownership over the component. */ -class Sensor +class Sensor final { public: + /// Constructor for a sensor component + /** + * The sensor is constructed with a precise SensorInterface implementation. + * The second parameter serves as a callback upon destruction, which is used + * to cleanup the component or signal its destruction to other entities such + * as the resource manager within the controller manager. + * + * \param impl Shared pointer to a precise implementation of the sensor component. + * \param deleter Callback function to be called upon destruction. + */ HARDWARE_INTERFACE_PUBLIC - Sensor() = default; + Sensor(std::shared_ptr impl, Deleter deleter); HARDWARE_INTERFACE_PUBLIC - virtual - ~Sensor() = default; + ~Sensor(); + /// Access the underlying implementation /** - * \brief Configure base sensor class based on the description in the robot's URDF file. + * The function returns a reference to the implementation. + * This allows for a full-access API for optimal use. * - * \param joint_info structure with data from URDF. - * \return return_type::OK if required data are provided and is successfully parsed, - * return_type::ERROR otherwise. + * \note: The given template parameter has to be directly convertible. + * A wrongly specified template argument might lead to UB. + * \return Reference to the casted interface. */ + template + auto & as() {return *std::static_pointer_cast(impl_);} + HARDWARE_INTERFACE_PUBLIC - virtual - return_type configure(const ComponentInfo & joint_info); + return_type configure(const ComponentInfo & sensor_info); - /** - * \brief Provide the list of state interfaces configured for the sensor. - * - * \return string list with state interfaces. - */ HARDWARE_INTERFACE_PUBLIC - virtual std::vector get_state_interfaces() const; - /** - * \brief Get state list from the sensor. This function is used by the controller to get the - * actual state of the hardware. The parameters state, and interfaces have the same order and - * number of elements. Using the interfaces list, the controller can choose which values to get. - * - * \param state list of doubles with states of the hardware. - * \param interfaces list of interfaces on which states have to be provided. - * \return return_type::INTERFACE_VALUE_SIZE_NOT_EQUAL if state and interfaces arguments do not - * have the same length; return_type::INTERFACE_NOT_FOUND if one of provided interfaces is not - * defined for the sensor; return return_type::INTERFACE_NOT_PROVIDED if the list of interfaces - * is empty; return_type::OK otherwise. - */ - HARDWARE_INTERFACE_EXPORT - virtual + HARDWARE_INTERFACE_PUBLIC return_type get_state( std::vector & state, const std::vector & interfaces) const; - /** - * \brief Get complete state list from the sensor. This function is used by the controller to get - * complete actual state of the hardware. The state values have the same order as interfaces which - * can be recived by get_state_interfaces() function. Return value is used for API consistency. - * - * \param state list of doubles with states of the hardware. - * \return return_type::OK always. - */ - HARDWARE_INTERFACE_EXPORT - virtual + HARDWARE_INTERFACE_PUBLIC return_type get_state(std::vector & state) const; - /** - * \brief Set state list for the sensor. This function is used by the hardware to set its actual - * state. The parameters state, and interfaces have the same order and number of elements. Using - * the interfaces list, the hardware can choose which values to set. - * - * \param state list of doubles with states of the hardware. - * \param interfaces list of interfaces on which states have to be provided. - * \return return_type::INTERFACE_VALUE_SIZE_NOT_EQUAL if state and interfaces arguments do not - * have the same length; return_type::INTERFACE_NOT_FOUND if one of provided interfaces is not - * defined for the sensor; return_type::OK otherwise. - */ - HARDWARE_INTERFACE_EXPORT - virtual + HARDWARE_INTERFACE_PUBLIC return_type set_state( const std::vector & state, const std::vector & interfaces); - /** - * \brief Set complete state list from the sensor.This function is used by the hardware to set its - * complete actual state. The state values have the same order as interfaces which can be recived - * by get_state_interfaces() function. - * - * \param state list of doubles with states from the hardware. - * \return return_type::INTERFACE_VALUE_SIZE_NOT_EQUAL is state size is not equal to number of - * sensor's state interfaces, return_type::OK otherwise. - */ - HARDWARE_INTERFACE_EXPORT - virtual + HARDWARE_INTERFACE_PUBLIC return_type set_state(const std::vector & state); -protected: - ComponentInfo info_; - std::vector states_; +private: + std::shared_ptr impl_; + Deleter deleter_; }; } // namespace components diff --git a/hardware_interface/include/hardware_interface/components/sensor_interface.hpp b/hardware_interface/include/hardware_interface/components/sensor_interface.hpp new file mode 100644 index 0000000000..f082a6be21 --- /dev/null +++ b/hardware_interface/include/hardware_interface/components/sensor_interface.hpp @@ -0,0 +1,128 @@ +// Copyright 2020 ros2_control Development Team +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef HARDWARE_INTERFACE__COMPONENTS__SENSOR_INTERFACE_HPP_ +#define HARDWARE_INTERFACE__COMPONENTS__SENSOR_INTERFACE_HPP_ + +#include +#include + +#include "hardware_interface/components/component_info.hpp" +#include "hardware_interface/types/hardware_interface_return_values.hpp" +#include "hardware_interface/visibility_control.h" + +namespace hardware_interface +{ +namespace components +{ + +/** + * \brief Interface for the "Sensor" component used as a basic build block for the devices which + * provide data. A sensor can have one or more interfaces (e.g., force, acceleration, etc.) to + * provide states for. The list of state interfaces defines this. + */ +class SensorInterface +{ +public: + HARDWARE_INTERFACE_PUBLIC + SensorInterface() = default; + + HARDWARE_INTERFACE_PUBLIC + virtual + ~SensorInterface() = default; + + /** + * \brief Configure base sensor class based on the description in the robot's URDF file. + * + * \param sensor_info structure with data from URDF. + * \return return_type::OK if required data are provided and is successfully parsed, + * return_type::ERROR otherwise. + */ + HARDWARE_INTERFACE_PUBLIC + virtual + return_type configure(const ComponentInfo & sensor_info) = 0; + + /** + * \brief Provide the list of state interfaces configured for the sensor. + * + * \return string list with state interfaces. + */ + HARDWARE_INTERFACE_PUBLIC + virtual + std::vector get_state_interfaces() const = 0; + + /** + * \brief Get state list from the sensor. This function is used by the controller to get the + * actual state of the hardware. The parameters state, and interfaces have the same order and + * number of elements. Using the interfaces list, the controller can choose which values to get. + * + * \param state list of doubles with states of the hardware. + * \param interfaces list of interfaces on which states have to be provided. + * \return return_type::INTERFACE_VALUE_SIZE_NOT_EQUAL if state and interfaces arguments do not + * have the same length; return_type::INTERFACE_NOT_FOUND if one of provided interfaces is not + * defined for the sensor; return return_type::INTERFACE_NOT_PROVIDED if the list of interfaces + * is empty; return_type::OK otherwise. + */ + HARDWARE_INTERFACE_PUBLIC + virtual + return_type get_state( + std::vector & state, + const std::vector & interfaces) const = 0; + + /** + * \brief Get complete state list from the sensor. This function is used by the controller to get + * complete actual state of the hardware. The state values have the same order as interfaces which + * can be recived by get_state_interfaces() function. Return value is used for API consistency. + * + * \param state list of doubles with states of the hardware. + * \return return_type::OK always. + */ + HARDWARE_INTERFACE_PUBLIC + virtual + return_type get_state(std::vector & state) const = 0; + + /** + * \brief Set state list for the sensor. This function is used by the hardware to set its actual + * state. The parameters state, and interfaces have the same order and number of elements. Using + * the interfaces list, the hardware can choose which values to set. + * + * \param state list of doubles with states of the hardware. + * \param interfaces list of interfaces on which states have to be provided. + * \return return_type::INTERFACE_VALUE_SIZE_NOT_EQUAL if state and interfaces arguments do not + * have the same length; return_type::INTERFACE_NOT_FOUND if one of provided interfaces is not + * defined for the sensor; return_type::OK otherwise. + */ + HARDWARE_INTERFACE_PUBLIC + virtual + return_type set_state( + const std::vector & state, + const std::vector & interfaces) = 0; + + /** + * \brief Set complete state list from the sensor.This function is used by the hardware to set its + * complete actual state. The state values have the same order as interfaces which can be recived + * by get_state_interfaces() function. + * + * \param state list of doubles with states from the hardware. + * \return return_type::INTERFACE_VALUE_SIZE_NOT_EQUAL is state size is not equal to number of + * sensor's state interfaces, return_type::OK otherwise. + */ + HARDWARE_INTERFACE_PUBLIC + virtual + return_type set_state(const std::vector & state) = 0; +}; + +} // namespace components +} // namespace hardware_interface +#endif // HARDWARE_INTERFACE__COMPONENTS__SENSOR_INTERFACE_HPP_ diff --git a/hardware_interface/include/hardware_interface/sensor_hardware.hpp b/hardware_interface/include/hardware_interface/sensor_hardware.hpp index 52a363ea0e..bec1e33ab9 100644 --- a/hardware_interface/include/hardware_interface/sensor_hardware.hpp +++ b/hardware_interface/include/hardware_interface/sensor_hardware.hpp @@ -21,6 +21,7 @@ #include #include "hardware_interface/hardware_info.hpp" +#include "hardware_interface/sensor_hardware_interface.hpp" #include "hardware_interface/types/hardware_interface_return_values.hpp" #include "hardware_interface/types/hardware_interface_status_values.hpp" #include "hardware_interface/visibility_control.h" @@ -32,13 +33,14 @@ namespace components { class Sensor; } // namespace components -class SensorHardwareInterface; class SensorHardware final { public: SensorHardware() = default; + explicit SensorHardware(SensorHardware && other) = default; + explicit SensorHardware(std::unique_ptr impl); ~SensorHardware() = default; diff --git a/hardware_interface/include/hardware_interface/system_hardware.hpp b/hardware_interface/include/hardware_interface/system_hardware.hpp index 113af16c72..6522bb1729 100644 --- a/hardware_interface/include/hardware_interface/system_hardware.hpp +++ b/hardware_interface/include/hardware_interface/system_hardware.hpp @@ -21,6 +21,7 @@ #include #include "hardware_interface/hardware_info.hpp" +#include "hardware_interface/system_hardware_interface.hpp" #include "hardware_interface/types/hardware_interface_return_values.hpp" #include "hardware_interface/types/hardware_interface_status_values.hpp" #include "hardware_interface/visibility_control.h" @@ -33,7 +34,6 @@ namespace components class Joint; class Sensor; } // namespace components -class SystemHardwareInterface; class SystemHardware final { @@ -41,6 +41,9 @@ class SystemHardware final HARDWARE_INTERFACE_PUBLIC explicit SystemHardware(std::unique_ptr impl); + HARDWARE_INTERFACE_PUBLIC + explicit SystemHardware(SystemHardware && other) = default; + virtual ~SystemHardware() = default; HARDWARE_INTERFACE_PUBLIC diff --git a/hardware_interface/include/hardware_interface/system_hardware_interface.hpp b/hardware_interface/include/hardware_interface/system_hardware_interface.hpp index 1d9a3089aa..6c38c0a937 100644 --- a/hardware_interface/include/hardware_interface/system_hardware_interface.hpp +++ b/hardware_interface/include/hardware_interface/system_hardware_interface.hpp @@ -112,8 +112,8 @@ class SystemHardwareInterface * \return return_type:OK if everything worked as expected, return_type::ERROR otherwise. */ HARDWARE_INTERFACE_PUBLIC - return_type virtual + return_type write_joints(const std::vector> & joints) = 0; }; diff --git a/hardware_interface/include/hardware_interface/types/hardware_interface_return_values.hpp b/hardware_interface/include/hardware_interface/types/hardware_interface_return_values.hpp index d09fea6e1a..047cbb75f3 100644 --- a/hardware_interface/include/hardware_interface/types/hardware_interface_return_values.hpp +++ b/hardware_interface/include/hardware_interface/types/hardware_interface_return_values.hpp @@ -35,6 +35,7 @@ enum class return_type : std::uint8_t INTERFACE_NOT_FOUND = 30, INTERFACE_VALUE_SIZE_NOT_EQUAL = 31, INTERFACE_NOT_PROVIDED = 32, + INTERFACE_DUPLICATES = 33, COMMAND_OUT_OF_LIMITS = 40, }; diff --git a/hardware_interface/src/components/joint.cpp b/hardware_interface/src/components/joint.cpp index 8aaa0c661d..82035c8efc 100644 --- a/hardware_interface/src/components/joint.cpp +++ b/hardware_interface/src/components/joint.cpp @@ -12,84 +12,87 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include #include +#include #include #include "hardware_interface/components/joint.hpp" #include "hardware_interface/components/component_info.hpp" #include "hardware_interface/types/hardware_interface_return_values.hpp" -#include "./component_lists_management.hpp" - namespace hardware_interface { namespace components { +Joint::Joint(std::shared_ptr impl, Deleter deleter) +: impl_(std::move(impl)), + deleter_(deleter) +{} + +Joint::~Joint() +{ + if (deleter_) {deleter_();} +} + return_type Joint::configure(const ComponentInfo & joint_info) { - info_ = joint_info; - if (info_.command_interfaces.size() > 0) { - commands_.resize(info_.command_interfaces.size()); - } - if (info_.state_interfaces.size() > 0) { - states_.resize(info_.state_interfaces.size()); - } - return return_type::OK; + return impl_->configure(joint_info); } std::vector Joint::get_command_interfaces() const { - return info_.command_interfaces; + return impl_->get_command_interfaces(); } std::vector Joint::get_state_interfaces() const { - return info_.state_interfaces; + return impl_->get_state_interfaces(); } return_type Joint::get_command( std::vector & command, const std::vector & interfaces) const { - return get_internal_values(command, interfaces, info_.command_interfaces, commands_); + return impl_->get_command(command, interfaces); } return_type Joint::get_command(std::vector & command) const { - return get_internal_values(command, commands_); + return impl_->get_command(command); } return_type Joint::set_command( const std::vector & command, const std::vector & interfaces) { - return set_internal_values(command, interfaces, info_.command_interfaces, commands_); + return impl_->set_command(command, interfaces); } return_type Joint::set_command(const std::vector & command) { - return set_internal_values(command, commands_); + return impl_->set_command(command); } return_type Joint::get_state( std::vector & state, const std::vector & interfaces) const { - return get_internal_values(state, interfaces, info_.state_interfaces, states_); + return impl_->get_state(state, interfaces); } return_type Joint::get_state(std::vector & state) const { - return get_internal_values(state, states_); + return impl_->get_state(state); } return_type Joint::set_state( const std::vector & state, const std::vector & interfaces) { - return set_internal_values(state, interfaces, info_.state_interfaces, states_); + return impl_->set_state(state, interfaces); } return_type Joint::set_state(const std::vector & state) { - return set_internal_values(state, states_); + return impl_->set_state(state); } } // namespace components diff --git a/hardware_interface/src/components/sensor.cpp b/hardware_interface/src/components/sensor.cpp index 397bee648d..f69720ad9b 100644 --- a/hardware_interface/src/components/sensor.cpp +++ b/hardware_interface/src/components/sensor.cpp @@ -12,7 +12,9 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include #include +#include #include #include "hardware_interface/components/sensor.hpp" @@ -26,40 +28,46 @@ namespace hardware_interface namespace components { -return_type Sensor::configure(const ComponentInfo & joint_info) +Sensor::Sensor(std::shared_ptr impl, Deleter deleter) +: impl_(std::move(impl)), + deleter_(deleter) +{} + +Sensor::~Sensor() +{ + if (deleter_) {deleter_();} +} + +return_type Sensor::configure(const ComponentInfo & sensor_info) { - info_ = joint_info; - if (info_.state_interfaces.size() > 0) { - states_.resize(info_.state_interfaces.size()); - } - return return_type::OK; + return impl_->configure(sensor_info); } std::vector Sensor::get_state_interfaces() const { - return info_.state_interfaces; + return impl_->get_state_interfaces(); } return_type Sensor::get_state( std::vector & state, const std::vector & interfaces) const { - return get_internal_values(state, interfaces, info_.state_interfaces, states_); + return impl_->get_state(state, interfaces); } return_type Sensor::get_state(std::vector & state) const { - return get_internal_values(state, states_); + return impl_->get_state(state); } return_type Sensor::set_state( const std::vector & state, const std::vector & interfaces) { - return set_internal_values(state, interfaces, info_.state_interfaces, states_); + return impl_->set_state(state, interfaces); } return_type Sensor::set_state(const std::vector & state) { - return set_internal_values(state, states_); + return impl_->set_state(state); } } // namespace components diff --git a/hardware_interface/test/test_component_interfaces.cpp b/hardware_interface/test/test_component_interfaces.cpp index 0526be9bf0..ae5aa8b5ec 100644 --- a/hardware_interface/test/test_component_interfaces.cpp +++ b/hardware_interface/test/test_component_interfaces.cpp @@ -40,86 +40,119 @@ namespace hardware_interface namespace hardware_interfaces_components_test { -class DummyPositionJoint : public components::Joint +class DummyPositionJoint : public components::JointInterface { public: return_type configure(const components::ComponentInfo & joint_info) { - if (Joint::configure(joint_info) != return_type::OK) { - return return_type::ERROR; - } + max_position_ = stod(joint_info.parameters.at("max_position")); + min_position_ = stod(joint_info.parameters.at("min_position")); + return return_type::OK; + } - if (info_.command_interfaces.size() > 1 || info_.state_interfaces.size() > 1) { - return return_type::ERROR; - } - if (info_.command_interfaces.size() == 0) { - info_.command_interfaces.push_back(HW_IF_POSITION); - commands_.resize(1); - } - if (info_.state_interfaces.size() == 0) { - info_.state_interfaces.push_back(HW_IF_POSITION); - states_.resize(1); - } + std::vector get_command_interfaces() const + { + return {HW_IF_POSITION}; + } - max_position_ = stod(info_.parameters["max_position"]); - min_position_ = stod(info_.parameters["min_position"]); + std::vector get_state_interfaces() const + { + return {HW_IF_POSITION}; + } + + return_type get_command( + std::vector & command, + const std::vector & interfaces) const + { + if (command.size() != 1u) {return return_type::ERROR;} + if (interfaces.size() != 1u) {return return_type::ERROR;} + if (interfaces[0] != HW_IF_POSITION) {return return_type::ERROR;} + + command[0] = command_; return return_type::OK; } -private: - double max_position_, min_position_; -}; + return_type get_command(std::vector & command) const + { + if (command.size() != 1u) {return return_type::ERROR;} -class DummyMultiJoint : public components::Joint -{ -public: - return_type configure(const components::ComponentInfo & joint_info) + command[0] = command_; + return return_type::OK; + } + + return_type set_command( + const std::vector & command, + const std::vector & interfaces) { - if (Joint::configure(joint_info) != return_type::OK) { - return return_type::ERROR; - } + if (command.size() != 1u) {return return_type::ERROR;} + if (interfaces.size() != 1u) {return return_type::ERROR;} + if (interfaces[0] != HW_IF_POSITION) {return return_type::ERROR;} - if (info_.command_interfaces.size() < 2) { - return return_type::ERROR; - } + command_ = command[0]; + return return_type::OK; + } - max_position_ = stod(info_.parameters["max_position"]); - min_position_ = stod(info_.parameters["min_position"]); - max_velocity_ = stod(info_.parameters["max_velocity"]); - min_velocity_ = stod(info_.parameters["min_velocity"]); + return_type set_command(const std::vector & command) + { + if (command.size() != 1u) {return return_type::ERROR;} + + command_ = command[0]; return return_type::OK; } -private: - double max_position_, min_position_; - double max_velocity_, min_velocity_; -}; + return_type get_state( + std::vector & state, + const std::vector & interfaces) const + { + if (state.size() != 1u) {return return_type::ERROR;} + if (interfaces.size() != 1u) {return return_type::ERROR;} + if (interfaces[0] != HW_IF_POSITION) {return return_type::ERROR;} -class DummyForceTorqueSensor : public components::Sensor -{ -public: - return_type configure(const components::ComponentInfo & sensor_info) + state[0] = state_; + return return_type::OK; + } + + return_type get_state(std::vector & state) const { - if (Sensor::configure(sensor_info) != return_type::OK) { - return return_type::ERROR; - } + if (state.size() != 1u) {return return_type::ERROR;} - if (info_.parameters["frame_id"] == "") { - return return_type::ERROR; - } - if (info_.state_interfaces.size() == 0) { - info_.state_interfaces.push_back("force_x"); - info_.state_interfaces.push_back("force_y"); - info_.state_interfaces.push_back("force_z"); - info_.state_interfaces.push_back("torque_x"); - info_.state_interfaces.push_back("torque_y"); - info_.state_interfaces.push_back("torque_z"); - } - states_ = {1.34, 5.67, 8.21, 5.63, 5.99, 4.32}; + state[0] = state_; return return_type::OK; } + + return_type set_state( + const std::vector & state, + const std::vector & interfaces) + { + if (state.size() != 1u) {return return_type::ERROR;} + if (interfaces.size() != 1u) {return return_type::ERROR;} + if (interfaces[0] != HW_IF_POSITION) {return return_type::ERROR;} + + state_ = state[0]; + return return_type::OK; + } + + return_type set_state(const std::vector & state) + { + if (state.size() != 1u) {return return_type::ERROR;} + + state_ = state[0]; + return return_type::OK; + } + +private: + double command_ = 0.0; + double state_ = 0.0; + + double max_position_, min_position_; }; +} // namespace hardware_interfaces_components_test +} // namespace hardware_interface + +// TODO(karsten1987): Transfer this test to the controller manager test resource plugin + +/* class DummyActuatorHardware : public ActuatorHardwareInterface { return_type configure(const HardwareInfo & actuator_info) override @@ -325,9 +358,6 @@ class DummySystemHardware : public SystemHardwareInterface std::vector joints_hw_values_ = {-1.575, -0.7543}; }; -} // namespace hardware_interfaces_components_test -} // namespace hardware_interface - using hardware_interface::components::ComponentInfo; using hardware_interface::components::Joint; using hardware_interface::components::Sensor; @@ -341,7 +371,6 @@ using hardware_interface::SensorHardwareInterface; using hardware_interface::SystemHardware; using hardware_interface::SystemHardwareInterface; using hardware_interface::hardware_interfaces_components_test::DummyForceTorqueSensor; -using hardware_interface::hardware_interfaces_components_test::DummyMultiJoint; using hardware_interface::hardware_interfaces_components_test::DummyPositionJoint; using hardware_interface::hardware_interfaces_components_test::DummyActuatorHardware; @@ -451,155 +480,6 @@ TEST_F(TestComponentInterfaces, joint_example_component_works) EXPECT_EQ(joint.configure(joint_info), return_type::ERROR); } -TEST_F(TestComponentInterfaces, multi_joint_example_component_works) -{ - DummyMultiJoint joint; - - joint_info.name = "DummyMultiJoint"; - joint_info.parameters["max_position"] = "3.14"; - joint_info.parameters["min_position"] = "-3.14"; - joint_info.parameters["max_velocity"] = "1.14"; - joint_info.parameters["min_velocity"] = "-1.14"; - - EXPECT_EQ(joint.configure(joint_info), return_type::ERROR); - - joint_info.command_interfaces.push_back(hardware_interface::HW_IF_POSITION); - joint_info.command_interfaces.push_back(hardware_interface::HW_IF_VELOCITY); - - EXPECT_EQ(joint.configure(joint_info), return_type::OK); - - ASSERT_THAT(joint.get_command_interfaces(), SizeIs(2)); - EXPECT_EQ(joint.get_command_interfaces()[0], hardware_interface::HW_IF_POSITION); - ASSERT_THAT(joint.get_state_interfaces(), SizeIs(0)); - - joint_info.state_interfaces.push_back(hardware_interface::HW_IF_POSITION); - joint_info.state_interfaces.push_back(hardware_interface::HW_IF_VELOCITY); - EXPECT_EQ(joint.configure(joint_info), return_type::OK); - ASSERT_THAT(joint.get_state_interfaces(), SizeIs(2)); - EXPECT_EQ(joint.get_command_interfaces()[1], hardware_interface::HW_IF_VELOCITY); - - // Command getters and setters - std::vector interfaces; - std::vector input; - input.push_back(2.1); - EXPECT_EQ(joint.set_command(input, interfaces), return_type::INTERFACE_NOT_PROVIDED); - interfaces.push_back(hardware_interface::HW_IF_EFFORT); - EXPECT_EQ(joint.set_command(input, interfaces), return_type::INTERFACE_NOT_FOUND); - interfaces.push_back(hardware_interface::HW_IF_VELOCITY); - EXPECT_EQ(joint.set_command(input, interfaces), return_type::INTERFACE_VALUE_SIZE_NOT_EQUAL); - interfaces.clear(); - interfaces.push_back(hardware_interface::HW_IF_VELOCITY); - input.clear(); - input.push_back(1.02); - EXPECT_EQ(joint.set_command(input, interfaces), return_type::OK); - - std::vector output; - EXPECT_EQ(joint.get_command(output, interfaces), return_type::OK); - ASSERT_THAT(output, SizeIs(1)); - EXPECT_EQ(output[0], 1.02); - interfaces.clear(); - interfaces.push_back(hardware_interface::HW_IF_EFFORT); - EXPECT_EQ(joint.get_command(output, interfaces), return_type::INTERFACE_NOT_FOUND); - - input.clear(); - EXPECT_EQ(joint.set_command(input), return_type::INTERFACE_VALUE_SIZE_NOT_EQUAL); - input.push_back(5.77); - EXPECT_EQ(joint.set_command(input), return_type::INTERFACE_VALUE_SIZE_NOT_EQUAL); - input.clear(); - input.push_back(1.2); - input.push_back(0.4); - EXPECT_EQ(joint.set_command(input), return_type::OK); - - EXPECT_EQ(joint.get_command(output), return_type::OK); - ASSERT_THAT(output, SizeIs(2)); - EXPECT_EQ(output[1], 0.4); - - // State getters and setters - interfaces.clear(); - input.clear(); - input.push_back(2.1); - EXPECT_EQ(joint.set_state(input, interfaces), return_type::INTERFACE_NOT_PROVIDED); - interfaces.push_back(hardware_interface::HW_IF_EFFORT); - EXPECT_EQ(joint.set_state(input, interfaces), return_type::INTERFACE_NOT_FOUND); - interfaces.push_back(hardware_interface::HW_IF_POSITION); - EXPECT_EQ(joint.set_state(input, interfaces), return_type::INTERFACE_VALUE_SIZE_NOT_EQUAL); - interfaces.clear(); - interfaces.push_back(hardware_interface::HW_IF_POSITION); - input.clear(); - input.push_back(1.2); - EXPECT_EQ(joint.set_state(input, interfaces), return_type::OK); - - output.clear(); - EXPECT_EQ(joint.get_state(output, interfaces), return_type::OK); - ASSERT_THAT(output, SizeIs(1)); - EXPECT_EQ(output[0], 1.2); - interfaces.clear(); - interfaces.push_back(hardware_interface::HW_IF_EFFORT); - EXPECT_EQ(joint.get_state(output, interfaces), return_type::INTERFACE_NOT_FOUND); - - input.clear(); - EXPECT_EQ(joint.set_state(input), return_type::INTERFACE_VALUE_SIZE_NOT_EQUAL); - input.push_back(2.1); - input.push_back(1.02); - EXPECT_EQ(joint.set_state(input), return_type::OK); - - EXPECT_EQ(joint.get_state(output), return_type::OK); - ASSERT_THAT(output, SizeIs(2)); - EXPECT_EQ(output[0], 2.1); -} - -TEST_F(TestComponentInterfaces, sensor_example_component_works) -{ - DummyForceTorqueSensor sensor; - - EXPECT_EQ(sensor.configure(sensor_info), return_type::OK); - ASSERT_THAT(sensor.get_state_interfaces(), SizeIs(6)); - EXPECT_EQ(sensor.get_state_interfaces()[0], "force_x"); - EXPECT_EQ(sensor.get_state_interfaces()[5], "torque_z"); - std::vector input = {5.23, 6.7, 2.5, 3.8, 8.9, 12.3}; - std::vector output; - std::vector interfaces; - EXPECT_EQ(sensor.get_state(output, interfaces), return_type::INTERFACE_NOT_PROVIDED); - interfaces.push_back("force_y"); - EXPECT_EQ(sensor.get_state(output, interfaces), return_type::OK); - ASSERT_THAT(output, SizeIs(1)); - EXPECT_EQ(output[0], 5.67); - - // State getters and setters - interfaces.clear(); - EXPECT_EQ(sensor.set_state(input, interfaces), return_type::INTERFACE_NOT_PROVIDED); - interfaces.push_back(hardware_interface::HW_IF_VELOCITY); - interfaces.push_back(hardware_interface::HW_IF_VELOCITY); - interfaces.push_back(hardware_interface::HW_IF_VELOCITY); - EXPECT_EQ(sensor.set_state(input, interfaces), return_type::INTERFACE_VALUE_SIZE_NOT_EQUAL); - interfaces.push_back(hardware_interface::HW_IF_VELOCITY); - interfaces.push_back(hardware_interface::HW_IF_VELOCITY); - interfaces.push_back(hardware_interface::HW_IF_VELOCITY); - EXPECT_EQ(sensor.set_state(input, interfaces), return_type::INTERFACE_NOT_FOUND); - interfaces.clear(); - interfaces = sensor.get_state_interfaces(); - EXPECT_EQ(sensor.set_state(input, interfaces), return_type::OK); - - output.clear(); - EXPECT_EQ(sensor.get_state(output, interfaces), return_type::OK); - ASSERT_THAT(output, SizeIs(6)); - EXPECT_EQ(output[0], 5.23); - interfaces.clear(); - interfaces.push_back(hardware_interface::HW_IF_VELOCITY); - EXPECT_EQ(sensor.get_state(output, interfaces), return_type::INTERFACE_NOT_FOUND); - - input.clear(); - EXPECT_EQ(sensor.set_state(input), return_type::INTERFACE_VALUE_SIZE_NOT_EQUAL); - input = {5.23, 6.7, 2.5, 3.8, 8.9, 12.3}; - EXPECT_EQ(sensor.set_state(input), return_type::OK); - - EXPECT_EQ(sensor.get_state(output), return_type::OK); - ASSERT_THAT(output, SizeIs(6)); - EXPECT_EQ(output[5], 12.3); - - sensor_info.parameters.clear(); - EXPECT_EQ(sensor.configure(sensor_info), return_type::ERROR); -} TEST_F(TestComponentInterfaces, actuator_hardware_interface_works) { @@ -718,3 +598,4 @@ TEST_F(TestComponentInterfaces, system_interface_with_hardware_works) EXPECT_EQ(system.stop(), return_type::OK); EXPECT_EQ(system.get_status(), status::STOPPED); } +*/ diff --git a/multi_interface_joint/CMakeLists.txt b/multi_interface_joint/CMakeLists.txt new file mode 100644 index 0000000000..7ba5c4f6a6 --- /dev/null +++ b/multi_interface_joint/CMakeLists.txt @@ -0,0 +1,68 @@ +cmake_minimum_required(VERSION 3.5) +project(multi_interface_joint) + +# Default to C99 +if(NOT CMAKE_C_STANDARD) + set(CMAKE_C_STANDARD 99) +endif() + +# Default to C++14 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) +endif() + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +find_package(ament_cmake_ros REQUIRED) +find_package(hardware_interface REQUIRED) + +add_library(multi_interface_joint src/multi_interface_joint.cpp) +target_include_directories(multi_interface_joint PUBLIC + $ + $) +ament_target_dependencies( + multi_interface_joint + "hardware_interface" +) + +# Causes the visibility macros to use dllexport rather than dllimport, +# which is appropriate when building the dll but not consuming it. +target_compile_definitions(multi_interface_joint PRIVATE "MULTI_INTERFACE_JOINT_BUILDING_LIBRARY") + +install( + DIRECTORY include/ + DESTINATION include +) +install( + TARGETS multi_interface_joint + EXPORT export_${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin +) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() + + ament_add_gmock(test_multi_interface_joint + test/test_multi_interface_joint.cpp + ) + target_link_libraries(test_multi_interface_joint multi_interface_joint) +endif() + +ament_export_include_directories( + include +) +ament_export_libraries( + multi_interface_joint +) +ament_export_targets( + export_${PROJECT_NAME} +) + +ament_package() diff --git a/multi_interface_joint/include/multi_interface_joint/multi_interface_joint.hpp b/multi_interface_joint/include/multi_interface_joint/multi_interface_joint.hpp new file mode 100644 index 0000000000..d8c10798da --- /dev/null +++ b/multi_interface_joint/include/multi_interface_joint/multi_interface_joint.hpp @@ -0,0 +1,88 @@ +// Copyright 2020 ros2_control Development Team +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef MULTI_INTERFACE_JOINT__MULTI_INTERFACE_JOINT_HPP_ +#define MULTI_INTERFACE_JOINT__MULTI_INTERFACE_JOINT_HPP_ + +#include +#include + +#include "multi_interface_joint/visibility_control.h" + +#include "hardware_interface/components/joint_interface.hpp" + +namespace multi_interface_joint +{ + +class MultiInterfaceJoint : public hardware_interface::components::JointInterface +{ +public: + MultiInterfaceJoint() = default; + + virtual ~MultiInterfaceJoint() = default; + + MULTI_INTERFACE_JOINT_PUBLIC + hardware_interface::return_type configure( + const hardware_interface::components::ComponentInfo & joint_info) override; + + MULTI_INTERFACE_JOINT_PUBLIC + std::vector get_command_interfaces() const override; + + MULTI_INTERFACE_JOINT_PUBLIC + std::vector get_state_interfaces() const override; + + MULTI_INTERFACE_JOINT_PUBLIC + hardware_interface::return_type get_command( + std::vector & command, + const std::vector & interfaces) const override; + + MULTI_INTERFACE_JOINT_PUBLIC + hardware_interface::return_type get_command( + std::vector & command) const override; + + MULTI_INTERFACE_JOINT_PUBLIC + hardware_interface::return_type set_command( + const std::vector & command, + const std::vector & interfaces) override; + + MULTI_INTERFACE_JOINT_PUBLIC + hardware_interface::return_type set_command(const std::vector & command) override; + + MULTI_INTERFACE_JOINT_PUBLIC + hardware_interface::return_type get_state( + std::vector & state, + const std::vector & interfaces) const override; + + MULTI_INTERFACE_JOINT_PUBLIC + hardware_interface::return_type get_state(std::vector & state) const override; + + MULTI_INTERFACE_JOINT_PUBLIC + hardware_interface::return_type set_state( + const std::vector & state, + const std::vector & interfaces) override; + + MULTI_INTERFACE_JOINT_PUBLIC + hardware_interface::return_type set_state(const std::vector & state) override; + +protected: + std::vector command_interfaces_; + std::vector command_values_; + + std::vector state_interfaces_; + std::vector state_values_; +}; + +} // namespace multi_interface_joint + +#endif // MULTI_INTERFACE_JOINT__MULTI_INTERFACE_JOINT_HPP_ diff --git a/multi_interface_joint/include/multi_interface_joint/visibility_control.h b/multi_interface_joint/include/multi_interface_joint/visibility_control.h new file mode 100644 index 0000000000..22b1151d0b --- /dev/null +++ b/multi_interface_joint/include/multi_interface_joint/visibility_control.h @@ -0,0 +1,49 @@ +// Copyright 2020 ros2_control Development Team +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef MULTI_INTERFACE_JOINT__VISIBILITY_CONTROL_H_ +#define MULTI_INTERFACE_JOINT__VISIBILITY_CONTROL_H_ + +// This logic was borrowed (then namespaced) from the examples on the gcc wiki: +// https://gcc.gnu.org/wiki/Visibility + +#if defined _WIN32 || defined __CYGWIN__ + #ifdef __GNUC__ + #define MULTI_INTERFACE_JOINT_EXPORT __attribute__ ((dllexport)) + #define MULTI_INTERFACE_JOINT_IMPORT __attribute__ ((dllimport)) + #else + #define MULTI_INTERFACE_JOINT_EXPORT __declspec(dllexport) + #define MULTI_INTERFACE_JOINT_IMPORT __declspec(dllimport) + #endif + #ifdef MULTI_INTERFACE_JOINT_BUILDING_LIBRARY + #define MULTI_INTERFACE_JOINT_PUBLIC MULTI_INTERFACE_JOINT_EXPORT + #else + #define MULTI_INTERFACE_JOINT_PUBLIC MULTI_INTERFACE_JOINT_IMPORT + #endif + #define MULTI_INTERFACE_JOINT_PUBLIC_TYPE MULTI_INTERFACE_JOINT_PUBLIC + #define MULTI_INTERFACE_JOINT_LOCAL +#else + #define MULTI_INTERFACE_JOINT_EXPORT __attribute__ ((visibility("default"))) + #define MULTI_INTERFACE_JOINT_IMPORT + #if __GNUC__ >= 4 + #define MULTI_INTERFACE_JOINT_PUBLIC __attribute__ ((visibility("default"))) + #define MULTI_INTERFACE_JOINT_LOCAL __attribute__ ((visibility("hidden"))) + #else + #define MULTI_INTERFACE_JOINT_PUBLIC + #define MULTI_INTERFACE_JOINT_LOCAL + #endif + #define MULTI_INTERFACE_JOINT_PUBLIC_TYPE +#endif + +#endif // MULTI_INTERFACE_JOINT__VISIBILITY_CONTROL_H_ diff --git a/multi_interface_joint/package.xml b/multi_interface_joint/package.xml new file mode 100644 index 0000000000..1be09da2c6 --- /dev/null +++ b/multi_interface_joint/package.xml @@ -0,0 +1,20 @@ + + + + multi_interface_joint + 0.0.0 + TODO: Package description + karsten + Apache license 2.0 + + ament_cmake_ros + + hardware_interface + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/multi_interface_joint/src/component_lists_management.hpp b/multi_interface_joint/src/component_lists_management.hpp new file mode 100644 index 0000000000..0865bdd15f --- /dev/null +++ b/multi_interface_joint/src/component_lists_management.hpp @@ -0,0 +1,140 @@ +// Copyright 2020 ros2_control Development Team +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef COMPONENT_LISTS_MANAGEMENT_HPP_ +#define COMPONENT_LISTS_MANAGEMENT_HPP_ + +#include +#include +#include + +#include "hardware_interface/types/hardware_interface_return_values.hpp" + +namespace multi_interface_joint +{ + +using hardware_interface::return_type; + +/** + * \brief Get values for queried_interfaces from the int_values. int_values data structure matches + * int_interfaces vector. + * + * \param values values to return. + * \param queried_interfaces interfaces for which values are queried. + * \param int_interfaces full list of interfaces of a component. + * \param int_values internal values of a component. + * \return return_type::INTERFACE_VALUE_SIZE_NOT_EQUAL if values and queried_interfaces arguments + * do not have the same length; return_type::INTERFACE_NOT_FOUND if one of queried_interfaces is + * not defined in int_interfaces; return return_type::INTERFACE_NOT_PROVIDED if queried_interfaces + * list is is empty; return_type::OK otherwise. + */ +inline return_type get_internal_values( + std::vector & values, const std::vector & queried_interfaces, + const std::vector & int_interfaces, const std::vector & int_values) +{ + if (queried_interfaces.size() == 0) { + return return_type::INTERFACE_NOT_PROVIDED; + } + + for (const auto & interface : queried_interfaces) { + auto it = std::find( + int_interfaces.begin(), int_interfaces.end(), interface); + if (it != int_interfaces.end()) { + values.push_back(int_values[std::distance(int_interfaces.begin(), it)]); + } else { + values.clear(); + return return_type::INTERFACE_NOT_FOUND; + } + } + return return_type::OK; +} + +/** + * \brief Set all internal values to to other vector. Return value is used for API consistency. + * + * \param values output list of values. + * \param int_values internal values of the component. + * \return return_type::OK always. + */ +inline return_type get_internal_values( + std::vector & values, const std::vector & int_values) +{ + values.clear(); + for (const auto & int_value : int_values) { + values.push_back(int_value); + } + return return_type::OK; +} + +/** + * \brief set values for queried_interfaces to the int_values. int_values data structure matches + * int_interfaces vector. + * + * \param values values to set. + * \param queried_interfaces interfaces for which values are queried. + * \param int_interfaces full list of interfaces of a component. + * \param int_values internal values of a component. + * \return return return_type::INTERFACE_NOT_PROVIDED if + * queried_interfaces list is is empty; return_type::INTERFACE_VALUE_SIZE_NOT_EQUAL if values and + * queried_interfaces arguments do not have the same length; return_type::INTERFACE_NOT_FOUND if + * one of queried_interfaces is not defined in int_interfaces; return_type::OK otherwise. + * + * \todo The error handling in this function could lead to incosistant command or state variables + * for different interfaces. This should be changed in the future. + * (see: https://github.com/ros-controls/ros2_control/issues/129) + */ +inline return_type set_internal_values( + const std::vector & values, const std::vector & queried_interfaces, + const std::vector & int_interfaces, std::vector & int_values) +{ + if (queried_interfaces.size() == 0) { + return return_type::INTERFACE_NOT_PROVIDED; + } + if (values.size() != queried_interfaces.size()) { + return return_type::INTERFACE_VALUE_SIZE_NOT_EQUAL; + } + + for (auto q_it = queried_interfaces.begin(); q_it != queried_interfaces.end(); ++q_it) { + auto it = std::find(int_interfaces.begin(), int_interfaces.end(), *q_it); + if (it != int_interfaces.end()) { + int_values[std::distance(int_interfaces.begin(), it)] = + values[std::distance(queried_interfaces.begin(), q_it)]; + } else { + return return_type::INTERFACE_NOT_FOUND; + } + } + return return_type::OK; +} + +/** + * \brief set all values to compoenents internal values. + * + * \param values values to set. + * \param int_values internal values of a component. + * \return return_type::INTERFACE_VALUE_SIZE_NOT_EQUAL if the size of the arguments is not equal, + * return_type::OK otherwise. + */ +inline return_type set_internal_values( + const std::vector & values, std::vector & int_values) +{ + if (values.size() == int_values.size()) { + int_values = values; + } else { + return return_type::INTERFACE_VALUE_SIZE_NOT_EQUAL; + } + return return_type::OK; +} + +} // namespace multi_interface_joint +#endif // COMPONENT_LISTS_MANAGEMENT_HPP_ diff --git a/multi_interface_joint/src/multi_interface_joint.cpp b/multi_interface_joint/src/multi_interface_joint.cpp new file mode 100644 index 0000000000..f838ea59b0 --- /dev/null +++ b/multi_interface_joint/src/multi_interface_joint.cpp @@ -0,0 +1,122 @@ +// Copyright 2020 ros2_control Development Team +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "multi_interface_joint/multi_interface_joint.hpp" + +#include +#include +#include + +#include "./component_lists_management.hpp" + +namespace multi_interface_joint +{ + +hardware_interface::return_type MultiInterfaceJoint::configure( + const hardware_interface::components::ComponentInfo & joint_info) +{ + const size_t command_interfaces_size = joint_info.command_interfaces.size(); + const size_t state_interfaces_size = joint_info.state_interfaces.size(); + + // fail if no interfaces at all are specified + if (command_interfaces_size == state_interfaces_size && command_interfaces_size == 0u) { + return hardware_interface::return_type::INTERFACE_NOT_PROVIDED; + } + + auto has_duplicates = [](const std::vector interfaces) -> bool + { + std::set set(interfaces.begin(), interfaces.end()); + return set.size() != interfaces.size(); + }; + + // fail if command interfaces has duplicates + if (has_duplicates(joint_info.command_interfaces)) { + return hardware_interface::return_type::INTERFACE_DUPLICATES; + } + + // fail if state interfaces has duplicates + if (has_duplicates(joint_info.state_interfaces)) { + return hardware_interface::return_type::INTERFACE_DUPLICATES; + } + + command_interfaces_ = joint_info.command_interfaces; + command_values_.resize(command_interfaces_size); + + state_interfaces_ = joint_info.state_interfaces; + state_values_.resize(state_interfaces_size); + + return hardware_interface::return_type::OK; +} + +std::vector MultiInterfaceJoint::get_command_interfaces() const +{ + return command_interfaces_; +} + +std::vector MultiInterfaceJoint::get_state_interfaces() const +{ + return state_interfaces_; +} + +hardware_interface::return_type MultiInterfaceJoint::get_command( + std::vector & command, + const std::vector & interfaces) const +{ + return get_internal_values(command, interfaces, command_interfaces_, command_values_); +} + +hardware_interface::return_type MultiInterfaceJoint::get_command( + std::vector & command) const +{ + return get_internal_values(command, command_values_); +} + +hardware_interface::return_type MultiInterfaceJoint::set_command( + const std::vector & command, + const std::vector & interfaces) +{ + return set_internal_values(command, interfaces, command_interfaces_, command_values_); +} + +hardware_interface::return_type MultiInterfaceJoint::set_command( + const std::vector & command) +{ + return set_internal_values(command, command_values_); +} + +hardware_interface::return_type MultiInterfaceJoint::get_state( + std::vector & state, + const std::vector & interfaces) const +{ + return get_internal_values(state, interfaces, state_interfaces_, state_values_); +} + +hardware_interface::return_type MultiInterfaceJoint::get_state(std::vector & state) const +{ + return get_internal_values(state, state_values_); +} + +hardware_interface::return_type MultiInterfaceJoint::set_state( + const std::vector & state, + const std::vector & interfaces) +{ + return set_internal_values(state, interfaces, state_interfaces_, state_values_); +} + +hardware_interface::return_type MultiInterfaceJoint::set_state(const std::vector & state) +{ + return set_internal_values(state, state_values_); +} + +} // namespace multi_interface_joint diff --git a/multi_interface_joint/test/test_multi_interface_joint.cpp b/multi_interface_joint/test/test_multi_interface_joint.cpp new file mode 100644 index 0000000000..fced3b0042 --- /dev/null +++ b/multi_interface_joint/test/test_multi_interface_joint.cpp @@ -0,0 +1,202 @@ +// Copyright 2020 ros2_control Development Team +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include + +#include "multi_interface_joint/multi_interface_joint.hpp" + +#include "hardware_interface/components/component_info.hpp" +#include "hardware_interface/types/hardware_interface_type_values.hpp" + +using namespace ::testing; // NOLINT + +class TestMultiInterfaceJoint : public Test +{ +}; + +TEST_F(TestMultiInterfaceJoint, empty_initialized) +{ + multi_interface_joint::MultiInterfaceJoint joint; + EXPECT_TRUE(joint.get_command_interfaces().empty()); + EXPECT_TRUE(joint.get_state_interfaces().empty()); +} + +TEST_F(TestMultiInterfaceJoint, wrong_initialized) +{ + { + multi_interface_joint::MultiInterfaceJoint joint; + hardware_interface::components::ComponentInfo joint_info; + EXPECT_EQ(hardware_interface::return_type::INTERFACE_NOT_PROVIDED, joint.configure(joint_info)); + } + + { + hardware_interface::components::ComponentInfo joint_info; + joint_info.command_interfaces.push_back("position"); + joint_info.command_interfaces.push_back("position"); + multi_interface_joint::MultiInterfaceJoint joint; + EXPECT_EQ(hardware_interface::return_type::INTERFACE_DUPLICATES, joint.configure(joint_info)); + } + + { + hardware_interface::components::ComponentInfo joint_info; + joint_info.state_interfaces.push_back("position"); + joint_info.state_interfaces.push_back("position"); + multi_interface_joint::MultiInterfaceJoint joint; + EXPECT_EQ(hardware_interface::return_type::INTERFACE_DUPLICATES, joint.configure(joint_info)); + } +} + +TEST_F(TestMultiInterfaceJoint, correct_initialized) +{ + { + hardware_interface::components::ComponentInfo joint_info; + joint_info.command_interfaces.push_back("position"); + joint_info.command_interfaces.push_back("velocity"); + joint_info.state_interfaces.push_back("effort"); + multi_interface_joint::MultiInterfaceJoint joint; + EXPECT_EQ(hardware_interface::return_type::OK, joint.configure(joint_info)); + ASSERT_EQ(2u, joint.get_command_interfaces().size()); + ASSERT_EQ(1u, joint.get_state_interfaces().size()); + EXPECT_EQ("position", joint.get_command_interfaces()[0]); + EXPECT_EQ("velocity", joint.get_command_interfaces()[1]); + EXPECT_EQ("effort", joint.get_state_interfaces()[0]); + } +} + +TEST_F(TestMultiInterfaceJoint, getters_and_setters) +{ + multi_interface_joint::MultiInterfaceJoint joint; + + hardware_interface::components::ComponentInfo joint_info; + joint_info.command_interfaces.push_back(hardware_interface::HW_IF_POSITION); + joint_info.command_interfaces.push_back(hardware_interface::HW_IF_VELOCITY); + joint_info.state_interfaces.push_back(hardware_interface::HW_IF_POSITION); + joint_info.state_interfaces.push_back(hardware_interface::HW_IF_VELOCITY); + ASSERT_EQ( + hardware_interface::return_type::OK, joint.configure(joint_info)); + + // Command getters and setters + std::vector interfaces; + std::vector input; + input.push_back(2.1); + EXPECT_EQ( + hardware_interface::return_type::INTERFACE_NOT_PROVIDED, + joint.set_command(input, interfaces)); + + interfaces.push_back(hardware_interface::HW_IF_EFFORT); + EXPECT_EQ( + hardware_interface::return_type::INTERFACE_NOT_FOUND, + joint.set_command(input, interfaces)); + + interfaces.push_back(hardware_interface::HW_IF_VELOCITY); + EXPECT_EQ( + hardware_interface::return_type::INTERFACE_VALUE_SIZE_NOT_EQUAL, + joint.set_command(input, interfaces)); + + interfaces.clear(); + input.clear(); + interfaces.push_back(hardware_interface::HW_IF_VELOCITY); + input.push_back(1.02); + EXPECT_EQ( + hardware_interface::return_type::OK, + joint.set_command(input, interfaces)); + + std::vector output; + EXPECT_EQ( + hardware_interface::return_type::OK, + joint.get_command(output, interfaces)); + ASSERT_EQ(1u, output.size()); + EXPECT_EQ(1.02, output[0]); + + interfaces.clear(); + interfaces.push_back(hardware_interface::HW_IF_EFFORT); + EXPECT_EQ( + hardware_interface::return_type::INTERFACE_NOT_FOUND, + joint.get_command(output, interfaces)); + + input.clear(); + EXPECT_EQ( + hardware_interface::return_type::INTERFACE_VALUE_SIZE_NOT_EQUAL, + joint.set_command(input)); + input.push_back(5.77); + EXPECT_EQ( + hardware_interface::return_type::INTERFACE_VALUE_SIZE_NOT_EQUAL, + joint.set_command(input)); + + input.clear(); + input.push_back(1.2); + input.push_back(0.4); + EXPECT_EQ( + hardware_interface::return_type::OK, + joint.set_command(input)); + EXPECT_EQ( + hardware_interface::return_type::OK, + joint.get_command(output)); + ASSERT_EQ(2u, output.size()); + EXPECT_EQ(0.4, output[1]); + + // State getters and setters + interfaces.clear(); + input.clear(); + input.push_back(2.1); + EXPECT_EQ( + hardware_interface::return_type::INTERFACE_NOT_PROVIDED, + joint.set_state(input, interfaces)); + interfaces.push_back(hardware_interface::HW_IF_EFFORT); + EXPECT_EQ( + hardware_interface::return_type::INTERFACE_NOT_FOUND, + joint.set_state(input, interfaces)); + interfaces.push_back(hardware_interface::HW_IF_POSITION); + EXPECT_EQ( + hardware_interface::return_type::INTERFACE_VALUE_SIZE_NOT_EQUAL, + joint.set_state(input, interfaces)); + + interfaces.clear(); + interfaces.push_back(hardware_interface::HW_IF_POSITION); + input.clear(); + input.push_back(1.2); + EXPECT_EQ( + hardware_interface::return_type::OK, + joint.set_state(input, interfaces)); + + output.clear(); + EXPECT_EQ( + hardware_interface::return_type::OK, + joint.get_state(output, interfaces)); + ASSERT_THAT(1u, output.size()); + EXPECT_EQ(1.2, output[0]); + + interfaces.clear(); + interfaces.push_back(hardware_interface::HW_IF_EFFORT); + EXPECT_EQ( + hardware_interface::return_type::INTERFACE_NOT_FOUND, + joint.get_state(output, interfaces)); + + input.clear(); + EXPECT_EQ( + hardware_interface::return_type::INTERFACE_VALUE_SIZE_NOT_EQUAL, + joint.set_state(input)); + input.push_back(2.1); + input.push_back(1.02); + EXPECT_EQ( + hardware_interface::return_type::OK, + joint.set_state(input)); + EXPECT_EQ( + hardware_interface::return_type::OK, + joint.get_state(output)); + ASSERT_EQ(2u, output.size()); + EXPECT_EQ(2.1, output[0]); +} diff --git a/multi_interface_sensor/CMakeLists.txt b/multi_interface_sensor/CMakeLists.txt new file mode 100644 index 0000000000..8810e8c100 --- /dev/null +++ b/multi_interface_sensor/CMakeLists.txt @@ -0,0 +1,68 @@ +cmake_minimum_required(VERSION 3.5) +project(multi_interface_sensor) + +# Default to C99 +if(NOT CMAKE_C_STANDARD) + set(CMAKE_C_STANDARD 99) +endif() + +# Default to C++14 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) +endif() + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +find_package(ament_cmake_ros REQUIRED) +find_package(hardware_interface REQUIRED) + +add_library(multi_interface_sensor src/multi_interface_sensor.cpp) +target_include_directories(multi_interface_sensor PUBLIC + $ + $) +ament_target_dependencies( + multi_interface_sensor + "hardware_interface" +) + +# Causes the visibility macros to use dllexport rather than dllimport, +# which is appropriate when building the dll but not consuming it. +target_compile_definitions(multi_interface_sensor PRIVATE "MULTI_INTERFACE_SENSOR_BUILDING_LIBRARY") + +install( + DIRECTORY include/ + DESTINATION include +) +install( + TARGETS multi_interface_sensor + EXPORT export_${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin +) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() + + ament_add_gmock(test_multi_interface_sensor + test/test_multi_interface_sensor + ) + target_link_libraries(test_multi_interface_sensor multi_interface_sensor) +endif() + +ament_export_include_directories( + include +) +ament_export_libraries( + multi_interface_sensor +) +ament_export_targets( + export_${PROJECT_NAME} +) + +ament_package() diff --git a/multi_interface_sensor/include/multi_interface_sensor/multi_interface_sensor.hpp b/multi_interface_sensor/include/multi_interface_sensor/multi_interface_sensor.hpp new file mode 100644 index 0000000000..eaf6716b6c --- /dev/null +++ b/multi_interface_sensor/include/multi_interface_sensor/multi_interface_sensor.hpp @@ -0,0 +1,65 @@ +// Copyright 2020 ros2_control Development Team +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef MULTI_INTERFACE_SENSOR__MULTI_INTERFACE_SENSOR_HPP_ +#define MULTI_INTERFACE_SENSOR__MULTI_INTERFACE_SENSOR_HPP_ + +#include +#include + +#include "multi_interface_sensor/visibility_control.h" + +#include "hardware_interface/components/sensor_interface.hpp" + +namespace multi_interface_sensor +{ + +class MultiInterfaceSensor : public hardware_interface::components::SensorInterface +{ +public: + MultiInterfaceSensor() = default; + + virtual ~MultiInterfaceSensor() = default; + + MULTI_INTERFACE_SENSOR_PUBLIC + hardware_interface::return_type configure( + const hardware_interface::components::ComponentInfo & sensor_info) override; + + MULTI_INTERFACE_SENSOR_PUBLIC + std::vector get_state_interfaces() const override; + + MULTI_INTERFACE_SENSOR_PUBLIC + hardware_interface::return_type get_state( + std::vector & state, + const std::vector & interfaces) const override; + + MULTI_INTERFACE_SENSOR_PUBLIC + hardware_interface::return_type get_state(std::vector & state) const override; + + MULTI_INTERFACE_SENSOR_PUBLIC + hardware_interface::return_type set_state( + const std::vector & state, + const std::vector & interfaces) override; + + MULTI_INTERFACE_SENSOR_PUBLIC + hardware_interface::return_type set_state(const std::vector & state) override; + +protected: + std::vector state_interfaces_; + std::vector state_values_; +}; + +} // namespace multi_interface_sensor + +#endif // MULTI_INTERFACE_SENSOR__MULTI_INTERFACE_SENSOR_HPP_ diff --git a/multi_interface_sensor/include/multi_interface_sensor/visibility_control.h b/multi_interface_sensor/include/multi_interface_sensor/visibility_control.h new file mode 100644 index 0000000000..43e489299a --- /dev/null +++ b/multi_interface_sensor/include/multi_interface_sensor/visibility_control.h @@ -0,0 +1,49 @@ +// Copyright 2020 ros2_control Development Team +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef MULTI_INTERFACE_SENSOR__VISIBILITY_CONTROL_H_ +#define MULTI_INTERFACE_SENSOR__VISIBILITY_CONTROL_H_ + +// This logic was borrowed (then namespaced) from the examples on the gcc wiki: +// https://gcc.gnu.org/wiki/Visibility + +#if defined _WIN32 || defined __CYGWIN__ + #ifdef __GNUC__ + #define MULTI_INTERFACE_SENSOR_EXPORT __attribute__ ((dllexport)) + #define MULTI_INTERFACE_SENSOR_IMPORT __attribute__ ((dllimport)) + #else + #define MULTI_INTERFACE_SENSOR_EXPORT __declspec(dllexport) + #define MULTI_INTERFACE_SENSOR_IMPORT __declspec(dllimport) + #endif + #ifdef MULTI_INTERFACE_SENSOR_BUILDING_LIBRARY + #define MULTI_INTERFACE_SENSOR_PUBLIC MULTI_INTERFACE_SENSOR_EXPORT + #else + #define MULTI_INTERFACE_SENSOR_PUBLIC MULTI_INTERFACE_SENSOR_IMPORT + #endif + #define MULTI_INTERFACE_SENSOR_PUBLIC_TYPE MULTI_INTERFACE_SENSOR_PUBLIC + #define MULTI_INTERFACE_SENSOR_LOCAL +#else + #define MULTI_INTERFACE_SENSOR_EXPORT __attribute__ ((visibility("default"))) + #define MULTI_INTERFACE_SENSOR_IMPORT + #if __GNUC__ >= 4 + #define MULTI_INTERFACE_SENSOR_PUBLIC __attribute__ ((visibility("default"))) + #define MULTI_INTERFACE_SENSOR_LOCAL __attribute__ ((visibility("hidden"))) + #else + #define MULTI_INTERFACE_SENSOR_PUBLIC + #define MULTI_INTERFACE_SENSOR_LOCAL + #endif + #define MULTI_INTERFACE_SENSOR_PUBLIC_TYPE +#endif + +#endif // MULTI_INTERFACE_SENSOR__VISIBILITY_CONTROL_H_ diff --git a/multi_interface_sensor/package.xml b/multi_interface_sensor/package.xml new file mode 100644 index 0000000000..2c18cc3c93 --- /dev/null +++ b/multi_interface_sensor/package.xml @@ -0,0 +1,20 @@ + + + + multi_interface_sensor + 0.0.0 + TODO: Package description + karsten + Apache license 2.0 + + ament_cmake_ros + + hardware_interface + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/multi_interface_sensor/src/component_lists_management.hpp b/multi_interface_sensor/src/component_lists_management.hpp new file mode 100644 index 0000000000..b73c8c6b28 --- /dev/null +++ b/multi_interface_sensor/src/component_lists_management.hpp @@ -0,0 +1,140 @@ +// Copyright 2020 ros2_control Development Team +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef COMPONENT_LISTS_MANAGEMENT_HPP_ +#define COMPONENT_LISTS_MANAGEMENT_HPP_ + +#include +#include +#include + +#include "hardware_interface/types/hardware_interface_return_values.hpp" + +namespace multi_interface_sensor +{ + +using hardware_interface::return_type; + +/** + * \brief Get values for queried_interfaces from the int_values. int_values data structure matches + * int_interfaces vector. + * + * \param values values to return. + * \param queried_interfaces interfaces for which values are queried. + * \param int_interfaces full list of interfaces of a component. + * \param int_values internal values of a component. + * \return return_type::INTERFACE_VALUE_SIZE_NOT_EQUAL if values and queried_interfaces arguments + * do not have the same length; return_type::INTERFACE_NOT_FOUND if one of queried_interfaces is + * not defined in int_interfaces; return return_type::INTERFACE_NOT_PROVIDED if queried_interfaces + * list is is empty; return_type::OK otherwise. + */ +inline return_type get_internal_values( + std::vector & values, const std::vector & queried_interfaces, + const std::vector & int_interfaces, const std::vector & int_values) +{ + if (queried_interfaces.size() == 0) { + return return_type::INTERFACE_NOT_PROVIDED; + } + + for (const auto & interface : queried_interfaces) { + auto it = std::find( + int_interfaces.begin(), int_interfaces.end(), interface); + if (it != int_interfaces.end()) { + values.push_back(int_values[std::distance(int_interfaces.begin(), it)]); + } else { + values.clear(); + return return_type::INTERFACE_NOT_FOUND; + } + } + return return_type::OK; +} + +/** + * \brief Set all internal values to to other vector. Return value is used for API consistency. + * + * \param values output list of values. + * \param int_values internal values of the component. + * \return return_type::OK always. + */ +inline return_type get_internal_values( + std::vector & values, const std::vector & int_values) +{ + values.clear(); + for (const auto & int_value : int_values) { + values.push_back(int_value); + } + return return_type::OK; +} + +/** + * \brief set values for queried_interfaces to the int_values. int_values data structure matches + * int_interfaces vector. + * + * \param values values to set. + * \param queried_interfaces interfaces for which values are queried. + * \param int_interfaces full list of interfaces of a component. + * \param int_values internal values of a component. + * \return return return_type::INTERFACE_NOT_PROVIDED if + * queried_interfaces list is is empty; return_type::INTERFACE_VALUE_SIZE_NOT_EQUAL if values and + * queried_interfaces arguments do not have the same length; return_type::INTERFACE_NOT_FOUND if + * one of queried_interfaces is not defined in int_interfaces; return_type::OK otherwise. + * + * \todo The error handling in this function could lead to incosistant command or state variables + * for different interfaces. This should be changed in the future. + * (see: https://github.com/ros-controls/ros2_control/issues/129) + */ +inline return_type set_internal_values( + const std::vector & values, const std::vector & queried_interfaces, + const std::vector & int_interfaces, std::vector & int_values) +{ + if (queried_interfaces.size() == 0) { + return return_type::INTERFACE_NOT_PROVIDED; + } + if (values.size() != queried_interfaces.size()) { + return return_type::INTERFACE_VALUE_SIZE_NOT_EQUAL; + } + + for (auto q_it = queried_interfaces.begin(); q_it != queried_interfaces.end(); ++q_it) { + auto it = std::find(int_interfaces.begin(), int_interfaces.end(), *q_it); + if (it != int_interfaces.end()) { + int_values[std::distance(int_interfaces.begin(), it)] = + values[std::distance(queried_interfaces.begin(), q_it)]; + } else { + return return_type::INTERFACE_NOT_FOUND; + } + } + return return_type::OK; +} + +/** + * \brief set all values to compoenents internal values. + * + * \param values values to set. + * \param int_values internal values of a component. + * \return return_type::INTERFACE_VALUE_SIZE_NOT_EQUAL if the size of the arguments is not equal, + * return_type::OK otherwise. + */ +inline return_type set_internal_values( + const std::vector & values, std::vector & int_values) +{ + if (values.size() == int_values.size()) { + int_values = values; + } else { + return return_type::INTERFACE_VALUE_SIZE_NOT_EQUAL; + } + return return_type::OK; +} + +} // namespace multi_interface_sensor +#endif // COMPONENT_LISTS_MANAGEMENT_HPP_ diff --git a/multi_interface_sensor/src/multi_interface_sensor.cpp b/multi_interface_sensor/src/multi_interface_sensor.cpp new file mode 100644 index 0000000000..591679f9d6 --- /dev/null +++ b/multi_interface_sensor/src/multi_interface_sensor.cpp @@ -0,0 +1,82 @@ +// Copyright 2020 ros2_control Development Team +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "multi_interface_sensor/multi_interface_sensor.hpp" + +#include +#include +#include + +#include "./component_lists_management.hpp" + +namespace multi_interface_sensor +{ + +hardware_interface::return_type MultiInterfaceSensor::configure( + const hardware_interface::components::ComponentInfo & sensor_info) +{ + const size_t state_interfaces_size = sensor_info.state_interfaces.size(); + + // fail if no interfaces at all are specified + if (state_interfaces_size == 0u) { + return hardware_interface::return_type::INTERFACE_NOT_PROVIDED; + } + + auto has_duplicates = [](const std::vector interfaces) -> bool + { + std::set set(interfaces.begin(), interfaces.end()); + return set.size() != interfaces.size(); + }; + + // fail if state interfaces has duplicates + if (has_duplicates(sensor_info.state_interfaces)) { + return hardware_interface::return_type::INTERFACE_DUPLICATES; + } + + state_interfaces_ = sensor_info.state_interfaces; + state_values_.resize(state_interfaces_size); + + return hardware_interface::return_type::OK; +} + +std::vector MultiInterfaceSensor::get_state_interfaces() const +{ + return state_interfaces_; +} + +hardware_interface::return_type MultiInterfaceSensor::get_state( + std::vector & state, + const std::vector & interfaces) const +{ + return get_internal_values(state, interfaces, state_interfaces_, state_values_); +} + +hardware_interface::return_type MultiInterfaceSensor::get_state(std::vector & state) const +{ + return get_internal_values(state, state_values_); +} + +hardware_interface::return_type MultiInterfaceSensor::set_state( + const std::vector & state, + const std::vector & interfaces) +{ + return set_internal_values(state, interfaces, state_interfaces_, state_values_); +} + +hardware_interface::return_type MultiInterfaceSensor::set_state(const std::vector & state) +{ + return set_internal_values(state, state_values_); +} + +} // namespace multi_interface_sensor diff --git a/multi_interface_sensor/test/test_multi_interface_sensor.cpp b/multi_interface_sensor/test/test_multi_interface_sensor.cpp new file mode 100644 index 0000000000..7c9d19aba4 --- /dev/null +++ b/multi_interface_sensor/test/test_multi_interface_sensor.cpp @@ -0,0 +1,152 @@ +// Copyright 2020 ros2_control Development Team +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include + +#include "multi_interface_sensor/multi_interface_sensor.hpp" + +#include "hardware_interface/components/component_info.hpp" +#include "hardware_interface/types/hardware_interface_type_values.hpp" + +using namespace ::testing; // NOLINT + +class TestMultiInterfaceSensor : public Test +{ +}; + +TEST_F(TestMultiInterfaceSensor, empty_initialized) +{ + multi_interface_sensor::MultiInterfaceSensor sensor; + EXPECT_TRUE(sensor.get_state_interfaces().empty()); +} + +TEST_F(TestMultiInterfaceSensor, wrong_initialized) +{ + { + multi_interface_sensor::MultiInterfaceSensor sensor; + hardware_interface::components::ComponentInfo sensor_info; + EXPECT_EQ( + hardware_interface::return_type::INTERFACE_NOT_PROVIDED, sensor.configure(sensor_info)); + } + + { + hardware_interface::components::ComponentInfo sensor_info; + sensor_info.state_interfaces.push_back("position"); + sensor_info.state_interfaces.push_back("position"); + multi_interface_sensor::MultiInterfaceSensor sensor; + EXPECT_EQ(hardware_interface::return_type::INTERFACE_DUPLICATES, sensor.configure(sensor_info)); + } +} + +TEST_F(TestMultiInterfaceSensor, correct_initialized) +{ + { + hardware_interface::components::ComponentInfo sensor_info; + sensor_info.state_interfaces.push_back("effort"); + multi_interface_sensor::MultiInterfaceSensor sensor; + EXPECT_EQ(hardware_interface::return_type::OK, sensor.configure(sensor_info)); + ASSERT_EQ(1u, sensor.get_state_interfaces().size()); + EXPECT_EQ("effort", sensor.get_state_interfaces()[0]); + } +} + +TEST_F(TestMultiInterfaceSensor, getters_and_setters) +{ + multi_interface_sensor::MultiInterfaceSensor sensor; + hardware_interface::components::ComponentInfo sensor_info; + sensor_info.state_interfaces.push_back("force_x"); + sensor_info.state_interfaces.push_back("force_y"); + sensor_info.state_interfaces.push_back("force_z"); + sensor_info.state_interfaces.push_back("torque_x"); + sensor_info.state_interfaces.push_back("torque_y"); + sensor_info.state_interfaces.push_back("torque_z"); + ASSERT_EQ( + hardware_interface::return_type::OK, sensor.configure(sensor_info)); + // default initialize values + ASSERT_EQ( + hardware_interface::return_type::OK, + sensor.set_state({1.34, 5.67, 8.21, 5.63, 5.99, 4.32})); + + EXPECT_EQ( + hardware_interface::return_type::OK, + sensor.configure(sensor_info)); + ASSERT_EQ(6u, sensor.get_state_interfaces().size()); + EXPECT_EQ(sensor.get_state_interfaces()[0], "force_x"); + EXPECT_EQ(sensor.get_state_interfaces()[5], "torque_z"); + std::vector input = {5.23, 6.7, 2.5, 3.8, 8.9, 12.3}; + std::vector output; + std::vector interfaces; + EXPECT_EQ( + hardware_interface::return_type::INTERFACE_NOT_PROVIDED, + sensor.get_state(output, interfaces)); + interfaces.push_back("force_y"); + EXPECT_EQ( + hardware_interface::return_type::OK, + sensor.get_state(output, interfaces)); + ASSERT_EQ(1u, output.size()); + EXPECT_EQ(5.67, output[0]); + + // State getters and setters + interfaces.clear(); + EXPECT_EQ( + hardware_interface::return_type::INTERFACE_NOT_PROVIDED, + sensor.set_state(input, interfaces)); + interfaces.push_back(hardware_interface::HW_IF_VELOCITY); + interfaces.push_back(hardware_interface::HW_IF_VELOCITY); + interfaces.push_back(hardware_interface::HW_IF_VELOCITY); + EXPECT_EQ( + hardware_interface::return_type::INTERFACE_VALUE_SIZE_NOT_EQUAL, + sensor.set_state(input, interfaces)); + interfaces.push_back(hardware_interface::HW_IF_VELOCITY); + interfaces.push_back(hardware_interface::HW_IF_VELOCITY); + interfaces.push_back(hardware_interface::HW_IF_VELOCITY); + EXPECT_EQ( + hardware_interface::return_type::INTERFACE_NOT_FOUND, + sensor.set_state(input, interfaces)); + interfaces.clear(); + interfaces = sensor.get_state_interfaces(); + EXPECT_EQ( + hardware_interface::return_type::OK, + sensor.set_state(input, interfaces)); + + output.clear(); + EXPECT_EQ( + hardware_interface::return_type::OK, + sensor.get_state(output, interfaces)); + ASSERT_EQ(6u, output.size()); + EXPECT_EQ(5.23, output[0]); + + interfaces.clear(); + interfaces.push_back(hardware_interface::HW_IF_VELOCITY); + EXPECT_EQ( + hardware_interface::return_type::INTERFACE_NOT_FOUND, + sensor.get_state(output, interfaces)); + + input.clear(); + EXPECT_EQ( + hardware_interface::return_type::INTERFACE_VALUE_SIZE_NOT_EQUAL, + sensor.set_state(input)); + input = {5.23, 6.7, 2.5, 3.8, 8.9, 12.3}; + EXPECT_EQ( + hardware_interface::return_type::OK, + sensor.set_state(input)); + + EXPECT_EQ( + hardware_interface::return_type::OK, + sensor.get_state(output)); + ASSERT_EQ(6u, output.size()); + EXPECT_EQ(12.3, output[5]); +} diff --git a/ros2_control/package.xml b/ros2_control/package.xml index e4673a0bbe..48e2d2e520 100644 --- a/ros2_control/package.xml +++ b/ros2_control/package.xml @@ -14,6 +14,8 @@ controller_interface controller_manager hardware_interface + multi_interface_joint + multi_interface_sensor transmission_interface test_robot_hardware