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.

Sign up for GitHub

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

move creation of stateinterfaces to systeminterface #9

Closed
wants to merge 8 commits into from
Closed

move creation of stateinterfaces to systeminterface #9

Show file tree
Hide file tree
Changes from 2 commits
Commits
Show all changes
8 commits
Select commit Hold shift + click to select a range
a8a3ab0
move creation of stateinterfaces to systeminterface
mamueluth Dec 7, 2023
9931106
store description in state_interface and provide functions for
mamueluth Dec 8, 2023
88f744f
return correct name for InterfaceDescription
mamueluth Dec 11, 2023
8f73a83
move parsing of interface description to component parser
mamueluth Dec 11, 2023
c49735d
adjusted actuator- and sensor_interface as well
mamueluth Dec 12, 2023
0782cdb
add first tests
mamueluth Dec 18, 2023
17f7a8f
adjust sensor_interface getting/setting and add tests
mamueluth Dec 18, 2023
7d4d939
add more tests
mamueluth Dec 18, 2023
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
14 changes: 14 additions & 0 deletions hardware_interface/include/hardware_interface/handle.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -18,6 +18,7 @@
#include <string>
#include <utility>

#include "hardware_interface/hardware_info.hpp"
#include "hardware_interface/macros.hpp"
#include "hardware_interface/visibility_control.h"

Expand Down Expand Up @@ -116,6 +117,13 @@ class ReadWriteHandle : public ReadOnlyHandle
class StateInterface : public ReadOnlyHandle
{
public:
explicit StateInterface(
const InterfaceDescription & interface_description, double * value_ptr = nullptr)
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I am not sure if we actually need to extend this here. We are not storing InterfaceDescription, but extending this might be useful in the future to support other types. What we also don't need here is the third argument value_ptr. Did you plan to remove it in the future? I mean we require this for backward compatibility, but for new approach only InterfaceDescription should sufficient.

Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Looking into this, it might be useful to move the storage here, when using getter and setters, which means that we don't need this method, but more something like:

Suggested change
const InterfaceDescription & interface_description, double * value_ptr = nullptr)
const InterfaceDescription & interface_description)

This should for now store the variable with value, and later we move it into handles, for example:

const InterfaceDescription & interface_description) : ReadOnlyHandle(interface_description)  // new method there see below)
{
  value = std::limits<double>::quietNan();  // it might be that we can move this even in the ReadOnlyHandle, which is even better so we don't need to duplicate this code in the interfaces --> this means if there is no pointer, we create our own storage of values.
  value_ptr_ = &value;
}  // This method might be removed later when we remove the old functionality

private:
  double value; // later we move this value into 

... [this should go above in the `ReadOnlyHandle`

explicit ReadOnlyHandle(const InterfaceDescription & interface_description)
  : prefix_name_(interface_description.prefix_name.), interface_name_(interface_description.interface_info.name), value_ptr_(nullptr)
{
}

Later changes within this file / handles are OK. Therefore, it is not so relevant where you move storage, but of course nicer the code the better. Only the changes of external API are critical.

Copy link
Member Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

working on this as a second step. Needs quite a lot of changes.

: ReadOnlyHandle(
interface_description.prefix_name, interface_description.interface_info.name, value_ptr)
{
}

StateInterface(const StateInterface & other) = default;

StateInterface(StateInterface && other) = default;
Expand All @@ -126,6 +134,12 @@ class StateInterface : public ReadOnlyHandle
class CommandInterface : public ReadWriteHandle
{
public:
explicit CommandInterface(
const InterfaceDescription & interface_description, double * value_ptr = nullptr)
: ReadWriteHandle(
interface_description.prefix_name, interface_description.interface_info.name, value_ptr)
{
}
/// CommandInterface copy constructor is actively deleted.
/**
* Command interfaces are having a unique ownership and thus
Expand Down
22 changes: 22 additions & 0 deletions hardware_interface/include/hardware_interface/hardware_info.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -102,6 +102,28 @@ struct TransmissionInfo
std::unordered_map<std::string, std::string> parameters;
};

/**
* This structure stores information about an interface for a specific hardware which should be
* instantiated internally.
*/
struct InterfaceDescription
{
InterfaceDescription(const std::string & prefix_name_in, const InterfaceInfo & interface_info_in)
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

This is a good idea to add this class, but please add also then type and size and so on. Maybe we have already something suitable here. What about InterfaceInfo structure?

Copy link
Member Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Not sure what you mean, i already use InterfaceInfo but i created this since the unique name of the component is missing and ComponentInfo does store all command/state Interfaces which we don't want either. Or am i mistaken?

: prefix_name(prefix_name_in), interface_info(interface_info_in)
{
}

/**
* Name of the interface defined by the user.
*/
std::string prefix_name;

InterfaceInfo interface_info;

std::string get_name() const { return prefix_name + interface_info.name; }
mamueluth marked this conversation as resolved.
Show resolved Hide resolved
std::string get_type() const { return interface_info.name; }
};

/// This structure stores information about hardware defined in a robot's URDF.
struct HardwareInfo
{
Expand Down
144 changes: 142 additions & 2 deletions hardware_interface/include/hardware_interface/system_interface.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -15,13 +15,15 @@
#ifndef HARDWARE_INTERFACE__SYSTEM_INTERFACE_HPP_
#define HARDWARE_INTERFACE__SYSTEM_INTERFACE_HPP_

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

#include "hardware_interface/handle.hpp"
#include "hardware_interface/hardware_info.hpp"
#include "hardware_interface/types/hardware_interface_return_values.hpp"
#include "hardware_interface/types/hardware_interface_type_values.hpp"
#include "hardware_interface/types/lifecycle_state_names.hpp"
#include "lifecycle_msgs/msg/state.hpp"
#include "rclcpp/duration.hpp"
Expand Down Expand Up @@ -98,9 +100,56 @@ 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();
return CallbackReturn::SUCCESS;
};

virtual void add_state_interface_descriptions()
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Method name is not so good. Why don't you use the stateless method, this means doesn't access to the global variables? This parsing will repeat in all components, so maybe adding this into URDF parser or Hardware Info might be enough. Actually, we have this already in the InterfaceInfo class. OK, only sorting into different vectors is not there… Perhaps something be added on the parser level, as it is repeatable.

Copy link
Member Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Cannot think of a better name. moved it to component parser

Copy link
Member Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

renamed it to import _<command/state>_interface_descriptions

{
for (const auto & joint : info_.joints)
{
for (const auto & state_interface : joint.state_interfaces)
{
joint_states_descr_.emplace_back(InterfaceDescription(joint.name, state_interface));
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Just write the full name, it is clearer.

Suggested change
joint_states_descr_.emplace_back(InterfaceDescription(joint.name, state_interface));
joint_states_description_.emplace_back(InterfaceDescription(joint.name, state_interface));

}
}

for (const auto & sensor : info_.sensors)
{
for (const auto & state_interface : sensor.state_interfaces)
{
sensor_states_descr_.emplace_back(InterfaceDescription(sensor.name, state_interface));
}
}

for (const auto & gpio : info_.gpios)
{
for (const auto & state_interface : gpio.state_interfaces)
{
gpio_states_descr_.emplace_back(InterfaceDescription(gpio.name, state_interface));
}
}
}

virtual void add_command_interface_descriptions()
{
for (const auto & joint : info_.joints)
{
for (const auto & command_interface : joint.command_interfaces)
{
joint_commands_descr_.emplace_back(InterfaceDescription(joint.name, command_interface));
}
}
for (const auto & gpio : info_.gpios)
{
for (const auto & command_interface : gpio.command_interfaces)
{
gpio_commands_descr_.emplace_back(InterfaceDescription(gpio.name, command_interface));
}
}
}

/// Exports all state interfaces for this hardware interface.
/**
* The state interfaces have to be created and transferred according
Expand All @@ -110,7 +159,19 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI
*
* \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_descr_.size());

for (const auto & descr : joint_states_descr_)
{
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.
/**
Expand All @@ -121,7 +182,19 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI
*
* \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_descr_.size());

for (const auto & descr : joint_commands_descr_)
{
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.
/**
Expand Down Expand Up @@ -203,8 +276,75 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI
*/
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;
}

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;
}

double gpio_state_get_value(const InterfaceDescription & interface_descr) const
{
return gpio_states_.at(interface_descr.get_name());
}

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

double gpio_command_get_value(const InterfaceDescription & interface_descr) const
{
return gpio_commands_.at(interface_descr.get_name());
}

void gpio_commands_set_value(const InterfaceDescription & interface_descr, const double & value)
{
gpio_commands_[interface_descr.get_name()] = value;
}
Comment on lines +312 to +360
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

This looks good!


protected:
HardwareInfo info_;
std::vector<InterfaceDescription> joint_states_descr_;
std::vector<InterfaceDescription> joint_commands_descr_;

std::vector<InterfaceDescription> sensor_states_descr_;

std::vector<InterfaceDescription> gpio_states_descr_;
std::vector<InterfaceDescription> gpio_commands_descr_;

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

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

std::map<std::string, double> gpio_states_;
std::map<std::string, double> gpio_commands_;

rclcpp_lifecycle::State lifecycle_state_;
};

Expand Down
Loading