Skip to content

Commit a7996c3

Browse files
mamueluthmamueluth
committed
add interface for warning, error and report
1 parent 720e11f commit a7996c3

11 files changed

+1523
-19
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
@@ -26,7 +26,10 @@
2626
#include "hardware_interface/component_parser.hpp"
2727
#include "hardware_interface/handle.hpp"
2828
#include "hardware_interface/hardware_info.hpp"
29+
#include "hardware_interface/types/hardware_interface_emergency_stop_signal.hpp"
30+
#include "hardware_interface/types/hardware_interface_error_signals.hpp"
2931
#include "hardware_interface/types/hardware_interface_return_values.hpp"
32+
#include "hardware_interface/types/hardware_interface_warning_signals.hpp"
3033
#include "hardware_interface/types/lifecycle_state_names.hpp"
3134
#include "lifecycle_msgs/msg/state.hpp"
3235
#include "rclcpp/duration.hpp"
@@ -104,6 +107,7 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod
104107
info_ = hardware_info;
105108
import_state_interface_descriptions(info_);
106109
import_command_interface_descriptions(info_);
110+
create_report_interfaces();
107111
return CallbackReturn::SUCCESS;
108112
};
109113

@@ -135,6 +139,52 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod
135139
}
136140
}
137141

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

@@ -325,6 +383,35 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod
325383
return actuator_commands_.at(interface_name)->get_value();
326384
}
327385

386+
void set_emergency_stop(const double & emergency_stop)
387+
{
388+
emergency_stop_->set_value(emergency_stop);
389+
}
390+
391+
double get_emergency_stop() const { return emergency_stop_->get_value(); }
392+
393+
void set_error_code(const double & error_code) { error_signal_->set_value(error_code); }
394+
395+
double get_error_code() const { return error_signal_->get_value(); }
396+
397+
void set_error_message(const double & error_message)
398+
{
399+
error_signal_message_->set_value(error_message);
400+
}
401+
402+
double get_error_message() const { return error_signal_message_->get_value(); }
403+
404+
void set_warning_code(const double & warning_codes) { warning_signal_->set_value(warning_codes); }
405+
406+
double get_warning_code() const { return warning_signal_->get_value(); }
407+
408+
void set_warning_message(const double & error_message)
409+
{
410+
warning_signal_message_->set_value(error_message);
411+
}
412+
413+
double get_warning_message() const { return warning_signal_message_->get_value(); }
414+
328415
protected:
329416
HardwareInfo info_;
330417
std::map<std::string, InterfaceDescription> joint_state_interfaces_;
@@ -333,6 +420,13 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod
333420
std::unordered_map<std::string, StateInterfaceSharedPtr> actuator_states_;
334421
std::unordered_map<std::string, CommandInterfaceSharedPtr> actuator_commands_;
335422

423+
private:
424+
std::shared_ptr<StateInterface> emergency_stop_;
425+
std::shared_ptr<StateInterface> error_signal_;
426+
std::shared_ptr<StateInterface> error_signal_message_;
427+
std::shared_ptr<StateInterface> warning_signal_;
428+
std::shared_ptr<StateInterface> warning_signal_message_;
429+
336430
rclcpp_lifecycle::State lifecycle_state_;
337431
};
338432

‎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
@@ -26,7 +26,9 @@
2626
#include "hardware_interface/component_parser.hpp"
2727
#include "hardware_interface/handle.hpp"
2828
#include "hardware_interface/hardware_info.hpp"
29+
#include "hardware_interface/types/hardware_interface_error_signals.hpp"
2930
#include "hardware_interface/types/hardware_interface_return_values.hpp"
31+
#include "hardware_interface/types/hardware_interface_warning_signals.hpp"
3032
#include "hardware_interface/types/lifecycle_state_names.hpp"
3133
#include "lifecycle_msgs/msg/state.hpp"
3234
#include "rclcpp/duration.hpp"
@@ -103,6 +105,7 @@ class SensorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI
103105
{
104106
info_ = hardware_info;
105107
import_state_interface_descriptions(info_);
108+
create_report_interfaces();
106109
return CallbackReturn::SUCCESS;
107110
};
108111

@@ -120,6 +123,45 @@ class SensorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI
120123
}
121124
}
122125

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

209+
// export warning signal interfaces
210+
state_interfaces.push_back(error_signal_);
211+
state_interfaces.push_back(error_signal_message_);
212+
state_interfaces.push_back(warning_signal_);
213+
state_interfaces.push_back(warning_signal_message_);
214+
167215
return state_interfaces;
168216
}
169217

@@ -207,13 +255,41 @@ class SensorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI
207255
return sensor_states_.at(interface_name)->get_value();
208256
}
209257

258+
void set_error_code(const double & error_code) { error_signal_->set_value(error_code); }
259+
260+
double get_error_code() const { return error_signal_->get_value(); }
261+
262+
void set_error_message(const double & error_message)
263+
{
264+
error_signal_message_->set_value(error_message);
265+
}
266+
267+
double get_error_message() const { return error_signal_message_->get_value(); }
268+
269+
void set_warning_code(const double & warning_codes) { warning_signal_->set_value(warning_codes); }
270+
271+
double get_warning_code() const { return warning_signal_->get_value(); }
272+
273+
void set_warning_message(const double & error_message)
274+
{
275+
warning_signal_message_->set_value(error_message);
276+
}
277+
278+
double get_warning_message() const { return warning_signal_message_->get_value(); }
279+
210280
protected:
211281
HardwareInfo info_;
212282

213283
std::map<std::string, InterfaceDescription> sensor_state_interfaces_;
214284

215285
std::unordered_map<std::string, StateInterfaceSharedPtr> sensor_states_;
216286

287+
private:
288+
std::shared_ptr<StateInterface> error_signal_;
289+
std::shared_ptr<StateInterface> error_signal_message_;
290+
std::shared_ptr<StateInterface> warning_signal_;
291+
std::shared_ptr<StateInterface> warning_signal_message_;
292+
217293
rclcpp_lifecycle::State lifecycle_state_;
218294
};
219295

0 commit comments

Comments
 (0)