Skip to content

Commit 57cf88d

Browse files
fix(start_planner): update drivable area info and enable idle to running state transition (#6172)
Update drivable area info and enable idle to running state transition Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>
1 parent ccd852b commit 57cf88d

File tree

3 files changed

+6
-6
lines changed

3 files changed

+6
-6
lines changed

planning/behavior_path_planner_common/src/utils/drivable_area_expansion/static_drivable_area.cpp

+4
Original file line numberDiff line numberDiff line change
@@ -1884,6 +1884,10 @@ DrivableAreaInfo combineDrivableAreaInfo(
18841884
drivable_area_info1.enable_expanding_intersection_areas ||
18851885
drivable_area_info2.enable_expanding_intersection_areas;
18861886

1887+
// drivable margin
1888+
combined_drivable_area_info.drivable_margin =
1889+
std::max(drivable_area_info1.drivable_margin, drivable_area_info2.drivable_margin);
1890+
18871891
return combined_drivable_area_info;
18881892
}
18891893

planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_module.hpp

+1-1
Original file line numberDiff line numberDiff line change
@@ -139,7 +139,7 @@ class StartPlannerModule : public SceneModuleInterface
139139

140140
bool canTransitFailureState() override { return false; }
141141

142-
bool canTransitIdleToRunningState() override;
142+
bool canTransitIdleToRunningState() override { return true; }
143143

144144
/**
145145
* @brief init member variables.

planning/behavior_path_start_planner_module/src/start_planner_module.cpp

+1-5
Original file line numberDiff line numberDiff line change
@@ -347,11 +347,6 @@ bool StartPlannerModule::canTransitSuccessState()
347347
return hasFinishedPullOut();
348348
}
349349

350-
bool StartPlannerModule::canTransitIdleToRunningState()
351-
{
352-
return isActivated() && !isWaitingApproval();
353-
}
354-
355350
BehaviorModuleOutput StartPlannerModule::plan()
356351
{
357352
if (isWaitingApproval()) {
@@ -1317,6 +1312,7 @@ bool StartPlannerModule::planFreespacePath()
13171312
void StartPlannerModule::setDrivableAreaInfo(BehaviorModuleOutput & output) const
13181313
{
13191314
if (status_.planner_type == PlannerType::FREESPACE) {
1315+
std::cerr << "Freespace planner updated drivable area." << std::endl;
13201316
const double drivable_area_margin = planner_data_->parameters.vehicle_width;
13211317
output.drivable_area_info.drivable_margin =
13221318
planner_data_->parameters.vehicle_width / 2.0 + drivable_area_margin;

0 commit comments

Comments
 (0)