Skip to content

Commit 0dcab13

Browse files
committed
fix(bpp): don't remove rtc cooperate status
Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>
1 parent b433f7d commit 0dcab13

File tree

8 files changed

+14
-35
lines changed

8 files changed

+14
-35
lines changed

planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/scene.hpp

-1
Original file line numberDiff line numberDiff line change
@@ -376,7 +376,6 @@ class AvoidanceModule : public SceneModuleInterface
376376
if (!path_shifter_.getShiftLines().empty()) {
377377
left_shift_array_.clear();
378378
right_shift_array_.clear();
379-
removeRTCStatus();
380379
}
381380

382381
generator_.reset();

planning/behavior_path_avoidance_module/src/scene.cpp

-1
Original file line numberDiff line numberDiff line change
@@ -1296,7 +1296,6 @@ void AvoidanceModule::updateData()
12961296
void AvoidanceModule::processOnEntry()
12971297
{
12981298
initVariables();
1299-
removeRTCStatus();
13001299
}
13011300

13021301
void AvoidanceModule::processOnExit()

planning/behavior_path_lane_change_module/src/interface.cpp

-2
Original file line numberDiff line numberDiff line change
@@ -117,7 +117,6 @@ BehaviorModuleOutput LaneChangeInterface::plan()
117117
updateSteeringFactorPtr(output);
118118
if (module_type_->isAbortState()) {
119119
waitApproval();
120-
removeRTCStatus();
121120
const auto candidate = planCandidate();
122121
path_candidate_ = std::make_shared<PathWithLaneId>(candidate.path_candidate);
123122
updateRTCStatus(
@@ -148,7 +147,6 @@ BehaviorModuleOutput LaneChangeInterface::planWaitingApproval()
148147
stop_pose_ = module_type_->getStopPose();
149148

150149
if (!module_type_->isValidPath()) {
151-
removeRTCStatus();
152150
path_candidate_ = std::make_shared<PathWithLaneId>();
153151
return out;
154152
}

planning/behavior_path_planner/include/behavior_path_planner/planner_manager.hpp

-3
Original file line numberDiff line numberDiff line change
@@ -290,8 +290,6 @@ class PlannerManager
290290

291291
module_ptr->updateCurrentState();
292292

293-
module_ptr->publishRTCStatus();
294-
295293
module_ptr->publishSteeringFactor();
296294

297295
module_ptr->publishObjectsOfInterestMarker();
@@ -323,7 +321,6 @@ class PlannerManager
323321
void deleteExpiredModules(SceneModulePtr & module_ptr) const
324322
{
325323
module_ptr->onExit();
326-
module_ptr->publishRTCStatus();
327324
module_ptr->publishObjectsOfInterestMarker();
328325
module_ptr.reset();
329326
}

planning/behavior_path_planner/src/planner_manager.cpp

+4-2
Original file line numberDiff line numberDiff line change
@@ -191,8 +191,10 @@ BehaviorModuleOutput PlannerManager::run(const std::shared_ptr<PlannerData> & da
191191
return BehaviorModuleOutput{}; // something wrong.
192192
}();
193193

194-
std::for_each(
195-
manager_ptrs_.begin(), manager_ptrs_.end(), [](const auto & m) { m->updateObserver(); });
194+
std::for_each(manager_ptrs_.begin(), manager_ptrs_.end(), [](const auto & m) {
195+
m->updateObserver();
196+
m->publishRTCStatus();
197+
});
196198

197199
generateCombinedDrivableArea(result_output, data);
198200

planning/behavior_path_planner_common/include/behavior_path_planner_common/interface/scene_module_interface.hpp

-23
Original file line numberDiff line numberDiff line change
@@ -187,8 +187,6 @@ class SceneModuleInterface
187187
RCLCPP_DEBUG(getLogger(), "%s %s", name_.c_str(), __func__);
188188

189189
clearWaitingApproval();
190-
removeRTCStatus();
191-
publishRTCStatus();
192190
unlockNewModuleLaunch();
193191
unlockOutputPath();
194192
steering_factor_interface_ptr_->clearSteeringFactors();
@@ -198,18 +196,6 @@ class SceneModuleInterface
198196
processOnExit();
199197
}
200198

201-
/**
202-
* @brief Publish status if the module is requested to run
203-
*/
204-
void publishRTCStatus()
205-
{
206-
for (const auto & [module_name, ptr] : rtc_interface_ptr_map_) {
207-
if (ptr) {
208-
ptr->publishCooperateStatus(clock_->now());
209-
}
210-
}
211-
}
212-
213199
void publishObjectsOfInterestMarker()
214200
{
215201
for (const auto & [module_name, ptr] : objects_of_interest_marker_interface_ptr_map_) {
@@ -540,15 +526,6 @@ class SceneModuleInterface
540526
return existApprovedRequest();
541527
}
542528

543-
void removeRTCStatus()
544-
{
545-
for (const auto & [module_name, ptr] : rtc_interface_ptr_map_) {
546-
if (ptr) {
547-
ptr->clearCooperateStatus();
548-
}
549-
}
550-
}
551-
552529
void setStopReason(const std::string & stop_reason, const PathWithLaneId & path)
553530
{
554531
stop_reason_.reason = stop_reason;

planning/behavior_path_planner_common/include/behavior_path_planner_common/interface/scene_module_manager_interface.hpp

+10-2
Original file line numberDiff line numberDiff line change
@@ -99,6 +99,16 @@ class SceneModuleManagerInterface
9999
observers_.erase(itr, observers_.end());
100100
}
101101

102+
void publishRTCStatus()
103+
{
104+
for (const auto & [module_name, ptr] : rtc_interface_ptr_map_) {
105+
if (ptr) {
106+
ptr->removeExpiredCooperateStatus();
107+
ptr->publishCooperateStatus(rclcpp::Clock(RCL_ROS_TIME).now());
108+
}
109+
}
110+
}
111+
102112
void publishVirtualWall() const
103113
{
104114
using tier4_autoware_utils::appendMarkerArray;
@@ -235,15 +245,13 @@ class SceneModuleManagerInterface
235245
std::for_each(observers_.begin(), observers_.end(), [](const auto & observer) {
236246
if (!observer.expired()) {
237247
observer.lock()->onExit();
238-
observer.lock()->publishRTCStatus();
239248
}
240249
});
241250

242251
observers_.clear();
243252

244253
if (idle_module_ptr_ != nullptr) {
245254
idle_module_ptr_->onExit();
246-
idle_module_ptr_->publishRTCStatus();
247255
idle_module_ptr_.reset();
248256
}
249257

planning/behavior_path_start_planner_module/src/start_planner_module.cpp

-1
Original file line numberDiff line numberDiff line change
@@ -1051,7 +1051,6 @@ void StartPlannerModule::updateStatusAfterBackwardDriving()
10511051
// request start_planner approval
10521052
waitApproval();
10531053
// To enable approval of the forward path, the RTC status is removed.
1054-
removeRTCStatus();
10551054
for (auto itr = uuid_map_.begin(); itr != uuid_map_.end(); ++itr) {
10561055
itr->second = generateUUID();
10571056
}

0 commit comments

Comments
 (0)