feat(strat_planner): add a prepare time for blinker before taking act…

…ion for approval (#6438) Signed-off-by: Mamoru Sobue <[email protected]> Signed-off-by: Kotaro Yoshimoto <[email protected]>
autowarefoundation · Mar 12, 2024 · 5b04c8e · 5b04c8e
1 parent c68ed71
commit 5b04c8e
Show file tree

Hide file tree

Showing 5 changed files with 21 additions and 1 deletion.
diff --git a/planning/behavior_path_start_planner_module/config/start_planner.param.yaml b/planning/behavior_path_start_planner_module/config/start_planner.param.yaml
@@ -38,6 +38,7 @@
       backward_path_update_duration: 3.0
       ignore_distance_from_lane_end: 15.0
       # turns signal
+      prepare_time_before_start: 0.0
       th_turn_signal_on_lateral_offset: 1.0
       intersection_search_length: 30.0
       length_ratio_for_turn_signal_deactivation_near_intersection: 0.5

diff --git a/...ior_path_start_planner_module/include/behavior_path_start_planner_module/data_structs.hpp b/...ior_path_start_planner_module/include/behavior_path_start_planner_module/data_structs.hpp
@@ -59,6 +59,7 @@ struct StartPlannerParameters
   double th_arrived_distance{0.0};
   double th_stopped_velocity{0.0};
   double th_stopped_time{0.0};
+  double prepare_time_before_start{0.0};
   double th_turn_signal_on_lateral_offset{0.0};
   double th_distance_to_middle_of_the_road{0.0};
   double intersection_search_length{0.0};

diff --git a/..._start_planner_module/include/behavior_path_start_planner_module/start_planner_module.hpp b/..._start_planner_module/include/behavior_path_start_planner_module/start_planner_module.hpp
@@ -70,6 +70,8 @@ struct PullOutStatus
   bool prev_is_safe_dynamic_objects{false};
   std::shared_ptr<PathWithLaneId> prev_stop_path_after_approval{nullptr};
   std::optional<Pose> stop_pose{std::nullopt};
+  //! record the first time when the state changed from !isActivated() to isActivated()
+  std::optional<rclcpp::Time> first_approved_time{std::nullopt};
 
   PullOutStatus() {}
 };
@@ -261,6 +263,7 @@ class StartPlannerModule : public SceneModuleInterface
     const lanelet::ConstLanelets & pull_out_lanes, const geometry_msgs::msg::Point & current_pose,
     const double velocity_threshold, const double object_check_backward_distance,
     const double object_check_forward_distance) const;
+  bool needToPrepareBlinkerBeforeStart() const;
   bool hasFinishedPullOut() const;
   bool hasFinishedBackwardDriving() const;
   bool hasCollisionWithDynamicObjects() const;

diff --git a/planning/behavior_path_start_planner_module/src/manager.cpp b/planning/behavior_path_start_planner_module/src/manager.cpp
@@ -36,6 +36,7 @@ void StartPlannerModuleManager::init(rclcpp::Node * node)
   p.th_arrived_distance = node->declare_parameter<double>(ns + "th_arrived_distance");
   p.th_stopped_velocity = node->declare_parameter<double>(ns + "th_stopped_velocity");
   p.th_stopped_time = node->declare_parameter<double>(ns + "th_stopped_time");
+  p.prepare_time_before_start = node->declare_parameter<double>(ns + "prepare_time_before_start");
   p.th_turn_signal_on_lateral_offset =
     node->declare_parameter<double>(ns + "th_turn_signal_on_lateral_offset");
   p.th_distance_to_middle_of_the_road =

diff --git a/planning/behavior_path_start_planner_module/src/start_planner_module.cpp b/planning/behavior_path_start_planner_module/src/start_planner_module.cpp
@@ -117,7 +117,7 @@ void StartPlannerModule::onFreespacePlannerTimer()
 BehaviorModuleOutput StartPlannerModule::run()
 {
   updateData();
-  if (!isActivated()) {
+  if (!isActivated() || needToPrepareBlinkerBeforeStart()) {
     return planWaitingApproval();
   }
 
@@ -176,6 +176,10 @@ void StartPlannerModule::updateData()
     DEBUG_PRINT("StartPlannerModule::updateData() received new route, reset status");
   }
 
+  if (!status_.first_approved_time && isActivated()) {
+    status_.first_approved_time = clock_->now();
+  }
+
   if (hasFinishedBackwardDriving()) {
     updateStatusAfterBackwardDriving();
     DEBUG_PRINT("StartPlannerModule::updateData() completed backward driving");
@@ -1078,6 +1082,16 @@ bool StartPlannerModule::hasFinishedPullOut() const
   return has_finished;
 }
 
+bool StartPlannerModule::needToPrepareBlinkerBeforeStart() const
+{
+  if (!status_.first_approved_time) {
+    return true;
+  }
+  const auto first_approved_time = status_.first_approved_time.value();
+  const double elapsed = rclcpp::Duration(clock_->now() - first_approved_time).seconds();
+  return elapsed < parameters_->prepare_time_before_start;
+}
+
 bool StartPlannerModule::isStuck()
 {
   if (!isStopped()) {