|
26 | 26 | #include <rclcpp/node.hpp>
|
27 | 27 | #include <rclcpp/time.hpp>
|
28 | 28 |
|
29 |
| -#include <limits> |
30 | 29 | #include <memory>
|
31 | 30 | #include <string>
|
32 | 31 | #include <unordered_map>
|
@@ -139,15 +138,9 @@ class AvoidanceModule : public SceneModuleInterface
|
139 | 138 | void removeCandidateRTCStatus()
|
140 | 139 | {
|
141 | 140 | if (rtc_interface_ptr_map_.at("left")->isRegistered(candidate_uuid_)) {
|
142 |
| - rtc_interface_ptr_map_.at("left")->updateCooperateStatus( |
143 |
| - candidate_uuid_, true, State::FAILED, std::numeric_limits<double>::lowest(), |
144 |
| - std::numeric_limits<double>::lowest(), clock_->now()); |
145 |
| - } |
146 |
| - |
147 |
| - if (rtc_interface_ptr_map_.at("right")->isRegistered(candidate_uuid_)) { |
148 |
| - rtc_interface_ptr_map_.at("right")->updateCooperateStatus( |
149 |
| - candidate_uuid_, true, State::FAILED, std::numeric_limits<double>::lowest(), |
150 |
| - std::numeric_limits<double>::lowest(), clock_->now()); |
| 141 | + rtc_interface_ptr_map_.at("left")->removeCooperateStatus(candidate_uuid_); |
| 142 | + } else if (rtc_interface_ptr_map_.at("right")->isRegistered(candidate_uuid_)) { |
| 143 | + rtc_interface_ptr_map_.at("right")->removeCooperateStatus(candidate_uuid_); |
151 | 144 | }
|
152 | 145 | }
|
153 | 146 |
|
@@ -369,21 +362,10 @@ class AvoidanceModule : public SceneModuleInterface
|
369 | 362 |
|
370 | 363 | unlockNewModuleLaunch();
|
371 | 364 |
|
372 |
| - for (const auto & left_shift : left_shift_array_) { |
373 |
| - rtc_interface_ptr_map_.at("left")->updateCooperateStatus( |
374 |
| - left_shift.uuid, true, State::FAILED, std::numeric_limits<double>::lowest(), |
375 |
| - std::numeric_limits<double>::lowest(), clock_->now()); |
376 |
| - } |
377 |
| - |
378 |
| - for (const auto & right_shift : right_shift_array_) { |
379 |
| - rtc_interface_ptr_map_.at("right")->updateCooperateStatus( |
380 |
| - right_shift.uuid, true, State::FAILED, std::numeric_limits<double>::lowest(), |
381 |
| - std::numeric_limits<double>::lowest(), clock_->now()); |
382 |
| - } |
383 |
| - |
384 | 365 | if (!path_shifter_.getShiftLines().empty()) {
|
385 | 366 | left_shift_array_.clear();
|
386 | 367 | right_shift_array_.clear();
|
| 368 | + removeRTCStatus(); |
387 | 369 | }
|
388 | 370 |
|
389 | 371 | generator_.reset();
|
|
0 commit comments