feat(behavior_velocity_planner): add hold stop margin distance in vtl…

… module (tier4#1454)

* feat(behavior_velocity_planner): add hold stop margin distance in vtl module

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>

* fix(behavior_velocity_planner): change default value

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>

planning/behavior_velocity_planner/config/virtual_traffic_light.param.yaml

@@ -4,5 +4,6 @@
       max_delay_sec: 3.0
       near_line_distance: 1.0
       dead_line_margin: 1.0
+      hold_stop_margin_distance: 0.0
       max_yaw_deviation_deg: 90.0
       check_timeout_after_stop_line: true

planning/behavior_velocity_planner/include/scene_module/virtual_traffic_light/scene.hpp

@@ -68,6 +68,7 @@ class VirtualTrafficLightModule : public SceneModuleInterface
     double max_delay_sec;
     double near_line_distance;
     double dead_line_margin;
+    double hold_stop_margin_distance;
     double max_yaw_deviation_rad;
     bool check_timeout_after_stop_line;
   };

planning/behavior_velocity_planner/src/scene_module/virtual_traffic_light/manager.cpp

@@ -35,6 +35,7 @@ VirtualTrafficLightModuleManager::VirtualTrafficLightModuleManager(rclcpp::Node
     p.max_delay_sec = node.declare_parameter(ns + ".max_delay_sec", 3.0);
     p.near_line_distance = node.declare_parameter(ns + ".near_line_distance", 1.0);
     p.dead_line_margin = node.declare_parameter(ns + ".dead_line_margin", 1.0);
+    p.hold_stop_margin_distance = node.declare_parameter(ns + ".hold_stop_margin_distance", 0.0);
     p.max_yaw_deviation_rad =
       tier4_autoware_utils::deg2rad(node.declare_parameter(ns + ".max_yaw_deviation_deg", 90.0));
     p.check_timeout_after_stop_line =

planning/behavior_velocity_planner/src/scene_module/virtual_traffic_light/scene.cpp

@@ -565,15 +565,40 @@ void VirtualTrafficLightModule::insertStopVelocityAtStopLine(
   tier4_planning_msgs::msg::StopReason * stop_reason)
 {
   const auto collision = findCollision(path->points, *map_data_.stop_line);
+  const auto offset = -planner_data_->vehicle_info_.max_longitudinal_offset_m;
 
   geometry_msgs::msg::Pose stop_pose{};
   if (!collision) {
     insertStopVelocityFromStart(path);
     stop_pose = planner_data_->current_pose.pose;
   } else {
-    const auto offset = -planner_data_->vehicle_info_.max_longitudinal_offset_m;
-    const auto insert_index = insertStopVelocityAtCollision(*collision, offset, path);
-    stop_pose = path->points.at(insert_index).point.pose;
+    const auto & ego_pos = planner_data_->current_pose.pose.position;
+    const auto stop_distance =
+      motion_utils::calcSignedArcLength(path->points, ego_pos, collision.get().point) + offset;
+    const auto is_stopped = planner_data_->isVehicleStopped();
+
+    if (stop_distance < planner_param_.hold_stop_margin_distance && is_stopped) {
+      SegmentIndexWithPoint new_collision;
+      const auto ego_pos_on_path =
+        motion_utils::calcLongitudinalOffsetPoint(path->points, ego_pos, 0.0);
+
+      if (ego_pos_on_path) {
+        new_collision.point = ego_pos_on_path.get();
+        new_collision.index = motion_utils::findNearestSegmentIndex(path->points, ego_pos);
+        insertStopVelocityAtCollision(new_collision, 0.0, path);
+      }
+
+      // for virtual wall
+      {
+        auto path_tmp = path;
+        const auto insert_index = insertStopVelocityAtCollision(*collision, offset, path_tmp);
+        stop_pose = path_tmp->points.at(insert_index).point.pose;
+      }
+
+    } else {
+      const auto insert_index = insertStopVelocityAtCollision(*collision, offset, path);
+      stop_pose = path->points.at(insert_index).point.pose;
+    }
   }
 
   // Set StopReason