Skip to content

Commit

Permalink
add interface for warning, error and report
Browse files Browse the repository at this point in the history
  • Loading branch information
@mamueluth
mamueluth committed Jan 29, 2024
1 parent b5f9b4d commit 884c472
Show file tree
Hide file tree
Showing 11 changed files with 1,263 additions and 10 deletions.
4 changes: 4 additions & 0 deletions hardware_interface/CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -80,6 +80,10 @@ if(BUILD_TESTING)
target_link_libraries(test_component_interfaces_custom_export hardware_interface)
ament_target_dependencies(test_component_interfaces_custom_export ros2_control_test_assets)

ament_add_gmock(test_error_warning_codes test/test_error_warning_codes.cpp)
target_link_libraries(test_error_warning_codes hardware_interface)
ament_target_dependencies(test_error_warning_codes ros2_control_test_assets)

ament_add_gmock(test_component_parser test/test_component_parser.cpp)
target_link_libraries(test_component_parser hardware_interface)
ament_target_dependencies(test_component_parser ros2_control_test_assets)
Expand Down
94 changes: 94 additions & 0 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,7 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod
info_ = hardware_info;
import_state_interface_descriptions(info_);
import_command_interface_descriptions(info_);
create_report_interfaces();
return CallbackReturn::SUCCESS;
};

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

/**
* Creates all interfaces used for reporting emergency stop, warning and error messages.
* The available report interfaces are: EMERGENCY_STOP_SIGNAL, ERROR_SIGNAL, ERROR_SIGNAL_MESSAGE,
* WARNING_SIGNAL and WARNING_SIGNAL_MESSAGE. Where the <report_type>_MESSAGE hold the message for
* the corresponding report signal.
* The interfaces are named like <hardware_name>/<report_interface_type>. E.g. if hardware is
* called joint_1 -> interface for WARNING_SIGNAL is called: joint_1/WARNING_SIGNAL
*/
void create_report_interfaces()
{
// EMERGENCY STOP
InterfaceInfo emergency_interface_info;
emergency_interface_info.name = hardware_interface::EMERGENCY_STOP_SIGNAL;
emergency_interface_info.data_type = "bool";
InterfaceDescription emergency_interface_descr(info_.name, emergency_interface_info);
emergency_stop_ = std::make_shared<StateInterface>(emergency_interface_descr);

// ERROR
// create error signal interface
InterfaceInfo error_interface_info;
error_interface_info.name = hardware_interface::ERROR_SIGNAL_INTERFACE_NAME;
error_interface_info.data_type = "std::array<uint8_t>";
InterfaceDescription error_interface_descr(info_.name, error_interface_info);
error_signal_ = std::make_shared<StateInterface>(error_interface_descr);
// create error signal report message interface
InterfaceInfo error_msg_interface_info;
error_msg_interface_info.name = hardware_interface::ERROR_SIGNAL_MESSAGE_INTERFACE_NAME;
error_msg_interface_info.data_type = "std::array<std::string>";
InterfaceDescription error_msg_interface_descr(info_.name, error_msg_interface_info);
error_signal_message_ = std::make_shared<StateInterface>(error_msg_interface_descr);

// WARNING
// create warning signal interface
InterfaceInfo warning_interface_info;
warning_interface_info.name = hardware_interface::WARNING_SIGNAL_INTERFACE_NAME;
warning_interface_info.data_type = "std::array<uint8_t>";
InterfaceDescription warning_interface_descr(info_.name, warning_interface_info);
warning_signal_ = std::make_shared<StateInterface>(warning_interface_descr);
// create warning signal report message interface
InterfaceInfo warning_msg_interface_info;
warning_msg_interface_info.name = hardware_interface::WARNING_SIGNAL_MESSAGE_INTERFACE_NAME;
warning_msg_interface_info.data_type = "std::array<std::string>";
InterfaceDescription warning_msg_interface_descr(info_.name, warning_msg_interface_info);
warning_signal_message_ = std::make_shared<StateInterface>(warning_msg_interface_descr);
}

/// Exports all state interfaces for this hardware interface.
/**
* Old way of exporting the StateInterfaces. If a empty vector is returned then
Expand Down Expand Up @@ -190,6 +240,14 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod
actuator_states_.insert(std::make_pair(name, state_interface));
state_interfaces.push_back(state_interface);
}

// export warning signal interfaces
state_interfaces.push_back(emergency_stop_);
state_interfaces.push_back(error_signal_);
state_interfaces.push_back(error_signal_message_);
state_interfaces.push_back(warning_signal_);
state_interfaces.push_back(warning_signal_message_);

return state_interfaces;
}

Expand Down Expand Up @@ -353,6 +411,35 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod
return actuator_commands_.at(interface_name)->get_value();
}

void set_emergency_stop(const double & emergency_stop)
{
emergency_stop_->set_value(emergency_stop);
}

double get_emergency_stop() const { return emergency_stop_->get_value(); }

void set_error_code(const double & error_code) { error_signal_->set_value(error_code); }

double get_error_code() const { return error_signal_->get_value(); }

void set_error_message(const double & error_message)
{
error_signal_message_->set_value(error_message);
}

double get_error_message() const { return error_signal_message_->get_value(); }

void set_warning_code(const double & warning_codes) { warning_signal_->set_value(warning_codes); }

double get_warning_code() const { return warning_signal_->get_value(); }

void set_warning_message(const double & error_message)
{
warning_signal_message_->set_value(error_message);
}

double get_warning_message() const { return warning_signal_message_->get_value(); }

protected:
HardwareInfo info_;
std::unordered_map<std::string, InterfaceDescription> joint_state_interfaces_;
Expand All @@ -361,6 +448,13 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod
std::unordered_map<std::string, std::shared_ptr<StateInterface>> actuator_states_;
std::unordered_map<std::string, std::shared_ptr<CommandInterface>> actuator_commands_;

private:
std::shared_ptr<StateInterface> emergency_stop_;
std::shared_ptr<StateInterface> error_signal_;
std::shared_ptr<StateInterface> error_signal_message_;
std::shared_ptr<StateInterface> warning_signal_;
std::shared_ptr<StateInterface> warning_signal_message_;

rclcpp_lifecycle::State lifecycle_state_;
};

Expand Down
4 changes: 3 additions & 1 deletion hardware_interface/include/hardware_interface/handle.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -15,6 +15,7 @@
#ifndef HARDWARE_INTERFACE__HANDLE_HPP_
#define HARDWARE_INTERFACE__HANDLE_HPP_

#include <array>
#include <limits>
#include <memory>
#include <string>
Expand All @@ -23,8 +24,9 @@

#include "hardware_interface/hardware_info.hpp"
#include "hardware_interface/macros.hpp"
#include "hardware_interface/types/hardware_interface_error_signals.hpp"
#include "hardware_interface/types/hardware_interface_warning_signals.hpp"
#include "hardware_interface/visibility_control.h"

namespace hardware_interface
{

Expand Down
76 changes: 76 additions & 0 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,7 @@ class SensorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI
{
info_ = hardware_info;
import_state_interface_descriptions(info_);
create_report_interfaces();
return CallbackReturn::SUCCESS;
};

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

/**
* Creates all interfaces used for reporting warning and error messages.
* The available report interfaces are: ERROR_SIGNAL, ERROR_SIGNAL_MESSAGE,
* WARNING_SIGNAL and WARNING_SIGNAL_MESSAGE. Where the <report_type>_MESSAGE hold the message for
* the corresponding report signal.
* The interfaces are named like <hardware_name>/<report_interface_type>. E.g. if hardware is
* called sensor_1 -> interface for WARNING_SIGNAL is called: sensor_1/WARNING_SIGNAL
*/
void create_report_interfaces()
{
// ERROR
// create error signal interface
InterfaceInfo error_interface_info;
error_interface_info.name = hardware_interface::ERROR_SIGNAL_INTERFACE_NAME;
error_interface_info.data_type = "std::array<uint8_t>";
InterfaceDescription error_interface_descr(info_.name, error_interface_info);
error_signal_ = std::make_shared<StateInterface>(error_interface_descr);
// create error signal report message interface
InterfaceInfo error_msg_interface_info;
error_msg_interface_info.name = hardware_interface::ERROR_SIGNAL_MESSAGE_INTERFACE_NAME;
error_msg_interface_info.data_type = "std::array<std::string>";
InterfaceDescription error_msg_interface_descr(info_.name, error_msg_interface_info);
error_signal_message_ = std::make_shared<StateInterface>(error_msg_interface_descr);

// WARNING
// create warning signal interface
InterfaceInfo warning_interface_info;
warning_interface_info.name = hardware_interface::WARNING_SIGNAL_INTERFACE_NAME;
warning_interface_info.data_type = "std::array<uint8_t>";
InterfaceDescription warning_interface_descr(info_.name, warning_interface_info);
warning_signal_ = std::make_shared<StateInterface>(warning_interface_descr);
// create warning signal report message interface
InterfaceInfo warning_msg_interface_info;
warning_msg_interface_info.name = hardware_interface::WARNING_SIGNAL_MESSAGE_INTERFACE_NAME;
warning_msg_interface_info.data_type = "std::array<std::string>";
InterfaceDescription warning_msg_interface_descr(info_.name, warning_msg_interface_info);
warning_signal_message_ = std::make_shared<StateInterface>(warning_msg_interface_descr);
}

/// Exports all state interfaces for this hardware interface.
/**
* Old way of exporting the StateInterfaces. If a empty vector is returned then
Expand Down Expand Up @@ -177,6 +219,12 @@ class SensorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI
state_interfaces.push_back(state_interface);
}

// export warning signal interfaces
state_interfaces.push_back(error_signal_);
state_interfaces.push_back(error_signal_message_);
state_interfaces.push_back(warning_signal_);
state_interfaces.push_back(warning_signal_message_);

return state_interfaces;
}

Expand Down Expand Up @@ -220,13 +268,41 @@ class SensorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI
return sensor_states_.at(interface_name)->get_value();
}

void set_error_code(const double & error_code) { error_signal_->set_value(error_code); }

double get_error_code() const { return error_signal_->get_value(); }

void set_error_message(const double & error_message)
{
error_signal_message_->set_value(error_message);
}

double get_error_message() const { return error_signal_message_->get_value(); }

void set_warning_code(const double & warning_codes) { warning_signal_->set_value(warning_codes); }

double get_warning_code() const { return warning_signal_->get_value(); }

void set_warning_message(const double & error_message)
{
warning_signal_message_->set_value(error_message);
}

double get_warning_message() const { return warning_signal_message_->get_value(); }

protected:
HardwareInfo info_;

std::unordered_map<std::string, InterfaceDescription> sensor_state_interfaces_;

std::unordered_map<std::string, std::shared_ptr<StateInterface>> sensor_states_;

private:
std::shared_ptr<StateInterface> error_signal_;
std::shared_ptr<StateInterface> error_signal_message_;
std::shared_ptr<StateInterface> warning_signal_;
std::shared_ptr<StateInterface> warning_signal_message_;

rclcpp_lifecycle::State lifecycle_state_;
};

Expand Down
Loading

0 comments on commit 884c472

Please sign in to comment.