diff --git a/controller_manager/CMakeLists.txt b/controller_manager/CMakeLists.txt index 59b20dbeaf..163a862eeb 100644 --- a/controller_manager/CMakeLists.txt +++ b/controller_manager/CMakeLists.txt @@ -19,7 +19,11 @@ find_package(hardware_interface REQUIRED) find_package(pluginlib REQUIRED) find_package(rclcpp REQUIRED) -add_library(controller_manager SHARED src/controller_manager.cpp src/controller_loader_pluginlib.cpp) +add_library(controller_manager SHARED + src/controller_manager.cpp + src/controller_loader_pluginlib.cpp + src/resource_manager.cpp +) target_include_directories(controller_manager PRIVATE include) ament_target_dependencies(controller_manager ament_index_cpp @@ -63,6 +67,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 @@ -99,11 +107,27 @@ 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) + ament_target_dependencies(test_hardware_resources + hardware_interface + pluginlib) + install(TARGETS test_hardware_resources DESTINATION lib ) + pluginlib_export_plugin_description_file( + hardware_interface test/test_hardware_resources.xml) + + ament_add_gtest( + test_resource_manager + test/test_resource_manager.cpp + ) + target_include_directories(test_resource_manager PRIVATE include src) + target_link_libraries(test_resource_manager controller_manager) endif() ament_export_libraries( diff --git a/controller_manager/include/controller_manager/controller_loader_pluginlib.hpp b/controller_manager/include/controller_manager/controller_loader_pluginlib.hpp index 6b56aa9684..6aadb2890a 100644 --- a/controller_manager/include/controller_manager/controller_loader_pluginlib.hpp +++ b/controller_manager/include/controller_manager/controller_loader_pluginlib.hpp @@ -36,10 +36,11 @@ class ControllerLoaderPluginlib : public ControllerLoaderInterface virtual ~ControllerLoaderPluginlib() = default; CONTROLLER_MANAGER_PUBLIC - controller_interface::ControllerInterfaceSharedPtr create(const std::string & controller_type); + controller_interface::ControllerInterfaceSharedPtr create(const std::string & controller_type) + override; CONTROLLER_MANAGER_PUBLIC - bool is_available(const std::string & controller_type) const; + bool is_available(const std::string & controller_type) const override; CONTROLLER_MANAGER_PUBLIC std::vector get_declared_classes() const override; diff --git a/controller_manager/src/resource_manager.cpp b/controller_manager/src/resource_manager.cpp new file mode 100644 index 0000000000..a3efaed710 --- /dev/null +++ b/controller_manager/src/resource_manager.cpp @@ -0,0 +1,193 @@ +// Copyright 2020 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include +#include + +#include "hardware_interface/actuator_hardware.hpp" +#include "hardware_interface/components/component_info.hpp" +#include "hardware_interface/component_parser.hpp" +#include "hardware_interface/hardware_info.hpp" +#include "hardware_interface/sensor_hardware.hpp" +#include "hardware_interface/system_hardware.hpp" + +#include "pluginlib/class_loader.hpp" + +#include "./resource_manager.hpp" + +namespace controller_manager +{ + +class ResourceStorage +{ + static constexpr const char * pkg_name = "hardware_interface"; + + static constexpr const char * joint_component_interface_name = + "hardware_interface::components::Joint"; + static constexpr const char * sensor_component_interface_name = + "hardware_interface::components::Sensor"; + + static constexpr const char * actuator_interface_name = + "hardware_interface::ActuatorHardwareInterface"; + static constexpr const char * sensor_interface_name = + "hardware_interface::SensorHardwareInterface"; + static constexpr const char * 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; + + void initialize_joint_component( + const hardware_interface::components::ComponentInfo & component_info) + { + joint_components_.emplace_back( + std::unique_ptr( + joint_component_loader_.createUnmanagedInstance(component_info.class_type))); + joint_components_.back()->configure(component_info); + } + + void initialize_sensor_component( + const hardware_interface::components::ComponentInfo & component_info) + { + sensor_components_.emplace_back( + std::unique_ptr( + sensor_component_loader_.createUnmanagedInstance(component_info.class_type))); + sensor_components_.back()->configure(component_info); + } + + template + void initialize_hardware( + const hardware_interface::HardwareInfo & hardware_info, + pluginlib::ClassLoader & loader, + std::vector & container) + { + // hardware_class_type has to match class name in plugin xml description + // TODO(karsten1987) extract package from hardware_class_type + // e.g.: / + auto interface = std::unique_ptr( + loader.createUnmanagedInstance(hardware_info.hardware_class_type)); + HardwareT actuator(std::move(interface)); + container.emplace_back(std::move(actuator)); + container.back().configure(hardware_info); + } + + void initialize_actuator(const hardware_interface::HardwareInfo & hardware_info) + { + initialize_hardware( + hardware_info, actuator_loader_, actuators_); + } + + void initialize_sensor(const hardware_interface::HardwareInfo & hardware_info) + { + initialize_hardware( + hardware_info, sensor_loader_, sensors_); + } + + void initialize_system(const hardware_interface::HardwareInfo & hardware_info) + { + initialize_hardware( + hardware_info, system_loader_, systems_); + } + + // components plugins + pluginlib::ClassLoader joint_component_loader_; + pluginlib::ClassLoader sensor_component_loader_; + + std::vector> joint_components_; + std::vector> sensor_components_; + + // hardware plugins + pluginlib::ClassLoader actuator_loader_; + pluginlib::ClassLoader sensor_loader_; + pluginlib::ClassLoader system_loader_; + + std::vector actuators_; + std::vector sensors_; + std::vector systems_; +}; + +ResourceManager::ResourceManager() +: resource_storage_(std::make_unique()) +{} + +ResourceManager::~ResourceManager() = default; + +ResourceManager::ResourceManager(const std::string & urdf) +: resource_storage_(std::make_unique()) +{ + const std::string system_type = "system"; + const std::string sensor_type = "sensor"; + const std::string actuator_type = "actuator"; + + auto hardware_info = hardware_interface::parse_control_resources_from_urdf(urdf); + + for (const auto & hardware : hardware_info) { + if (hardware.type == actuator_type) { + resource_storage_->initialize_actuator(hardware); + } + if (hardware.type == sensor_type) { + resource_storage_->initialize_sensor(hardware); + } + if (hardware.type == system_type) { + resource_storage_->initialize_system(hardware); + } + + for (const auto & joint : hardware.joints) { + resource_storage_->initialize_joint_component(joint); + } + + for (const auto & sensor : hardware.sensors) { + resource_storage_->initialize_sensor_component(sensor); + } + } +} + +size_t ResourceManager::joint_components_size() const +{ + return resource_storage_->joint_components_.size(); +} + +size_t ResourceManager::sensor_components_size() const +{ + return resource_storage_->sensor_components_.size(); +} + +size_t ResourceManager::actuator_interfaces_size() const +{ + return resource_storage_->actuators_.size(); +} + +size_t ResourceManager::sensor_interfaces_size() const +{ + return resource_storage_->sensors_.size(); +} + +size_t ResourceManager::system_interfaces_size() const +{ + return resource_storage_->systems_.size(); +} +} // namespace controller_manager diff --git a/controller_manager/src/resource_manager.hpp b/controller_manager/src/resource_manager.hpp new file mode 100644 index 0000000000..3646317619 --- /dev/null +++ b/controller_manager/src/resource_manager.hpp @@ -0,0 +1,55 @@ +// Copyright 2020 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef RESOURCE_MANAGER_HPP_ +#define RESOURCE_MANAGER_HPP_ + +#include +#include + +namespace controller_manager +{ + +class ResourceStorage; + +class ResourceManager +{ +public: + ResourceManager(); + + explicit ResourceManager(const std::string & urdf); + + ResourceManager(const ResourceManager &) = delete; + + ~ResourceManager(); + + size_t joint_components_size() const; + + size_t sensor_components_size() const; + + size_t actuator_interfaces_size() const; + + size_t sensor_interfaces_size() const; + + size_t system_interfaces_size() const; + + // loan_joint(const std::string & name); + // loan_sensor(const std::string & name); + +private: + std::unique_ptr resource_storage_; +}; + +} // namespace controller_manager +#endif // RESOURCE_MANAGER_HPP_ diff --git a/controller_manager/test/controller_manager_test_common.hpp b/controller_manager/test/controller_manager_test_common.hpp index bd7a606a1e..b57c955771 100644 --- a/controller_manager/test/controller_manager_test_common.hpp +++ b/controller_manager/test/controller_manager_test_common.hpp @@ -71,13 +71,15 @@ class ControllerLoaderMock : public controller_manager::ControllerLoaderInterfac ControllerLoaderMock() : controller_manager::ControllerLoaderInterface("controller_interface::MockControllerInterface") {} - MOCK_METHOD1(create, controller_interface::ControllerInterfaceSharedPtr(const std::string &)); - MOCK_CONST_METHOD1(is_available, bool(const std::string &)); + MOCK_METHOD( + controller_interface::ControllerInterfaceSharedPtr, create, (const std::string &), + (override)); + MOCK_METHOD(bool, is_available, (const std::string &), (const, override)); std::vector get_declared_classes() const override { return {MOCK_TEST_CONTROLLER_NAME}; } - MOCK_METHOD0(reload, void()); + MOCK_METHOD(void, reload, (), (override)); }; #endif // CONTROLLER_MANAGER_TEST_COMMON_HPP_ diff --git a/controller_manager/test/test_controller_manager_srvs.cpp b/controller_manager/test/test_controller_manager_srvs.cpp index 9a0f6e8b9c..c60fdc7895 100644 --- a/controller_manager/test/test_controller_manager_srvs.cpp +++ b/controller_manager/test/test_controller_manager_srvs.cpp @@ -32,7 +32,6 @@ using ::testing::Return; using namespace std::chrono_literals; - class TestControllerManagerSrvs : public TestControllerManager { public: @@ -137,7 +136,6 @@ TEST_F(TestControllerManagerSrvs, list_controller_types) ::testing::Contains("controller_interface::MockControllerInterface")); } - TEST_F(TestControllerManagerSrvs, list_controllers_srv) { rclcpp::executors::SingleThreadedExecutor srv_executor; rclcpp::Node::SharedPtr srv_node = std::make_shared("srv_client"); @@ -176,7 +174,6 @@ TEST_F(TestControllerManagerSrvs, list_controllers_srv) { result->controller.size()); ASSERT_EQ("active", result->controller[0].state); - cm_->switch_controller( {}, {test_controller::TEST_CONTROLLER_NAME}, controller_manager_msgs::srv::SwitchController::Request::STRICT, true, @@ -207,7 +204,6 @@ TEST_F(TestControllerManagerSrvs, reload_controller_libraries_srv) { auto request = std::make_shared(); - std::shared_ptr mock_loader(new ControllerLoaderMock); cm_->register_controller_loader(mock_loader); @@ -219,7 +215,6 @@ TEST_F(TestControllerManagerSrvs, reload_controller_libraries_srv) { auto result = call_service_and_wait(*client, request, srv_executor); ASSERT_TRUE(result->ok); - // Add a controller, but stopped auto test_controller = cm_->load_controller( test_controller::TEST_CONTROLLER_NAME, @@ -257,7 +252,6 @@ TEST_F(TestControllerManagerSrvs, reload_controller_libraries_srv) { lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_controller->get_lifecycle_node()->get_current_state().id()); - // Failed reload due to active controller request->force_kill = false; EXPECT_CALL(*mock_loader, reload).Times(0); @@ -278,7 +272,6 @@ TEST_F(TestControllerManagerSrvs, reload_controller_libraries_srv) { result = call_service_and_wait(*client, request, srv_executor, true); ASSERT_TRUE(result->ok); - ASSERT_EQ( test_controller.use_count(), 1) << "No more references to the controller after reloading."; @@ -320,7 +313,6 @@ TEST_F(TestControllerManagerSrvs, unload_controller_srv) { auto result = call_service_and_wait(*client, request, srv_executor); ASSERT_FALSE(result->ok) << "Controller not loaded: " << request->name; - auto test_controller = std::make_shared(); auto abstract_test_controller = cm_->add_controller( test_controller, test_controller::TEST_CONTROLLER_NAME, diff --git a/controller_manager/test/test_hardware_resources.xml b/controller_manager/test/test_hardware_resources.xml new file mode 100644 index 0000000000..d0494d45ec --- /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..68c6a30481 --- /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::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; +} + +hardware_interface_status +TestActuatorHardware::get_status() const +{ + return hardware_interface_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..daaaded4fc --- /dev/null +++ b/controller_manager/test/test_hardware_resources/test_actuator_hardware.hpp @@ -0,0 +1,44 @@ +// Copyright 2020 ros2_control Development Team +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef TEST_HARDWARE_RESOURCES__TEST_ACTUATOR_HARDWARE_HPP_ +#define TEST_HARDWARE_RESOURCES__TEST_ACTUATOR_HARDWARE_HPP_ + +#include + +#include "hardware_interface/actuator_hardware_interface.hpp" + +class TestActuatorHardware : public hardware_interface::ActuatorHardwareInterface +{ +public: + hardware_interface::return_type + configure(const hardware_interface::HardwareInfo & actuator_info) override; + + hardware_interface::return_type + start() override; + + hardware_interface::return_type + stop() override; + + hardware_interface::hardware_interface_status + get_status() const override; + + hardware_interface::return_type + read_joint(std::shared_ptr joint) const override; + + hardware_interface::return_type + write_joint(const std::shared_ptr joint) override; +}; + +#endif // TEST_HARDWARE_RESOURCES__TEST_ACTUATOR_HARDWARE_HPP_ diff --git a/controller_manager/test/test_hardware_resources/test_joint_component.cpp b/controller_manager/test/test_hardware_resources/test_joint_component.cpp new file mode 100644 index 0000000000..3ab5858eb0 --- /dev/null +++ b/controller_manager/test/test_hardware_resources/test_joint_component.cpp @@ -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. + +#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"}; +} + +#include "pluginlib/class_list_macros.hpp" // NOLINT + +PLUGINLIB_EXPORT_CLASS(TestJointComponent, hardware_interface::components::Joint) diff --git a/controller_manager/test/test_hardware_resources/test_joint_component.hpp b/controller_manager/test/test_hardware_resources/test_joint_component.hpp new file mode 100644 index 0000000000..b754cfa993 --- /dev/null +++ b/controller_manager/test/test_hardware_resources/test_joint_component.hpp @@ -0,0 +1,35 @@ +// Copyright 2020 ros2_control Development Team +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef TEST_HARDWARE_RESOURCES__TEST_JOINT_COMPONENT_HPP_ +#define TEST_HARDWARE_RESOURCES__TEST_JOINT_COMPONENT_HPP_ + +#include +#include +#include + +#include "hardware_interface/components/joint.hpp" + +class TestJointComponent : public hardware_interface::components::Joint +{ +public: + hardware_interface::return_type + configure(const hardware_interface::components::ComponentInfo & joint_info) override; + + std::vector get_command_interfaces() const override; + + std::vector get_state_interfaces() const override; +}; + +#endif // TEST_HARDWARE_RESOURCES__TEST_JOINT_COMPONENT_HPP_ diff --git a/controller_manager/test/test_hardware_resources/test_sensor_component.cpp b/controller_manager/test/test_hardware_resources/test_sensor_component.cpp new file mode 100644 index 0000000000..7ee7c6f0c6 --- /dev/null +++ b/controller_manager/test/test_hardware_resources/test_sensor_component.cpp @@ -0,0 +1,38 @@ +// 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"}; +} + +#include "pluginlib/class_list_macros.hpp" // NOLINT + +PLUGINLIB_EXPORT_CLASS(TestSensorComponent, hardware_interface::components::Sensor) diff --git a/controller_manager/test/test_hardware_resources/test_sensor_component.hpp b/controller_manager/test/test_hardware_resources/test_sensor_component.hpp new file mode 100644 index 0000000000..ce7e8c1998 --- /dev/null +++ b/controller_manager/test/test_hardware_resources/test_sensor_component.hpp @@ -0,0 +1,33 @@ +// Copyright 2020 ros2_control Development Team +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef TEST_HARDWARE_RESOURCES__TEST_SENSOR_COMPONENT_HPP_ +#define TEST_HARDWARE_RESOURCES__TEST_SENSOR_COMPONENT_HPP_ + +#include +#include +#include + +#include "hardware_interface/components/sensor.hpp" + +class TestSensorComponent : public hardware_interface::components::Sensor +{ +public: + hardware_interface::return_type + configure(const hardware_interface::components::ComponentInfo & sensor_info) override; + + std::vector get_state_interfaces() const override; +}; + +#endif // TEST_HARDWARE_RESOURCES__TEST_SENSOR_COMPONENT_HPP_ diff --git a/controller_manager/test/test_hardware_resources/test_sensor_hardware.cpp b/controller_manager/test/test_hardware_resources/test_sensor_hardware.cpp new file mode 100644 index 0000000000..b418c5b896 --- /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::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; +} + +hardware_interface_status +TestSensorHardware::get_status() const +{ + return hardware_interface_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..23b82b7f2f --- /dev/null +++ b/controller_manager/test/test_hardware_resources/test_sensor_hardware.hpp @@ -0,0 +1,43 @@ +// Copyright 2020 ros2_control Development Team +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef TEST_HARDWARE_RESOURCES__TEST_SENSOR_HARDWARE_HPP_ +#define TEST_HARDWARE_RESOURCES__TEST_SENSOR_HARDWARE_HPP_ + +#include +#include + +#include "hardware_interface/sensor_hardware_interface.hpp" + +class TestSensorHardware : public hardware_interface::SensorHardwareInterface +{ +public: + hardware_interface::return_type + configure(const hardware_interface::HardwareInfo & sensor_info) override; + + hardware_interface::return_type + start() override; + + hardware_interface::return_type + stop() override; + + hardware_interface::hardware_interface_status + get_status() const override; + + hardware_interface::return_type + read_sensors(const std::vector> & sensors) + const override; +}; + +#endif // TEST_HARDWARE_RESOURCES__TEST_SENSOR_HARDWARE_HPP_ diff --git a/controller_manager/test/test_hardware_resources/test_system_hardware.cpp b/controller_manager/test/test_hardware_resources/test_system_hardware.cpp new file mode 100644 index 0000000000..e69efdd4e5 --- /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::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; +} + +hardware_interface_status +TestSystemHardware::get_status() const +{ + return hardware_interface_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..c2d62bd8e6 --- /dev/null +++ b/controller_manager/test/test_hardware_resources/test_system_hardware.hpp @@ -0,0 +1,51 @@ +// Copyright 2020 ros2_control Development Team +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef TEST_HARDWARE_RESOURCES__TEST_SYSTEM_HARDWARE_HPP_ +#define TEST_HARDWARE_RESOURCES__TEST_SYSTEM_HARDWARE_HPP_ + +#include +#include + +#include "hardware_interface/system_hardware_interface.hpp" + +class TestSystemHardware : public hardware_interface::SystemHardwareInterface +{ +public: + hardware_interface::return_type + configure(const hardware_interface::HardwareInfo & system_info) override; + + hardware_interface::return_type + start() override; + + hardware_interface::return_type + stop() override; + + hardware_interface::hardware_interface_status + get_status() const override; + + hardware_interface::return_type + read_sensors( + std::vector> & sensors) const override; + + hardware_interface::return_type + read_joints( + std::vector> & joints) const override; + + hardware_interface::return_type + write_joints( + const std::vector> & joints) override; +}; + +#endif // TEST_HARDWARE_RESOURCES__TEST_SYSTEM_HARDWARE_HPP_ diff --git a/controller_manager/test/test_resource_manager.cpp b/controller_manager/test/test_resource_manager.cpp new file mode 100644 index 0000000000..93d76ce0ac --- /dev/null +++ b/controller_manager/test/test_resource_manager.cpp @@ -0,0 +1,155 @@ +// Copyright 2017 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include + +#include "resource_manager.hpp" + +class TestResourceManager : public ::testing::Test +{ +public: + static void SetUpTestCase() + { + } + + void SetUp() + { + urdf_head_ = + R"( + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +)"; + + urdf_tail_ = + R"( + +)"; + + test_hardware_resource_system_ = + R"( + + + test_actuator_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.actuator_interfaces_size()); + EXPECT_EQ(0u, rm.sensor_interfaces_size()); + EXPECT_EQ(0u, rm.system_interfaces_size()); +} + +TEST_F(TestResourceManager, initialization_with_urdf) { + auto urdf = urdf_head_ + test_hardware_resource_system_ + urdf_tail_; + controller_manager::ResourceManager rm(urdf); + + EXPECT_EQ(3u, rm.joint_components_size()); + EXPECT_EQ(1u, rm.sensor_components_size()); + + EXPECT_EQ(1u, rm.actuator_interfaces_size()); + EXPECT_EQ(1u, rm.sensor_interfaces_size()); + EXPECT_EQ(1u, rm.system_interfaces_size()); +} diff --git a/hardware_interface/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 1bcfbafe60..282cc20c18 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 64ede0a2bc..9f2fef052e 100644 --- a/hardware_interface/include/hardware_interface/components/joint.hpp +++ b/hardware_interface/include/hardware_interface/components/joint.hpp @@ -50,6 +50,7 @@ class Joint * return_type::ERROR otherwise. */ HARDWARE_INTERFACE_PUBLIC + virtual return_type configure(const ComponentInfo & joint_info); /** @@ -58,6 +59,7 @@ class Joint * \return string list with command interfaces. */ HARDWARE_INTERFACE_PUBLIC + virtual std::vector get_command_interfaces() const; /** @@ -66,6 +68,7 @@ class Joint * \return string list with state interfaces. */ HARDWARE_INTERFACE_PUBLIC + virtual std::vector get_state_interfaces() const; /** @@ -81,7 +84,8 @@ class Joint * defined for the joint; return return_type::INTERFACE_NOT_PROVIDED if the list of interfaces * is empty; return_type::OK otherwise. */ - HARDWARE_INTERFACE_EXPORT + HARDWARE_INTERFACE_PUBLIC + virtual return_type get_command( std::vector & command, const std::vector & interfaces) const; @@ -94,7 +98,8 @@ class Joint * \param command list of doubles with commands for the hardware. * \return return_type::OK always. */ - HARDWARE_INTERFACE_EXPORT + HARDWARE_INTERFACE_PUBLIC + virtual return_type get_command(std::vector & command) const; /** @@ -113,7 +118,8 @@ class Joint * for different interfaces. This should be changed in the future. * (see: https://github.com/ros-controls/ros2_control/issues/129) */ - HARDWARE_INTERFACE_EXPORT + HARDWARE_INTERFACE_PUBLIC + virtual return_type set_command( const std::vector & command, const std::vector & interfaces); @@ -128,7 +134,8 @@ class Joint * joint's command interfaces; return_type::COMMAND_OUT_OF_LIMITS if one of the command values is out * of limits; return_type::OK otherwise. */ - HARDWARE_INTERFACE_EXPORT + HARDWARE_INTERFACE_PUBLIC + virtual return_type set_command(const std::vector & command); /** @@ -143,7 +150,8 @@ class Joint * defined for the joint; return return_type::INTERFACE_NOT_PROVIDED if the list of interfaces * is empty; return_type::OK otherwise. */ - HARDWARE_INTERFACE_EXPORT + HARDWARE_INTERFACE_PUBLIC + virtual return_type get_state( std::vector & state, const std::vector & interfaces) const; @@ -156,7 +164,8 @@ class Joint * \param state list of doubles with states of the hardware. * \return return_type::OK always. */ - HARDWARE_INTERFACE_EXPORT + HARDWARE_INTERFACE_PUBLIC + virtual return_type get_state(std::vector & state) const; /** @@ -170,7 +179,8 @@ class Joint * have the same length; return_type::INTERFACE_NOT_FOUND if one of provided interfaces is not * defined for the joint; return_type::OK otherwise. */ - HARDWARE_INTERFACE_EXPORT + HARDWARE_INTERFACE_PUBLIC + virtual return_type set_state( const std::vector & state, const std::vector & interfaces); @@ -184,7 +194,8 @@ class Joint * \return return_type::INTERFACE_VALUE_SIZE_NOT_EQUAL is command size is not equal to number of * joint's state interfaces, return_type::OK otherwise. */ - HARDWARE_INTERFACE_EXPORT + HARDWARE_INTERFACE_PUBLIC + virtual return_type set_state(const std::vector & state); protected: diff --git a/hardware_interface/include/hardware_interface/components/sensor.hpp b/hardware_interface/include/hardware_interface/components/sensor.hpp index a99c85eec9..5d52fccf48 100644 --- a/hardware_interface/include/hardware_interface/components/sensor.hpp +++ b/hardware_interface/include/hardware_interface/components/sensor.hpp @@ -50,7 +50,8 @@ class Sensor * return_type::ERROR otherwise. */ HARDWARE_INTERFACE_PUBLIC - return_type configure(const ComponentInfo & joint_info); + virtual + return_type configure(const ComponentInfo & sensor_info); /** * \brief Provide the list of state interfaces configured for the sensor. @@ -58,7 +59,8 @@ class Sensor * \return string list with state interfaces. */ HARDWARE_INTERFACE_PUBLIC - std::vector get_state_interfaces(); + virtual + std::vector get_state_interfaces() const; /** * \brief Get state list from the sensor. This function is used by the controller to get the @@ -72,7 +74,8 @@ class Sensor * defined for the sensor; return return_type::INTERFACE_NOT_PROVIDED if the list of interfaces * is empty; return_type::OK otherwise. */ - HARDWARE_INTERFACE_EXPORT + HARDWARE_INTERFACE_PUBLIC + virtual return_type get_state( std::vector & state, const std::vector & interfaces) const; @@ -85,7 +88,8 @@ class Sensor * \param state list of doubles with states of the hardware. * \return return_type::OK always. */ - HARDWARE_INTERFACE_EXPORT + HARDWARE_INTERFACE_PUBLIC + virtual return_type get_state(std::vector & state) const; /** @@ -99,7 +103,8 @@ class Sensor * have the same length; return_type::INTERFACE_NOT_FOUND if one of provided interfaces is not * defined for the sensor; return_type::OK otherwise. */ - HARDWARE_INTERFACE_EXPORT + HARDWARE_INTERFACE_PUBLIC + virtual return_type set_state( const std::vector & state, const std::vector & interfaces); @@ -113,7 +118,8 @@ class Sensor * \return return_type::INTERFACE_VALUE_SIZE_NOT_EQUAL is state size is not equal to number of * sensor's state interfaces, return_type::OK otherwise. */ - HARDWARE_INTERFACE_EXPORT + HARDWARE_INTERFACE_PUBLIC + virtual return_type set_state(const std::vector & state); protected: diff --git a/hardware_interface/include/hardware_interface/sensor_hardware.hpp b/hardware_interface/include/hardware_interface/sensor_hardware.hpp index 95bb04f4cf..4385eada32 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 5a64148c29..3bd61dc01d 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 722c75c435..465a96d658 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/src/components/sensor.cpp b/hardware_interface/src/components/sensor.cpp index 0229e8b14f..5c51ed8848 100644 --- a/hardware_interface/src/components/sensor.cpp +++ b/hardware_interface/src/components/sensor.cpp @@ -36,7 +36,7 @@ return_type Sensor::configure(const ComponentInfo & joint_info) return return_type::OK; } -std::vector Sensor::get_state_interfaces() +std::vector Sensor::get_state_interfaces() const { return info_.state_interfaces; }