Skip to content

Commit 884c472

Browse files
mamueluthmamueluth
committed
add interface for warning, error and report
1 parent b5f9b4d commit 884c472

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
@@ -80,6 +80,10 @@ if(BUILD_TESTING)
8080
target_link_libraries(test_component_interfaces_custom_export hardware_interface)
8181
ament_target_dependencies(test_component_interfaces_custom_export ros2_control_test_assets)
8282

83+
ament_add_gmock(test_error_warning_codes test/test_error_warning_codes.cpp)
84+
target_link_libraries(test_error_warning_codes hardware_interface)
85+
ament_target_dependencies(test_error_warning_codes ros2_control_test_assets)
86+
8387
ament_add_gmock(test_component_parser test/test_component_parser.cpp)
8488
target_link_libraries(test_component_parser hardware_interface)
8589
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
@@ -190,6 +240,14 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod
190240
actuator_states_.insert(std::make_pair(name, state_interface));
191241
state_interfaces.push_back(state_interface);
192242
}
243+
244+
// export warning signal interfaces
245+
state_interfaces.push_back(emergency_stop_);
246+
state_interfaces.push_back(error_signal_);
247+
state_interfaces.push_back(error_signal_message_);
248+
state_interfaces.push_back(warning_signal_);
249+
state_interfaces.push_back(warning_signal_message_);
250+
193251
return state_interfaces;
194252
}
195253

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

414+
void set_emergency_stop(const double & emergency_stop)
415+
{
416+
emergency_stop_->set_value(emergency_stop);
417+
}
418+
419+
double get_emergency_stop() const { return emergency_stop_->get_value(); }
420+
421+
void set_error_code(const double & error_code) { error_signal_->set_value(error_code); }
422+
423+
double get_error_code() const { return error_signal_->get_value(); }
424+
425+
void set_error_message(const double & error_message)
426+
{
427+
error_signal_message_->set_value(error_message);
428+
}
429+
430+
double get_error_message() const { return error_signal_message_->get_value(); }
431+
432+
void set_warning_code(const double & warning_codes) { warning_signal_->set_value(warning_codes); }
433+
434+
double get_warning_code() const { return warning_signal_->get_value(); }
435+
436+
void set_warning_message(const double & error_message)
437+
{
438+
warning_signal_message_->set_value(error_message);
439+
}
440+
441+
double get_warning_message() const { return warning_signal_message_->get_value(); }
442+
356443
protected:
357444
HardwareInfo info_;
358445
std::unordered_map<std::string, InterfaceDescription> joint_state_interfaces_;
@@ -361,6 +448,13 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod
361448
std::unordered_map<std::string, std::shared_ptr<StateInterface>> actuator_states_;
362449
std::unordered_map<std::string, std::shared_ptr<CommandInterface>> actuator_commands_;
363450

451+
private:
452+
std::shared_ptr<StateInterface> emergency_stop_;
453+
std::shared_ptr<StateInterface> error_signal_;
454+
std::shared_ptr<StateInterface> error_signal_message_;
455+
std::shared_ptr<StateInterface> warning_signal_;
456+
std::shared_ptr<StateInterface> warning_signal_message_;
457+
364458
rclcpp_lifecycle::State lifecycle_state_;
365459
};
366460

‎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
@@ -177,6 +219,12 @@ class SensorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI
177219
state_interfaces.push_back(state_interface);
178220
}
179221

222+
// export warning signal interfaces
223+
state_interfaces.push_back(error_signal_);
224+
state_interfaces.push_back(error_signal_message_);
225+
state_interfaces.push_back(warning_signal_);
226+
state_interfaces.push_back(warning_signal_message_);
227+
180228
return state_interfaces;
181229
}
182230

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

271+
void set_error_code(const double & error_code) { error_signal_->set_value(error_code); }
272+
273+
double get_error_code() const { return error_signal_->get_value(); }
274+
275+
void set_error_message(const double & error_message)
276+
{
277+
error_signal_message_->set_value(error_message);
278+
}
279+
280+
double get_error_message() const { return error_signal_message_->get_value(); }
281+
282+
void set_warning_code(const double & warning_codes) { warning_signal_->set_value(warning_codes); }
283+
284+
double get_warning_code() const { return warning_signal_->get_value(); }
285+
286+
void set_warning_message(const double & error_message)
287+
{
288+
warning_signal_message_->set_value(error_message);
289+
}
290+
291+
double get_warning_message() const { return warning_signal_message_->get_value(); }
292+
223293
protected:
224294
HardwareInfo info_;
225295

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

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

300+
private:
301+
std::shared_ptr<StateInterface> error_signal_;
302+
std::shared_ptr<StateInterface> error_signal_message_;
303+
std::shared_ptr<StateInterface> warning_signal_;
304+
std::shared_ptr<StateInterface> warning_signal_message_;
305+
230306
rclcpp_lifecycle::State lifecycle_state_;
231307
};
232308

0 commit comments

Comments
 (0)