Skip to content

Commit a48e726

Browse files
mamueluthmamueluth
committed
add interface for warning, error and report
1 parent e2f1564 commit a48e726

11 files changed

+1263
-10
lines changed

‎hardware_interface/CMakeLists.txt‎

Lines changed: 4 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -82,6 +82,10 @@ if(BUILD_TESTING)
8282
target_link_libraries(test_component_interfaces_custom_export hardware_interface)
8383
ament_target_dependencies(test_component_interfaces_custom_export ros2_control_test_assets)
8484

85+
ament_add_gmock(test_error_warning_codes test/test_error_warning_codes.cpp)
86+
target_link_libraries(test_error_warning_codes hardware_interface)
87+
ament_target_dependencies(test_error_warning_codes ros2_control_test_assets)
88+
8589
ament_add_gmock(test_component_parser test/test_component_parser.cpp)
8690
target_link_libraries(test_component_parser hardware_interface)
8791
ament_target_dependencies(test_component_parser ros2_control_test_assets)

‎hardware_interface/include/hardware_interface/actuator_interface.hpp‎

Lines changed: 94 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -25,7 +25,10 @@
2525
#include "hardware_interface/component_parser.hpp"
2626
#include "hardware_interface/handle.hpp"
2727
#include "hardware_interface/hardware_info.hpp"
28+
#include "hardware_interface/types/hardware_interface_emergency_stop_signal.hpp"
29+
#include "hardware_interface/types/hardware_interface_error_signals.hpp"
2830
#include "hardware_interface/types/hardware_interface_return_values.hpp"
31+
#include "hardware_interface/types/hardware_interface_warning_signals.hpp"
2932
#include "hardware_interface/types/lifecycle_state_names.hpp"
3033
#include "lifecycle_msgs/msg/state.hpp"
3134
#include "rclcpp/duration.hpp"
@@ -103,6 +106,7 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod
103106
info_ = hardware_info;
104107
import_state_interface_descriptions(info_);
105108
import_command_interface_descriptions(info_);
109+
create_report_interfaces();
106110
return CallbackReturn::SUCCESS;
107111
};
108112

@@ -134,6 +138,52 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod
134138
}
135139
}
136140

141+
/**
142+
* Creates all interfaces used for reporting emergency stop, warning and error messages.
143+
* The available report interfaces are: EMERGENCY_STOP_SIGNAL, ERROR_SIGNAL, ERROR_SIGNAL_MESSAGE,
144+
* WARNING_SIGNAL and WARNING_SIGNAL_MESSAGE. Where the <report_type>_MESSAGE hold the message for
145+
* the corresponding report signal.
146+
* The interfaces are named like <hardware_name>/<report_interface_type>. E.g. if hardware is
147+
* called joint_1 -> interface for WARNING_SIGNAL is called: joint_1/WARNING_SIGNAL
148+
*/
149+
void create_report_interfaces()
150+
{
151+
// EMERGENCY STOP
152+
InterfaceInfo emergency_interface_info;
153+
emergency_interface_info.name = hardware_interface::EMERGENCY_STOP_SIGNAL;
154+
emergency_interface_info.data_type = "bool";
155+
InterfaceDescription emergency_interface_descr(info_.name, emergency_interface_info);
156+
emergency_stop_ = std::make_shared<StateInterface>(emergency_interface_descr);
157+
158+
// ERROR
159+
// create error signal interface
160+
InterfaceInfo error_interface_info;
161+
error_interface_info.name = hardware_interface::ERROR_SIGNAL_INTERFACE_NAME;
162+
error_interface_info.data_type = "std::array<uint8_t>";
163+
InterfaceDescription error_interface_descr(info_.name, error_interface_info);
164+
error_signal_ = std::make_shared<StateInterface>(error_interface_descr);
165+
// create error signal report message interface
166+
InterfaceInfo error_msg_interface_info;
167+
error_msg_interface_info.name = hardware_interface::ERROR_SIGNAL_MESSAGE_INTERFACE_NAME;
168+
error_msg_interface_info.data_type = "std::array<std::string>";
169+
InterfaceDescription error_msg_interface_descr(info_.name, error_msg_interface_info);
170+
error_signal_message_ = std::make_shared<StateInterface>(error_msg_interface_descr);
171+
172+
// WARNING
173+
// create warning signal interface
174+
InterfaceInfo warning_interface_info;
175+
warning_interface_info.name = hardware_interface::WARNING_SIGNAL_INTERFACE_NAME;
176+
warning_interface_info.data_type = "std::array<uint8_t>";
177+
InterfaceDescription warning_interface_descr(info_.name, warning_interface_info);
178+
warning_signal_ = std::make_shared<StateInterface>(warning_interface_descr);
179+
// create warning signal report message interface
180+
InterfaceInfo warning_msg_interface_info;
181+
warning_msg_interface_info.name = hardware_interface::WARNING_SIGNAL_MESSAGE_INTERFACE_NAME;
182+
warning_msg_interface_info.data_type = "std::array<std::string>";
183+
InterfaceDescription warning_msg_interface_descr(info_.name, warning_msg_interface_info);
184+
warning_signal_message_ = std::make_shared<StateInterface>(warning_msg_interface_descr);
185+
}
186+
137187
/// Exports all state interfaces for this hardware interface.
138188
/**
139189
* Old way of exporting the StateInterfaces. If a empty vector is returned then
@@ -207,6 +257,14 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod
207257
actuator_states_.insert(std::make_pair(name, state_interface));
208258
state_interfaces.push_back(state_interface);
209259
}
260+
261+
// export warning signal interfaces
262+
state_interfaces.push_back(emergency_stop_);
263+
state_interfaces.push_back(error_signal_);
264+
state_interfaces.push_back(error_signal_message_);
265+
state_interfaces.push_back(warning_signal_);
266+
state_interfaces.push_back(warning_signal_message_);
267+
210268
return state_interfaces;
211269
}
212270

@@ -386,6 +444,35 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod
386444
return actuator_commands_.at(interface_name)->get_value();
387445
}
388446

447+
void set_emergency_stop(const double & emergency_stop)
448+
{
449+
emergency_stop_->set_value(emergency_stop);
450+
}
451+
452+
double get_emergency_stop() const { return emergency_stop_->get_value(); }
453+
454+
void set_error_code(const double & error_code) { error_signal_->set_value(error_code); }
455+
456+
double get_error_code() const { return error_signal_->get_value(); }
457+
458+
void set_error_message(const double & error_message)
459+
{
460+
error_signal_message_->set_value(error_message);
461+
}
462+
463+
double get_error_message() const { return error_signal_message_->get_value(); }
464+
465+
void set_warning_code(const double & warning_codes) { warning_signal_->set_value(warning_codes); }
466+
467+
double get_warning_code() const { return warning_signal_->get_value(); }
468+
469+
void set_warning_message(const double & error_message)
470+
{
471+
warning_signal_message_->set_value(error_message);
472+
}
473+
474+
double get_warning_message() const { return warning_signal_message_->get_value(); }
475+
389476
protected:
390477
HardwareInfo info_;
391478
std::unordered_map<std::string, InterfaceDescription> joint_state_interfaces_;
@@ -394,6 +481,13 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod
394481
std::unordered_map<std::string, InterfaceDescription> unlisted_state_interfaces_;
395482
std::unordered_map<std::string, InterfaceDescription> unlisted_command_interfaces_;
396483

484+
private:
485+
std::shared_ptr<StateInterface> emergency_stop_;
486+
std::shared_ptr<StateInterface> error_signal_;
487+
std::shared_ptr<StateInterface> error_signal_message_;
488+
std::shared_ptr<StateInterface> warning_signal_;
489+
std::shared_ptr<StateInterface> warning_signal_message_;
490+
397491
rclcpp_lifecycle::State lifecycle_state_;
398492

399493
private:

‎hardware_interface/include/hardware_interface/handle.hpp‎

Lines changed: 3 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -15,6 +15,7 @@
1515
#ifndef HARDWARE_INTERFACE__HANDLE_HPP_
1616
#define HARDWARE_INTERFACE__HANDLE_HPP_
1717

18+
#include <array>
1819
#include <limits>
1920
#include <memory>
2021
#include <string>
@@ -23,8 +24,9 @@
2324

2425
#include "hardware_interface/hardware_info.hpp"
2526
#include "hardware_interface/macros.hpp"
27+
#include "hardware_interface/types/hardware_interface_error_signals.hpp"
28+
#include "hardware_interface/types/hardware_interface_warning_signals.hpp"
2629
#include "hardware_interface/visibility_control.h"
27-
2830
namespace hardware_interface
2931
{
3032

‎hardware_interface/include/hardware_interface/sensor_interface.hpp‎

Lines changed: 76 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -25,7 +25,9 @@
2525
#include "hardware_interface/component_parser.hpp"
2626
#include "hardware_interface/handle.hpp"
2727
#include "hardware_interface/hardware_info.hpp"
28+
#include "hardware_interface/types/hardware_interface_error_signals.hpp"
2829
#include "hardware_interface/types/hardware_interface_return_values.hpp"
30+
#include "hardware_interface/types/hardware_interface_warning_signals.hpp"
2931
#include "hardware_interface/types/lifecycle_state_names.hpp"
3032
#include "lifecycle_msgs/msg/state.hpp"
3133
#include "rclcpp/duration.hpp"
@@ -102,6 +104,7 @@ class SensorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI
102104
{
103105
info_ = hardware_info;
104106
import_state_interface_descriptions(info_);
107+
create_report_interfaces();
105108
return CallbackReturn::SUCCESS;
106109
};
107110

@@ -119,6 +122,45 @@ class SensorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI
119122
}
120123
}
121124

125+
/**
126+
* Creates all interfaces used for reporting warning and error messages.
127+
* The available report interfaces are: ERROR_SIGNAL, ERROR_SIGNAL_MESSAGE,
128+
* WARNING_SIGNAL and WARNING_SIGNAL_MESSAGE. Where the <report_type>_MESSAGE hold the message for
129+
* the corresponding report signal.
130+
* The interfaces are named like <hardware_name>/<report_interface_type>. E.g. if hardware is
131+
* called sensor_1 -> interface for WARNING_SIGNAL is called: sensor_1/WARNING_SIGNAL
132+
*/
133+
void create_report_interfaces()
134+
{
135+
// ERROR
136+
// create error signal interface
137+
InterfaceInfo error_interface_info;
138+
error_interface_info.name = hardware_interface::ERROR_SIGNAL_INTERFACE_NAME;
139+
error_interface_info.data_type = "std::array<uint8_t>";
140+
InterfaceDescription error_interface_descr(info_.name, error_interface_info);
141+
error_signal_ = std::make_shared<StateInterface>(error_interface_descr);
142+
// create error signal report message interface
143+
InterfaceInfo error_msg_interface_info;
144+
error_msg_interface_info.name = hardware_interface::ERROR_SIGNAL_MESSAGE_INTERFACE_NAME;
145+
error_msg_interface_info.data_type = "std::array<std::string>";
146+
InterfaceDescription error_msg_interface_descr(info_.name, error_msg_interface_info);
147+
error_signal_message_ = std::make_shared<StateInterface>(error_msg_interface_descr);
148+
149+
// WARNING
150+
// create warning signal interface
151+
InterfaceInfo warning_interface_info;
152+
warning_interface_info.name = hardware_interface::WARNING_SIGNAL_INTERFACE_NAME;
153+
warning_interface_info.data_type = "std::array<uint8_t>";
154+
InterfaceDescription warning_interface_descr(info_.name, warning_interface_info);
155+
warning_signal_ = std::make_shared<StateInterface>(warning_interface_descr);
156+
// create warning signal report message interface
157+
InterfaceInfo warning_msg_interface_info;
158+
warning_msg_interface_info.name = hardware_interface::WARNING_SIGNAL_MESSAGE_INTERFACE_NAME;
159+
warning_msg_interface_info.data_type = "std::array<std::string>";
160+
InterfaceDescription warning_msg_interface_descr(info_.name, warning_msg_interface_info);
161+
warning_signal_message_ = std::make_shared<StateInterface>(warning_msg_interface_descr);
162+
}
163+
122164
/// Exports all state interfaces for this hardware interface.
123165
/**
124166
* Old way of exporting the StateInterfaces. If a empty vector is returned then
@@ -194,6 +236,12 @@ class SensorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI
194236
state_interfaces.push_back(state_interface);
195237
}
196238

239+
// export warning signal interfaces
240+
state_interfaces.push_back(error_signal_);
241+
state_interfaces.push_back(error_signal_message_);
242+
state_interfaces.push_back(warning_signal_);
243+
state_interfaces.push_back(warning_signal_message_);
244+
197245
return state_interfaces;
198246
}
199247

@@ -237,12 +285,40 @@ class SensorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI
237285
return sensor_states_.at(interface_name)->get_value();
238286
}
239287

288+
void set_error_code(const double & error_code) { error_signal_->set_value(error_code); }
289+
290+
double get_error_code() const { return error_signal_->get_value(); }
291+
292+
void set_error_message(const double & error_message)
293+
{
294+
error_signal_message_->set_value(error_message);
295+
}
296+
297+
double get_error_message() const { return error_signal_message_->get_value(); }
298+
299+
void set_warning_code(const double & warning_codes) { warning_signal_->set_value(warning_codes); }
300+
301+
double get_warning_code() const { return warning_signal_->get_value(); }
302+
303+
void set_warning_message(const double & error_message)
304+
{
305+
warning_signal_message_->set_value(error_message);
306+
}
307+
308+
double get_warning_message() const { return warning_signal_message_->get_value(); }
309+
240310
protected:
241311
HardwareInfo info_;
242312

243313
std::unordered_map<std::string, InterfaceDescription> sensor_state_interfaces_;
244314
std::unordered_map<std::string, InterfaceDescription> unlisted_state_interfaces_;
245315

316+
private:
317+
std::shared_ptr<StateInterface> error_signal_;
318+
std::shared_ptr<StateInterface> error_signal_message_;
319+
std::shared_ptr<StateInterface> warning_signal_;
320+
std::shared_ptr<StateInterface> warning_signal_message_;
321+
246322
rclcpp_lifecycle::State lifecycle_state_;
247323

248324
private:

0 commit comments

Comments
 (0)