Merge pull request #417 from tier4/hotfix/behavior-path

fix(behavior_path_planner): hotfix for behavior path planner
tier4 · May 10, 2023 · a4a173e · a4a173e
2 parents dc3633c + 45a4d43
commit a4a173e
Show file tree

Hide file tree

Showing 6 changed files with 38 additions and 3 deletions.
diff --git a/...havior_path_planner/include/behavior_path_planner/scene_module/lane_change/base_class.hpp b/...havior_path_planner/include/behavior_path_planner/scene_module/lane_change/base_class.hpp
@@ -60,6 +60,7 @@ class LaneChangeBase
     prev_module_reference_path_ = std::make_shared<PathWithLaneId>();
     prev_module_path_ = std::make_shared<PathWithLaneId>();
     prev_drivable_area_info_ = std::make_shared<DrivableAreaInfo>();
+    prev_turn_signal_info_ = std::make_shared<TurnSignalInfo>();
   }
 
   LaneChangeBase(const LaneChangeBase &) = delete;
@@ -107,6 +108,11 @@ class LaneChangeBase
     }
   }
 
+  virtual void setPreviousTurnSignalInfo(const TurnSignalInfo & prev_turn_signal_info)
+  {
+    *prev_turn_signal_info_ = prev_turn_signal_info;
+  }
+
   virtual void updateSpecialData() {}
 
   const LaneChangeStatus & getLaneChangeStatus() const { return status_; }
@@ -232,6 +238,7 @@ class LaneChangeBase
   std::shared_ptr<PathWithLaneId> prev_module_reference_path_{};
   std::shared_ptr<PathWithLaneId> prev_module_path_{};
   std::shared_ptr<DrivableAreaInfo> prev_drivable_area_info_{};
+  std::shared_ptr<TurnSignalInfo> prev_turn_signal_info_{};
 
   PathWithLaneId prev_approved_path_{};
 

diff --git a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp
@@ -2783,7 +2783,18 @@ BehaviorModuleOutput AvoidanceModule::plan()
   }
 
   BehaviorModuleOutput output;
-  output.turn_signal_info = calcTurnSignalInfo(avoidance_path);
+
+  // turn signal info
+  {
+    const auto original_signal = getPreviousModuleOutput().turn_signal_info;
+    const auto new_signal = calcTurnSignalInfo(avoidance_path);
+    const auto current_seg_idx = planner_data_->findEgoSegmentIndex(avoidance_path.path.points);
+    output.turn_signal_info = planner_data_->turn_signal_decider.use_prior_turn_signal(
+      avoidance_path.path, getEgoPose(), current_seg_idx, original_signal, new_signal,
+      planner_data_->parameters.ego_nearest_dist_threshold,
+      planner_data_->parameters.ego_nearest_yaw_threshold);
+  }
+
   // sparse resampling for computational cost
   {
     avoidance_path.path =

diff --git a/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp b/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp
@@ -707,7 +707,13 @@ BehaviorModuleOutput GoalPlannerModule::planWithGoalModification()
 
   // set hazard and turn signal
   if (status_.has_decided_path) {
-    output.turn_signal_info = calcTurnSignalInfo();
+    const auto original_signal = getPreviousModuleOutput().turn_signal_info;
+    const auto new_signal = calcTurnSignalInfo();
+    const auto current_seg_idx = planner_data_->findEgoSegmentIndex(output.path->points);
+    output.turn_signal_info = planner_data_->turn_signal_decider.use_prior_turn_signal(
+      *output.path, getEgoPose(), current_seg_idx, original_signal, new_signal,
+      planner_data_->parameters.ego_nearest_dist_threshold,
+      planner_data_->parameters.ego_nearest_yaw_threshold);
   }
 
   const auto distance_to_path_change = calcDistanceToPathChange();

diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/interface.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/interface.cpp
@@ -131,6 +131,7 @@ BehaviorModuleOutput LaneChangeInterface::plan()
   }
 
   module_type_->setPreviousDrivableAreaInfo(getPreviousModuleOutput().drivable_area_info);
+  module_type_->setPreviousTurnSignalInfo(getPreviousModuleOutput().turn_signal_info);
   auto output = module_type_->generateOutput();
   path_reference_ = output.reference_path;
   *prev_approved_path_ = *getPreviousModuleOutput().path;

diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp
@@ -105,6 +105,16 @@ BehaviorModuleOutput NormalLaneChange::generateOutput()
   output.reference_path = std::make_shared<PathWithLaneId>(getReferencePath());
   output.turn_signal_info = updateOutputTurnSignal();
 
+  if (!prev_turn_signal_info_) {
+    return output;
+  }
+
+  const auto current_seg_idx = planner_data_->findEgoSegmentIndex(output.path->points);
+  output.turn_signal_info = planner_data_->turn_signal_decider.use_prior_turn_signal(
+    *output.path, getEgoPose(), current_seg_idx, *prev_turn_signal_info_, output.turn_signal_info,
+    planner_data_->parameters.ego_nearest_dist_threshold,
+    planner_data_->parameters.ego_nearest_yaw_threshold);
+
   return output;
 }
 

diff --git a/planning/behavior_path_planner/src/utils/avoidance/utils.cpp b/planning/behavior_path_planner/src/utils/avoidance/utils.cpp
@@ -187,7 +187,7 @@ void fillLongitudinalAndLengthByClosestEnvelopeFootprint(
   const PathWithLaneId & path, const Point & ego_pos, ObjectData & obj)
 {
   double min_distance = std::numeric_limits<double>::max();
-  double max_distance = std::numeric_limits<double>::min();
+  double max_distance = std::numeric_limits<double>::lowest();
   for (const auto & p : obj.envelope_poly.outer()) {
     const auto point = tier4_autoware_utils::createPoint(p.x(), p.y(), 0.0);
     const double arc_length = motion_utils::calcSignedArcLength(path.points, ego_pos, point);