From 7c6d89c517a432130e7b36127b713e86ac4fe83f Mon Sep 17 00:00:00 2001 From: NestorDP Date: Sun, 21 Jul 2024 13:58:04 -0300 Subject: [PATCH] basic hardware component structure --- littlebot_base/CMakeLists.txt | 41 +- .../littlebot_base/littlebot_hardware.hpp | 234 +----------- littlebot_base/src/littlebot_hardware.cpp | 351 +++--------------- 3 files changed, 102 insertions(+), 524 deletions(-) diff --git a/littlebot_base/CMakeLists.txt b/littlebot_base/CMakeLists.txt index 28a9c46..80b7e0d 100755 --- a/littlebot_base/CMakeLists.txt +++ b/littlebot_base/CMakeLists.txt @@ -138,6 +138,34 @@ ament_export_dependencies( ament_package() + + + + + + + + + + + + + + + + + + + + + + + + + + + + cmake_minimum_required(VERSION 3.8) project(bravo_seven_driver) @@ -172,6 +200,12 @@ ament_target_dependencies(bravo_seven_udp_communication PUBLIC ${THIS_PACKAGE_INCLUDE_DEPENDS} ) +pluginlib_export_plugin_description_file(bravo_seven_driver + bravo_seven_udp_communication.xml +) + + + add_library(bravo_seven_hardware_component SHARED src/bravo_seven_hardware_component.cpp ) @@ -181,17 +215,14 @@ target_include_directories(bravo_seven_hardware_component PUBLIC $ ) -pluginlib_export_plugin_description_file(bravo_seven_driver - bravo_seven_udp_communication.xml +ament_target_dependencies(bravo_seven_hardware_component PUBLIC + ${THIS_PACKAGE_INCLUDE_DEPENDS} ) pluginlib_export_plugin_description_file(hardware_interface bravo_seven_hardware_plugin.xml ) -ament_target_dependencies(bravo_seven_hardware_component PUBLIC - ${THIS_PACKAGE_INCLUDE_DEPENDS} -) install( TARGETS bravo_seven_udp_communication diff --git a/littlebot_base/include/littlebot_base/littlebot_hardware.hpp b/littlebot_base/include/littlebot_base/littlebot_hardware.hpp index a4733cc..e4ce3b9 100644 --- a/littlebot_base/include/littlebot_base/littlebot_hardware.hpp +++ b/littlebot_base/include/littlebot_base/littlebot_hardware.hpp @@ -26,7 +26,7 @@ class LittlebotHardwareComponent : public hardware_interface::SystemInterface * @brief Deconstructor for the LittlebotHardwareComponent class * */ - ~LittlebotHardwareComponent() override; + // ~LittlebotHardwareComponent() override; /** * @brief @@ -87,266 +87,70 @@ class LittlebotHardwareComponent : public hardware_interface::SystemInterface const rclcpp::Time & time, const rclcpp::Duration & period) override; - private: - /** - * @brief Get the state interfaces from the Force Torque Sensor through the Urdf. - * - * This method checks the Force Torque state interfaces listed in Urdf and saves them - * in the @ref all_sensor_state_interface_names_ variable. - * - * @param sensors The vector of sensors (ComponentInfo). - * @throw std::invalid_argument If the Force Torque is not listed. - */ - void fillForceTorqueStateInterfacesMapFromUrdf( - const std::vector & sensors); - - /** - * @brief Get the Joints passed through the Urdf. - * - * This method calls @ref areJointParametersValid and @ref getJointStateInterfaces to check the parameters and map the state interfaces to each joint, respectively. When the joints are correctly configured, they are saved in the vector @ref joints_. - * - * @param joints The vector of joints (ComponentInfo). - * @throw std::invalid_argument If there are not eight joints correctly named. - * @throw std::invalid_argument If the parameters have invalid values. - */ - void fillJointsConfigurationMapFromUrdf( - const std::vector & joints); - - /** - * @brief The Data structure of one joint of the Bravo 7 Manipulator. - * - */ - struct ManipulatorJointData - { - /// The name of the joint. - std::string name; - /// The ID of the joint, according to the map @ref bravo_seven_devices_map_. - types::DeviceId id; - /// The map of state interfaces. - std::unordered_map state_interfaces; - /// The map of limit values, the first one the minimum limit and the second one the maximum. - std::unordered_map> limit_values; - }; - - /** - * @brief Check the parameters of the Joint component. - * - * @param valid_joint The joint data, where the parameter values are saved. - * @return true If the parameters are valid. - * @return false If the parameters have invalid values. - */ - bool areJointParametersValid(ManipulatorJointData & valid_joint); - - /** - * @brief Get the Joint State Interfaces. - * - * This method checks if the state interfaces are valid and saves them. Otherwise the - * state interface will not be exported. - * - * @param state_interfaces The vector of state interfaces. - * @param valid_joint The data of the joint. - */ - void getJointStateInterfaces( - const std::vector & state_interfaces, - ManipulatorJointData & valid_joint); - - /** - * @brief Configure the limits of the joints. - * - * This method calls the @ref setJointLimits to configure the positions, current - * and voltage limits, to all the joints mapped in @ref configurations_map_. If no limit - * parameter is set for a joint, the limits are not set. - * - */ - void configureLimitsOfJoints(); - - /** - * @brief Get the Float Component Parameter object - * - * @param joint_data - * @param float_parameter - * @param parameter_name - */ - void getFloatComponentParameter( - ManipulatorJointData & joint_data, float & float_parameter, std::string parameter_name); - - /** - * @brief Get the Joint Position Limits Parameters passed through the Urdf. - * - * @param joint_name The name of the joint to get the parameters. - * @param minimum_position The minimum limit to set. - * @param maximum_position The maximum limit to set. - */ - void getJointPositionLimitsParameters( - std::string joint_name, float & minimum_position, float & maximum_position); - /** * @brief The name of the hardware component. */ const std::string hardware_component_name_{"LittlebotHardwareComponent"}; /** - * @brief This class uses this pointer to store the concrete LittlebotCommunicationInterface. - */ - std::shared_ptr manipulator_communication_ptr_; - - /** - * @brief Unique pointer to an instance of the PluginlibLoaderWrapper class. - * - * The ros2_control_toolbox::PluginlibLoaderWrapper loads and unloads a communication plugin, - * from base class LittlebotCommunicationInterface. - */ - std::unique_ptr> plugin_loader_wrapper_ptr_; - - /** - * @brief The name of the package where the communication plugin is implemented. - * - */ - const std::string communication_plugin_package_name_{"littlebot_base"}; - - /** - * @brief The name of the communication plugin. - * - */ - std::string communication_plugin_name_{""}; - - /** - * @brief The base class of the communication plugin. - * + * @brief command interface. */ - const std::string communication_plugin_base_class_{"LittlebotCommunicationInterface"}; + std::vector hw_commands_; /** - * @brief The driver_base uses this URI to know the communication channel with the manipulator. + * @brief position state interface. */ - std::string uri_{""}; + std::vector hw_positions_; /** - * @brief Default driver_base read timeout. + * @brief velocity state interface. */ - std::chrono::milliseconds read_timeout_{0}; + std::vector hw_velocities_; /** - * @brief Default driver_base write timeout. + * @brief */ - std::chrono::milliseconds write_timeout_{0}; + static constexpr int kNumCommandInterface_{1}; /** - * @brief Default read timeout. + * @brief */ - static constexpr std::chrono::milliseconds kDefaultReadTimeout{1000}; + static constexpr int kNumStateInterface_{2}; - /** - * @brief Default write timeout. - */ - static constexpr std::chrono::milliseconds kDefaultWriteTimeout{1000}; - /** - * @brief The pointer used to access the HardwareDescriptionHelper methods, which gets the - * hardware component parameters. - * - */ - std::unique_ptr hardware_description_helper_ptr_; - /** - * @brief Maps the state interfaces by the joints ID. - * - */ - std::unordered_map> state_interfaces_map_; - /** - * @brief Maps the configuration parameters by the joints ID. - * - */ - std::unordered_map> configurations_map_; - /** - * @brief Maps the state interface values of the Force Torque sensor. - * - */ - std::unordered_map force_torque_state_interfaces_map_; - /** - * @brief Lists the sensor state interfaces that should be exported. - * - */ - std::vector sensor_state_interfaces_in_use_; - /** - * @brief Maps the configuration parameters with the function related (listed in - * @ref LittlebotCommunicationInterface::PacketId). - * - */ - std::unordered_map configurations_functions_map_{ - {"maximum_velocity", types::PacketId::SET_VELOCITY_LIMITS}, - {"maximum_current", types::PacketId::SET_CURRENT_LIMITS}, - {"maximum_position", types::PacketId::SET_POSITION_LIMITS}, - }; - /** - * @brief The data vector of the manipulator's joints. - * - */ - std::array joints_; - /** - * @brief The actual data of the Force Torque sensor. - * - */ - types::ForceTorque force_toque_measurements_; - /** - * @brief Number of sensors in the Bravo Seven Manipulator. - * - */ - static constexpr uint8_t kSensorsInManipulator{1}; - - /** - * @brief The name of the Force Torque sensor. - */ - std::string sensor_name_{"force_torque"}; /** - * @brief Number of state interfaces in the force torque sensor (hardware interface). - * - */ - static constexpr uint8_t kSensorStateInterfaces{6}; - - /** - * @brief Number of state interfaces available for each joint. - * + * @brief */ - static constexpr uint8_t kJointStateInterfacesAvailable{8}; + //std::shared_ptr littlebot_communication_ptr_; /** - * @brief Numbers of parameters by joint. + * @brief The name of the package where the communication plugin is implemented. * */ - static constexpr uint8_t kJointParameters{4}; + //const std::string communication_plugin_package_name_{"littlebot_base"}; /** - * @brief List of all the state interface names available for one joint. + * @brief The name of the communication plugin. * */ - std::array all_joint_state_interface_names_ = - {"position", "relative_position", "velocity", "voltage", "current", "internal_humidity", - "internal_temperature", "internal_pressure"}; + //std::string communication_plugin_name_{""}; /** - * @brief List all the configuration parameters available for one joint. + * @brief The base class of the communication plugin. * */ - std::array configuration_parameters_names_ = - {"maximum_velocity", "maximum_current", "minimum_position", "maximum_position"}; + //const std::string communication_plugin_base_class_{"LittlebotCommunicationInterface"}; - /** - * @brief List all the state interface names available for the Force Torque sensor. - * - */ - std::array all_sensor_state_interface_names_ = {"force_x", - "force_y", "force_z", "torque_x", "torque_y", "torque_z"}; }; } // namespace littlebot_base \ No newline at end of file diff --git a/littlebot_base/src/littlebot_hardware.cpp b/littlebot_base/src/littlebot_hardware.cpp index ef642ec..6d55a62 100644 --- a/littlebot_base/src/littlebot_hardware.cpp +++ b/littlebot_base/src/littlebot_hardware.cpp @@ -7,10 +7,9 @@ namespace littlebot_base { -LittlebotHardwareComponent::~LittlebotHardwareComponent() -{ - -} +// LittlebotHardwareComponent::~LittlebotHardwareComponent() +// { +// } hardware_interface::CallbackReturn LittlebotHardwareComponent::on_init( const hardware_interface::HardwareInfo & info) @@ -27,8 +26,7 @@ hardware_interface::CallbackReturn LittlebotHardwareComponent::on_init( for (const hardware_interface::ComponentInfo & joint : info_.joints) { - // LittlebotSystem has exactly two states and one command interface on each joint - if (joint.command_interfaces.size() != 1) + if (joint.command_interfaces.size() != kNumCommandInterface_) { RCLCPP_FATAL( rclcpp::get_logger("LittlebotSystemHardware"), @@ -46,7 +44,7 @@ hardware_interface::CallbackReturn LittlebotHardwareComponent::on_init( return hardware_interface::CallbackReturn::ERROR; } - if (joint.state_interfaces.size() != 2) + if (joint.state_interfaces.size() != kNumStateInterface_) { RCLCPP_FATAL( rclcpp::get_logger("LittlebotSystemHardware"), @@ -73,6 +71,7 @@ hardware_interface::CallbackReturn LittlebotHardwareComponent::on_init( return hardware_interface::CallbackReturn::ERROR; } } + return hardware_interface::CallbackReturn::SUCCESS; } @@ -164,330 +163,74 @@ hardware_interface::CallbackReturn LittlebotHardwareComponent::on_error( std::vector LittlebotHardwareComponent::export_state_interfaces() { std::vector state_interfaces; - - for (ManipulatorJointData & joint : joints_) { - for (auto & state_interface : joint.state_interfaces) { - state_interfaces.emplace_back( - hardware_interface::StateInterface( - joint.name, state_interface.first, &state_interface.second)); - } + 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, &hw_positions_[i])); + state_interfaces.emplace_back(hardware_interface::StateInterface( + info_.joints[i].name, hardware_interface::HW_IF_VELOCITY, &hw_velocities_[i])); } - for (const std::string & sensor_state_interface_name : sensor_state_interfaces_in_use_) { - state_interfaces.emplace_back( - hardware_interface::StateInterface( - sensor_name_, sensor_state_interface_name, - &force_torque_state_interfaces_map_[sensor_state_interface_name])); - } return state_interfaces; } -hardware_interface::return_type LittlebotHardwareComponent::read( - [[maybe_unused]] const rclcpp::Time & time, - [[maybe_unused]] const rclcpp::Duration & period) +std::vector DiffBotSystemHardware::export_command_interfaces() { - for (const std::string & state_interface_name : all_joint_state_interface_names_) { - for (const types::DeviceId & joint_id : state_interfaces_map_[state_interface_name]) { - uint8_t joint_index = (static_cast(joint_id) - 1); - - try { - if (state_interface_name == "voltage") { - joints_[joint_index].state_interfaces[state_interface_name] = - manipulator_communication_ptr_->getVoltageInVolts(joint_id); - } - if (state_interface_name == "velocity") { - joints_[joint_index].state_interfaces[state_interface_name] = - manipulator_communication_ptr_->getVelocityInRadiansPerSecond(joint_id); - } - if (state_interface_name == "position") { - joints_[joint_index].state_interfaces[state_interface_name] = - manipulator_communication_ptr_->getPositionInRadians(joint_id); - } - if (state_interface_name == "relative_position") { - joints_[joint_index].state_interfaces[state_interface_name] = - manipulator_communication_ptr_->getRelativePositionInRadians(joint_id); - } - if (state_interface_name == "current") { - joints_[joint_index].state_interfaces[state_interface_name] = - manipulator_communication_ptr_->getCurrentInMiliampere(joint_id); - } - if (state_interface_name == "internal_humidity") { - joints_[joint_index].state_interfaces[state_interface_name] = - manipulator_communication_ptr_->getInternalHumidityPercent(joint_id); - } - if (state_interface_name == "internal_temperature") { - joints_[joint_index].state_interfaces[state_interface_name] = - manipulator_communication_ptr_->getInternalTemperatureInDegreesCelsius(joint_id); - } - if (state_interface_name == "internal_pressure") { - joints_[joint_index].state_interfaces[state_interface_name] = - manipulator_communication_ptr_->getInternalPressureInBar(joint_id); - } - } catch (const driver_base::TimeoutError & e) { - RCLCPP_ERROR_STREAM( - rclcpp::get_logger( - hardware_component_name_), - "Timeout error while reading the " << state_interface_name << "of the manipulator joints: " << - e.what()); - - joints_[joint_index].state_interfaces[state_interface_name] = - std::numeric_limits::quiet_NaN(); - } catch (const std::exception & e) { - RCLCPP_ERROR_STREAM( - rclcpp::get_logger( - hardware_component_name_), - "Error while reading the " << state_interface_name << "of the manipulator joints: " << - e.what()); - - joints_[joint_index].state_interfaces[state_interface_name] = - std::numeric_limits::quiet_NaN(); - } - } - } - - try { - types::ForceTorque sensor_reading = - manipulator_communication_ptr_->getForceTorqueReading(); - - force_torque_state_interfaces_map_[all_sensor_state_interface_names_[0]] = - sensor_reading.force_x; - force_torque_state_interfaces_map_[all_sensor_state_interface_names_[1]] = - sensor_reading.force_y; - force_torque_state_interfaces_map_[all_sensor_state_interface_names_[2]] = - sensor_reading.force_z; - force_torque_state_interfaces_map_[all_sensor_state_interface_names_[3]] = - sensor_reading.torque_x; - force_torque_state_interfaces_map_[all_sensor_state_interface_names_[4]] = - sensor_reading.torque_y; - force_torque_state_interfaces_map_[all_sensor_state_interface_names_[5]] = - sensor_reading.torque_z; - } catch (const driver_base::TimeoutError & e) { - RCLCPP_ERROR( - rclcpp::get_logger( - hardware_component_name_), - "Timeout error while reading the Force Torque sensor: %s", e.what()); - - for (auto & sensor_state_interface : force_torque_state_interfaces_map_) { - sensor_state_interface.second = std::numeric_limits::quiet_NaN(); - } - } catch (const std::exception & e) { - RCLCPP_ERROR( - rclcpp::get_logger( - hardware_component_name_), - "An error occurred while reading the Force Torque sensor: %s", e.what()); - - for (auto & sensor_state_interface : force_torque_state_interfaces_map_) { - sensor_state_interface.second = std::numeric_limits::quiet_NaN(); - } + std::vector command_interfaces; + 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, &hw_commands_[i])); } - return hardware_interface::return_type::OK; -} - -std::vector LittlebotHardwareComponent:: -export_command_interfaces() -{ - std::vector command_interfaces; return command_interfaces; } -hardware_interface::return_type LittlebotHardwareComponent::write( +hardware_interface::return_type LittlebotHardwareComponent::read( [[maybe_unused]] const rclcpp::Time & time, [[maybe_unused]] const rclcpp::Duration & period) { - return hardware_interface::return_type::OK; -} - -void LittlebotHardwareComponent::configureLimitsOfJoints() -{ - for (const std::string & configuration_name : configuration_parameters_names_) { - for (const types::DeviceId & joint_id : configurations_map_[configuration_name]) { - if (configuration_name == "maximum_position") { - continue; // The maximum position is configured in the same time as the minimum position. - } - - float minimum_limit = - joints_[static_cast(joint_id) - 1].limit_values[configuration_name].first; - - float maximum_limit = - joints_[static_cast(joint_id) - 1].limit_values[configuration_name].second; - - manipulator_communication_ptr_->setJointLimits( - configurations_functions_map_[configuration_name], minimum_limit, maximum_limit, - joint_id); - } - } -} - -void LittlebotHardwareComponent::fillForceTorqueStateInterfacesMapFromUrdf( - const std::vector & sensors) -{ - if (sensors.size() != kSensorsInManipulator || sensors[0].name != sensor_name_) { - throw std::invalid_argument( - "The BravoSevenHardware component should include the configured Force Torque sensor."); - } - - for (const hardware_interface::InterfaceInfo & state_interface : sensors[0].state_interfaces) { - auto vector_iterator = std::find( - all_sensor_state_interface_names_.begin(), - all_sensor_state_interface_names_.end(), state_interface.name); - - if (vector_iterator == all_sensor_state_interface_names_.end()) { - RCLCPP_ERROR( - rclcpp::get_logger( - hardware_component_name_), - "The %s state interface will not be configured, because it is not listed on the BravoSevenHardware.", - state_interface.name.c_str()); - } else { - sensor_state_interfaces_in_use_.push_back(state_interface.name); - force_torque_state_interfaces_map_[state_interface.name] = 0.0; - } - } -} + // BEGIN: This part here is for exemplary purposes - Please do not copy to your production code + for (std::size_t i = 0; i < hw_velocities_.size(); i++) + { + // Simulate DiffBot wheels's movement as a first-order system + // Update the joint status: this is a revolute joint without any limit. + // Simply integrates + hw_positions_[i] = hw_positions_[i] + period.seconds() * hw_velocities_[i]; -void LittlebotHardwareComponent::fillJointsConfigurationMapFromUrdf( - const std::vector & joints) -{ - if (joints.size() != types::kJointsInManipulator) { - throw std::invalid_argument( - "The BravoSevenHardware component must have eight joint interfaces."); + RCLCPP_INFO( + rclcpp::get_logger("DiffBotSystemHardware"), + "Got position state %.5f and velocity state %.5f for '%s'!", hw_positions_[i], + hw_velocities_[i], info_.joints[i].name.c_str()); } + // END: This part here is for exemplary purposes - Please do not copy to your production code - for (const hardware_interface::ComponentInfo & joint_info : joints) { - ManipulatorJointData valid_joint; - if (types::bravo_seven_devices_map_.find(joint_info.name) == - types::bravo_seven_devices_map_.end()) - { - throw std::invalid_argument( - "The joint " + joint_info.name + - " has an invalid name. The joints of the bravo seven Manipulator are: rotate_base, bend_shoulder," - " bend_elbow, rotate_elbow, bend_forearm, rotate_end_effector and end_effector."); - } - - for (const ManipulatorJointData & joint : joints_) { - if (joint_info.name == joint.name) { - throw std::invalid_argument( - "The joint " + joint_info.name + " has already been configured."); - } - } - - valid_joint.name = joint_info.name; - - if (!this->areJointParametersValid(valid_joint)) { - throw std::invalid_argument( - "The joint " + joint_info.name + " has invalid parameters, check the values. " - ); - } - - this->getJointStateInterfaces(joint_info.state_interfaces, valid_joint); - - joints_.at(static_cast(valid_joint.id) - 1) = valid_joint; - } + return hardware_interface::return_type::OK; } -bool LittlebotHardwareComponent::areJointParametersValid( - ManipulatorJointData & valid_joint) -{ - valid_joint.id = types::bravo_seven_devices_map_.at(valid_joint.name); - - for (const std::string & parameter_name : configuration_parameters_names_) { - std::string parameter_as_str = hardware_description_helper_ptr_->getComponentParameter( - valid_joint.name, parameter_name); - - if (!parameter_as_str.empty()) { - float parameter_as_float; - try { - parameter_as_float = std::stof(parameter_as_str); - } catch (const std::invalid_argument & e) { - return false; - } - - float minimum_limit; - float maximum_limit; - - if (parameter_name == "minimum_position" || parameter_name == "maximum_position") { - try { - this->getJointPositionLimitsParameters( - valid_joint.name, minimum_limit, maximum_limit); - } catch (const std::invalid_argument & e) { - RCLCPP_ERROR_STREAM( - rclcpp::get_logger(hardware_component_name_), - "Error while reading the parameters of the joint " << valid_joint.name << ": " << - e.what()); - - return false; - } - } else { - minimum_limit = -parameter_as_float; - maximum_limit = parameter_as_float; - } - valid_joint.limit_values[parameter_name] = std::make_pair(minimum_limit, maximum_limit); - - configurations_map_[parameter_name].push_back(valid_joint.id); - } - } - return true; -} -void LittlebotHardwareComponent::getJointPositionLimitsParameters( - std::string valid_joint_name, float & minimum_position, float & maximum_position) +hardware_interface::return_type LittlebotHardwareComponent::write( + [[maybe_unused]] const rclcpp::Time & time, + [[maybe_unused]] const rclcpp::Duration & period) { - std::string maximum_position_as_str = - hardware_description_helper_ptr_->getComponentParameter( - valid_joint_name, "maximum_position"); - - std::string minimum_position_as_str = - hardware_description_helper_ptr_->getComponentParameter( - valid_joint_name, "minimum_position"); - - if (maximum_position_as_str.empty()) { - throw std::invalid_argument( - "The parameter maximum_position cannot be empty."); - } + // BEGIN: This part here is for exemplary purposes - Please do not copy to your production code + RCLCPP_INFO(rclcpp::get_logger("DiffBotSystemHardware"), "Writing..."); - if (minimum_position_as_str.empty()) { - throw std::invalid_argument( - "The parameter minimum_position cannot be empty."); - } + for (auto i = 0u; i < hw_commands_.size(); i++) + { + // Simulate sending commands to the hardware + RCLCPP_INFO( + rclcpp::get_logger("DiffBotSystemHardware"), "Got command %.5f for '%s'!", hw_commands_[i], + info_.joints[i].name.c_str()); - try { - minimum_position = std::stof(minimum_position_as_str); - maximum_position = std::stof(maximum_position_as_str); - } catch (const std::invalid_argument & e) { - throw std::invalid_argument( - "The position limits contain invalid values."); + hw_velocities_[i] = hw_commands_[i]; } -} + RCLCPP_INFO(rclcpp::get_logger("DiffBotSystemHardware"), "Joints successfully written!"); + // END: This part here is for exemplary purposes - Please do not copy to your production code -void LittlebotHardwareComponent::getJointStateInterfaces( - const std::vector & state_interfaces, - ManipulatorJointData & valid_joint) -{ - if (!state_interfaces.empty()) { - for (const hardware_interface::InterfaceInfo & state_interface : state_interfaces) { - auto vector_iterator = std::find( - all_joint_state_interface_names_.begin(), - all_joint_state_interface_names_.end(), state_interface.name); - - if (vector_iterator == all_joint_state_interface_names_.end()) { - RCLCPP_ERROR( - rclcpp::get_logger( - hardware_component_name_), - "The %s state interface will not be configured, because it is not listed on the BravoSevenHardware.", - state_interface.name.c_str()); - } else { - state_interfaces_map_[state_interface.name].push_back(valid_joint.id); - valid_joint.state_interfaces[state_interface.name] = 0; - } - } - } + return hardware_interface::return_type::OK; } -void LittlebotHardwareComponent::setBravoSevenCommunication( - std::shared_ptr ptr) -{ - manipulator_communication_ptr_ = ptr; -} } // namespace littlebot_base #include "pluginlib/class_list_macros.hpp"