blob: c1abded9234de0947ccadba8c90910f18c1d00e9 (
plain)
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
|
--- a/picknik_reset_fault_controller/src/picknik_reset_fault_controller.cpp
+++ b/picknik_reset_fault_controller/src/picknik_reset_fault_controller.cpp
@@ -64,12 +64,8 @@
{
if (realtime_publisher_)
{
- auto state_op = state_interfaces_[StateInterfaces::IN_FAULT].get_optional();
- if (state_op.has_value())
- {
- state_.data = static_cast<bool>(state_op.value());
- }
- realtime_publisher_->try_publish(state_);
+ state_.data = static_cast<bool>(state_interfaces_[StateInterfaces::IN_FAULT].get_value());
+ realtime_publisher_->tryPublish(state_);
}
return controller_interface::return_type::OK;
@@ -124,23 +120,15 @@
RCLCPP_INFO(get_node()->get_logger(), "Trying to reset faults on hardware controller.");
- while (command_interfaces_[CommandInterfaces::RESET_FAULT_ASYNC_SUCCESS].get_optional().value_or(
- ASYNC_WAITING) == ASYNC_WAITING)
+ while (command_interfaces_[CommandInterfaces::RESET_FAULT_ASYNC_SUCCESS].get_value() ==
+ ASYNC_WAITING)
{
// Asynchronous wait until the hardware interface has set the io value
std::this_thread::sleep_for(std::chrono::milliseconds(50));
}
- auto async_result_op =
- command_interfaces_[CommandInterfaces::RESET_FAULT_ASYNC_SUCCESS].get_optional();
- if (!async_result_op.has_value())
- {
- RCLCPP_ERROR(
- get_node()->get_logger(),
- "Failed to get result of fault reset command from hardware controller.");
- resp->success = false;
- return resp->success;
- }
- resp->success = static_cast<bool>(async_result_op.value());
+ double async_result =
+ command_interfaces_[CommandInterfaces::RESET_FAULT_ASYNC_SUCCESS].get_value();
+ resp->success = static_cast<bool>(async_result);
RCLCPP_INFO(
get_node()->get_logger(), "Resetting fault on hardware controller '%s'!",
|
