feat(behavior_velocity): add insertDecel point function

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

planning/behavior_velocity_planner/include/scene_module/crosswalk/scene_crosswalk.hpp

@@ -120,7 +120,7 @@ class CrosswalkModule : public SceneModuleInterface
 
   std::pair<double, double> getAttentionRange(const PathWithLaneId & ego_path);
 
-  void insertDecelPoint(
+  void insertDecelPointWithDebugInfo(
     const geometry_msgs::msg::Point & stop_point, const float target_velocity,
     PathWithLaneId & output);
 

planning/behavior_velocity_planner/include/utilization/util.hpp

@@ -347,6 +347,10 @@ std::set<int64_t> getLaneletIdSetOnPath(
   return id_set;
 }
 
+boost::optional<geometry_msgs::msg::Pose> insertDecelPoint(
+  const geometry_msgs::msg::Point & stop_point, PathWithLaneId & output,
+  const float target_velocity);
+
 std::vector<lanelet::ConstLanelet> getLaneletsOnPath(
   const PathWithLaneId & path, const lanelet::LaneletMapPtr lanelet_map,
   const geometry_msgs::msg::Pose & current_pose);

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

@@ -200,18 +200,18 @@ bool CrosswalkModule::modifyPathVelocity(PathWithLaneId * path, StopReason * sto
     }
 
     const auto target_velocity = calcTargetVelocity(nearest_stop_point.get(), ego_path);
-    insertDecelPoint(
+    insertDecelPointWithDebugInfo(
       nearest_stop_point.get(), std::max(planner_param_.min_slow_down_velocity, target_velocity),
       *path);
 
     return true;
   }
 
   if (nearest_stop_point) {
-    insertDecelPoint(nearest_stop_point.get(), 0.0, *path);
+    insertDecelPointWithDebugInfo(nearest_stop_point.get(), 0.0, *path);
     planning_utils::appendStopReason(stop_factor, stop_reason);
   } else if (rtc_stop_point) {
-    insertDecelPoint(rtc_stop_point.get(), 0.0, *path);
+    insertDecelPointWithDebugInfo(rtc_stop_point.get(), 0.0, *path);
     planning_utils::appendStopReason(stop_factor_rtc, stop_reason);
   }
 
@@ -430,32 +430,24 @@ std::pair<double, double> CrosswalkModule::getAttentionRange(const PathWithLaneI
   return std::make_pair(std::max(0.0, near_attention_range), std::max(0.0, far_attention_range));
 }
 
-void CrosswalkModule::insertDecelPoint(
+void CrosswalkModule::insertDecelPointWithDebugInfo(
   const geometry_msgs::msg::Point & stop_point, const float target_velocity,
   PathWithLaneId & output)
 {
-  const auto & ego_pos = planner_data_->current_pose.pose.position;
-  const size_t base_idx = findNearestSegmentIndex(output.points, stop_point);
-  const auto insert_idx = insertTargetPoint(base_idx, stop_point, output.points);
-
-  if (!insert_idx) {
+  const auto stop_pose = planning_utils::insertDecelPoint(stop_point, output, target_velocity);
+  if (!stop_pose) {
     return;
   }
+  const auto & ego_pos = planner_data_->current_pose.pose.position;
 
-  for (size_t i = insert_idx.get(); i < output.points.size(); ++i) {
-    const auto & original_velocity = output.points.at(i).point.longitudinal_velocity_mps;
-    output.points.at(i).point.longitudinal_velocity_mps =
-      std::min(original_velocity, target_velocity);
-  }
-
-  setDistance(calcSignedArcLength(output.points, ego_pos, stop_point));
+  setDistance(calcSignedArcLength(output.points, ego_pos, stop_pose->position));
 
-  debug_data_.first_stop_pose = getPose(output.points.at(insert_idx.get()));
+  debug_data_.first_stop_pose = getPose(*stop_pose);
 
   if (std::abs(target_velocity) < 1e-3) {
-    debug_data_.stop_poses.push_back(getPose(output.points.at(insert_idx.get())));
+    debug_data_.stop_poses.push_back(*stop_pose);
   } else {
-    debug_data_.slow_poses.push_back(getPose(output.points.at(insert_idx.get())));
+    debug_data_.slow_poses.push_back(*stop_pose);
   }
 }
 

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

@@ -54,7 +54,7 @@ void applySafeVelocityConsideringPossibleCollision(
     const double safe_velocity = calculateInsertVelocity(v_slow_down, v_safe, v_min, original_vel);
     possible_collision.obstacle_info.safe_motion.safe_velocity = safe_velocity;
     const auto & pose = possible_collision.collision_with_margin.pose;
-    insertSafeVelocityToPath(pose, safe_velocity, param, inout_path);
+    planning_utils::insertDecelPoint(pose.position, *inout_path, safe_velocity);
   }
 }
 

planning/behavior_velocity_planner/src/utilization/util.cpp

@@ -246,6 +246,28 @@ void insertVelocity(
   setVelocityFromIndex(insert_index, v, &path);
 }
 
+boost::optional<geometry_msgs::msg::Pose> insertDecelPoint(
+  const geometry_msgs::msg::Point & stop_point, PathWithLaneId & output,
+  const float target_velocity)
+{
+  // TODO(tanaka): consider proper overlap threshold for inserting decel point
+  const double overlap_threshold = 5e-2;
+  const size_t base_idx = motion_utils::findNearestSegmentIndex(output.points, stop_point);
+  const auto insert_idx =
+    motion_utils::insertTargetPoint(base_idx, stop_point, output.points, overlap_threshold);
+
+  if (!insert_idx) {
+    return {};
+  }
+
+  for (size_t i = insert_idx.get(); i < output.points.size(); ++i) {
+    const auto & original_velocity = output.points.at(i).point.longitudinal_velocity_mps;
+    output.points.at(i).point.longitudinal_velocity_mps =
+      std::min(original_velocity, target_velocity);
+  }
+  return tier4_autoware_utils::getPose(output.points.at(insert_idx.get()));
+}
+
 Polygon2d toFootprintPolygon(const PredictedObject & object)
 {
   Polygon2d obj_footprint;