Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Add Interfaces for reporting of error and warning messages from HW #1319

Open
wants to merge 25 commits into
base: master
Choose a base branch
from
Open
Changes from 1 commit
Commits
Show all changes
25 commits
Select commit Hold shift + click to select a range
2a065a3
move creation of stateinterfaces to systeminterface
mamueluth Dec 7, 2023
1e35d1f
store description in state_interface and provide functions for
mamueluth Dec 8, 2023
8334d9f
return correct name for InterfaceDescription
mamueluth Dec 11, 2023
e3ca9ef
move parsing of interface description to component parser
mamueluth Dec 11, 2023
112f20f
adjusted actuator- and sensor_interface as well
mamueluth Dec 12, 2023
e6d49e3
add first tests
mamueluth Dec 18, 2023
4b43db4
adjust sensor_interface getting/setting and add tests
mamueluth Dec 18, 2023
7f7d106
add more tests
mamueluth Dec 18, 2023
4f3ce18
first steps towards variants and storing of value in handle, missing:
mamueluth Dec 19, 2023
dedf062
add variant support
mamueluth Dec 19, 2023
5e244a6
adjusted resource manager to work with shared_ptr adapt actuator,sensor
mamueluth Dec 20, 2023
8313284
cleanup
mamueluth Dec 20, 2023
322412d
fix failing tests
mamueluth Dec 20, 2023
5421081
change rest of component_interface test and mark what should be removed
mamueluth Dec 21, 2023
6b214a2
code review suggestions and:
mamueluth Jan 23, 2024
8ea003b
undo prefix_ -> prefix (HWInfo) and Command-/StateIntefaceSharedPtr
mamueluth Jan 26, 2024
373b182
merge parser functions
mamueluth Jan 26, 2024
18301da
export_interfaces_2() virtual for custom interface export
mamueluth Jan 29, 2024
d7b2fb1
use unordered map and adjust tests
mamueluth Jan 29, 2024
e2f1564
make states and commands of component interfaces private
mamueluth Feb 1, 2024
a48e726
add interface for warning, error and report
mamueluth Jan 11, 2024
63c523f
review suggestions
mamueluth Jan 24, 2024
b692f61
add basic test for error codes setting
mamueluth Jan 29, 2024
ccfe700
remove rebase artefacts
mamueluth Feb 1, 2024
3d017a6
docs for error and warning signals
mamueluth Apr 11, 2024
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
Prev Previous commit
Next Next commit
adjusted actuator- and sensor_interface as well
mamueluth committed Apr 11, 2024
commit 112f20f0707e86333c5efbfc0d91b9bf3a98117b
Original file line number Diff line number Diff line change
@@ -15,10 +15,13 @@
#ifndef HARDWARE_INTERFACE__ACTUATOR_INTERFACE_HPP_
#define HARDWARE_INTERFACE__ACTUATOR_INTERFACE_HPP_

#include <limits>
#include <map>
#include <memory>
#include <string>
#include <vector>

#include "hardware_interface/component_parser.hpp"
#include "hardware_interface/handle.hpp"
#include "hardware_interface/hardware_info.hpp"
#include "hardware_interface/types/hardware_interface_return_values.hpp"
@@ -97,30 +100,85 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod
virtual CallbackReturn on_init(const HardwareInfo & hardware_info)
{
info_ = hardware_info;
import_state_interface_descriptions(info_);
import_command_interface_descriptions(info_);
return CallbackReturn::SUCCESS;
};

/**
* Import the InterfaceDescription for the StateInterfaces from the HardwareInfo.
* Separate them into the possible types: Joint and store them.
*/
virtual void import_state_interface_descriptions(const HardwareInfo & hardware_info)
{
joint_states_descriptions_ =
parse_joint_state_interface_descriptions_from_hardware_info(hardware_info);
}

/**
* Import the InterfaceDescription for the CommandInterfaces from the HardwareInfo.
* Separate them into the possible types: Joint and store them.
*/
virtual void import_command_interface_descriptions(const HardwareInfo & hardware_info)
{
joint_commands_descriptions_ =
parse_joint_command_interface_descriptions_from_hardware_info(hardware_info);
}

/// Exports all state interfaces for this hardware interface.
/**
* Default implementation for exporting the StateInterfaces. The StateInterfaces are created
* according to the InterfaceDescription. The memory accessed by the controllers and hardware is
* assigned here and resides in the system_interface.
*
* If overwritten:
* The state interfaces have to be created and transferred according
* to the hardware info passed in for the configuration.
*
* Note the ownership over the state interfaces is transferred to the caller.
*
* \return vector of state interfaces
*/
virtual std::vector<StateInterface> export_state_interfaces() = 0;
virtual std::vector<StateInterface> export_state_interfaces()
{
std::vector<hardware_interface::StateInterface> state_interfaces;
state_interfaces.reserve(joint_states_descriptions_.size());

for (const auto & descr : joint_states_descriptions_)
{
joint_states_[descr.get_name()] = std::numeric_limits<double>::quiet_NaN();
state_interfaces.emplace_back(StateInterface(descr, &joint_states_[descr.get_name()]));
}

return state_interfaces;
}

/// Exports all command interfaces for this hardware interface.
/**
* Default implementation for exporting the CommandInterfaces. The CommandInterfaces are created
* according to the InterfaceDescription. The memory accessed by the controllers and hardware is
* assigned here and resides in the system_interface.
*
* The command interfaces have to be created and transferred according
* to the hardware info passed in for the configuration.
*
* Note the ownership over the state interfaces is transferred to the caller.
*
* \return vector of command interfaces
*/
virtual std::vector<CommandInterface> export_command_interfaces() = 0;
virtual std::vector<CommandInterface> export_command_interfaces()
{
std::vector<hardware_interface::CommandInterface> command_interfaces;
command_interfaces.reserve(joint_commands_descriptions_.size());

for (const auto & descr : joint_commands_descriptions_)
{
joint_commands_[descr.get_name()] = std::numeric_limits<double>::quiet_NaN();
command_interfaces.emplace_back(CommandInterface(descr, &joint_commands_[descr.get_name()]));
}

return command_interfaces;
}

/// Prepare for a new command interface switch.
/**
@@ -202,8 +260,35 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod
*/
void set_state(const rclcpp_lifecycle::State & new_state) { lifecycle_state_ = new_state; }

double joint_state_get_value(const InterfaceDescription & interface_descr) const
{
return joint_states_.at(interface_descr.get_name());
}

void joint_state_set_value(const InterfaceDescription & interface_descr, const double & value)
{
joint_states_[interface_descr.get_name()] = value;
}

double joint_command_get_value(const InterfaceDescription & interface_descr) const
{
return joint_commands_.at(interface_descr.get_name());
}

void joint_command_set_value(const InterfaceDescription & interface_descr, const double & value)
{
joint_commands_[interface_descr.get_name()] = value;
}

protected:
HardwareInfo info_;
std::vector<InterfaceDescription> joint_states_descriptions_;
std::vector<InterfaceDescription> joint_commands_descriptions_;

private:
std::map<std::string, double> joint_states_;
std::map<std::string, double> joint_commands_;

rclcpp_lifecycle::State lifecycle_state_;
};

Original file line number Diff line number Diff line change
@@ -15,10 +15,13 @@
#ifndef HARDWARE_INTERFACE__SENSOR_INTERFACE_HPP_
#define HARDWARE_INTERFACE__SENSOR_INTERFACE_HPP_

#include <limits>
#include <map>
#include <memory>
#include <string>
#include <vector>

#include "hardware_interface/component_parser.hpp"
#include "hardware_interface/handle.hpp"
#include "hardware_interface/hardware_info.hpp"
#include "hardware_interface/types/hardware_interface_return_values.hpp"
@@ -97,19 +100,47 @@ class SensorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI
virtual CallbackReturn on_init(const HardwareInfo & hardware_info)
{
info_ = hardware_info;
import_state_interface_descriptions(info_);
return CallbackReturn::SUCCESS;
};

/**
* Import the InterfaceDescription for the StateInterfaces from the HardwareInfo.
* Separate them into the possible types: Sensor and store them.
*/
virtual void import_state_interface_descriptions(const HardwareInfo & hardware_info)
{
sensor_states_descriptions_ =
parse_sensor_state_interface_descriptions_from_hardware_info(hardware_info);
}

/// Exports all state interfaces for this hardware interface.
/**
* Default implementation for exporting the StateInterfaces. The StateInterfaces are created
* according to the InterfaceDescription. The memory accessed by the controllers and hardware is
* assigned here and resides in the system_interface.
*
* If overwritten:
* The state interfaces have to be created and transferred according
* to the hardware info passed in for the configuration.
*
* Note the ownership over the state interfaces is transferred to the caller.
*
* \return vector of state interfaces
*/
virtual std::vector<StateInterface> export_state_interfaces() = 0;
virtual std::vector<StateInterface> export_state_interfaces()
{
std::vector<hardware_interface::StateInterface> state_interfaces;
state_interfaces.reserve(sensor_states_descriptions_.size());

for (const auto & descr : sensor_states_descriptions_)
{
sensor_states_[descr.get_name()] = std::numeric_limits<double>::quiet_NaN();
state_interfaces.emplace_back(StateInterface(descr, &sensor_states_[descr.get_name()]));
}

return state_interfaces;
}

/// Read the current state values from the actuator.
/**
@@ -141,8 +172,24 @@ class SensorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI
*/
void set_state(const rclcpp_lifecycle::State & new_state) { lifecycle_state_ = new_state; }

double sensor_state_get_value(const InterfaceDescription & interface_descr) const
{
return sensor_states_.at(interface_descr.get_name());
}

void sensor_state_set_value(const InterfaceDescription & interface_descr, const double & value)
{
sensor_states_[interface_descr.get_name()] = value;
}

protected:
HardwareInfo info_;

std::vector<InterfaceDescription> sensor_states_descriptions_;

private:
std::map<std::string, double> sensor_states_;

rclcpp_lifecycle::State lifecycle_state_;
};

62 changes: 51 additions & 11 deletions hardware_interface/include/hardware_interface/system_interface.hpp
Original file line number Diff line number Diff line change
@@ -102,29 +102,44 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI
virtual CallbackReturn on_init(const HardwareInfo & hardware_info)
{
info_ = hardware_info;
add_state_interface_descriptions();
add_command_interface_descriptions();
import_state_interface_descriptions(info_);
import_command_interface_descriptions(info_);
return CallbackReturn::SUCCESS;
};

virtual void add_state_interface_descriptions()
/**
* Import the InterfaceDescription for the StateInterfaces from the HardwareInfo.
* Separate them into the possible types: Joint, GPIO, Sensor and store them.
*/
void import_state_interface_descriptions(const HardwareInfo & hardware_info)
{
joint_states_descriptions_ = parse_joint_state_interface_descriptions_from_hardware_info(info_);
gpio_states_descriptions_ = parse_gpio_state_interface_descriptions_from_hardware_info(info_);
joint_states_descriptions_ =
parse_joint_state_interface_descriptions_from_hardware_info(hardware_info);
gpio_states_descriptions_ =
parse_gpio_state_interface_descriptions_from_hardware_info(hardware_info);
sensor_states_descriptions_ =
parse_sensor_state_interface_descriptions_from_hardware_info(info_);
parse_sensor_state_interface_descriptions_from_hardware_info(hardware_info);
}

virtual void add_command_interface_descriptions()
/**
* Import the InterfaceDescription for the CommandInterfaces from the HardwareInfo.
* Separate them into the possible types: Joint and GPIO and store them.
*/
void import_command_interface_descriptions(const HardwareInfo & hardware_info)
{
joint_commands_descriptions_ =
parse_joint_command_interface_descriptions_from_hardware_info(info_);
parse_joint_command_interface_descriptions_from_hardware_info(hardware_info);
gpio_commands_descriptions_ =
parse_gpio_command_interface_descriptions_from_hardware_info(info_);
parse_gpio_command_interface_descriptions_from_hardware_info(hardware_info);
}

/// Exports all state interfaces for this hardware interface.
/**
* Default implementation for exporting the StateInterfaces. The StateInterfaces are created
* according to the InterfaceDescription. The memory accessed by the controllers and hardware is
* assigned here and resides in the system_interface.
*
* If overwritten:
* The state interfaces have to be created and transferred according
* to the hardware info passed in for the configuration.
*
@@ -135,19 +150,37 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI
virtual std::vector<StateInterface> export_state_interfaces()
{
std::vector<hardware_interface::StateInterface> state_interfaces;
state_interfaces.reserve(joint_states_descriptions_.size());
state_interfaces.reserve(
joint_states_descriptions_.size() + sensor_states_descriptions_.size() +
gpio_states_descriptions_.size());

for (const auto & descr : joint_states_descriptions_)
{
joint_states_[descr.get_name()] = std::numeric_limits<double>::quiet_NaN();
state_interfaces.emplace_back(StateInterface(descr, &joint_states_[descr.get_name()]));
}

for (const auto & descr : sensor_states_descriptions_)
{
sensor_states_[descr.get_name()] = std::numeric_limits<double>::quiet_NaN();
state_interfaces.emplace_back(StateInterface(descr, &sensor_states_[descr.get_name()]));
}

for (const auto & descr : gpio_states_descriptions_)
{
gpio_states_[descr.get_name()] = std::numeric_limits<double>::quiet_NaN();
state_interfaces.emplace_back(StateInterface(descr, &gpio_states_[descr.get_name()]));
}

return state_interfaces;
}

/// Exports all command interfaces for this hardware interface.
/**
* Default implementation for exporting the CommandInterfaces. The CommandInterfaces are created
* according to the InterfaceDescription. The memory accessed by the controllers and hardware is
* assigned here and resides in the system_interface.
*
* The command interfaces have to be created and transferred according
* to the hardware info passed in for the configuration.
*
@@ -158,14 +191,21 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI
virtual std::vector<CommandInterface> export_command_interfaces()
{
std::vector<hardware_interface::CommandInterface> command_interfaces;
command_interfaces.reserve(joint_commands_descriptions_.size());
command_interfaces.reserve(
joint_commands_descriptions_.size() + gpio_commands_descriptions_.size());

for (const auto & descr : joint_commands_descriptions_)
{
joint_commands_[descr.get_name()] = std::numeric_limits<double>::quiet_NaN();
command_interfaces.emplace_back(CommandInterface(descr, &joint_commands_[descr.get_name()]));
}

for (const auto & descr : gpio_commands_descriptions_)
{
gpio_commands_[descr.get_name()] = std::numeric_limits<double>::quiet_NaN();
command_interfaces.emplace_back(CommandInterface(descr, &gpio_commands_[descr.get_name()]));
}

return command_interfaces;
}