From d3d1c95797cb8e7efcf365ac498a1ece78913b36 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Mon, 29 Jul 2024 20:01:57 +0200 Subject: [PATCH] [RM] Add `get_hardware_info` method to the Hardware Components (#1643) --- doc/release_notes.rst | 4 +- .../hardware_interface/actuator_interface.hpp | 6 + .../hardware_interface/sensor_interface.hpp | 6 + .../hardware_interface/system_interface.hpp | 6 + .../src/mock_components/generic_system.cpp | 105 ++++++++++-------- .../test_force_torque_sensor.cpp | 4 +- .../test_imu_sensor.cpp | 4 +- .../test_single_joint_actuator.cpp | 10 +- .../test_system_with_command_modes.cpp | 35 +++--- .../test_two_joint_system.cpp | 14 ++- .../test/test_components/test_actuator.cpp | 26 +++-- .../test/test_components/test_sensor.cpp | 5 +- .../test/test_components/test_system.cpp | 27 ++--- 13 files changed, 146 insertions(+), 106 deletions(-) diff --git a/doc/release_notes.rst b/doc/release_notes.rst index e2b22bf7a6..3c516e6320 100644 --- a/doc/release_notes.rst +++ b/doc/release_notes.rst @@ -21,8 +21,7 @@ For details see the controller_manager section. * Pass controller manager update rate on the init of the controller interface (`#1141 `_) * A method to get node options to setup the controller node #api-breaking (`#1169 `_) * Export state interfaces from the chainable controller #api-breaking (`#1021 `_) - - * All chainable controllers must implement the method ``export_state_interfaces`` to export the state interfaces, similar to ``export_reference_interfaces`` method that is exporting the reference interfaces. +* All chainable controllers must implement the method ``export_state_interfaces`` to export the state interfaces, similar to ``export_reference_interfaces`` method that is exporting the reference interfaces. controller_manager ****************** @@ -100,6 +99,7 @@ hardware_interface * Soft limits are also parsed from the URDF into the ``HardwareInfo`` structure for the defined joints (`#1488 `_) * Access to logger and clock through ``get_logger`` and ``get_clock`` methods in ResourceManager and HardwareComponents ``Actuator``, ``Sensor`` and ``System`` (`#1585 `_) +* Added ``get_hardware_info`` method to the hardware components interface to access the ``HardwareInfo`` instead of accessing the variable ``info_`` directly (`#1643 `_) joint_limits ************ diff --git a/hardware_interface/include/hardware_interface/actuator_interface.hpp b/hardware_interface/include/hardware_interface/actuator_interface.hpp index b637068fe3..1228d88bcd 100644 --- a/hardware_interface/include/hardware_interface/actuator_interface.hpp +++ b/hardware_interface/include/hardware_interface/actuator_interface.hpp @@ -241,6 +241,12 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod */ rclcpp::Clock::SharedPtr get_clock() const { return clock_interface_->get_clock(); } + /// Get the hardware info of the ActuatorInterface. + /** + * \return hardware info of the ActuatorInterface. + */ + const HardwareInfo & get_hardware_info() const { return info_; } + HardwareInfo info_; rclcpp_lifecycle::State lifecycle_state_; diff --git a/hardware_interface/include/hardware_interface/sensor_interface.hpp b/hardware_interface/include/hardware_interface/sensor_interface.hpp index 2cff8b2a3a..6fde3f8891 100644 --- a/hardware_interface/include/hardware_interface/sensor_interface.hpp +++ b/hardware_interface/include/hardware_interface/sensor_interface.hpp @@ -180,6 +180,12 @@ class SensorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI */ rclcpp::Clock::SharedPtr get_clock() const { return clock_interface_->get_clock(); } + /// Get the hardware info of the SensorInterface. + /** + * \return hardware info of the SensorInterface. + */ + const HardwareInfo & get_hardware_info() const { return info_; } + HardwareInfo info_; rclcpp_lifecycle::State lifecycle_state_; diff --git a/hardware_interface/include/hardware_interface/system_interface.hpp b/hardware_interface/include/hardware_interface/system_interface.hpp index f9173fb754..06358f205e 100644 --- a/hardware_interface/include/hardware_interface/system_interface.hpp +++ b/hardware_interface/include/hardware_interface/system_interface.hpp @@ -242,6 +242,12 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI */ rclcpp::Clock::SharedPtr get_clock() const { return clock_interface_->get_clock(); } + /// Get the hardware info of the SystemInterface. + /** + * \return hardware info of the SystemInterface. + */ + const HardwareInfo & get_hardware_info() const { return info_; } + HardwareInfo info_; rclcpp_lifecycle::State lifecycle_state_; diff --git a/hardware_interface/src/mock_components/generic_system.cpp b/hardware_interface/src/mock_components/generic_system.cpp index abe36ba262..8b46d21f4d 100644 --- a/hardware_interface/src/mock_components/generic_system.cpp +++ b/hardware_interface/src/mock_components/generic_system.cpp @@ -59,8 +59,8 @@ CallbackReturn GenericSystem::on_init(const hardware_interface::HardwareInfo & i }; // check if to create mock command interface for sensor - auto it = info_.hardware_parameters.find("mock_sensor_commands"); - if (it != info_.hardware_parameters.end()) + auto it = get_hardware_info().hardware_parameters.find("mock_sensor_commands"); + if (it != get_hardware_info().hardware_parameters.end()) { use_mock_sensor_command_interfaces_ = hardware_interface::parse_bool(it->second); } @@ -70,8 +70,8 @@ CallbackReturn GenericSystem::on_init(const hardware_interface::HardwareInfo & i } // check if to create mock command interface for gpio - it = info_.hardware_parameters.find("mock_gpio_commands"); - if (it != info_.hardware_parameters.end()) + it = get_hardware_info().hardware_parameters.find("mock_gpio_commands"); + if (it != get_hardware_info().hardware_parameters.end()) { use_mock_gpio_command_interfaces_ = hardware_interface::parse_bool(it->second); } @@ -82,7 +82,7 @@ CallbackReturn GenericSystem::on_init(const hardware_interface::HardwareInfo & i // check if there is parameter that disables commands // this way we simulate disconnected driver - it = info_.hardware_parameters.find("disable_commands"); + it = get_hardware_info().hardware_parameters.find("disable_commands"); if (it != info.hardware_parameters.end()) { command_propagation_disabled_ = hardware_interface::parse_bool(it->second); @@ -93,7 +93,7 @@ CallbackReturn GenericSystem::on_init(const hardware_interface::HardwareInfo & i } // check if there is parameter that enables dynamic calculation - it = info_.hardware_parameters.find("calculate_dynamics"); + it = get_hardware_info().hardware_parameters.find("calculate_dynamics"); if (it != info.hardware_parameters.end()) { calculate_dynamics_ = hardware_interface::parse_bool(it->second); @@ -107,12 +107,12 @@ CallbackReturn GenericSystem::on_init(const hardware_interface::HardwareInfo & i position_state_following_offset_ = 0.0; custom_interface_with_following_offset_ = ""; - it = info_.hardware_parameters.find("position_state_following_offset"); - if (it != info_.hardware_parameters.end()) + it = get_hardware_info().hardware_parameters.find("position_state_following_offset"); + if (it != get_hardware_info().hardware_parameters.end()) { position_state_following_offset_ = hardware_interface::stod(it->second); - it = info_.hardware_parameters.find("custom_interface_with_following_offset"); - if (it != info_.hardware_parameters.end()) + it = get_hardware_info().hardware_parameters.find("custom_interface_with_following_offset"); + if (it != get_hardware_info().hardware_parameters.end()) { custom_interface_with_following_offset_ = it->second; } @@ -121,9 +121,10 @@ CallbackReturn GenericSystem::on_init(const hardware_interface::HardwareInfo & i index_custom_interface_with_following_offset_ = std::numeric_limits::max(); // Initialize storage for standard interfaces - initialize_storage_vectors(joint_commands_, joint_states_, standard_interfaces_, info_.joints); + initialize_storage_vectors( + joint_commands_, joint_states_, standard_interfaces_, get_hardware_info().joints); // set all values without initial values to 0 - for (auto i = 0u; i < info_.joints.size(); i++) + for (auto i = 0u; i < get_hardware_info().joints.size(); i++) { for (auto j = 0u; j < standard_interfaces_.size(); j++) { @@ -135,7 +136,7 @@ CallbackReturn GenericSystem::on_init(const hardware_interface::HardwareInfo & i } // search for non-standard joint interfaces - for (const auto & joint : info_.joints) + for (const auto & joint : get_hardware_info().joints) { // populate non-standard command interfaces to other_interfaces_ populate_non_standard_interfaces(joint.command_interfaces, other_interfaces_); @@ -145,7 +146,8 @@ CallbackReturn GenericSystem::on_init(const hardware_interface::HardwareInfo & i } // Initialize storage for non-standard interfaces - initialize_storage_vectors(other_commands_, other_states_, other_interfaces_, info_.joints); + initialize_storage_vectors( + other_commands_, other_states_, other_interfaces_, get_hardware_info().joints); // when following offset is used on custom interface then find its index if (!custom_interface_with_following_offset_.empty()) @@ -170,7 +172,7 @@ CallbackReturn GenericSystem::on_init(const hardware_interface::HardwareInfo & i } } - for (const auto & sensor : info_.sensors) + for (const auto & sensor : get_hardware_info().sensors) { for (const auto & interface : sensor.state_interfaces) { @@ -183,10 +185,10 @@ CallbackReturn GenericSystem::on_init(const hardware_interface::HardwareInfo & i } } initialize_storage_vectors( - sensor_mock_commands_, sensor_states_, sensor_interfaces_, info_.sensors); + sensor_mock_commands_, sensor_states_, sensor_interfaces_, get_hardware_info().sensors); // search for gpio interfaces - for (const auto & gpio : info_.gpios) + for (const auto & gpio : get_hardware_info().gpios) { // populate non-standard command interfaces to gpio_interfaces_ populate_non_standard_interfaces(gpio.command_interfaces, gpio_interfaces_); @@ -198,12 +200,14 @@ CallbackReturn GenericSystem::on_init(const hardware_interface::HardwareInfo & i // Mock gpio command interfaces if (use_mock_gpio_command_interfaces_) { - initialize_storage_vectors(gpio_mock_commands_, gpio_states_, gpio_interfaces_, info_.gpios); + initialize_storage_vectors( + gpio_mock_commands_, gpio_states_, gpio_interfaces_, get_hardware_info().gpios); } // Real gpio command interfaces else { - initialize_storage_vectors(gpio_commands_, gpio_states_, gpio_interfaces_, info_.gpios); + initialize_storage_vectors( + gpio_commands_, gpio_states_, gpio_interfaces_, get_hardware_info().gpios); } return CallbackReturn::SUCCESS; @@ -214,9 +218,9 @@ std::vector GenericSystem::export_state_inte std::vector state_interfaces; // Joints' state interfaces - for (auto i = 0u; i < info_.joints.size(); i++) + for (auto i = 0u; i < get_hardware_info().joints.size(); i++) { - const auto & joint = info_.joints[i]; + const auto & joint = get_hardware_info().joints[i]; for (const auto & interface : joint.state_interfaces) { // Add interface: if not in the standard list then use "other" interface list @@ -236,14 +240,15 @@ std::vector GenericSystem::export_state_inte // Sensor state interfaces if (!populate_interfaces( - info_.sensors, sensor_interfaces_, sensor_states_, state_interfaces, true)) + get_hardware_info().sensors, sensor_interfaces_, sensor_states_, state_interfaces, true)) { throw std::runtime_error( "Interface is not found in the standard nor other list. This should never happen!"); }; // GPIO state interfaces - if (!populate_interfaces(info_.gpios, gpio_interfaces_, gpio_states_, state_interfaces, true)) + if (!populate_interfaces( + get_hardware_info().gpios, gpio_interfaces_, gpio_states_, state_interfaces, true)) { throw std::runtime_error("Interface is not found in the gpio list. This should never happen!"); } @@ -256,9 +261,9 @@ std::vector GenericSystem::export_command_ std::vector command_interfaces; // Joints' state interfaces - for (size_t i = 0; i < info_.joints.size(); ++i) + for (size_t i = 0; i < get_hardware_info().joints.size(); ++i) { - const auto & joint = info_.joints[i]; + const auto & joint = get_hardware_info().joints[i]; for (const auto & interface : joint.command_interfaces) { // Add interface: if not in the standard list than use "other" interface list @@ -278,13 +283,14 @@ std::vector GenericSystem::export_command_ } } // Set position control mode per default - joint_control_mode_.resize(info_.joints.size(), POSITION_INTERFACE_INDEX); + joint_control_mode_.resize(get_hardware_info().joints.size(), POSITION_INTERFACE_INDEX); // Mock sensor command interfaces if (use_mock_sensor_command_interfaces_) { if (!populate_interfaces( - info_.sensors, sensor_interfaces_, sensor_mock_commands_, command_interfaces, true)) + get_hardware_info().sensors, sensor_interfaces_, sensor_mock_commands_, + command_interfaces, true)) { throw std::runtime_error( "Interface is not found in the standard nor other list. This should never happen!"); @@ -295,7 +301,8 @@ std::vector GenericSystem::export_command_ if (use_mock_gpio_command_interfaces_) { if (!populate_interfaces( - info_.gpios, gpio_interfaces_, gpio_mock_commands_, command_interfaces, true)) + get_hardware_info().gpios, gpio_interfaces_, gpio_mock_commands_, command_interfaces, + true)) { throw std::runtime_error( "Interface is not found in the gpio list. This should never happen!"); @@ -305,7 +312,7 @@ std::vector GenericSystem::export_command_ else { if (!populate_interfaces( - info_.gpios, gpio_interfaces_, gpio_commands_, command_interfaces, false)) + get_hardware_info().gpios, gpio_interfaces_, gpio_commands_, command_interfaces, false)) { throw std::runtime_error( "Interface is not found in the gpio list. This should never happen!"); @@ -328,29 +335,30 @@ return_type GenericSystem::prepare_command_mode_switch( const size_t FOUND_ONCE_FLAG = 1000000; + const auto & info = get_hardware_info(); std::vector joint_found_in_x_requests_; - joint_found_in_x_requests_.resize(info_.joints.size(), 0); + joint_found_in_x_requests_.resize(info.joints.size(), 0); for (const auto & key : start_interfaces) { // check if interface is joint auto joint_it_found = std::find_if( - info_.joints.begin(), info_.joints.end(), + info.joints.begin(), info.joints.end(), [key](const auto & joint) { return (key.find(joint.name) != std::string::npos); }); - if (joint_it_found != info_.joints.end()) + if (joint_it_found != info.joints.end()) { - const size_t joint_index = std::distance(info_.joints.begin(), joint_it_found); + const size_t joint_index = std::distance(info.joints.begin(), joint_it_found); if (joint_found_in_x_requests_[joint_index] == 0) { joint_found_in_x_requests_[joint_index] = FOUND_ONCE_FLAG; } - if (key == info_.joints[joint_index].name + "/" + hardware_interface::HW_IF_POSITION) + if (key == info.joints[joint_index].name + "/" + hardware_interface::HW_IF_POSITION) { joint_found_in_x_requests_[joint_index] += 1; } - if (key == info_.joints[joint_index].name + "/" + hardware_interface::HW_IF_VELOCITY) + if (key == info.joints[joint_index].name + "/" + hardware_interface::HW_IF_VELOCITY) { if (!calculate_dynamics_) { @@ -358,11 +366,11 @@ return_type GenericSystem::prepare_command_mode_switch( get_logger(), "Requested velocity mode for joint '%s' without dynamics calculation enabled - this " "might lead to wrong feedback and unexpected behavior.", - info_.joints[joint_index].name.c_str()); + info.joints[joint_index].name.c_str()); } joint_found_in_x_requests_[joint_index] += 1; } - if (key == info_.joints[joint_index].name + "/" + hardware_interface::HW_IF_ACCELERATION) + if (key == info.joints[joint_index].name + "/" + hardware_interface::HW_IF_ACCELERATION) { if (!calculate_dynamics_) { @@ -370,7 +378,7 @@ return_type GenericSystem::prepare_command_mode_switch( get_logger(), "Requested acceleration mode for joint '%s' without dynamics calculation enabled - " "this might lead to wrong feedback and unexpected behavior.", - info_.joints[joint_index].name.c_str()); + info.joints[joint_index].name.c_str()); } joint_found_in_x_requests_[joint_index] += 1; } @@ -382,14 +390,14 @@ return_type GenericSystem::prepare_command_mode_switch( } } - for (size_t i = 0; i < info_.joints.size(); ++i) + for (size_t i = 0; i < info.joints.size(); ++i) { // There has to always be at least one control mode from the above three set if (joint_found_in_x_requests_[i] == FOUND_ONCE_FLAG) { RCLCPP_ERROR( get_logger(), "Joint '%s' has to have '%s', '%s', or '%s' interface!", - info_.joints[i].name.c_str(), hardware_interface::HW_IF_POSITION, + info.joints[i].name.c_str(), hardware_interface::HW_IF_POSITION, hardware_interface::HW_IF_VELOCITY, hardware_interface::HW_IF_ACCELERATION); ret_val = hardware_interface::return_type::ERROR; } @@ -401,7 +409,7 @@ return_type GenericSystem::prepare_command_mode_switch( get_logger(), "Got multiple (%zu) starting interfaces for joint '%s' - this is not " "supported!", - joint_found_in_x_requests_[i] - FOUND_ONCE_FLAG, info_.joints[i].name.c_str()); + joint_found_in_x_requests_[i] - FOUND_ONCE_FLAG, info.joints[i].name.c_str()); ret_val = hardware_interface::return_type::ERROR; } } @@ -421,23 +429,24 @@ return_type GenericSystem::perform_command_mode_switch( for (const auto & key : start_interfaces) { // check if interface is joint + const auto & info = get_hardware_info(); auto joint_it_found = std::find_if( - info_.joints.begin(), info_.joints.end(), + info.joints.begin(), info.joints.end(), [key](const auto & joint) { return (key.find(joint.name) != std::string::npos); }); - if (joint_it_found != info_.joints.end()) + if (joint_it_found != info.joints.end()) { - const size_t joint_index = std::distance(info_.joints.begin(), joint_it_found); + const size_t joint_index = std::distance(info.joints.begin(), joint_it_found); - if (key == info_.joints[joint_index].name + "/" + hardware_interface::HW_IF_POSITION) + if (key == info.joints[joint_index].name + "/" + hardware_interface::HW_IF_POSITION) { joint_control_mode_[joint_index] = POSITION_INTERFACE_INDEX; } - if (key == info_.joints[joint_index].name + "/" + hardware_interface::HW_IF_VELOCITY) + if (key == info.joints[joint_index].name + "/" + hardware_interface::HW_IF_VELOCITY) { joint_control_mode_[joint_index] = VELOCITY_INTERFACE_INDEX; } - if (key == info_.joints[joint_index].name + "/" + hardware_interface::HW_IF_ACCELERATION) + if (key == info.joints[joint_index].name + "/" + hardware_interface::HW_IF_ACCELERATION) { joint_control_mode_[joint_index] = ACCELERATION_INTERFACE_INDEX; } @@ -565,7 +574,7 @@ return_type GenericSystem::read(const rclcpp::Time & /*time*/, const rclcpp::Dur return return_type::ERROR; } - for (const auto & mimic_joint : info_.mimic_joints) + for (const auto & mimic_joint : get_hardware_info().mimic_joints) { for (auto i = 0u; i < joint_states_.size(); ++i) { diff --git a/hardware_interface/test/test_hardware_components/test_force_torque_sensor.cpp b/hardware_interface/test/test_hardware_components/test_force_torque_sensor.cpp index ff57d6a63d..acec0e5917 100644 --- a/hardware_interface/test/test_hardware_components/test_force_torque_sensor.cpp +++ b/hardware_interface/test/test_hardware_components/test_force_torque_sensor.cpp @@ -33,7 +33,7 @@ class TestForceTorqueSensor : public SensorInterface return CallbackReturn::ERROR; } - const auto & state_interfaces = info_.sensors[0].state_interfaces; + const auto & state_interfaces = get_hardware_info().sensors[0].state_interfaces; if (state_interfaces.size() != 6) { return CallbackReturn::ERROR; @@ -57,7 +57,7 @@ class TestForceTorqueSensor : public SensorInterface { std::vector state_interfaces; - const auto & sensor_name = info_.sensors[0].name; + const auto & sensor_name = get_hardware_info().sensors[0].name; state_interfaces.emplace_back( hardware_interface::StateInterface(sensor_name, "fx", &values_.fx)); state_interfaces.emplace_back( diff --git a/hardware_interface/test/test_hardware_components/test_imu_sensor.cpp b/hardware_interface/test/test_hardware_components/test_imu_sensor.cpp index bb41f16228..0bd9e9121c 100644 --- a/hardware_interface/test/test_hardware_components/test_imu_sensor.cpp +++ b/hardware_interface/test/test_hardware_components/test_imu_sensor.cpp @@ -38,7 +38,7 @@ class TestIMUSensor : public SensorInterface return CallbackReturn::ERROR; } - const auto & state_interfaces = info_.sensors[0].state_interfaces; + const auto & state_interfaces = get_hardware_info().sensors[0].state_interfaces; if (state_interfaces.size() != 10) { return CallbackReturn::ERROR; @@ -65,7 +65,7 @@ class TestIMUSensor : public SensorInterface { std::vector state_interfaces; - const std::string & sensor_name = info_.sensors[0].name; + const std::string & sensor_name = get_hardware_info().sensors[0].name; state_interfaces.emplace_back( hardware_interface::StateInterface(sensor_name, "orientation.x", &orientation_.x)); state_interfaces.emplace_back( diff --git a/hardware_interface/test/test_hardware_components/test_single_joint_actuator.cpp b/hardware_interface/test/test_hardware_components/test_single_joint_actuator.cpp index dacbf308fa..eb61c3de5a 100644 --- a/hardware_interface/test/test_hardware_components/test_single_joint_actuator.cpp +++ b/hardware_interface/test/test_hardware_components/test_single_joint_actuator.cpp @@ -34,12 +34,12 @@ class TestSingleJointActuator : public ActuatorInterface } // can only control one joint - if (info_.joints.size() != 1) + if (get_hardware_info().joints.size() != 1) { return CallbackReturn::ERROR; } // can only control in position - const auto & command_interfaces = info_.joints[0].command_interfaces; + const auto & command_interfaces = get_hardware_info().joints[0].command_interfaces; if (command_interfaces.size() != 1) { return CallbackReturn::ERROR; @@ -49,7 +49,7 @@ class TestSingleJointActuator : public ActuatorInterface return CallbackReturn::ERROR; } // can only give feedback state for position and velocity - const auto & state_interfaces = info_.joints[0].state_interfaces; + const auto & state_interfaces = get_hardware_info().joints[0].state_interfaces; if (state_interfaces.size() < 1) { return CallbackReturn::ERROR; @@ -71,7 +71,7 @@ class TestSingleJointActuator : public ActuatorInterface { std::vector state_interfaces; - const auto & joint_name = info_.joints[0].name; + const auto & joint_name = get_hardware_info().joints[0].name; state_interfaces.emplace_back(hardware_interface::StateInterface( joint_name, hardware_interface::HW_IF_POSITION, &position_state_)); state_interfaces.emplace_back(hardware_interface::StateInterface( @@ -84,7 +84,7 @@ class TestSingleJointActuator : public ActuatorInterface { std::vector command_interfaces; - const auto & joint_name = info_.joints[0].name; + const auto & joint_name = get_hardware_info().joints[0].name; command_interfaces.emplace_back(hardware_interface::CommandInterface( joint_name, hardware_interface::HW_IF_POSITION, &position_command_)); diff --git a/hardware_interface/test/test_hardware_components/test_system_with_command_modes.cpp b/hardware_interface/test/test_hardware_components/test_system_with_command_modes.cpp index aba2f86fe5..a511344f52 100644 --- a/hardware_interface/test/test_hardware_components/test_system_with_command_modes.cpp +++ b/hardware_interface/test/test_hardware_components/test_system_with_command_modes.cpp @@ -32,11 +32,11 @@ class TestSystemCommandModes : public hardware_interface::SystemInterface } // Can only control two joints - if (info_.joints.size() != 2) + if (get_hardware_info().joints.size() != 2) { return CallbackReturn::ERROR; } - for (const hardware_interface::ComponentInfo & joint : info_.joints) + for (const hardware_interface::ComponentInfo & joint : get_hardware_info().joints) { // Can control in position or velocity const auto & command_interfaces = joint.command_interfaces; @@ -77,14 +77,17 @@ class TestSystemCommandModes : public hardware_interface::SystemInterface std::vector export_state_interfaces() override { std::vector state_interfaces; - for (auto i = 0u; i < info_.joints.size(); i++) + for (auto i = 0u; i < get_hardware_info().joints.size(); i++) { state_interfaces.emplace_back(hardware_interface::StateInterface( - info_.joints[i].name, hardware_interface::HW_IF_POSITION, &position_state_[i])); + get_hardware_info().joints[i].name, hardware_interface::HW_IF_POSITION, + &position_state_[i])); state_interfaces.emplace_back(hardware_interface::StateInterface( - info_.joints[i].name, hardware_interface::HW_IF_VELOCITY, &velocity_state_[i])); + get_hardware_info().joints[i].name, hardware_interface::HW_IF_VELOCITY, + &velocity_state_[i])); state_interfaces.emplace_back(hardware_interface::StateInterface( - info_.joints[i].name, hardware_interface::HW_IF_ACCELERATION, &acceleration_state_[i])); + get_hardware_info().joints[i].name, hardware_interface::HW_IF_ACCELERATION, + &acceleration_state_[i])); } return state_interfaces; @@ -93,12 +96,14 @@ class TestSystemCommandModes : public hardware_interface::SystemInterface std::vector export_command_interfaces() override { std::vector command_interfaces; - for (auto i = 0u; i < info_.joints.size(); i++) + for (auto i = 0u; i < get_hardware_info().joints.size(); i++) { command_interfaces.emplace_back(hardware_interface::CommandInterface( - info_.joints[i].name, hardware_interface::HW_IF_POSITION, &position_command_[i])); + get_hardware_info().joints[i].name, hardware_interface::HW_IF_POSITION, + &position_command_[i])); command_interfaces.emplace_back(hardware_interface::CommandInterface( - info_.joints[i].name, hardware_interface::HW_IF_VELOCITY, &velocity_command_[i])); + get_hardware_info().joints[i].name, hardware_interface::HW_IF_VELOCITY, + &velocity_command_[i])); } return command_interfaces; @@ -127,20 +132,20 @@ class TestSystemCommandModes : public hardware_interface::SystemInterface stop_modes_.clear(); for (const auto & key : start_interfaces) { - for (auto i = 0u; i < info_.joints.size(); i++) + for (auto i = 0u; i < get_hardware_info().joints.size(); i++) { - if (key == info_.joints[i].name + "/" + hardware_interface::HW_IF_POSITION) + if (key == get_hardware_info().joints[i].name + "/" + hardware_interface::HW_IF_POSITION) { start_modes_.push_back(hardware_interface::HW_IF_POSITION); } - if (key == info_.joints[i].name + "/" + hardware_interface::HW_IF_VELOCITY) + if (key == get_hardware_info().joints[i].name + "/" + hardware_interface::HW_IF_VELOCITY) { start_modes_.push_back(hardware_interface::HW_IF_VELOCITY); } } } // Example Criteria 1 - Starting: All interfaces must be given a new mode at the same time - if (start_modes_.size() != 0 && start_modes_.size() != info_.joints.size()) + if (start_modes_.size() != 0 && start_modes_.size() != get_hardware_info().joints.size()) { return hardware_interface::return_type::ERROR; } @@ -148,9 +153,9 @@ class TestSystemCommandModes : public hardware_interface::SystemInterface // Stopping interfaces for (const auto & key : stop_interfaces) { - for (auto i = 0u; i < info_.joints.size(); i++) + for (auto i = 0u; i < get_hardware_info().joints.size(); i++) { - if (key.find(info_.joints[i].name) != std::string::npos) + if (key.find(get_hardware_info().joints[i].name) != std::string::npos) { stop_modes_.push_back(true); } diff --git a/hardware_interface/test/test_hardware_components/test_two_joint_system.cpp b/hardware_interface/test/test_hardware_components/test_two_joint_system.cpp index 5942f14d42..7bc06a3e1e 100644 --- a/hardware_interface/test/test_hardware_components/test_two_joint_system.cpp +++ b/hardware_interface/test/test_hardware_components/test_two_joint_system.cpp @@ -35,11 +35,11 @@ class TestTwoJointSystem : public SystemInterface } // can only control two joint - if (info_.joints.size() != 2) + if (get_hardware_info().joints.size() != 2) { return CallbackReturn::ERROR; } - for (const auto & joint : info_.joints) + for (const auto & joint : get_hardware_info().joints) { // can only control in position const auto & command_interfaces = joint.command_interfaces; @@ -70,10 +70,11 @@ class TestTwoJointSystem : public SystemInterface std::vector export_state_interfaces() override { std::vector state_interfaces; - for (auto i = 0u; i < info_.joints.size(); ++i) + for (auto i = 0u; i < get_hardware_info().joints.size(); ++i) { state_interfaces.emplace_back(hardware_interface::StateInterface( - info_.joints[i].name, hardware_interface::HW_IF_POSITION, &position_state_[i])); + get_hardware_info().joints[i].name, hardware_interface::HW_IF_POSITION, + &position_state_[i])); } return state_interfaces; @@ -82,10 +83,11 @@ class TestTwoJointSystem : public SystemInterface std::vector export_command_interfaces() override { std::vector command_interfaces; - for (auto i = 0u; i < info_.joints.size(); ++i) + for (auto i = 0u; i < get_hardware_info().joints.size(); ++i) { command_interfaces.emplace_back(hardware_interface::CommandInterface( - info_.joints[i].name, hardware_interface::HW_IF_POSITION, &position_command_[i])); + get_hardware_info().joints[i].name, hardware_interface::HW_IF_POSITION, + &position_command_[i])); } return command_interfaces; diff --git a/hardware_interface_testing/test/test_components/test_actuator.cpp b/hardware_interface_testing/test/test_components/test_actuator.cpp index b7149369ed..86debc1f4d 100644 --- a/hardware_interface_testing/test/test_components/test_actuator.cpp +++ b/hardware_interface_testing/test/test_components/test_actuator.cpp @@ -31,7 +31,7 @@ class TestActuator : public ActuatorInterface return CallbackReturn::ERROR; } - if (info_.joints[0].state_interfaces[1].name == "does_not_exist") + if (get_hardware_info().joints[0].state_interfaces[1].name == "does_not_exist") { return CallbackReturn::ERROR; } @@ -40,12 +40,12 @@ class TestActuator : public ActuatorInterface * a hardware can optional prove for incorrect info here. * * // can only control one joint - * if (info_.joints.size() != 1) {return CallbackReturn::ERROR;} + * if (get_hardware_info().joints.size() != 1) {return CallbackReturn::ERROR;} * // can only control in position - * if (info_.joints[0].command_interfaces.size() != 1) {return + * if (get_hardware_info().joints[0].command_interfaces.size() != 1) {return * CallbackReturn::ERROR;} * // can only give feedback state for position and velocity - * if (info_.joints[0].state_interfaces.size() != 2) {return + * if (get_hardware_info().joints[0].state_interfaces.size() != 2) {return * CallbackReturn::ERROR;} */ @@ -56,11 +56,13 @@ class TestActuator : public ActuatorInterface { std::vector state_interfaces; state_interfaces.emplace_back(hardware_interface::StateInterface( - info_.joints[0].name, info_.joints[0].state_interfaces[0].name, &position_state_)); + get_hardware_info().joints[0].name, get_hardware_info().joints[0].state_interfaces[0].name, + &position_state_)); state_interfaces.emplace_back(hardware_interface::StateInterface( - info_.joints[0].name, info_.joints[0].state_interfaces[1].name, &velocity_state_)); - state_interfaces.emplace_back( - hardware_interface::StateInterface(info_.joints[0].name, "some_unlisted_interface", nullptr)); + get_hardware_info().joints[0].name, get_hardware_info().joints[0].state_interfaces[1].name, + &velocity_state_)); + state_interfaces.emplace_back(hardware_interface::StateInterface( + get_hardware_info().joints[0].name, "some_unlisted_interface", nullptr)); return state_interfaces; } @@ -69,12 +71,14 @@ class TestActuator : public ActuatorInterface { std::vector command_interfaces; command_interfaces.emplace_back(hardware_interface::CommandInterface( - info_.joints[0].name, info_.joints[0].command_interfaces[0].name, &velocity_command_)); + get_hardware_info().joints[0].name, get_hardware_info().joints[0].command_interfaces[0].name, + &velocity_command_)); - if (info_.joints[0].command_interfaces.size() > 1) + if (get_hardware_info().joints[0].command_interfaces.size() > 1) { command_interfaces.emplace_back(hardware_interface::CommandInterface( - info_.joints[0].name, info_.joints[0].command_interfaces[1].name, &max_velocity_command_)); + get_hardware_info().joints[0].name, + get_hardware_info().joints[0].command_interfaces[1].name, &max_velocity_command_)); } return command_interfaces; diff --git a/hardware_interface_testing/test/test_components/test_sensor.cpp b/hardware_interface_testing/test/test_components/test_sensor.cpp index 7f5a476c14..16692f55d9 100644 --- a/hardware_interface_testing/test/test_components/test_sensor.cpp +++ b/hardware_interface_testing/test/test_components/test_sensor.cpp @@ -29,7 +29,7 @@ class TestSensor : public SensorInterface return CallbackReturn::ERROR; } // can only give feedback state for velocity - if (info_.sensors[0].state_interfaces.size() == 2) + if (get_hardware_info().sensors[0].state_interfaces.size() == 2) { return CallbackReturn::ERROR; } @@ -40,7 +40,8 @@ class TestSensor : public SensorInterface { std::vector state_interfaces; state_interfaces.emplace_back(hardware_interface::StateInterface( - info_.sensors[0].name, info_.sensors[0].state_interfaces[0].name, &velocity_state_)); + get_hardware_info().sensors[0].name, get_hardware_info().sensors[0].state_interfaces[0].name, + &velocity_state_)); return state_interfaces; } diff --git a/hardware_interface_testing/test/test_components/test_system.cpp b/hardware_interface_testing/test/test_components/test_system.cpp index 6724a1d019..e30b74488e 100644 --- a/hardware_interface_testing/test/test_components/test_system.cpp +++ b/hardware_interface_testing/test/test_components/test_system.cpp @@ -34,7 +34,7 @@ class TestSystem : public SystemInterface } // Simulating initialization error - if (info_.joints[0].state_interfaces[1].name == "does_not_exist") + if (get_hardware_info().joints[0].state_interfaces[1].name == "does_not_exist") { return CallbackReturn::ERROR; } @@ -44,22 +44,23 @@ class TestSystem : public SystemInterface std::vector export_state_interfaces() override { + const auto info = get_hardware_info(); std::vector state_interfaces; - for (auto i = 0u; i < info_.joints.size(); ++i) + for (auto i = 0u; i < info.joints.size(); ++i) { state_interfaces.emplace_back(hardware_interface::StateInterface( - 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( - 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( - info_.joints[i].name, hardware_interface::HW_IF_ACCELERATION, &acceleration_state_[i])); + info.joints[i].name, hardware_interface::HW_IF_ACCELERATION, &acceleration_state_[i])); } - if (info_.gpios.size() > 0) + if (info.gpios.size() > 0) { // Add configuration/max_tcp_jerk interface state_interfaces.emplace_back(hardware_interface::StateInterface( - info_.gpios[0].name, info_.gpios[0].state_interfaces[0].name, &configuration_state_)); + info.gpios[0].name, info.gpios[0].state_interfaces[0].name, &configuration_state_)); } return state_interfaces; @@ -67,22 +68,22 @@ class TestSystem : public SystemInterface std::vector export_command_interfaces() override { + const auto info = get_hardware_info(); std::vector command_interfaces; - for (auto i = 0u; i < info_.joints.size(); ++i) + for (auto i = 0u; i < info.joints.size(); ++i) { command_interfaces.emplace_back(hardware_interface::CommandInterface( - info_.joints[i].name, hardware_interface::HW_IF_VELOCITY, &velocity_command_[i])); + info.joints[i].name, hardware_interface::HW_IF_VELOCITY, &velocity_command_[i])); } // Add max_acceleration command interface command_interfaces.emplace_back(hardware_interface::CommandInterface( - info_.joints[0].name, info_.joints[0].command_interfaces[1].name, - &max_acceleration_command_)); + info.joints[0].name, info.joints[0].command_interfaces[1].name, &max_acceleration_command_)); - if (info_.gpios.size() > 0) + if (info.gpios.size() > 0) { // Add configuration/max_tcp_jerk interface command_interfaces.emplace_back(hardware_interface::CommandInterface( - info_.gpios[0].name, info_.gpios[0].command_interfaces[0].name, &configuration_command_)); + info.gpios[0].name, info.gpios[0].command_interfaces[0].name, &configuration_command_)); } return command_interfaces;