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>

Author: kyoichi-sugahara

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

@@ -1884,6 +1884,10 @@ DrivableAreaInfo combineDrivableAreaInfo(
     drivable_area_info1.enable_expanding_intersection_areas ||
     drivable_area_info2.enable_expanding_intersection_areas;
 
+  // drivable margin
+  combined_drivable_area_info.drivable_margin =
+    std::max(drivable_area_info1.drivable_margin, drivable_area_info2.drivable_margin);
+
   return combined_drivable_area_info;
 }
 

planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_module.hpp

@@ -139,7 +139,7 @@ class StartPlannerModule : public SceneModuleInterface
 
   bool canTransitFailureState() override { return false; }
 
-  bool canTransitIdleToRunningState() override;
+  bool canTransitIdleToRunningState() override { return true; }
 
   /**
    * @brief init member variables.

planning/behavior_path_start_planner_module/src/start_planner_module.cpp

@@ -347,11 +347,6 @@ bool StartPlannerModule::canTransitSuccessState()
   return hasFinishedPullOut();
 }
 
-bool StartPlannerModule::canTransitIdleToRunningState()
-{
-  return isActivated() && !isWaitingApproval();
-}
-
 BehaviorModuleOutput StartPlannerModule::plan()
 {
   if (isWaitingApproval()) {
@@ -1317,6 +1312,7 @@ bool StartPlannerModule::planFreespacePath()
 void StartPlannerModule::setDrivableAreaInfo(BehaviorModuleOutput & output) const
 {
   if (status_.planner_type == PlannerType::FREESPACE) {
+    std::cerr << "Freespace planner updated drivable area." << std::endl;
     const double drivable_area_margin = planner_data_->parameters.vehicle_width;
     output.drivable_area_info.drivable_margin =
       planner_data_->parameters.vehicle_width / 2.0 + drivable_area_margin;