fix(behavior_velocity): fix detection area ,stop line,traffic lights …

…stop line insertion ,occlusion spot (#505)

Signed-off-by: tanaka3 <ttatcoder@outlook.jp>

planning/behavior_velocity_planner/include/utilization/util.hpp

@@ -58,6 +58,8 @@ namespace behavior_velocity_planner
 {
 using Point2d = boost::geometry::model::d2::point_xy<double>;
 using Polygons2d = std::vector<lanelet::BasicPolygon2d>;
+using autoware_auto_planning_msgs::msg::PathPointWithLaneId;
+using autoware_auto_planning_msgs::msg::PathWithLaneId;
 namespace planning_utils
 {
 inline geometry_msgs::msg::Point getPoint(const geometry_msgs::msg::Point & p) { return p; }
@@ -96,6 +98,10 @@ inline geometry_msgs::msg::Pose getPose(
   return traj.points.at(idx).pose;
 }
 void getAllPartitionLanelets(const lanelet::LaneletMapConstPtr ll, Polygons2d & polys);
+void setVelocityFrom(const size_t idx, const double vel, PathWithLaneId * input);
+void insertVelocity(
+  PathWithLaneId & path, const PathPointWithLaneId & path_point, const double v,
+  size_t & insert_index, const double min_distance = 0.001);
 inline int64_t bitShift(int64_t original_id) { return original_id << (sizeof(int32_t) * 8 / 2); }
 
 inline double square(const double & a) { return a * a; }

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

@@ -460,15 +460,15 @@ autoware_auto_planning_msgs::msg::PathWithLaneId TrafficLightModule::insertStopP
   modified_path = input;
 
   // Create stop pose
-  const int target_velocity_point_idx = std::max(static_cast<int>(insert_target_point_idx - 1), 0);
+  size_t target_velocity_point_idx = std::max(static_cast<int>(insert_target_point_idx - 1), 0);
   auto target_point_with_lane_id = modified_path.points.at(target_velocity_point_idx);
   target_point_with_lane_id.point.pose.position.x = target_point.x();
   target_point_with_lane_id.point.pose.position.y = target_point.y();
   target_point_with_lane_id.point.longitudinal_velocity_mps = 0.0;
 
   // Insert stop pose into path or replace with zero velocity
-  size_t insert_index = insert_target_point_idx;
-  planning_utils::insertVelocity(modified_path, target_point_with_lane_id, 0.0, insert_index);
+  planning_utils::insertVelocity(
+    modified_path, target_point_with_lane_id, 0.0, target_velocity_point_idx);
   if (static_cast<int>(target_velocity_point_idx) < first_stop_path_point_index_) {
     first_stop_path_point_index_ = static_cast<int>(target_velocity_point_idx);
     debug_data_.first_stop_pose = target_point_with_lane_id.point.pose;

planning/behavior_velocity_planner/src/utilization/util.cpp

@@ -62,6 +62,43 @@ SearchRangeIndex getPathIndexRangeIncludeLaneId(
   setVelocityFromIndex(insert_index, v, &path);
 }
 
+void setVelocityFromIndex(const size_t begin_idx, const double vel, PathWithLaneId * input)
+{
+  for (size_t i = begin_idx; i < input->points.size(); ++i) {
+    input->points.at(i).point.longitudinal_velocity_mps =
+      std::min(static_cast<float>(vel), input->points.at(i).point.longitudinal_velocity_mps);
+  }
+  return;
+}
+
+void insertVelocity(
+  PathWithLaneId & path, const PathPointWithLaneId & path_point, const double v,
+  size_t & insert_index, const double min_distance)
+{
+  bool already_has_path_point = false;
+  // consider front/back point is near to insert point or not
+  int min_idx = std::max(0, static_cast<int>(insert_index - 1));
+  int max_idx =
+    std::min(static_cast<int>(insert_index + 1), static_cast<int>(path.points.size() - 1));
+  for (int i = min_idx; i <= max_idx; i++) {
+    if (
+      tier4_autoware_utils::calcDistance2d(path.points.at(static_cast<size_t>(i)), path_point) <
+      min_distance) {
+      path.points.at(i).point.longitudinal_velocity_mps = 0;
+      already_has_path_point = true;
+      insert_index = static_cast<size_t>(i);
+      // set velocity from is going to insert min velocity later
+      break;
+    }
+  }
+  //! insert velocity point only if there is no close point on path
+  if (!already_has_path_point) {
+    path.points.insert(path.points.begin() + insert_index, path_point);
+  }
+  // set zero velocity from insert index
+  setVelocityFromIndex(insert_index, v, &path);
+}
+
 Polygon2d toFootprintPolygon(const autoware_auto_perception_msgs::msg::PredictedObject & object)
 {
   Polygon2d obj_footprint;