Merge pull request #23 from tier4/fix-behavior-velocity-duplicated-point

fix(behavior_velocity): fix detection area ,stop line,traffic lights stop line insertion ,occlusion spot (#505)

planning/behavior_velocity_planner/include/utilization/util.hpp

@@ -39,6 +39,8 @@
 #include <pcl/point_types.h>
 #include <tf2/utils.h>
 
+#include <algorithm>
+#include <limits>
 #include <string>
 #include <vector>
 
@@ -55,6 +57,8 @@ inline geometry_msgs::msg::Point getPoint(
 namespace behavior_velocity_planner
 {
 using Point2d = boost::geometry::model::d2::point_xy<double>;
+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; }
@@ -93,6 +97,10 @@ inline geometry_msgs::msg::Pose getPose(
   return traj.points.at(idx).pose;
 }
 
+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/blind_spot/scene.cpp

@@ -242,11 +242,13 @@ bool BlindSpotModule::generateStopLine(
   }
 
   /* insert judge point */
-  const int pass_judge_idx_ip = std::min(
+  // need to remove const because pass judge idx will be changed by insert stop point
+  int pass_judge_idx_ip = std::min(
     static_cast<int>(path_ip.points.size()) - 1, std::max(stop_idx_ip - pass_judge_idx_dist, 0));
   if (has_prior_stopline || stop_idx_ip == pass_judge_idx_ip) {
     *pass_judge_line_idx = *stop_line_idx;
   } else {
+    //! insertPoint check if there is no duplicated point
     *pass_judge_line_idx = insertPoint(pass_judge_idx_ip, path_ip, path);
     ++(*stop_line_idx);  // stop index is incremented by judge line insertion
   }
@@ -309,6 +311,14 @@ int BlindSpotModule::insertPoint(
     // copy from previous point
     inserted_point = inout_path->points.at(std::max(insert_idx - 1, 0));
     inserted_point.point.pose = path_ip.points[insert_idx_ip].point.pose;
+    constexpr double min_dist = 0.001;
+    //! avoid to insert duplicated point
+    if (
+      planning_utils::calcDist2d(inserted_point, inout_path->points.at(insert_idx).point) <
+      min_dist) {
+      inout_path->points.at(insert_idx).point.longitudinal_velocity_mps = 0.0;
+      return insert_idx;
+    }
     inout_path->points.insert(it, inserted_point);
   }
   return insert_idx;

planning/behavior_velocity_planner/src/scene_module/crosswalk/util.cpp

@@ -157,15 +157,9 @@ bool insertTargetVelocityPoint(
     }
     // ----------------
 
-    // insert target point
-    output.points.insert(
-      output.points.begin() + insert_target_point_idx, target_point_with_lane_id);
+    planning_utils::insertVelocity(
+      output, target_point_with_lane_id, velocity, insert_target_point_idx);
 
-    // insert 0 velocity after target point
-    for (size_t j = insert_target_point_idx; j < output.points.size(); ++j) {
-      output.points.at(j).point.longitudinal_velocity_mps =
-        std::min(static_cast<float>(velocity), output.points.at(j).point.longitudinal_velocity_mps);
-    }
     return true;
   }
   return false;
@@ -300,15 +294,9 @@ bool insertTargetVelocityPoint(
     }
     // ----------------
 
-    // insert target point
-    output.points.insert(
-      output.points.begin() + insert_target_point_idx, target_point_with_lane_id);
-
-    // insert 0 velocity after target point
-    for (size_t j = insert_target_point_idx; j < output.points.size(); ++j) {
-      output.points.at(j).point.longitudinal_velocity_mps =
-        std::min(static_cast<float>(velocity), output.points.at(j).point.longitudinal_velocity_mps);
-    }
+    // insert target point or replace with 0 velocity if same points found
+    planning_utils::insertVelocity(
+      output, target_point_with_lane_id, velocity, insert_target_point_idx);
     return true;
   }
   return false;

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

@@ -375,7 +375,7 @@ autoware_auto_planning_msgs::msg::PathWithLaneId DetectionAreaModule::insertStop
 {
   auto output_path = path;
 
-  const auto insert_idx = stop_point.first + 1;
+  size_t insert_idx = static_cast<size_t>(stop_point.first + 1);
   const auto stop_pose = stop_point.second;
 
   // To PathPointWithLaneId
@@ -384,14 +384,8 @@ autoware_auto_planning_msgs::msg::PathWithLaneId DetectionAreaModule::insertStop
   stop_point_with_lane_id.point.pose = stop_pose;
   stop_point_with_lane_id.point.longitudinal_velocity_mps = 0.0;
 
-  // Insert stop point
-  output_path.points.insert(output_path.points.begin() + insert_idx, stop_point_with_lane_id);
-
-  // Insert 0 velocity after stop point
-  for (size_t j = insert_idx; j < output_path.points.size(); ++j) {
-    output_path.points.at(j).point.longitudinal_velocity_mps = 0.0;
-  }
-
+  // Insert stop point or replace with zero velocity
+  planning_utils::insertVelocity(output_path, stop_point_with_lane_id, 0.0, insert_idx);
   return output_path;
 }
 

planning/behavior_velocity_planner/src/scene_module/no_stopping_area/scene_no_stopping_area.cpp

@@ -397,7 +397,7 @@ bool NoStoppingAreaModule::isStoppable(
 void NoStoppingAreaModule::insertStopPoint(
   autoware_auto_planning_msgs::msg::PathWithLaneId & path, const PathIndexWithPose & stop_point)
 {
-  const auto insert_idx = stop_point.first + 1;
+  size_t insert_idx = static_cast<size_t>(stop_point.first + 1);
   const auto stop_pose = stop_point.second;
 
   // To PathPointWithLaneId
@@ -406,13 +406,8 @@ void NoStoppingAreaModule::insertStopPoint(
   stop_point_with_lane_id.point.pose = stop_pose;
   stop_point_with_lane_id.point.longitudinal_velocity_mps = 0.0;
 
-  // Insert stop point
-  path.points.insert(path.points.begin() + insert_idx, stop_point_with_lane_id);
-
-  // Insert 0 velocity after stop point
-  for (size_t j = insert_idx; j < path.points.size(); ++j) {
-    path.points.at(j).point.longitudinal_velocity_mps = 0.0;
-  }
+  // Insert stop point or replace with zero velocity
+  planning_utils::insertVelocity(path, stop_point_with_lane_id, 0.0, insert_idx);
 }
 
 boost::optional<PathIndexWithPose> NoStoppingAreaModule::createTargetPoint(

planning/behavior_velocity_planner/src/scene_module/occlusion_spot/risk_predictive_braking.cpp

@@ -85,23 +85,16 @@ int insertSafeVelocityToPath(
   }
   PathPointWithLaneId inserted_point;
   inserted_point = inout_path->points.at(closest_idx);
-  int insert_idx = closest_idx;
+  size_t insert_idx = closest_idx;
   // insert velocity to path if distance is not too close else insert new collision point
   // if original path has narrow points it's better to set higher distance threshold
-  if (planning_utils::calcDist2d(in_pose, inserted_point.point) > 0.3) {
-    if (isAheadOf(in_pose, inout_path->points.at(closest_idx).point.pose)) {
-      ++insert_idx;
-    }
-    // return if index is after the last path point
-    if (insert_idx == static_cast<int>(inout_path->points.size())) {
-      return -1;
-    }
-    auto it = inout_path->points.begin() + insert_idx;
-    inserted_point = inout_path->points.at(closest_idx);
-    inserted_point.point.pose = in_pose;
-    inout_path->points.insert(it, inserted_point);
+  if (isAheadOf(in_pose, inout_path->points.at(closest_idx).point.pose)) {
+    ++insert_idx;
+    if (insert_idx == static_cast<size_t>(inout_path->points.size())) return -1;
   }
-  setVelocityFrom(insert_idx, safe_vel, inout_path);
+  // return if index is after the last path point
+  inserted_point.point.pose = in_pose;
+  planning_utils::insertVelocity(*inout_path, inserted_point, safe_vel, insert_idx);
   return 0;
 }
 

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

@@ -162,22 +162,17 @@ autoware_auto_planning_msgs::msg::PathWithLaneId StopLineModule::insertStopPose(
   auto modified_path = path;
 
   // Insert stop pose to between segment start and end
-  const int insert_index = stop_pose_with_index.index + 1;
+  size_t insert_index = static_cast<size_t>(stop_pose_with_index.index + 1);
   auto stop_point_with_lane_id = modified_path.points.at(insert_index);
   stop_point_with_lane_id.point.pose = stop_pose_with_index.pose;
   stop_point_with_lane_id.point.longitudinal_velocity_mps = 0.0;
 
   // Update first stop index
-  first_stop_path_point_index_ = insert_index;
+  first_stop_path_point_index_ = static_cast<int>(insert_index);
   debug_data_.stop_pose = stop_point_with_lane_id.point.pose;
 
-  // Insert stop point
-  modified_path.points.insert(modified_path.points.begin() + insert_index, stop_point_with_lane_id);
-
-  // Insert 0 velocity after stop point
-  for (size_t j = insert_index; j < modified_path.points.size(); ++j) {
-    modified_path.points.at(j).point.longitudinal_velocity_mps = 0.0;
-  }
+  // Insert stop point or replace with zero velocity
+  planning_utils::insertVelocity(modified_path, stop_point_with_lane_id, 0.0, insert_index);
 
   // Get stop point and stop factor
   {

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

@@ -466,17 +466,11 @@ autoware_auto_planning_msgs::msg::PathWithLaneId TrafficLightModule::insertStopP
   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
-  modified_path.points.insert(
-    modified_path.points.begin() + insert_target_point_idx, target_point_with_lane_id);
-  debug_data_.stop_poses.push_back(target_point_with_lane_id.point.pose);
-
-  // insert 0 velocity after target point
-  for (size_t j = insert_target_point_idx; j < modified_path.points.size(); ++j) {
-    modified_path.points.at(j).point.longitudinal_velocity_mps = 0.0;
-  }
-  if (target_velocity_point_idx < first_stop_path_point_index_) {
-    first_stop_path_point_index_ = target_velocity_point_idx;
+  // 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);
+  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/scene_module/virtual_traffic_light/scene.cpp

@@ -279,17 +279,11 @@ size_t insertStopVelocityAtCollision(
     return 0;
   }
 
-  const auto insert_index = offset_segment.index + 1;
+  auto insert_index = static_cast<size_t>(offset_segment.index + 1);
   auto insert_point = path->points.at(insert_index);
   insert_point.point.pose = interpolated_pose;
-
-  path->points.insert(path->points.begin() + insert_index, insert_point);
-
-  // Insert 0 velocity after stop point
-  for (size_t i = insert_index; i < path->points.size(); ++i) {
-    path->points.at(i).point.longitudinal_velocity_mps = 0.0;
-  }
-
+  // Insert 0 velocity after stop point or replace velocity with 0
+  behavior_velocity_planner::planning_utils::insertVelocity(*path, insert_point, 0.0, insert_index);
   return insert_index;
 }
 }  // namespace

planning/behavior_velocity_planner/src/utilization/util.cpp

@@ -23,6 +23,44 @@ namespace behavior_velocity_planner
 {
 namespace planning_utils
 {
+
+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;