Skip to content

Commit 730f2e7

Browse files
mamueluthmamueluth
committed
add interface for warning, error and report
1 parent 923866c commit 730f2e7

10 files changed

+1494
-4
lines changed

‎hardware_interface/CMakeLists.txt

Lines changed: 4 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -75,6 +75,10 @@ if(BUILD_TESTING)
7575
target_link_libraries(test_component_interfaces hardware_interface)
7676
ament_target_dependencies(test_component_interfaces ros2_control_test_assets)
7777

78+
ament_add_gmock(test_error_warning_codes test/test_error_warning_codes.cpp)
79+
target_link_libraries(test_error_warning_codes hardware_interface)
80+
ament_target_dependencies(test_error_warning_codes ros2_control_test_assets)
81+
7882
ament_add_gmock(test_component_parser test/test_component_parser.cpp)
7983
target_link_libraries(test_component_parser hardware_interface)
8084
ament_target_dependencies(test_component_parser ros2_control_test_assets)

‎hardware_interface/include/hardware_interface/actuator_interface.hpp

Lines changed: 93 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
* Default implementation for exporting the StateInterfaces. The StateInterfaces are created
@@ -170,6 +220,14 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod
170220
actuator_states_.insert(std::make_pair(name, std::make_shared<StateInterface>(descr)));
171221
state_interfaces.push_back(actuator_states_.at(name));
172222
}
223+
224+
// export warning signal interfaces
225+
state_interfaces.push_back(emergency_stop_);
226+
state_interfaces.push_back(error_signal_);
227+
state_interfaces.push_back(error_signal_message_);
228+
state_interfaces.push_back(warning_signal_);
229+
state_interfaces.push_back(warning_signal_message_);
230+
173231
return state_interfaces;
174232
}
175233
/// Exports all command interfaces for this hardware interface.
@@ -310,13 +368,48 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod
310368
return actuator_commands_.at(interface_name)->get_value();
311369
}
312370

371+
void set_emergency_stop(const double & emergency_stop)
372+
{
373+
emergency_stop_->set_value(emergency_stop);
374+
}
375+
376+
double get_emergency_stop() const { return emergency_stop_->get_value(); }
377+
378+
void set_error_code(const double & error_code) { error_signal_->set_value(error_code); }
379+
380+
double get_error_code() const { return error_signal_->get_value(); }
381+
382+
void set_error_message(const double & error_message)
383+
{
384+
error_signal_message_->set_value(error_message);
385+
}
386+
387+
double get_error_message() const { return error_signal_message_->get_value(); }
388+
389+
void set_warning_code(const double & warning_codes) { warning_signal_->set_value(warning_codes); }
390+
391+
double get_warning_code() const { return warning_signal_->get_value(); }
392+
393+
void set_warning_message(const double & error_message)
394+
{
395+
warning_signal_message_->set_value(error_message);
396+
}
397+
398+
double get_warning_message() const { return warning_signal_message_->get_value(); }
399+
313400
protected:
314401
HardwareInfo info_;
315402
std::map<std::string, InterfaceDescription> joint_state_interfaces_;
316403
std::map<std::string, InterfaceDescription> joint_command_interfaces_;
317404

318405
private:
319406
std::map<std::string, std::shared_ptr<StateInterface>> actuator_states_;
407+
std::shared_ptr<StateInterface> emergency_stop_;
408+
std::shared_ptr<StateInterface> error_signal_;
409+
std::shared_ptr<StateInterface> error_signal_message_;
410+
std::shared_ptr<StateInterface> warning_signal_;
411+
std::shared_ptr<StateInterface> warning_signal_message_;
412+
320413
std::map<std::string, std::shared_ptr<CommandInterface>> actuator_commands_;
321414

322415
rclcpp_lifecycle::State lifecycle_state_;

‎hardware_interface/include/hardware_interface/handle.hpp

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

18+
#include <array>
1819
#include <limits>
1920
#include <string>
2021
#include <utility>
2122
#include <variant>
2223

2324
#include "hardware_interface/hardware_info.hpp"
2425
#include "hardware_interface/macros.hpp"
26+
#include "hardware_interface/types/hardware_interface_error_signals.hpp"
27+
#include "hardware_interface/types/hardware_interface_warning_signals.hpp"
2528
#include "hardware_interface/visibility_control.h"
26-
2729
namespace hardware_interface
2830
{
2931

‎hardware_interface/include/hardware_interface/sensor_interface.hpp

Lines changed: 74 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
* Default implementation for exporting the StateInterfaces. The StateInterfaces are created
@@ -157,6 +199,12 @@ class SensorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI
157199
state_interfaces.push_back(sensor_states_.at(name));
158200
}
159201

202+
// export warning signal interfaces
203+
state_interfaces.push_back(error_signal_);
204+
state_interfaces.push_back(error_signal_message_);
205+
state_interfaces.push_back(warning_signal_);
206+
state_interfaces.push_back(warning_signal_message_);
207+
160208
return state_interfaces;
161209
}
162210

@@ -200,13 +248,39 @@ class SensorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI
200248
return sensor_states_.at(interface_name)->get_value();
201249
}
202250

251+
void set_error_code(const double & error_code) { error_signal_->set_value(error_code); }
252+
253+
double get_error_code() const { return error_signal_->get_value(); }
254+
255+
void set_error_message(const double & error_message)
256+
{
257+
error_signal_message_->set_value(error_message);
258+
}
259+
260+
double get_error_message() const { return error_signal_message_->get_value(); }
261+
262+
void set_warning_code(const double & warning_codes) { warning_signal_->set_value(warning_codes); }
263+
264+
double get_warning_code() const { return warning_signal_->get_value(); }
265+
266+
void set_warning_message(const double & error_message)
267+
{
268+
warning_signal_message_->set_value(error_message);
269+
}
270+
271+
double get_warning_message() const { return warning_signal_message_->get_value(); }
272+
203273
protected:
204274
HardwareInfo info_;
205275

206276
std::map<std::string, InterfaceDescription> sensor_state_interfaces_;
207277

208278
private:
209279
std::map<std::string, std::shared_ptr<StateInterface>> sensor_states_;
280+
std::shared_ptr<StateInterface> error_signal_;
281+
std::shared_ptr<StateInterface> error_signal_message_;
282+
std::shared_ptr<StateInterface> warning_signal_;
283+
std::shared_ptr<StateInterface> warning_signal_message_;
210284

211285
rclcpp_lifecycle::State lifecycle_state_;
212286
};

0 commit comments

Comments
 (0)