From c904c43310b98d4f87b5469760befb77cceaa36b Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Sun, 15 Nov 2020 15:09:51 +0100 Subject: [PATCH 01/10] Added starting of resources into CM and RM --- controller_manager/src/controller_manager.cpp | 3 +++ .../hardware_interface/resource_manager.hpp | 6 +++++ hardware_interface/src/resource_manager.cpp | 26 +++++++++++++++++++ 3 files changed, 35 insertions(+) diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 4219b5eb91..d6ca890ee9 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -71,6 +71,9 @@ ControllerManager::ControllerManager( resource_manager_->load_urdf(robot_description); + //TODO(all): Here we should start only "auto-start" resources + resource_manager_->start_all(); + init_services(); } diff --git a/hardware_interface/include/hardware_interface/resource_manager.hpp b/hardware_interface/include/hardware_interface/resource_manager.hpp index bbfcce6e85..3eee8969d0 100644 --- a/hardware_interface/include/hardware_interface/resource_manager.hpp +++ b/hardware_interface/include/hardware_interface/resource_manager.hpp @@ -190,6 +190,12 @@ class ResourceManager */ void import_component(std::unique_ptr system); + /// Start all loaded hardware components. + void start_all(); + + /// Stops all loaded hardware components. + void stop_all(); + /// Reads all loaded hardware components. void read(); diff --git a/hardware_interface/src/resource_manager.cpp b/hardware_interface/src/resource_manager.cpp index e1ef098e08..4fd1091fa3 100644 --- a/hardware_interface/src/resource_manager.cpp +++ b/hardware_interface/src/resource_manager.cpp @@ -291,6 +291,32 @@ size_t ResourceManager::system_components_size() const return resource_storage_->systems_.size(); } +void ResourceManager::start_all() +{ + for (auto & component : resource_storage_->actuators_) { + component.start(); + } + for (auto & component : resource_storage_->sensors_) { + component.start(); + } + for (auto & component : resource_storage_->systems_) { + component.start(); + } +} + +void ResourceManager::stop_all() +{ + for (auto & component : resource_storage_->actuators_) { + component.stop(); + } + for (auto & component : resource_storage_->sensors_) { + component.stop(); + } + for (auto & component : resource_storage_->systems_) { + component.stop(); + } +} + void ResourceManager::read() { for (auto & component : resource_storage_->actuators_) { From 898ed8daa5d2520083f57b561fb521c7db8f9025 Mon Sep 17 00:00:00 2001 From: Karsten Knese Date: Tue, 17 Nov 2020 13:30:33 -0800 Subject: [PATCH 02/10] adapt controller manager test Signed-off-by: Karsten Knese --- controller_manager/src/controller_manager.cpp | 6 ++++++ controller_manager/test/test_controller_manager_srvs.cpp | 6 ++++++ 2 files changed, 12 insertions(+) diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index d6ca890ee9..94baba8eab 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -98,6 +98,12 @@ void ControllerManager::init_services() rclcpp::CallbackGroupType::MutuallyExclusive); using namespace std::placeholders; + list_controller_interfaces_service_ = + create_service( + "~/list_controller_interfaces", + std::bind(&ControllerManager::list_controller_interfaces_srv_cb, this, _1, _2), + rmw_qos_profile_services_default, + best_effort_callback_group_); list_controllers_service_ = create_service( "~/list_controllers", std::bind(&ControllerManager::list_controllers_srv_cb, this, _1, _2), diff --git a/controller_manager/test/test_controller_manager_srvs.cpp b/controller_manager/test/test_controller_manager_srvs.cpp index e9849bdb73..7e42695bed 100644 --- a/controller_manager/test/test_controller_manager_srvs.cpp +++ b/controller_manager/test/test_controller_manager_srvs.cpp @@ -208,10 +208,13 @@ TEST_F(TestControllerManagerSrvs, reload_controller_libraries_srv) { auto test_controller = cm_->load_controller( test_controller::TEST_CONTROLLER_NAME, test_controller::TEST_CONTROLLER_CLASS_NAME); +<<<<<<< HEAD // weak_ptr so the only controller shared_ptr instance is owned by the controller_manager and // can be completely destroyed before reloading the library std::weak_ptr test_controller_weak(test_controller); +======= +>>>>>>> adapt controller manager test ASSERT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, @@ -236,7 +239,10 @@ TEST_F(TestControllerManagerSrvs, reload_controller_libraries_srv) { test_controller = cm_->load_controller( test_controller::TEST_CONTROLLER_NAME, test_controller::TEST_CONTROLLER_CLASS_NAME); +<<<<<<< HEAD test_controller_weak = test_controller; +======= +>>>>>>> adapt controller manager test // Start Controller cm_->switch_controller( {test_controller::TEST_CONTROLLER_NAME}, {}, From c94428d94cfe9569d3c6030ac44309eec7d5baaf Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Fri, 20 Nov 2020 14:49:33 +0100 Subject: [PATCH 03/10] Add Base Implementation and extend hardware interfaces with get_name --- .../components/actuator.hpp | 3 + .../components/actuator_interface.hpp | 11 +++- .../components/base_inteface.hpp | 63 +++++++++++++++++++ .../hardware_interface/components/sensor.hpp | 3 + .../components/sensor_interface.hpp | 11 +++- .../hardware_interface/components/system.hpp | 3 + .../components/system_interface.hpp | 9 +++ .../src/components/actuator.cpp | 5 ++ hardware_interface/src/components/sensor.cpp | 5 ++ hardware_interface/src/components/system.cpp | 5 ++ 10 files changed, 116 insertions(+), 2 deletions(-) create mode 100644 hardware_interface/include/hardware_interface/components/base_inteface.hpp diff --git a/hardware_interface/include/hardware_interface/components/actuator.hpp b/hardware_interface/include/hardware_interface/components/actuator.hpp index 720d7e93d1..94d0ca56b2 100644 --- a/hardware_interface/include/hardware_interface/components/actuator.hpp +++ b/hardware_interface/include/hardware_interface/components/actuator.hpp @@ -58,6 +58,9 @@ class Actuator final HARDWARE_INTERFACE_PUBLIC return_type stop(); + HARDWARE_INTERFACE_PUBLIC + std::string get_name() const; + HARDWARE_INTERFACE_PUBLIC status get_status() const; diff --git a/hardware_interface/include/hardware_interface/components/actuator_interface.hpp b/hardware_interface/include/hardware_interface/components/actuator_interface.hpp index 4e1cc3e6d9..e40c44a3e2 100644 --- a/hardware_interface/include/hardware_interface/components/actuator_interface.hpp +++ b/hardware_interface/include/hardware_interface/components/actuator_interface.hpp @@ -16,6 +16,7 @@ #define HARDWARE_INTERFACE__COMPONENTS__ACTUATOR_INTERFACE_HPP_ #include +#include #include #include "hardware_interface/handle.hpp" @@ -91,7 +92,15 @@ class ActuatorInterface return_type stop() = 0; /** - * \brief Get current state of the system hardware. + * \brief Get name of the actuator hardware. + * + * \return std::string name. + */ + virtual + std::string get_name() const = 0; + + /** + * \brief Get current state of the actuator hardware. * * \return status current status. */ diff --git a/hardware_interface/include/hardware_interface/components/base_inteface.hpp b/hardware_interface/include/hardware_interface/components/base_inteface.hpp new file mode 100644 index 0000000000..a5bf764bc3 --- /dev/null +++ b/hardware_interface/include/hardware_interface/components/base_inteface.hpp @@ -0,0 +1,63 @@ +// 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__BASE_INTERFACE_HPP_ +#define HARDWARE_INTERFACE__COMPONENTS__BASE_INTERFACE_HPP_ + +#include + +#include "hardware_interface/hardware_info.hpp" +#include "hardware_interface/types/hardware_interface_return_values.hpp" +#include "hardware_interface/types/hardware_interface_status_values.hpp" + +namespace hardware_interface +{ +namespace components +{ + +template +class BaseInterface : public InterfaceType +{ +public: + return_type configure(const HardwareInfo & info) override + { + info_ = info; + status_ = status::CONFIGURED; + return return_type::OK; + } + + return_type configure_default(const HardwareInfo & info) + { + return BaseInterface::configure(info); + } + + std::string get_name() const final + { + return info_.name; + } + + status get_status() const final + { + return status_; + } + +protected: + HardwareInfo info_; + status status_; +}; + +} // namespace components +} // namespace hardware_interface + +#endif // HARDWARE_INTERFACE__COMPONENTS__BASE_INTERFACE_HPP_ diff --git a/hardware_interface/include/hardware_interface/components/sensor.hpp b/hardware_interface/include/hardware_interface/components/sensor.hpp index 18762b28cc..3780d8d9a3 100644 --- a/hardware_interface/include/hardware_interface/components/sensor.hpp +++ b/hardware_interface/include/hardware_interface/components/sensor.hpp @@ -57,6 +57,9 @@ class Sensor final HARDWARE_INTERFACE_PUBLIC return_type stop(); + HARDWARE_INTERFACE_PUBLIC + std::string get_name() const; + HARDWARE_INTERFACE_PUBLIC status get_status() const; diff --git a/hardware_interface/include/hardware_interface/components/sensor_interface.hpp b/hardware_interface/include/hardware_interface/components/sensor_interface.hpp index 399040004d..7517c9936b 100644 --- a/hardware_interface/include/hardware_interface/components/sensor_interface.hpp +++ b/hardware_interface/include/hardware_interface/components/sensor_interface.hpp @@ -16,6 +16,7 @@ #define HARDWARE_INTERFACE__COMPONENTS__SENSOR_INTERFACE_HPP_ #include +#include #include #include "hardware_interface/handle.hpp" @@ -80,7 +81,15 @@ class SensorInterface return_type stop() = 0; /** - * \brief Get current state of the system hardware. + * \brief Get name of the sensor hardware. + * + * \return std::string name. + */ + virtual + std::string get_name() const = 0; + + /** + * \brief Get current state of the sensor hardware. * * \return status current status. */ diff --git a/hardware_interface/include/hardware_interface/components/system.hpp b/hardware_interface/include/hardware_interface/components/system.hpp index d11474db56..0a95ad350d 100644 --- a/hardware_interface/include/hardware_interface/components/system.hpp +++ b/hardware_interface/include/hardware_interface/components/system.hpp @@ -58,6 +58,9 @@ class System final HARDWARE_INTERFACE_PUBLIC return_type stop(); + HARDWARE_INTERFACE_PUBLIC + std::string get_name() const; + HARDWARE_INTERFACE_PUBLIC status get_status() const; diff --git a/hardware_interface/include/hardware_interface/components/system_interface.hpp b/hardware_interface/include/hardware_interface/components/system_interface.hpp index dc93c0d564..11323b931a 100644 --- a/hardware_interface/include/hardware_interface/components/system_interface.hpp +++ b/hardware_interface/include/hardware_interface/components/system_interface.hpp @@ -16,6 +16,7 @@ #define HARDWARE_INTERFACE__COMPONENTS__SYSTEM_INTERFACE_HPP_ #include +#include #include #include "hardware_interface/handle.hpp" @@ -93,6 +94,14 @@ class SystemInterface virtual return_type stop() = 0; + /** + * \brief Get name of the system hardware. + * + * \return std::string name. + */ + virtual + std::string get_name() const = 0; + /** * \brief Get current state of the system hardware. * diff --git a/hardware_interface/src/components/actuator.cpp b/hardware_interface/src/components/actuator.cpp index d299ace130..63d64ce636 100644 --- a/hardware_interface/src/components/actuator.cpp +++ b/hardware_interface/src/components/actuator.cpp @@ -62,6 +62,11 @@ return_type Actuator::stop() return impl_->stop(); } +std::string Actuator::get_name() const +{ + return impl_->get_name(); +} + status Actuator::get_status() const { return impl_->get_status(); diff --git a/hardware_interface/src/components/sensor.cpp b/hardware_interface/src/components/sensor.cpp index f16a9a0095..65b550515c 100644 --- a/hardware_interface/src/components/sensor.cpp +++ b/hardware_interface/src/components/sensor.cpp @@ -54,6 +54,11 @@ return_type Sensor::stop() return impl_->stop(); } +std::string Sensor::get_name() const +{ + return impl_->get_name(); +} + status Sensor::get_status() const { return impl_->get_status(); diff --git a/hardware_interface/src/components/system.cpp b/hardware_interface/src/components/system.cpp index dd903a0374..2fe421a9a7 100644 --- a/hardware_interface/src/components/system.cpp +++ b/hardware_interface/src/components/system.cpp @@ -59,6 +59,11 @@ return_type System::stop() return impl_->stop(); } +std::string System::get_name() const +{ + return impl_->get_name(); +} + status System::get_status() const { return impl_->get_status(); From 98f3675ff7b908fb29c1a9b61bfc935cd04739ac Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Sat, 21 Nov 2020 09:51:02 +0100 Subject: [PATCH 04/10] Components return names and status now; Moved base_interface; added tests for starting --- .../{base_inteface.hpp => base_interface.hpp} | 0 .../hardware_interface/resource_manager.hpp | 7 +++ hardware_interface/src/resource_manager.cpp | 22 ++++++- .../test/test_component_interfaces.cpp | 15 +++++ .../test/test_components/test_actuator.cpp | 38 ++++++------ .../test/test_components/test_sensor.cpp | 24 ++++---- .../test/test_components/test_system.cpp | 24 +++----- .../test/test_resource_manager.cpp | 58 +++++++++++++++---- 8 files changed, 129 insertions(+), 59 deletions(-) rename hardware_interface/include/hardware_interface/components/{base_inteface.hpp => base_interface.hpp} (100%) diff --git a/hardware_interface/include/hardware_interface/components/base_inteface.hpp b/hardware_interface/include/hardware_interface/components/base_interface.hpp similarity index 100% rename from hardware_interface/include/hardware_interface/components/base_inteface.hpp rename to hardware_interface/include/hardware_interface/components/base_interface.hpp diff --git a/hardware_interface/include/hardware_interface/resource_manager.hpp b/hardware_interface/include/hardware_interface/resource_manager.hpp index 3eee8969d0..25f672b6f3 100644 --- a/hardware_interface/include/hardware_interface/resource_manager.hpp +++ b/hardware_interface/include/hardware_interface/resource_manager.hpp @@ -24,6 +24,7 @@ #include "hardware_interface/loaned_command_interface.hpp" #include "hardware_interface/loaned_state_interface.hpp" #include "hardware_interface/hardware_info.hpp" +#include "hardware_interface/types/hardware_interface_status_values.hpp" namespace hardware_interface { @@ -190,6 +191,12 @@ class ResourceManager */ void import_component(std::unique_ptr system); + /// Return status for all components. + /** + * \return map of hardware names and their status + */ + std::unordered_map get_components_status(); + /// Start all loaded hardware components. void start_all(); diff --git a/hardware_interface/src/resource_manager.cpp b/hardware_interface/src/resource_manager.cpp index 4fd1091fa3..9fc4613e35 100644 --- a/hardware_interface/src/resource_manager.cpp +++ b/hardware_interface/src/resource_manager.cpp @@ -62,8 +62,9 @@ class ResourceStorage // 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)); + HardwareT hardware(std::move(interface)); + container.emplace_back(std::move(hardware)); + hardware_status_map_.emplace(std::make_pair(container.back().get_name(), container.back().get_status())); } template @@ -137,6 +138,8 @@ class ResourceStorage std::vector sensors_; std::vector systems_; + std::unordered_map hardware_status_map_; + std::unordered_map state_interface_map_; std::unordered_map command_interface_map_; }; @@ -291,6 +294,21 @@ size_t ResourceManager::system_components_size() const return resource_storage_->systems_.size(); } +std::unordered_map ResourceManager::get_components_status() +{ + for (auto & component : resource_storage_->actuators_) { + resource_storage_->hardware_status_map_[component.get_name()] = component.get_status(); + } + for (auto & component : resource_storage_->sensors_) { + resource_storage_->hardware_status_map_[component.get_name()] = component.get_status(); + } + for (auto & component : resource_storage_->systems_) { + resource_storage_->hardware_status_map_[component.get_name()] = component.get_status(); + } + + return resource_storage_->hardware_status_map_; +} + void ResourceManager::start_all() { for (auto & component : resource_storage_->actuators_) { diff --git a/hardware_interface/test/test_component_interfaces.cpp b/hardware_interface/test/test_component_interfaces.cpp index 02866a5ba0..1416fc2076 100644 --- a/hardware_interface/test/test_component_interfaces.cpp +++ b/hardware_interface/test/test_component_interfaces.cpp @@ -80,6 +80,11 @@ class DummyActuator : public hardware_interface::components::ActuatorInterface return hardware_interface::return_type::OK; } + std::string get_name() const override + { + return std::string("DummyActuator"); + } + hardware_interface::status get_status() const override { return hardware_interface::status::UNKNOWN; @@ -134,6 +139,11 @@ class DummySensor : public hardware_interface::components::SensorInterface return hardware_interface::return_type::OK; } + std::string get_name() const override + { + return std::string("DummySensor"); + } + hardware_interface::status get_status() const override { return hardware_interface::status::UNKNOWN; @@ -211,6 +221,11 @@ class DummySystem : public hardware_interface::components::SystemInterface return hardware_interface::return_type::OK; } + std::string get_name() const override + { + return std::string("DummySystem"); + } + hardware_interface::status get_status() const override { return hardware_interface::status::UNKNOWN; diff --git a/hardware_interface/test/test_components/test_actuator.cpp b/hardware_interface/test/test_components/test_actuator.cpp index 6c1c6daba9..c56cea9539 100644 --- a/hardware_interface/test/test_components/test_actuator.cpp +++ b/hardware_interface/test/test_components/test_actuator.cpp @@ -15,6 +15,7 @@ #include #include +#include "hardware_interface/components/base_interface.hpp" #include "hardware_interface/components/actuator_interface.hpp" using hardware_interface::status; @@ -22,21 +23,24 @@ using hardware_interface::return_type; using hardware_interface::StateInterface; using hardware_interface::CommandInterface; -class TestActuator : public hardware_interface::components::ActuatorInterface +class TestActuator : public + hardware_interface::components::BaseInterface { - return_type configure(const hardware_interface::HardwareInfo & actuator_info) override + return_type configure(const hardware_interface::HardwareInfo & sensor_info) override { - actuator_info_ = actuator_info; + if (configure_default(sensor_info) != return_type::OK) { + return return_type::ERROR; + } /* * a hardware can optional prove for incorrect info here. * * // can only control one joint - * if (actuator_info_.joints.size() != 1) {return return_type::ERROR;} + * if (info_.joints.size() != 1) {return return_type::ERROR;} * // can only control in position - * if (actuator_info_.joints[0].command_interfaces.size() != 1) {return return_type::ERROR;} + * if (info_.joints[0].command_interfaces.size() != 1) {return return_type::ERROR;} * // can only give feedback state for position and velocity - * if (actuator_info_.joints[0].state_interfaces.size() != 2) {return return_type::ERROR;} + * if (info_.joints[0].state_interfaces.size() != 2) {return return_type::ERROR;} */ return return_type::OK; @@ -47,17 +51,17 @@ class TestActuator : public hardware_interface::components::ActuatorInterface std::vector state_interfaces; state_interfaces.emplace_back( hardware_interface::StateInterface( - actuator_info_.joints[0].name, - actuator_info_.joints[0].state_interfaces[0].name, + info_.joints[0].name, + info_.joints[0].state_interfaces[0].name, &position_state_)); state_interfaces.emplace_back( hardware_interface::StateInterface( - actuator_info_.joints[0].name, - actuator_info_.joints[0].state_interfaces[1].name, + info_.joints[0].name, + info_.joints[0].state_interfaces[1].name, &velocity_state_)); state_interfaces.emplace_back( hardware_interface::StateInterface( - actuator_info_.joints[0].name, + info_.joints[0].name, "some_unlisted_interface", nullptr)); @@ -69,8 +73,8 @@ class TestActuator : public hardware_interface::components::ActuatorInterface std::vector command_interfaces; command_interfaces.emplace_back( hardware_interface::CommandInterface( - actuator_info_.joints[0].name, - actuator_info_.joints[0].command_interfaces[0].name, + info_.joints[0].name, + info_.joints[0].command_interfaces[0].name, &velocity_command_)); return command_interfaces; @@ -78,19 +82,16 @@ class TestActuator : public hardware_interface::components::ActuatorInterface return_type start() override { + status_ = status::STARTED; return return_type::OK; } return_type stop() override { + status_ = status::STOPPED; return return_type::OK; } - status get_status() const override - { - return status::UNKNOWN; - } - return_type read() override { return return_type::OK; @@ -105,7 +106,6 @@ class TestActuator : public hardware_interface::components::ActuatorInterface double position_state_ = 0.0; double velocity_state_ = 0.0; double velocity_command_ = 0.0; - hardware_interface::HardwareInfo actuator_info_; }; #include "pluginlib/class_list_macros.hpp" // NOLINT diff --git a/hardware_interface/test/test_components/test_sensor.cpp b/hardware_interface/test/test_components/test_sensor.cpp index 868878341f..c3406dc326 100644 --- a/hardware_interface/test/test_components/test_sensor.cpp +++ b/hardware_interface/test/test_components/test_sensor.cpp @@ -15,19 +15,25 @@ #include #include +#include "hardware_interface/components/base_interface.hpp" #include "hardware_interface/components/sensor_interface.hpp" using hardware_interface::status; using hardware_interface::return_type; using hardware_interface::StateInterface; -class TestSensor : public hardware_interface::components::SensorInterface +class TestSensor : public + hardware_interface::components::BaseInterface { return_type configure(const hardware_interface::HardwareInfo & sensor_info) override { - sensor_info_ = sensor_info; + if (configure_default(sensor_info) != return_type::OK) { + return return_type::ERROR; + } // can only give feedback state for velocity - if (sensor_info_.sensors[0].state_interfaces.size() != 1) {return return_type::ERROR;} + if (info_.sensors[0].state_interfaces.size() != 1) { + return return_type::ERROR; + } return return_type::OK; } @@ -36,8 +42,8 @@ class TestSensor : public hardware_interface::components::SensorInterface std::vector state_interfaces; state_interfaces.emplace_back( hardware_interface::StateInterface( - sensor_info_.sensors[0].name, - sensor_info_.sensors[0].state_interfaces[0].name, + info_.sensors[0].name, + info_.sensors[0].state_interfaces[0].name, &velocity_state_)); return state_interfaces; @@ -45,19 +51,16 @@ class TestSensor : public hardware_interface::components::SensorInterface return_type start() override { + status_ = status::STARTED; return return_type::OK; } return_type stop() override { + status_ = status::STOPPED; return return_type::OK; } - status get_status() const override - { - return status::UNKNOWN; - } - return_type read() override { return return_type::OK; @@ -65,7 +68,6 @@ class TestSensor : public hardware_interface::components::SensorInterface private: double velocity_state_ = 0.0; - hardware_interface::HardwareInfo sensor_info_; }; #include "pluginlib/class_list_macros.hpp" // NOLINT diff --git a/hardware_interface/test/test_components/test_system.cpp b/hardware_interface/test/test_components/test_system.cpp index 81b6d309f2..1340464e7a 100644 --- a/hardware_interface/test/test_components/test_system.cpp +++ b/hardware_interface/test/test_components/test_system.cpp @@ -16,26 +16,22 @@ #include #include -#include "hardware_interface/types/hardware_interface_type_values.hpp" +#include "hardware_interface/components/base_interface.hpp" #include "hardware_interface/components/system_interface.hpp" +#include "hardware_interface/types/hardware_interface_type_values.hpp" using hardware_interface::status; using hardware_interface::return_type; using hardware_interface::StateInterface; using hardware_interface::CommandInterface; -class TestSystem : public hardware_interface::components::SystemInterface +class TestSystem : public + hardware_interface::components::BaseInterface { - return_type configure(const hardware_interface::HardwareInfo & system_info) override - { - system_info_ = system_info; - return return_type::OK; - } - std::vector export_state_interfaces() override { std::vector state_interfaces; - for (auto i = 0u; i < system_info_.joints.size(); ++i) { + for (auto i = 0u; i < info_.joints.size(); ++i) { state_interfaces.emplace_back( hardware_interface::StateInterface( system_info_.joints[i].name, hardware_interface::HW_IF_POSITION, &position_state_[i])); @@ -54,7 +50,7 @@ class TestSystem : public hardware_interface::components::SystemInterface std::vector export_command_interfaces() override { std::vector command_interfaces; - for (auto i = 0u; i < system_info_.joints.size(); ++i) { + for (auto i = 0u; i < info_.joints.size(); ++i) { command_interfaces.emplace_back( hardware_interface::CommandInterface( system_info_.joints[i].name, hardware_interface::HW_IF_VELOCITY, &velocity_command_[i])); @@ -65,19 +61,16 @@ class TestSystem : public hardware_interface::components::SystemInterface return_type start() override { + status_ = status::STARTED; return return_type::OK; } return_type stop() override { + status_ = status::STOPPED; return return_type::OK; } - status get_status() const override - { - return status::UNKNOWN; - } - return_type read() override { return return_type::OK; @@ -93,7 +86,6 @@ class TestSystem : public hardware_interface::components::SystemInterface std::array position_state_ = {0.0, 0.0}; std::array velocity_state_ = {0.0, 0.0}; std::array acceleration_state_ = {0.0, 0.0}; - hardware_interface::HardwareInfo system_info_; }; #include "pluginlib/class_list_macros.hpp" // NOLINT diff --git a/hardware_interface/test/test_resource_manager.cpp b/hardware_interface/test/test_resource_manager.cpp index f9a4c8b14c..de57ee30b1 100644 --- a/hardware_interface/test/test_resource_manager.cpp +++ b/hardware_interface/test/test_resource_manager.cpp @@ -88,7 +88,7 @@ class TestResourceManager : public ::testing::Test )"; - test_hardware_resource_system_ = + hardware_resources_for_test_ = R"( @@ -127,7 +127,7 @@ class TestResourceManager : public ::testing::Test )"; - test_hardware_resource_system_missing_keys_ = + hardware_resources_for_test_missing_keys_ = R"( @@ -175,8 +175,8 @@ class TestResourceManager : public ::testing::Test } std::string urdf_head_; - std::string test_hardware_resource_system_; - std::string test_hardware_resource_system_missing_keys_; + std::string hardware_resources_for_test_; + std::string hardware_resources_for_test_missing_keys_; std::string urdf_tail_; }; @@ -185,18 +185,18 @@ TEST_F(TestResourceManager, initialization_empty) { } TEST_F(TestResourceManager, initialization_with_urdf) { - auto urdf = urdf_head_ + test_hardware_resource_system_ + urdf_tail_; + auto urdf = urdf_head_ + hardware_resources_for_test_ + urdf_tail_; ASSERT_NO_THROW(hardware_interface::ResourceManager rm(urdf)); } TEST_F(TestResourceManager, post_initialization_with_urdf) { - auto urdf = urdf_head_ + test_hardware_resource_system_ + urdf_tail_; + auto urdf = urdf_head_ + hardware_resources_for_test_ + urdf_tail_; hardware_interface::ResourceManager rm; ASSERT_NO_THROW(rm.load_urdf(urdf)); } TEST_F(TestResourceManager, initialization_with_urdf_manual_validation) { - auto urdf = urdf_head_ + test_hardware_resource_system_ + urdf_tail_; + auto urdf = urdf_head_ + hardware_resources_for_test_ + urdf_tail_; // we validate the results manually hardware_interface::ResourceManager rm(urdf, false); @@ -220,7 +220,7 @@ TEST_F(TestResourceManager, initialization_with_urdf_manual_validation) { } TEST_F(TestResourceManager, initialization_with_wrong_urdf) { - auto urdf = urdf_head_ + test_hardware_resource_system_missing_keys_ + urdf_tail_; + auto urdf = urdf_head_ + hardware_resources_for_test_missing_keys_ + urdf_tail_; try { hardware_interface::ResourceManager rm(urdf); FAIL(); @@ -231,7 +231,7 @@ TEST_F(TestResourceManager, initialization_with_wrong_urdf) { } TEST_F(TestResourceManager, initialization_with_urdf_unclaimed) { - auto urdf = urdf_head_ + test_hardware_resource_system_ + urdf_tail_; + auto urdf = urdf_head_ + hardware_resources_for_test_ + urdf_tail_; // we validate the results manually hardware_interface::ResourceManager rm(urdf); @@ -247,8 +247,39 @@ TEST_F(TestResourceManager, initialization_with_urdf_unclaimed) { } } +TEST_F(TestResourceManager, resource_status) { + auto urdf = urdf_head_ + hardware_resources_for_test_ + urdf_tail_; + hardware_interface::ResourceManager rm(urdf); + + std::unordered_map status_map; + + status_map = rm.get_components_status(); + EXPECT_EQ(status_map["TestActuatorHardware"], hardware_interface::status::CONFIGURED); + EXPECT_EQ(status_map["TestSensorHardware"], hardware_interface::status::CONFIGURED); + EXPECT_EQ(status_map["TestSystemHardware"], hardware_interface::status::CONFIGURED); +} + +TEST_F(TestResourceManager, starting_and_stopping_resources) { + auto urdf = urdf_head_ + hardware_resources_for_test_ + urdf_tail_; + hardware_interface::ResourceManager rm(urdf); + + std::unordered_map status_map; + + rm.start_all(); + status_map = rm.get_components_status(); + EXPECT_EQ(status_map["TestActuatorHardware"], hardware_interface::status::STARTED); + EXPECT_EQ(status_map["TestSensorHardware"], hardware_interface::status::STARTED); + EXPECT_EQ(status_map["TestSystemHardware"], hardware_interface::status::STARTED); + + rm.stop_all(); + status_map = rm.get_components_status(); + EXPECT_EQ(status_map["TestActuatorHardware"], hardware_interface::status::STOPPED); + EXPECT_EQ(status_map["TestSensorHardware"], hardware_interface::status::STOPPED); + EXPECT_EQ(status_map["TestSystemHardware"], hardware_interface::status::STOPPED); +} + TEST_F(TestResourceManager, resource_claiming) { - auto urdf = urdf_head_ + test_hardware_resource_system_ + urdf_tail_; + auto urdf = urdf_head_ + hardware_resources_for_test_ + urdf_tail_; hardware_interface::ResourceManager rm(urdf); const auto key = "joint1/position"; @@ -342,6 +373,11 @@ class ExternalComponent : public hardware_interface::components::ActuatorInterfa return hardware_interface::return_type::OK; } + std::string get_name() const override + { + return std::string("ExternalComponent"); + } + hardware_interface::status get_status() const override { return hardware_interface::status::UNKNOWN; @@ -359,7 +395,7 @@ class ExternalComponent : public hardware_interface::components::ActuatorInterfa }; TEST_F(TestResourceManager, post_initialization_add_components) { - auto urdf = urdf_head_ + test_hardware_resource_system_ + urdf_tail_; + auto urdf = urdf_head_ + hardware_resources_for_test_ + urdf_tail_; // we validate the results manually hardware_interface::ResourceManager rm(urdf, false); From 03e76fcd47a4d6b04fc03783737a8d64b85cafd5 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Sun, 22 Nov 2020 09:19:12 +0100 Subject: [PATCH 05/10] Update hardware_interface/include/hardware_interface/components/base_interface.hpp Co-authored-by: Bence Magyar --- .../include/hardware_interface/components/base_interface.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/hardware_interface/include/hardware_interface/components/base_interface.hpp b/hardware_interface/include/hardware_interface/components/base_interface.hpp index a5bf764bc3..80b89c7709 100644 --- a/hardware_interface/include/hardware_interface/components/base_interface.hpp +++ b/hardware_interface/include/hardware_interface/components/base_interface.hpp @@ -1,4 +1,4 @@ -// Copyright 2020 ROS2-Control Development Team +// 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. From 7ebee0717c1daf0a06c69aa7e95ca3e7fb695bfd Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Sun, 22 Nov 2020 09:26:40 +0100 Subject: [PATCH 06/10] Update hardware_interface/src/resource_manager.cpp Co-authored-by: Bence Magyar --- hardware_interface/src/resource_manager.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/hardware_interface/src/resource_manager.cpp b/hardware_interface/src/resource_manager.cpp index 9fc4613e35..dcfc8dc0dc 100644 --- a/hardware_interface/src/resource_manager.cpp +++ b/hardware_interface/src/resource_manager.cpp @@ -64,7 +64,8 @@ class ResourceStorage loader.createUnmanagedInstance(hardware_info.hardware_class_type)); HardwareT hardware(std::move(interface)); container.emplace_back(std::move(hardware)); - hardware_status_map_.emplace(std::make_pair(container.back().get_name(), container.back().get_status())); + hardware_status_map_.emplace( + std::make_pair(container.back().get_name(), container.back().get_status())); } template From 59c5abae65c6b70cf1db5fbaf59e198403c7f77b Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Tue, 24 Nov 2020 08:42:36 +0100 Subject: [PATCH 07/10] Updated to the comments --- controller_manager/src/controller_manager.cpp | 4 ++-- .../include/hardware_interface/resource_manager.hpp | 4 ++-- hardware_interface/src/resource_manager.cpp | 4 ++-- hardware_interface/test/test_components/test_actuator.cpp | 5 +++-- hardware_interface/test/test_components/test_sensor.cpp | 5 +++-- hardware_interface/test/test_components/test_system.cpp | 7 ++++--- hardware_interface/test/test_resource_manager.cpp | 4 ++-- 7 files changed, 18 insertions(+), 15 deletions(-) diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 94baba8eab..279fecb9f6 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -71,8 +71,8 @@ ControllerManager::ControllerManager( resource_manager_->load_urdf(robot_description); - //TODO(all): Here we should start only "auto-start" resources - resource_manager_->start_all(); + // TODO(all): Here we should start only "auto-start" resources + resource_manager_->start_components(); init_services(); } diff --git a/hardware_interface/include/hardware_interface/resource_manager.hpp b/hardware_interface/include/hardware_interface/resource_manager.hpp index 25f672b6f3..2f94515b62 100644 --- a/hardware_interface/include/hardware_interface/resource_manager.hpp +++ b/hardware_interface/include/hardware_interface/resource_manager.hpp @@ -198,10 +198,10 @@ class ResourceManager std::unordered_map get_components_status(); /// Start all loaded hardware components. - void start_all(); + void start_components(); /// Stops all loaded hardware components. - void stop_all(); + void stop_components(); /// Reads all loaded hardware components. void read(); diff --git a/hardware_interface/src/resource_manager.cpp b/hardware_interface/src/resource_manager.cpp index dcfc8dc0dc..401203e11b 100644 --- a/hardware_interface/src/resource_manager.cpp +++ b/hardware_interface/src/resource_manager.cpp @@ -310,7 +310,7 @@ std::unordered_map ResourceManager::get_components_status() return resource_storage_->hardware_status_map_; } -void ResourceManager::start_all() +void ResourceManager::start_components() { for (auto & component : resource_storage_->actuators_) { component.start(); @@ -323,7 +323,7 @@ void ResourceManager::start_all() } } -void ResourceManager::stop_all() +void ResourceManager::stop_components() { for (auto & component : resource_storage_->actuators_) { component.stop(); diff --git a/hardware_interface/test/test_components/test_actuator.cpp b/hardware_interface/test/test_components/test_actuator.cpp index c56cea9539..4ec1e30238 100644 --- a/hardware_interface/test/test_components/test_actuator.cpp +++ b/hardware_interface/test/test_components/test_actuator.cpp @@ -22,9 +22,10 @@ using hardware_interface::status; using hardware_interface::return_type; using hardware_interface::StateInterface; using hardware_interface::CommandInterface; +using hardware_interface::components::ActuatorInterface; +using hardware_interface::components::BaseInterface; -class TestActuator : public - hardware_interface::components::BaseInterface +class TestActuator : public BaseInterface { return_type configure(const hardware_interface::HardwareInfo & sensor_info) override { diff --git a/hardware_interface/test/test_components/test_sensor.cpp b/hardware_interface/test/test_components/test_sensor.cpp index c3406dc326..dbc2fffd2c 100644 --- a/hardware_interface/test/test_components/test_sensor.cpp +++ b/hardware_interface/test/test_components/test_sensor.cpp @@ -20,10 +20,11 @@ using hardware_interface::status; using hardware_interface::return_type; +using hardware_interface::components::BaseInterface; +using hardware_interface::components::SensorInterface; using hardware_interface::StateInterface; -class TestSensor : public - hardware_interface::components::BaseInterface +class TestSensor : public BaseInterface { return_type configure(const hardware_interface::HardwareInfo & sensor_info) override { diff --git a/hardware_interface/test/test_components/test_system.cpp b/hardware_interface/test/test_components/test_system.cpp index 1340464e7a..826b5aa59c 100644 --- a/hardware_interface/test/test_components/test_system.cpp +++ b/hardware_interface/test/test_components/test_system.cpp @@ -22,11 +22,12 @@ using hardware_interface::status; using hardware_interface::return_type; -using hardware_interface::StateInterface; using hardware_interface::CommandInterface; +using hardware_interface::StateInterface; +using hardware_interface::components::BaseInterface; +using hardware_interface::components::SystemInterface; -class TestSystem : public - hardware_interface::components::BaseInterface +class TestSystem : public BaseInterface { std::vector export_state_interfaces() override { diff --git a/hardware_interface/test/test_resource_manager.cpp b/hardware_interface/test/test_resource_manager.cpp index de57ee30b1..b0ab574731 100644 --- a/hardware_interface/test/test_resource_manager.cpp +++ b/hardware_interface/test/test_resource_manager.cpp @@ -265,13 +265,13 @@ TEST_F(TestResourceManager, starting_and_stopping_resources) { std::unordered_map status_map; - rm.start_all(); + rm.start_components(); status_map = rm.get_components_status(); EXPECT_EQ(status_map["TestActuatorHardware"], hardware_interface::status::STARTED); EXPECT_EQ(status_map["TestSensorHardware"], hardware_interface::status::STARTED); EXPECT_EQ(status_map["TestSystemHardware"], hardware_interface::status::STARTED); - rm.stop_all(); + rm.stop_components(); status_map = rm.get_components_status(); EXPECT_EQ(status_map["TestActuatorHardware"], hardware_interface::status::STOPPED); EXPECT_EQ(status_map["TestSensorHardware"], hardware_interface::status::STOPPED); From d648c10e8667bfec621c2fcc4ae15328c9c9bd17 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Fri, 27 Nov 2020 00:00:55 +0100 Subject: [PATCH 08/10] Correct all issues after rebasing. --- controller_manager/src/controller_manager.cpp | 6 ------ controller_manager/test/test_controller_manager_srvs.cpp | 6 ------ .../include/hardware_interface/components/actuator.hpp | 1 + hardware_interface/test/test_components/test_actuator.cpp | 4 ++-- hardware_interface/test/test_components/test_system.cpp | 8 ++++---- hardware_interface/test/test_resource_manager.cpp | 1 + 6 files changed, 8 insertions(+), 18 deletions(-) diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 279fecb9f6..3d24f99ddb 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -98,12 +98,6 @@ void ControllerManager::init_services() rclcpp::CallbackGroupType::MutuallyExclusive); using namespace std::placeholders; - list_controller_interfaces_service_ = - create_service( - "~/list_controller_interfaces", - std::bind(&ControllerManager::list_controller_interfaces_srv_cb, this, _1, _2), - rmw_qos_profile_services_default, - best_effort_callback_group_); list_controllers_service_ = create_service( "~/list_controllers", std::bind(&ControllerManager::list_controllers_srv_cb, this, _1, _2), diff --git a/controller_manager/test/test_controller_manager_srvs.cpp b/controller_manager/test/test_controller_manager_srvs.cpp index 7e42695bed..e9849bdb73 100644 --- a/controller_manager/test/test_controller_manager_srvs.cpp +++ b/controller_manager/test/test_controller_manager_srvs.cpp @@ -208,13 +208,10 @@ TEST_F(TestControllerManagerSrvs, reload_controller_libraries_srv) { auto test_controller = cm_->load_controller( test_controller::TEST_CONTROLLER_NAME, test_controller::TEST_CONTROLLER_CLASS_NAME); -<<<<<<< HEAD // weak_ptr so the only controller shared_ptr instance is owned by the controller_manager and // can be completely destroyed before reloading the library std::weak_ptr test_controller_weak(test_controller); -======= ->>>>>>> adapt controller manager test ASSERT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, @@ -239,10 +236,7 @@ TEST_F(TestControllerManagerSrvs, reload_controller_libraries_srv) { test_controller = cm_->load_controller( test_controller::TEST_CONTROLLER_NAME, test_controller::TEST_CONTROLLER_CLASS_NAME); -<<<<<<< HEAD test_controller_weak = test_controller; -======= ->>>>>>> adapt controller manager test // Start Controller cm_->switch_controller( {test_controller::TEST_CONTROLLER_NAME}, {}, diff --git a/hardware_interface/include/hardware_interface/components/actuator.hpp b/hardware_interface/include/hardware_interface/components/actuator.hpp index 94d0ca56b2..23ef1e5879 100644 --- a/hardware_interface/include/hardware_interface/components/actuator.hpp +++ b/hardware_interface/include/hardware_interface/components/actuator.hpp @@ -16,6 +16,7 @@ #define HARDWARE_INTERFACE__COMPONENTS__ACTUATOR_HPP_ #include +#include #include #include "hardware_interface/handle.hpp" diff --git a/hardware_interface/test/test_components/test_actuator.cpp b/hardware_interface/test/test_components/test_actuator.cpp index 4ec1e30238..94b2ec7130 100644 --- a/hardware_interface/test/test_components/test_actuator.cpp +++ b/hardware_interface/test/test_components/test_actuator.cpp @@ -27,9 +27,9 @@ using hardware_interface::components::BaseInterface; class TestActuator : public BaseInterface { - return_type configure(const hardware_interface::HardwareInfo & sensor_info) override + return_type configure(const hardware_interface::HardwareInfo & info) override { - if (configure_default(sensor_info) != return_type::OK) { + if (configure_default(info) != return_type::OK) { return return_type::ERROR; } diff --git a/hardware_interface/test/test_components/test_system.cpp b/hardware_interface/test/test_components/test_system.cpp index 826b5aa59c..a76e5a4fa7 100644 --- a/hardware_interface/test/test_components/test_system.cpp +++ b/hardware_interface/test/test_components/test_system.cpp @@ -35,13 +35,13 @@ class TestSystem : public BaseInterface for (auto i = 0u; i < info_.joints.size(); ++i) { state_interfaces.emplace_back( hardware_interface::StateInterface( - system_info_.joints[i].name, hardware_interface::HW_IF_POSITION, &position_state_[i])); + info_.joints[i].name, hardware_interface::HW_IF_POSITION, &position_state_[i])); state_interfaces.emplace_back( hardware_interface::StateInterface( - system_info_.joints[i].name, hardware_interface::HW_IF_VELOCITY, &velocity_state_[i])); + info_.joints[i].name, hardware_interface::HW_IF_VELOCITY, &velocity_state_[i])); state_interfaces.emplace_back( hardware_interface::StateInterface( - system_info_.joints[i].name, + info_.joints[i].name, hardware_interface::HW_IF_ACCELERATION, &acceleration_state_[i])); } @@ -54,7 +54,7 @@ class TestSystem : public BaseInterface for (auto i = 0u; i < info_.joints.size(); ++i) { command_interfaces.emplace_back( hardware_interface::CommandInterface( - system_info_.joints[i].name, hardware_interface::HW_IF_VELOCITY, &velocity_command_[i])); + info_.joints[i].name, hardware_interface::HW_IF_VELOCITY, &velocity_command_[i])); } return command_interfaces; diff --git a/hardware_interface/test/test_resource_manager.cpp b/hardware_interface/test/test_resource_manager.cpp index b0ab574731..f88b63aa73 100644 --- a/hardware_interface/test/test_resource_manager.cpp +++ b/hardware_interface/test/test_resource_manager.cpp @@ -18,6 +18,7 @@ #include #include #include +#include #include "hardware_interface/components/actuator_interface.hpp" #include "hardware_interface/resource_manager.hpp" From a7d7a25d6e043b28632bdde4c80835f44f1699ce Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Fri, 27 Nov 2020 00:26:21 +0100 Subject: [PATCH 09/10] Update test_robot_hardware package to use constants and BaseInterface --- .../test/test_components/test_sensor.cpp | 6 +-- .../src/test_force_torque_sensor.cpp | 19 +++++---- .../src/test_single_joint_actuator.cpp | 40 ++++++++++--------- .../src/test_two_joint_system.cpp | 36 +++++++++-------- 4 files changed, 53 insertions(+), 48 deletions(-) diff --git a/hardware_interface/test/test_components/test_sensor.cpp b/hardware_interface/test/test_components/test_sensor.cpp index dbc2fffd2c..e9b64afc4b 100644 --- a/hardware_interface/test/test_components/test_sensor.cpp +++ b/hardware_interface/test/test_components/test_sensor.cpp @@ -20,15 +20,15 @@ using hardware_interface::status; using hardware_interface::return_type; +using hardware_interface::StateInterface; using hardware_interface::components::BaseInterface; using hardware_interface::components::SensorInterface; -using hardware_interface::StateInterface; class TestSensor : public BaseInterface { - return_type configure(const hardware_interface::HardwareInfo & sensor_info) override + return_type configure(const hardware_interface::HardwareInfo & info) override { - if (configure_default(sensor_info) != return_type::OK) { + if (configure_default(info) != return_type::OK) { return return_type::ERROR; } // can only give feedback state for velocity diff --git a/test_robot_hardware/src/test_force_torque_sensor.cpp b/test_robot_hardware/src/test_force_torque_sensor.cpp index baf4304d33..e09ed0d6f6 100644 --- a/test_robot_hardware/src/test_force_torque_sensor.cpp +++ b/test_robot_hardware/src/test_force_torque_sensor.cpp @@ -17,22 +17,27 @@ #include #include +#include "hardware_interface/components/base_interface.hpp" #include "hardware_interface/components/sensor_interface.hpp" using hardware_interface::status; using hardware_interface::return_type; using hardware_interface::StateInterface; +using hardware_interface::components::BaseInterface; +using hardware_interface::components::SensorInterface; namespace test_robot_hardware { -class TestForceTorqueSensor : public hardware_interface::components::SensorInterface +class TestForceTorqueSensor : public BaseInterface { return_type configure(const hardware_interface::HardwareInfo & sensor_info) override { - sensor_info_ = sensor_info; + if (configure_default(sensor_info) != return_type::OK) { + return return_type::ERROR; + } - const auto & state_interfaces = sensor_info_.sensors[0].state_interfaces; + const auto & state_interfaces = info_.sensors[0].state_interfaces; if (state_interfaces.size() != 6) {return return_type::ERROR;} for (const auto & ft_key : {"fx", "fy", "fz", "tx", "ty", "tz"}) { if (std::find_if( @@ -52,7 +57,7 @@ class TestForceTorqueSensor : public hardware_interface::components::SensorInter { std::vector state_interfaces; - const auto & sensor_name = sensor_info_.sensors[0].name; + const auto & sensor_name = info_.sensors[0].name; state_interfaces.emplace_back( hardware_interface::StateInterface( sensor_name, @@ -97,11 +102,6 @@ class TestForceTorqueSensor : public hardware_interface::components::SensorInter return return_type::OK; } - status get_status() const override - { - return status::UNKNOWN; - } - return_type read() override { values_.fx = fmod((values_.fx + 1.0), 10); @@ -125,7 +125,6 @@ class TestForceTorqueSensor : public hardware_interface::components::SensorInter }; FTValues values_; - hardware_interface::HardwareInfo sensor_info_; }; } // namespace test_robot_hardware diff --git a/test_robot_hardware/src/test_single_joint_actuator.cpp b/test_robot_hardware/src/test_single_joint_actuator.cpp index 0bcccfd3f2..1f7479158f 100644 --- a/test_robot_hardware/src/test_single_joint_actuator.cpp +++ b/test_robot_hardware/src/test_single_joint_actuator.cpp @@ -15,34 +15,42 @@ #include #include +#include "hardware_interface/components/base_interface.hpp" #include "hardware_interface/components/actuator_interface.hpp" +#include "hardware_interface/types/hardware_interface_type_values.hpp" using hardware_interface::status; using hardware_interface::return_type; using hardware_interface::StateInterface; using hardware_interface::CommandInterface; +using hardware_interface::components::ActuatorInterface; +using hardware_interface::components::BaseInterface; namespace test_robot_hardware { -class TestSingleJointActuator : public hardware_interface::components::ActuatorInterface +class TestSingleJointActuator : public BaseInterface { return_type configure(const hardware_interface::HardwareInfo & actuator_info) override { - actuator_info_ = actuator_info; + if (configure_default(actuator_info) != return_type::OK) { + return return_type::ERROR; + } // can only control one joint - if (actuator_info_.joints.size() != 1) {return return_type::ERROR;} + if (info_.joints.size() != 1) {return return_type::ERROR;} // can only control in position - const auto & command_interfaces = actuator_info_.joints[0].command_interfaces; + const auto & command_interfaces = info_.joints[0].command_interfaces; if (command_interfaces.size() != 1) {return return_type::ERROR;} - if (command_interfaces[0].name != "position") {return return_type::ERROR;} + if (command_interfaces[0].name != hardware_interface::HW_IF_POSITION) { + return return_type::ERROR; + } // can only give feedback state for position and velocity - const auto & state_interfaces = actuator_info_.joints[0].state_interfaces; + const auto & state_interfaces = info_.joints[0].state_interfaces; if (state_interfaces.size() < 1) {return return_type::ERROR;} for (const auto & state_interface : state_interfaces) { - if ((state_interface.name != "position") && - (state_interface.name != "velocity")) + if ((state_interface.name != hardware_interface::HW_IF_POSITION) && + (state_interface.name != hardware_interface::HW_IF_VELOCITY)) { return return_type::ERROR; } @@ -55,16 +63,16 @@ class TestSingleJointActuator : public hardware_interface::components::ActuatorI { std::vector state_interfaces; - const auto & joint_name = actuator_info_.joints[0].name; + const auto & joint_name = info_.joints[0].name; state_interfaces.emplace_back( hardware_interface::StateInterface( joint_name, - "position", + hardware_interface::HW_IF_POSITION, &position_state_)); state_interfaces.emplace_back( hardware_interface::StateInterface( joint_name, - "velocity", + hardware_interface::HW_IF_VELOCITY, &velocity_state_)); return state_interfaces; @@ -74,11 +82,11 @@ class TestSingleJointActuator : public hardware_interface::components::ActuatorI { std::vector command_interfaces; - const auto & joint_name = actuator_info_.joints[0].name; + const auto & joint_name = info_.joints[0].name; command_interfaces.emplace_back( hardware_interface::CommandInterface( joint_name, - "position", + hardware_interface::HW_IF_POSITION, &position_command_)); return command_interfaces; @@ -94,11 +102,6 @@ class TestSingleJointActuator : public hardware_interface::components::ActuatorI return return_type::OK; } - status get_status() const override - { - return status::UNKNOWN; - } - return_type read() override { return return_type::OK; @@ -115,7 +118,6 @@ class TestSingleJointActuator : public hardware_interface::components::ActuatorI double position_state_ = 0.0; double velocity_state_ = 0.0; double position_command_ = 0.0; - hardware_interface::HardwareInfo actuator_info_; }; } // namespace test_robot_hardware diff --git a/test_robot_hardware/src/test_two_joint_system.cpp b/test_robot_hardware/src/test_two_joint_system.cpp index 01d8d452ff..adba3a6ca7 100644 --- a/test_robot_hardware/src/test_two_joint_system.cpp +++ b/test_robot_hardware/src/test_two_joint_system.cpp @@ -16,33 +16,43 @@ #include #include +#include "hardware_interface/components/base_interface.hpp" #include "hardware_interface/components/system_interface.hpp" +#include "hardware_interface/types/hardware_interface_type_values.hpp" using hardware_interface::status; using hardware_interface::return_type; using hardware_interface::StateInterface; using hardware_interface::CommandInterface; +using hardware_interface::components::BaseInterface; +using hardware_interface::components::SystemInterface; namespace test_robot_hardware { -class TestTwoJointSystem : public hardware_interface::components::SystemInterface +class TestTwoJointSystem : public BaseInterface { return_type configure(const hardware_interface::HardwareInfo & system_info) override { - system_info_ = system_info; + if (configure_default(system_info) != return_type::OK) { + return return_type::ERROR; + } // can only control two joint - if (system_info_.joints.size() != 2) {return return_type::ERROR;} - for (const auto & joint : system_info_.joints) { + if (info_.joints.size() != 2) {return return_type::ERROR;} + for (const auto & joint : info_.joints) { // can only control in position const auto & command_interfaces = joint.command_interfaces; if (command_interfaces.size() != 1) {return return_type::ERROR;} - if (command_interfaces[0].name != "position") {return return_type::ERROR;} + if (command_interfaces[0].name != hardware_interface::HW_IF_POSITION) { + return return_type::ERROR; + } // can only give feedback state for position and velocity const auto & state_interfaces = joint.state_interfaces; if (state_interfaces.size() != 1) {return return_type::ERROR;} - if (state_interfaces[0].name != "position") {return return_type::ERROR;} + if (state_interfaces[0].name != hardware_interface::HW_IF_POSITION) { + return return_type::ERROR; + } } fprintf(stderr, "TestTwoJointSystem configured successfully.\n"); @@ -52,10 +62,10 @@ class TestTwoJointSystem : public hardware_interface::components::SystemInterfac std::vector export_state_interfaces() override { std::vector state_interfaces; - for (auto i = 0u; i < system_info_.joints.size(); ++i) { + for (auto i = 0u; i < info_.joints.size(); ++i) { state_interfaces.emplace_back( hardware_interface::StateInterface( - system_info_.joints[i].name, "position", &position_state_[i])); + info_.joints[i].name, hardware_interface::HW_IF_POSITION, &position_state_[i])); } return state_interfaces; @@ -64,10 +74,10 @@ class TestTwoJointSystem : public hardware_interface::components::SystemInterfac std::vector export_command_interfaces() override { std::vector command_interfaces; - for (auto i = 0u; i < system_info_.joints.size(); ++i) { + for (auto i = 0u; i < info_.joints.size(); ++i) { command_interfaces.emplace_back( hardware_interface::CommandInterface( - system_info_.joints[i].name, "position", &position_command_[i])); + info_.joints[i].name, hardware_interface::HW_IF_POSITION, &position_command_[i])); } return command_interfaces; @@ -83,11 +93,6 @@ class TestTwoJointSystem : public hardware_interface::components::SystemInterfac return return_type::OK; } - status get_status() const override - { - return status::UNKNOWN; - } - return_type read() override { return return_type::OK; @@ -101,7 +106,6 @@ class TestTwoJointSystem : public hardware_interface::components::SystemInterfac private: std::array position_command_ = {0.0, 0.0}; std::array position_state_ = {0.0, 0.0}; - hardware_interface::HardwareInfo system_info_; }; } // namespace test_robot_hardware From 01dc00851a6f039b0a5a2064a0adbcbe44da635e Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Wed, 2 Dec 2020 09:03:01 +0000 Subject: [PATCH 10/10] shorter --- hardware_interface/test/test_component_interfaces.cpp | 6 +++--- hardware_interface/test/test_resource_manager.cpp | 2 +- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/hardware_interface/test/test_component_interfaces.cpp b/hardware_interface/test/test_component_interfaces.cpp index 1416fc2076..f483d24fa9 100644 --- a/hardware_interface/test/test_component_interfaces.cpp +++ b/hardware_interface/test/test_component_interfaces.cpp @@ -82,7 +82,7 @@ class DummyActuator : public hardware_interface::components::ActuatorInterface std::string get_name() const override { - return std::string("DummyActuator"); + return "DummyActuator"; } hardware_interface::status get_status() const override @@ -141,7 +141,7 @@ class DummySensor : public hardware_interface::components::SensorInterface std::string get_name() const override { - return std::string("DummySensor"); + return "DummySensor"; } hardware_interface::status get_status() const override @@ -223,7 +223,7 @@ class DummySystem : public hardware_interface::components::SystemInterface std::string get_name() const override { - return std::string("DummySystem"); + return "DummySystem"; } hardware_interface::status get_status() const override diff --git a/hardware_interface/test/test_resource_manager.cpp b/hardware_interface/test/test_resource_manager.cpp index f88b63aa73..c104ac043c 100644 --- a/hardware_interface/test/test_resource_manager.cpp +++ b/hardware_interface/test/test_resource_manager.cpp @@ -376,7 +376,7 @@ class ExternalComponent : public hardware_interface::components::ActuatorInterfa std::string get_name() const override { - return std::string("ExternalComponent"); + return "ExternalComponent"; } hardware_interface::status get_status() const override