Skip to content

Proposal for solving multiple issues with implict interface in hardware. #714

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

Draft
wants to merge 1 commit into
base: master
Choose a base branch
from
Draft

Proposal for solving multiple issues with implict interface in hardware. #714

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
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
123 changes: 116 additions & 7 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 @@ -16,19 +16,126 @@
#define HARDWARE_INTERFACE__HANDLE_HPP_

#include <string>
#include <vector>
#include <utility>

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

#include <hardware_interface/hardware_info.hpp>

namespace hardware_interface
{
struct InterfaceConfiguration
{
std::string joint_name;
InterfaceInfo interface_info;
};

/// A variant class used to store value on a given interface
class Variant
Copy link
Contributor

Choose a reason for hiding this comment

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

I think a better name is needed. Maybe InterfaceVariant, HWInterfaceVariant, or HardwareInterfaceVariant

Copy link
Contributor

Choose a reason for hiding this comment

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

I think it would be better to just use std::variant from c++17, it is a much nicer implementation of a sum type than this.

Copy link
Member Author

Choose a reason for hiding this comment

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

As discussed, the problem with std::variant is that it does not support arrays or vectors – which we want to add in forceable future (cf. #490)

Copy link
Contributor

Choose a reason for hiding this comment

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

you can't copy a vector without calling new, I don't understand why you need vectors when you can use template arguments to specify the size of sets of values.

{
enum value_type
{
boolean = 0,
integer,
floating,
vector_boolean,
vector_integer,
vector_floating,
};

public:
Variant(bool value)
{
value_type_ = value_type::boolean;
bool_value_ = value;
}

Variant(int value)
{
value_type_ = value_type::integer;
int_value_ = value;
}

Variant(double value)
{
value_type_ = value_type::floating;
double_value_ = value;
}

Variant(const std::vector<bool> & value)
{
value_type_ = value_type::vector_boolean;
vector_bool_value_ = value;
}

Variant(const std::vector<int> & value)
{
value_type_ = value_type::vector_integer;
vector_int_value_ = value;
}

Variant(const std::vector<double> & value)
{
value_type_ = value_type::vector_floating;
vector_double_value_ = value;
}

void set_value(bool value)
{
bool_value_ = value;
}

void set_value(int value)
{
int_value_ = value;
}

void set_value(double value)
{
double_value_ = value;
}

template <typename DataT, typename std::enable_if<std::is_integral<DataT>, bool> = true>
DataT get_value()
{
return bool_value_;
}

template <typename DataT, typename std::enable_if<std::is_integral<DataT>, int> = true>
DataT get_value()
{
return int_value_;
}

template <typename DataT, typename std::enable_if<std::is_floating_point<DataT>, double> = true>
DataT get_value()
{
return double_value_;
}

bool is_valid()
{
// Check if nan or empty to return "false"
}

private:
value_type value_type_;
bool bool_value_;
int int_value_;
double double_value_;
std::vector<bool> vector_bool_value_;
std::vector<int> vector_int_value_;
std::vector<double> vector_double_value_;
Copy link
Contributor

Choose a reason for hiding this comment

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

Maybe you should add a member that would store whether this interface is initialized and a getter function.

bool is_initialized_;
bool is_initialized() { return is_initialized_; }

Copy link
Contributor

Choose a reason for hiding this comment

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

I would really like to be rid of initialized flag values and instead use RAII like it was meant to be used. All constructed objects are either valid or don't exist. We should use the type system for things like this.

};

/// A handle used to get and set a value on a given interface.
class ReadOnlyHandle
{
public:
ReadOnlyHandle(
const std::string & name, const std::string & interface_name, double * value_ptr = nullptr)
const std::string & name, const std::string & interface_name, Variant * value_ptr = nullptr)
Copy link
Contributor

Choose a reason for hiding this comment

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

This doesn't fix the fundamental problem I see with the interfaces of ros2_control. With this change, you are still using a pointer to some memory in the hardware_interface for communication between the hardware_interface and the controllers.

Copy link
Member Author

Choose a reason for hiding this comment

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

I just don't know any other approach that would work will all the types we want to support and provides all the flexibility and strictness to manage this sensibly with controller manager.

Using functions does not solve fully all the issues, but adds complexity on the users' side of the code.

Copy link
Contributor

Choose a reason for hiding this comment

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

Using functions makes it explicit and not out of band.

: name_(name), interface_name_(interface_name), value_ptr_(value_ptr)
{
}
Expand Down Expand Up @@ -62,23 +169,24 @@ class ReadOnlyHandle

const std::string get_full_name() const { return name_ + "/" + interface_name_; }

double get_value() const
template <typedef DataT>
DataT get_value() const
{
THROW_ON_NULLPTR(value_ptr_);
return *value_ptr_;
return value_ptr_->get_value();
}

protected:
std::string name_;
std::string interface_name_;
double * value_ptr_;
Variant * value_ptr_;
};

class ReadWriteHandle : public ReadOnlyHandle
{
public:
ReadWriteHandle(
const std::string & name, const std::string & interface_name, double * value_ptr = nullptr)
const std::string & name, const std::string & interface_name, Variant * value_ptr = nullptr)
: ReadOnlyHandle(name, interface_name, value_ptr)
{
}
Expand All @@ -97,10 +205,11 @@ class ReadWriteHandle : public ReadOnlyHandle

virtual ~ReadWriteHandle() = default;

void set_value(double value)
template <typename DataT>
void set_value(DataT value)
{
THROW_ON_NULLPTR(this->value_ptr_);
*this->value_ptr_ = value;
this->value_ptr_->set_value(value);
}
};

Expand Down
3 changes: 3 additions & 0 deletions hardware_interface/include/hardware_interface/system.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -91,6 +91,9 @@ class System final

private:
std::unique_ptr<SystemInterface> impl_;

std::vector<Variant> commands_;
std::vector<Variant> states_;
};

} // namespace hardware_interface
Expand Down
8 changes: 6 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 @@ -97,6 +97,10 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI
return CallbackReturn::SUCCESS;
};

virtual std::vector<InterfaceConfiguration> export_state_interfaces_info() = 0;

virtual std::vector<InterfaceConfiguration> export_command_interfaces_info() = 0;

/// Exports all state interfaces for this hardware interface.
/**
* The state interfaces have to be created and transferred according
Expand Down Expand Up @@ -166,7 +170,7 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI
*
* \return return_type::OK if the read was successful, return_type::ERROR otherwise.
*/
virtual return_type read() = 0;
virtual return_type read(std::vector<Variant> & states) = 0;
Copy link
Contributor

Choose a reason for hiding this comment

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

I would support what @tylerjw suggested here: NoDiscard.

This will force the user to check the return value.

Suggested change
virtual return_type read(std::vector<Variant> & states) = 0;
virtual [[nodiscard]] return_type read(std::vector<Variant> & states) = 0;

Copy link
Member Author

Choose a reason for hiding this comment

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

I don't understand why a controller should deal with a failure of an interface that belongs to a hardware. Controller should do it's thing.

The issue you actually have is missing functionality in controller manager. Everything is known for months already, but someone has to write some code at some point. Unfortunately, most of the users decide to do some external solutions instead of writing 50 lines of code in the controller manager.


/// Write the current command values to the actuator.
/**
Expand All @@ -175,7 +179,7 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI
*
* \return return_type::OK if the read was successful, return_type::ERROR otherwise.
*/
virtual return_type write() = 0;
virtual return_type write(std::vector<Variant> & commands) = 0;
Copy link
Contributor

Choose a reason for hiding this comment

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

NoDiscard would be good here, too.

Copy link
Contributor

Choose a reason for hiding this comment

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

Agree NoDiscard should be used on these and the return here should be propagated to the controllers so the controller fails when the hardware it is commanding fails.

Copy link
Member Author

Choose a reason for hiding this comment

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

This is not, and it will never be propagated to controllers. Controller manager should deal with this. (see comment above)

Copy link
Contributor

Choose a reason for hiding this comment

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

The controller is the interface to the user in many cases as the controllers create ros interfaces. This means that failures are communicated out of band from how external ros processes are interacting with ros2_control. I want to get rid of all of these sorts of "out of band", "side-affect", "implicit" communication channels and make the handling of errors part of the explicit path of the code.


/// Get name of the actuator hardware.
/**
Expand Down
69 changes: 66 additions & 3 deletions hardware_interface/src/system.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -186,11 +186,74 @@ const rclcpp_lifecycle::State & System::error()

std::vector<StateInterface> System::export_state_interfaces()
{
return impl_->export_state_interfaces();
std::vector<StateInterface> state_interfaces;

auto state_interfaces_info = impl_->export_state_interfaces_info();
states_.reserve(state_interfaces_info.size());
state_interfaces.reserve(state_interfaces_info.size());

for (auto const & itf : state_interfaces_info)
{
if (itf.interface_info.data_type.empty()) // default is double (check in parser if not already set)
{
if (itf.interface_info.initial_value.empty())
{
states_.emplace_back(hardware_interface::Variant(std::numeric_limits<double>::quiet_NaN()));
}
else
{
states_.emplace_back(hardware_interface::Variant(std::stod(itf.interface_info.initial_value)));
}
}
else if (itf.interface_info.data_type == "bool")
{
if (itf.interface_info.initial_value == "true" || itf.interface_info.initial_value == "True")
{
states_.emplace_back(hardware_interface::Variant(true));
}
else
{
states_.emplace_back(hardware_interface::Variant(false));
}
}
else if (itf.interface_info.data_type == "int")
{
if (itf.interface_info.initial_value.empty())
{
states_.emplace_back(hardware_interface::Variant(std::numeric_limits<int>::quiet_NaN()));
}
else
{
states_.emplace_back(hardware_interface::Variant(std::stoi(itf.interface_info.initial_value)));
}
}
else if (itf.interface_info.data_type == "double")
{
if (itf.interface_info.initial_value.empty())
{
states_.emplace_back(hardware_interface::Variant(std::numeric_limits<double>::quiet_NaN()));
}
else
{
states_.emplace_back(hardware_interface::Variant(std::stod(itf.interface_info.initial_value)));
}
}
else if (itf.interface_info.data_type == "vec<bool>") // somewhere we should have "size"
{
states_.emplace_back(hardware_interface::Variant(std::vector<bool>().reserve(std::stoi(itf.interface_info.size))));
}

// .... and so on for other types

state_interfaces.emplace_back(StateInterface(
itf.joint_name, itf.interface_info.name, &states_.back()));
}
}

std::vector<CommandInterface> System::export_command_interfaces()
{
// The same change as for state interfaces

return impl_->export_command_interfaces();
}

Expand Down Expand Up @@ -219,7 +282,7 @@ return_type System::read()
impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE ||
impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE)
{
result = impl_->read();
result = impl_->read(state_);
if (result == return_type::ERROR)
{
error();
Expand All @@ -235,7 +298,7 @@ return_type System::write()
impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE ||
impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE)
{
result = impl_->write();
result = impl_->write(commands_);
if (result == return_type::ERROR)
{
error();
Expand Down
Loading