feat(behavior_velocity_planner): replace first_stop_path_point_index

planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/scene.cpp

@@ -195,13 +195,14 @@
 
   // Create legacy StopReason
   {
-    const auto insert_idx = stop_point->first + 1;
+    const double stop_path_point_distance = autoware::motion_utils::calcSignedArcLength(
+      path->points, 0, stop_pose.position, stop_point->first);
 
     if (
-      !first_stop_path_point_index_ ||
-      static_cast<int>(insert_idx) < first_stop_path_point_index_) {
+      !first_stop_path_point_distance_ ||
+      stop_path_point_distance < first_stop_path_point_distance_.value()) {
       debug_data_.first_stop_pose = stop_point->second;
-      first_stop_path_point_index_ = static_cast<int>(insert_idx);
+      first_stop_path_point_distance_ = stop_path_point_distance;
     }
   }
 

planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.cpp

@@ -153,12 +153,14 @@
 
     // Create legacy StopReason
     {
-      const auto insert_idx = stop_point->first + 1;
+      const double stop_path_point_distance = autoware::motion_utils::calcSignedArcLength(
+        path->points, 0, stop_pose.position, stop_point->first);
+
       if (
-        !first_stop_path_point_index_ ||
-        static_cast<int>(insert_idx) < first_stop_path_point_index_) {
-        debug_data_.first_stop_pose = stop_pose;
-        first_stop_path_point_index_ = static_cast<int>(insert_idx);
+        !first_stop_path_point_distance_ ||
+        stop_path_point_distance < first_stop_path_point_distance_.value()) {
+        debug_data_.first_stop_pose = stop_point->second;
+        first_stop_path_point_distance_ = stop_path_point_distance;
       }
     }
   } else if (state_machine_.getState() == StateMachine::State::GO) {

planning/behavior_velocity_planner/autoware_behavior_velocity_planner/src/planner_manager.cpp

@@ -14,9 +14,13 @@
 
 #include "planner_manager.hpp"
 
+#include <autoware/motion_utils/trajectory/interpolation.hpp>
+#include <autoware/motion_utils/trajectory/trajectory.hpp>
+
 #include <boost/format.hpp>
 
 #include <memory>
+#include <optional>
 #include <string>
 
 namespace autoware::behavior_velocity_planner
@@ -103,24 +107,28 @@ tier4_planning_msgs::msg::PathWithLaneId BehaviorVelocityPlannerManager::planPat
 {
   tier4_planning_msgs::msg::PathWithLaneId output_path_msg = input_path_msg;
 
-  int first_stop_path_point_index = static_cast<int>(output_path_msg.points.size() - 1);
+  double first_stop_path_point_distance =
+    autoware::motion_utils::calcArcLength(output_path_msg.points);
   std::string stop_reason_msg("path_end");
 
   for (const auto & plugin : scene_manager_plugins_) {
     plugin->updateSceneModuleInstances(planner_data, input_path_msg);
     plugin->plan(&output_path_msg);
-    const auto firstStopPathPointIndex = plugin->getFirstStopPathPointIndex();
+    const std::optional<double> firstStopPathPointDistance =
+      plugin->getFirstStopPathPointDistance();
 
-    if (firstStopPathPointIndex) {
-      if (firstStopPathPointIndex.value() < first_stop_path_point_index) {
-        first_stop_path_point_index = firstStopPathPointIndex.value();
+    if (firstStopPathPointDistance.has_value()) {
+      if (firstStopPathPointDistance.value() < first_stop_path_point_distance) {
+        first_stop_path_point_distance = firstStopPathPointDistance.value();
         stop_reason_msg = plugin->getModuleName();
       }
     }
   }
 
-  stop_reason_diag_ = makeStopReasonDiag(
-    stop_reason_msg, output_path_msg.points[first_stop_path_point_index].point.pose);
+  geometry_msgs::msg::Pose stop_pose = autoware::motion_utils::calcInterpolatedPose(
+    output_path_msg.points, first_stop_path_point_distance);
+
+  stop_reason_diag_ = makeStopReasonDiag(stop_reason_msg, stop_pose);
 
   return output_path_msg;
 }

planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/plugin_interface.hpp

@@ -34,7 +34,7 @@ class PluginInterface
   virtual void updateSceneModuleInstances(
     const std::shared_ptr<const PlannerData> & planner_data,
     const tier4_planning_msgs::msg::PathWithLaneId & path) = 0;
-  virtual std::optional<int> getFirstStopPathPointIndex() = 0;
+  virtual std::optional<double> getFirstStopPathPointDistance() = 0;
   virtual const char * getModuleName() = 0;
 };
 

planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/plugin_wrapper.hpp

@@ -38,9 +38,9 @@ class PluginWrapper : public PluginInterface
   {
     scene_manager_->updateSceneModuleInstances(planner_data, path);
   }
-  std::optional<int> getFirstStopPathPointIndex() override
+  std::optional<double> getFirstStopPathPointDistance() override
   {
-    return scene_manager_->getFirstStopPathPointIndex();
+    return scene_manager_->getFirstStopPathPointDistance();
   }
   const char * getModuleName() override { return scene_manager_->getModuleName(); }
 

planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/scene_module_interface.hpp

@@ -37,6 +37,7 @@
 #include <unique_identifier_msgs/msg/uuid.hpp>
 
 #include <memory>
+#include <optional>
 #include <set>
 #include <string>
 #include <unordered_map>
@@ -115,7 +116,7 @@ class SceneModuleInterface
 
   std::shared_ptr<universe_utils::TimeKeeper> getTimeKeeper() { return time_keeper_; }
 
-  std::optional<int> getFirstStopPathPointIndex() { return first_stop_path_point_index_; }
+  std::optional<double> getFirstStopPathPointDistance() { return first_stop_path_point_distance_; }
 
   void setActivation(const bool activated) { activated_ = activated; }
   void setRTCEnabled(const bool enable_rtc) { rtc_enabled_ = enable_rtc; }
@@ -138,7 +139,7 @@ class SceneModuleInterface
   rclcpp::Clock::SharedPtr clock_;
   std::shared_ptr<const PlannerData> planner_data_;
   std::optional<tier4_v2x_msgs::msg::InfrastructureCommand> infrastructure_command_;
-  std::optional<int> first_stop_path_point_index_;
+  std::optional<double> first_stop_path_point_distance_;
   autoware::motion_utils::VelocityFactorInterface velocity_factor_;
   std::vector<ObjectOfInterest> objects_of_interest_;
   mutable std::shared_ptr<universe_utils::TimeKeeper> time_keeper_;
@@ -173,7 +174,7 @@ class SceneModuleManagerInterface
 
   virtual const char * getModuleName() = 0;
 
-  std::optional<int> getFirstStopPathPointIndex() { return first_stop_path_point_index_; }
+  std::optional<double> getFirstStopPathPointDistance() { return first_stop_path_point_distance_; }
 
   void updateSceneModuleInstances(
     const std::shared_ptr<const PlannerData> & planner_data,
@@ -207,7 +208,7 @@ class SceneModuleManagerInterface
   std::shared_ptr<const PlannerData> planner_data_;
   autoware::motion_utils::VirtualWallMarkerCreator virtual_wall_marker_creator_;
 
-  std::optional<int> first_stop_path_point_index_;
+  std::optional<double> first_stop_path_point_distance_;
   rclcpp::Node & node_;
   rclcpp::Clock::SharedPtr clock_;
   // Debug

planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/scene_module_interface.cpp

@@ -117,7 +117,7 @@ void SceneModuleManagerInterface::modifyPathVelocity(
   tier4_v2x_msgs::msg::InfrastructureCommandArray infrastructure_command_array;
   infrastructure_command_array.stamp = clock_->now();
 
-  first_stop_path_point_index_ = static_cast<int>(path->points.size()) - 1;
+  first_stop_path_point_distance_ = autoware::motion_utils::calcArcLength(path->points);
   for (const auto & scene_module : scene_modules_) {
     tier4_planning_msgs::msg::StopReason stop_reason;
     scene_module->resetVelocityFactor();
@@ -137,8 +137,8 @@ void SceneModuleManagerInterface::modifyPathVelocity(
       infrastructure_command_array.commands.push_back(*command);
     }
 
-    if (scene_module->getFirstStopPathPointIndex() < first_stop_path_point_index_) {
-      first_stop_path_point_index_ = scene_module->getFirstStopPathPointIndex();
+    if (scene_module->getFirstStopPathPointDistance() < first_stop_path_point_distance_) {
+      first_stop_path_point_distance_ = scene_module->getFirstStopPathPointDistance();
     }
 
     for (const auto & marker : scene_module->createDebugMarkerArray().markers) {

planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/scene.cpp

@@ -48,50 +48,51 @@
   if (path->points.empty()) return true;
   const auto base_link2front = planner_data_->vehicle_info_.max_longitudinal_offset_m;
   debug_data_.base_link2front = base_link2front;
-  first_stop_path_point_index_ = static_cast<int>(path->points.size()) - 1;
+  first_stop_path_point_distance_ = autoware::motion_utils::calcArcLength(path->points);
   *stop_reason = planning_utils::initializeStopReason(StopReason::STOP_LINE);
 
   const LineString2d stop_line = planning_utils::extendLine(
     stop_line_[0], stop_line_[1], planner_data_->stop_line_extend_length);
 
   time_keeper_->start_track("createTargetPoint");
   // Calculate stop pose and insert index
   const auto stop_point = arc_lane_utils::createTargetPoint(
     *path, stop_line, planner_param_.stop_margin,
     planner_data_->vehicle_info_.max_longitudinal_offset_m);
   time_keeper_->end_track("createTargetPoint");
   // If no collision found, do nothing
   if (!stop_point) {
     RCLCPP_DEBUG_THROTTLE(logger_, *clock_, 5000 /* ms */, "is no collision");
     return true;
   }
 
   const auto stop_point_idx = stop_point->first;
   auto stop_pose = stop_point->second;
 
   /**
    * @brief : calculate signed arc length consider stop margin from stop line
    *
    * |----------------------------|
    * s---ego----------x--|--------g
    */
   time_keeper_->start_track(
     "calcSegmentIndexFromPointIndex & findEgoSegmentIndex & calcSignedArcLength");
   const size_t stop_line_seg_idx = planning_utils::calcSegmentIndexFromPointIndex(
     path->points, stop_pose.position, stop_point_idx);
   const size_t current_seg_idx = findEgoSegmentIndex(path->points);
   const double signed_arc_dist_to_stop_point = autoware::motion_utils::calcSignedArcLength(
     path->points, planner_data_->current_odometry->pose.position, current_seg_idx,
     stop_pose.position, stop_line_seg_idx);
   time_keeper_->end_track(
     "calcSegmentIndexFromPointIndex & findEgoSegmentIndex & calcSignedArcLength");
   switch (state_) {
     case State::APPROACH: {
       // Insert stop pose
       planning_utils::insertStopPoint(stop_pose.position, stop_line_seg_idx, *path);
 
       // Update first stop index
-      first_stop_path_point_index_ = static_cast<int>(stop_point_idx);
+      first_stop_path_point_distance_ = autoware::motion_utils::calcSignedArcLength(
+        path->points, 0, stop_pose.position, stop_line_seg_idx);
       debug_data_.stop_pose = stop_pose;
 
       // Get stop point and stop factor

planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/manager.cpp

@@ -15,6 +15,7 @@
 #include "manager.hpp"
 
 #include <autoware/behavior_velocity_planner_common/utilization/util.hpp>
+#include <autoware/motion_utils/trajectory/trajectory.hpp>
 #include <autoware/universe_utils/ros/parameter.hpp>
 
 #include <tf2/utils.h>
@@ -60,7 +61,7 @@ void TrafficLightModuleManager::modifyPathVelocity(tier4_planning_msgs::msg::Pat
   stop_reason_array.header.frame_id = "map";
   stop_reason_array.header.stamp = path->header.stamp;
 
-  first_stop_path_point_index_ = static_cast<int>(path->points.size() - 1);
+  first_stop_path_point_distance_ = autoware::motion_utils::calcArcLength(path->points);
   nearest_ref_stop_path_point_index_ = static_cast<int>(path->points.size() - 1);
   for (const auto & scene_module : scene_modules_) {
     tier4_planning_msgs::msg::StopReason stop_reason;
@@ -79,8 +80,10 @@ void TrafficLightModuleManager::modifyPathVelocity(tier4_planning_msgs::msg::Pat
       stop_reason_array.stop_reasons.emplace_back(stop_reason);
     }
 
-    if (traffic_light_scene_module->getFirstStopPathPointIndex() < first_stop_path_point_index_) {
-      first_stop_path_point_index_ = traffic_light_scene_module->getFirstStopPathPointIndex();
+    if (
+      traffic_light_scene_module->getFirstStopPathPointDistance() <
+      first_stop_path_point_distance_) {
+      first_stop_path_point_distance_ = traffic_light_scene_module->getFirstStopPathPointDistance();
     }
     if (
       traffic_light_scene_module->getFirstRefStopPathPointIndex() <

planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/scene.cpp

@@ -58,7 +58,7 @@ bool TrafficLightModule::modifyPathVelocity(PathWithLaneId * path, StopReason *
 {
   debug_data_ = DebugData();
   debug_data_.base_link2front = planner_data_->vehicle_info_.max_longitudinal_offset_m;
-  first_stop_path_point_index_ = static_cast<int>(path->points.size()) - 1;
+  first_stop_path_point_distance_ = autoware::motion_utils::calcArcLength(path->points);
   first_ref_stop_path_point_index_ = static_cast<int>(path->points.size()) - 1;
   *stop_reason = planning_utils::initializeStopReason(StopReason::TRAFFIC_LIGHT);
 
@@ -291,8 +291,11 @@ tier4_planning_msgs::msg::PathWithLaneId TrafficLightModule::insertStopPose(
   // 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);
+
+  const double target_velocity_point_distance = autoware::motion_utils::calcArcLength(std::vector(
+    modified_path.points.begin(), modified_path.points.begin() + target_velocity_point_idx));
+  if (target_velocity_point_distance < first_stop_path_point_distance_) {
+    first_stop_path_point_distance_ = target_velocity_point_distance;
     debug_data_.first_stop_pose = target_point_with_lane_id.point.pose;
   }