Skip to content

Commit

Permalink
create std interfaces for errors and warnings
Browse files Browse the repository at this point in the history
  • Loading branch information
@mamueluth
mamueluth committed Jan 11, 2024
1 parent ce485b2 commit d3d8cdb
Show file tree
Hide file tree
Showing 10 changed files with 321 additions and 211 deletions.
69 changes: 55 additions & 14 deletions hardware_interface/include/hardware_interface/actuator_interface.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -25,7 +25,10 @@
#include "hardware_interface/component_parser.hpp"
#include "hardware_interface/handle.hpp"
#include "hardware_interface/hardware_info.hpp"
#include "hardware_interface/types/hardware_interface_emergency_stop_signal.hpp"
#include "hardware_interface/types/hardware_interface_error_signals.hpp"
#include "hardware_interface/types/hardware_interface_return_values.hpp"
#include "hardware_interface/types/hardware_interface_warning_signals.hpp"
#include "hardware_interface/types/lifecycle_state_names.hpp"
#include "lifecycle_msgs/msg/state.hpp"
#include "rclcpp/duration.hpp"
Expand Down Expand Up @@ -103,6 +106,9 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod
info_ = hardware_info;
import_state_interface_descriptions(info_);
import_command_interface_descriptions(info_);
create_emergency_stop_interface();
create_error_interfaces();
create_warning_interfaces();
return CallbackReturn::SUCCESS;
};

Expand Down Expand Up @@ -134,6 +140,41 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod
}
}

void create_emergency_stop_interface()
{
InterfaceInfo interface_info;
interface_info.name = hardware_interface::EMERGENCY_STOP_SIGNAL;
interface_info.data_type = "bool";
InterfaceDescription interface_descr(info_.name, interface_info);
emergency_stop_ = std::make_shared<StateInterface>(interface_descr);
}

void create_error_interfaces()
{
for (const auto error_signal : hardware_interface::ERROR_SIGNALS)
{
InterfaceInfo interface_info;
interface_info.name = error_signal;
interface_info.data_type = "uint8_t";
InterfaceDescription interface_descr(info_.name, interface_info);
error_signals_.insert(std::make_pair(
interface_descr.get_name(), std::make_shared<StateInterface>(interface_descr)));
}
}

void create_warning_interfaces()
{
for (const auto warning_signal : hardware_interface::WARNING_SIGNALS)
{
InterfaceInfo interface_info;
interface_info.name = warning_signal;
interface_info.data_type = "int8_t";
InterfaceDescription interface_descr(info_.name, interface_info);
warning_signals_.insert(std::make_pair(
interface_descr.get_name(), std::make_shared<StateInterface>(interface_descr)));
}
}

/// Exports all state interfaces for this hardware interface.
/**
* Default implementation for exporting the StateInterfaces. The StateInterfaces are created
Expand Down Expand Up @@ -310,34 +351,31 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod
return actuator_commands_.at(interface_name)->get_value();
}

void set_warning_code(const std::string & interface_name, const double & warning_code)
void set_emergency_stop(const double & emergency_stop)
{
actuator_states_.at(interface_name)->warning_code(warning_code);
emergency_stop_->emergency_stop(emergency_stop);
}

double get_warning_code(const std::string & interface_name) const
{
return actuator_states_.at(interface_name)->warning_code();
}
double get_emergency_stop() const { return emergency_stop_->emergency_stop(); }

void set_error_code(const std::string & interface_name, const double & error_code)
void set_warning_code(const std::string & warning_signal, const int8_t & warning_code)
{
actuator_states_.at(interface_name)->error_code(error_code);
warning_signals_.at(warning_signal)->warning_code(warning_code);
}

double get_error_code(const std::string & interface_name) const
int8_t get_warning_code(const std::string & warning_signal) const
{
return actuator_states_.at(interface_name)->error_code();
return warning_signals_.at(warning_signal)->warning_code();
}

void set_report_message(const std::string & interface_name, const double & report_message)
void set_error_code(const std::string & error_signal, const uint8_t & error_code)
{
actuator_states_.at(interface_name)->report_message(report_message);
warning_signals_.at(error_signal)->error_code(error_code);
}

double get_report_message(const std::string & interface_name) const
uint8_t get_error_code(const std::string & error_signal) const
{
return actuator_states_.at(interface_name)->report_message();
return warning_signals_.at(error_signal)->error_code();
}

protected:
Expand All @@ -348,6 +386,9 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod
private:
std::map<std::string, std::shared_ptr<StateInterface>> actuator_states_;
std::map<std::string, std::shared_ptr<CommandInterface>> actuator_commands_;
std::shared_ptr<StateInterface> emergency_stop_;
std::map<std::string, std::shared_ptr<StateInterface>> error_signals_;
std::map<std::string, std::shared_ptr<StateInterface>> warning_signals_;

rclcpp_lifecycle::State lifecycle_state_;
};
Expand Down
29 changes: 9 additions & 20 deletions hardware_interface/include/hardware_interface/handle.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -27,7 +27,7 @@
namespace hardware_interface
{

typedef std::variant<double> HANDLE_DATATYPE;
typedef std::variant<bool, double, int8_t, uint8_t> HANDLE_DATATYPE;

/// A handle used to get and set a value on a given interface.
class Handle
Expand Down Expand Up @@ -132,31 +132,31 @@ class StateInterface : public Handle

StateInterface(StateInterface && other) = default;

double emergency_stop() const
bool emergency_stop() const
{
const double * emergency_stop = std::get_if<double>(&value_);
const auto emergency_stop = std::get_if<bool>(&value_);
// This means the value does not store the expected datatype. How should we handle this
// properly?
THROW_ON_NOT_NULLPTR(emergency_stop);
return *emergency_stop;
}

void emergency_stop(const double & emergency_stop) { value_ = emergency_stop; }
void emergency_stop(const bool & emergency_stop) { value_ = emergency_stop; }

double warning_code() const
int8_t warning_code() const
{
const double * warning_code = std::get_if<double>(&value_);
const auto warning_code = std::get_if<int8_t>(&value_);
// This means the value does not store the expected datatype. How should we handle this
// properly?
THROW_ON_NOT_NULLPTR(warning_code);
return *warning_code;
}

void warning_code(const double & warning_code) { value_ = warning_code; }
void warning_code(const int8_t & warning_code) { value_ = warning_code; }

double error_code() const
uint8_t error_code() const
{
const auto error_code = std::get_if<double>(&value_);
const auto error_code = std::get_if<uint8_t>(&value_);
// This means the value does not store the expected datatype. How should we handle this
// properly?
THROW_ON_NOT_NULLPTR(error_code);
Expand All @@ -165,17 +165,6 @@ class StateInterface : public Handle

void error_code(const double & error_code) { value_ = error_code; }

double report_message() const
{
const auto report_message = std::get_if<double>(&value_);
// This means the value does not store the expected datatype. How should we handle this
// properly?
THROW_ON_NOT_NULLPTR(report_message);
return *report_message;
}

void report_message(const double & report_message) { value_ = report_message; }

using Handle::Handle;
};

Expand Down
8 changes: 3 additions & 5 deletions hardware_interface/include/hardware_interface/loaned_state_interface.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -65,13 +65,11 @@ class LoanedStateInterface

double get_value() const { return state_interface_.get_value(); }

double has_emergency_stop() const { return state_interface_.emergency_stop(); }
bool has_emergency_stop() const { return state_interface_.emergency_stop(); }

double get_warning_code() const { return state_interface_.warning_code(); }
int8_t get_warning_code() const { return state_interface_.warning_code(); }

double get_error_code() const { return state_interface_.error_code(); }

double get_report_message() const { return state_interface_.report_message(); }
uint8_t get_error_code() const { return state_interface_.error_code(); }

protected:
StateInterface & state_interface_;
Expand Down
68 changes: 40 additions & 28 deletions hardware_interface/include/hardware_interface/sensor_interface.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -25,7 +25,9 @@
#include "hardware_interface/component_parser.hpp"
#include "hardware_interface/handle.hpp"
#include "hardware_interface/hardware_info.hpp"
#include "hardware_interface/types/hardware_interface_error_signals.hpp"
#include "hardware_interface/types/hardware_interface_return_values.hpp"
#include "hardware_interface/types/hardware_interface_warning_signals.hpp"
#include "hardware_interface/types/lifecycle_state_names.hpp"
#include "lifecycle_msgs/msg/state.hpp"
#include "rclcpp/duration.hpp"
Expand Down Expand Up @@ -102,6 +104,8 @@ class SensorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI
{
info_ = hardware_info;
import_state_interface_descriptions(info_);
create_error_interfaces();
create_warning_interfaces();
return CallbackReturn::SUCCESS;
};

Expand All @@ -119,6 +123,32 @@ class SensorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI
}
}

void create_error_interfaces()
{
for (const auto error_signal : hardware_interface::ERROR_SIGNALS)
{
InterfaceInfo interface_info;
interface_info.name = error_signal;
interface_info.data_type = "uint8_t";
InterfaceDescription interface_descr(info_.name, interface_info);
error_signals_.insert(std::make_pair(
interface_descr.get_name(), std::make_shared<StateInterface>(interface_descr)));
}
}

void create_warning_interfaces()
{
for (const auto warning_signal : hardware_interface::WARNING_SIGNALS)
{
InterfaceInfo interface_info;
interface_info.name = warning_signal;
interface_info.data_type = "int8_t";
InterfaceDescription interface_descr(info_.name, interface_info);
warning_signals_.insert(std::make_pair(
interface_descr.get_name(), std::make_shared<StateInterface>(interface_descr)));
}
}

/// Exports all state interfaces for this hardware interface.
/**
* Default implementation for exporting the StateInterfaces. The StateInterfaces are created
Expand Down Expand Up @@ -200,44 +230,24 @@ class SensorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI
return sensor_states_.at(interface_name)->get_value();
}

void set_emergency_stop(const std::string & interface_name, const double & emergency_stop)
{
sensor_states_.at(interface_name)->emergency_stop(emergency_stop);
}

double get_emergency_stop(const std::string & interface_name) const
{
return sensor_states_.at(interface_name)->emergency_stop();
}

void set_warning_code(const std::string & interface_name, const double & warning_code)
{
sensor_states_.at(interface_name)->warning_code(warning_code);
}

double get_warning_code(const std::string & interface_name) const
{
return sensor_states_.at(interface_name)->warning_code();
}

void set_error_code(const std::string & interface_name, const double & error_code)
void set_warning_code(const std::string & warning_signal, const int8_t & warning_code)
{
sensor_states_.at(interface_name)->error_code(error_code);
warning_signals_.at(warning_signal)->warning_code(warning_code);
}

double get_error_code(const std::string & interface_name) const
int8_t get_warning_code(const std::string & warning_signal) const
{
return sensor_states_.at(interface_name)->error_code();
return warning_signals_.at(warning_signal)->warning_code();
}

void set_report_message(const std::string & interface_name, const double & report_message)
void set_error_code(const std::string & error_signal, const uint8_t & error_code)
{
sensor_states_.at(interface_name)->report_message(report_message);
warning_signals_.at(error_signal)->error_code(error_code);
}

double get_report_message(const std::string & interface_name) const
uint8_t get_error_code(const std::string & error_signal) const
{
return sensor_states_.at(interface_name)->report_message();
return warning_signals_.at(error_signal)->error_code();
}

protected:
Expand All @@ -247,6 +257,8 @@ class SensorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI

private:
std::map<std::string, std::shared_ptr<StateInterface>> sensor_states_;
std::map<std::string, std::shared_ptr<StateInterface>> error_signals_;
std::map<std::string, std::shared_ptr<StateInterface>> warning_signals_;

rclcpp_lifecycle::State lifecycle_state_;
};
Expand Down
Loading

0 comments on commit d3d8cdb

Please sign in to comment.