fix(behavior_velocity): fix stop line arc length consider path index (t…

…ier4#405)

* feat(behavior_velocity): add index to index calculation to stop line

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

* chore(behavior_velocity): add debug cout

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

* chore(behavior_velocity): devide pair

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

* feat(behavior_velocity): update functions

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

* chore(behaivor_velocity): remove debug

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

* fix(behavior_velocity): calculate signed arc length consider stop margin from stop line

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

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

@@ -17,6 +17,7 @@
 
 #include <string>
 #include <unordered_map>
+#include <utility>
 #include <vector>
 
 #define EIGEN_MPL2_ONLY
@@ -26,6 +27,7 @@
 #include <rclcpp/rclcpp.hpp>
 #include <scene_module/scene_module_interface.hpp>
 #include <utilization/boost_geometry_helper.hpp>
+#include <utilization/util.hpp>
 
 #include <lanelet2_core/LaneletMap.h>
 #include <lanelet2_routing/RoutingGraph.h>
@@ -34,6 +36,8 @@ namespace behavior_velocity_planner
 {
 class StopLineModule : public SceneModuleInterface
 {
+  using StopLineWithLaneId = std::pair<lanelet::ConstLineString3d, int64_t>;
+
 public:
   enum class State { APPROACH, STOPPED, START };
 
@@ -70,7 +74,7 @@ class StopLineModule : public SceneModuleInterface
 
 public:
   StopLineModule(
-    const int64_t module_id, const lanelet::ConstLineString3d & stop_line,
+    const int64_t module_id, const size_t lane_id, const lanelet::ConstLineString3d & stop_line,
     const PlannerParam & planner_param, const rclcpp::Logger logger,
     const rclcpp::Clock::SharedPtr clock);
 
@@ -86,7 +90,8 @@ class StopLineModule : public SceneModuleInterface
   geometry_msgs::msg::Point getCenterOfStopLine(const lanelet::ConstLineString3d & stop_line);
 
   boost::optional<StopLineModule::SegmentIndexWithPoint2d> findCollision(
-    const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const LineString2d & stop_line);
+    const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const LineString2d & stop_line,
+    const SearchRangeIndex & search_index);
 
   boost::optional<StopLineModule::SegmentIndexWithOffset> findOffsetSegment(
     const autoware_auto_planning_msgs::msg::PathWithLaneId & path,
@@ -102,6 +107,7 @@ class StopLineModule : public SceneModuleInterface
     tier4_planning_msgs::msg::StopReason * stop_reason);
 
   lanelet::ConstLineString3d stop_line_;
+  int64_t lane_id_;
   State state_;
 
   // Parameter

planning/behavior_velocity_planner/include/utilization/util.hpp

@@ -17,6 +17,7 @@
 
 #include <lanelet2_extension/utility/query.hpp>
 #include <tier4_autoware_utils/geometry/geometry.hpp>
+#include <tier4_autoware_utils/trajectory/trajectory.hpp>
 #include <utilization/boost_geometry_helper.hpp>
 
 #include <autoware_auto_perception_msgs/msg/predicted_object.hpp>
@@ -25,6 +26,7 @@
 #include <autoware_auto_planning_msgs/msg/path_with_lane_id.hpp>
 #include <autoware_auto_planning_msgs/msg/trajectory.hpp>
 #include <autoware_auto_planning_msgs/msg/trajectory_point.hpp>
+#include <geometry_msgs/msg/point.hpp>
 #include <geometry_msgs/msg/pose_stamped.hpp>
 #include <geometry_msgs/msg/quaternion.hpp>
 #include <tier4_planning_msgs/msg/stop_reason.hpp>
@@ -39,7 +41,6 @@
 #include <pcl/point_types.h>
 #include <tf2/utils.h>
 
-#include <algorithm>
 #include <limits>
 #include <string>
 #include <vector>
@@ -56,9 +57,18 @@ 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;
+struct SearchRangeIndex
+{
+  size_t min_idx;
+  size_t max_idx;
+};
+struct PointWithSearchRangeIndex
+{
+  geometry_msgs::msg::Point point;
+  SearchRangeIndex index;
+};
 using autoware_auto_planning_msgs::msg::PathWithLaneId;
+using Point2d = boost::geometry::model::d2::point_xy<double>;
 namespace planning_utils
 {
 inline geometry_msgs::msg::Point getPoint(const geometry_msgs::msg::Point & p) { return p; }
@@ -97,10 +107,6 @@ 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; }
@@ -132,6 +138,88 @@ geometry_msgs::msg::Pose transformRelCoordinate2D(
   const geometry_msgs::msg::Pose & target, const geometry_msgs::msg::Pose & origin);
 geometry_msgs::msg::Pose transformAbsCoordinate2D(
   const geometry_msgs::msg::Pose & relative, const geometry_msgs::msg::Pose & origin);
+SearchRangeIndex getPathIndexRangeIncludeLaneId(
+  const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const int64_t lane_id);
+/**
+ * @brief find nearest segment index with search range
+ */
+template <class T>
+size_t findNearestSegmentIndex(const T & points, const PointWithSearchRangeIndex & point_with_index)
+{
+  const auto & index = point_with_index.index;
+  const auto point = point_with_index.point;
+
+  tier4_autoware_utils::validateNonEmpty(points);
+
+  double min_dist = std::numeric_limits<double>::max();
+  size_t nearest_idx = 0;
+
+  for (size_t i = index.min_idx; i <= index.max_idx; ++i) {
+    const auto dist = tier4_autoware_utils::calcSquaredDistance2d(points.at(i), point);
+    if (dist < min_dist) {
+      min_dist = dist;
+      nearest_idx = i;
+    }
+  }
+
+  if (nearest_idx == 0) {
+    return 0;
+  }
+  if (nearest_idx == points.size() - 1) {
+    return points.size() - 2;
+  }
+
+  const double signed_length =
+    tier4_autoware_utils::calcLongitudinalOffsetToSegment(points, nearest_idx, point);
+
+  if (signed_length <= 0) {
+    return nearest_idx - 1;
+  }
+
+  return nearest_idx;
+}
+/**
+ * @brief find nearest segment index within distance threshold
+ */
+template <class T>
+PointWithSearchRangeIndex findFirstNearSearchRangeIndex(
+  const T & points, const geometry_msgs::msg::Point & point, const double distance_thresh = 9.0)
+{
+  tier4_autoware_utils::validateNonEmpty(points);
+
+  bool min_idx_found = false;
+  PointWithSearchRangeIndex point_with_range = {point, {static_cast<size_t>(0), points.size() - 1}};
+  for (size_t i = 0; i < points.size(); i++) {
+    const auto & p = points.at(i).point.pose.position;
+    const double dist = std::hypot(point.x - p.x, point.y - p.y);
+    if (dist < distance_thresh) {
+      if (!min_idx_found) {
+        point_with_range.index.min_idx = i;
+        min_idx_found = true;
+      }
+      point_with_range.index.max_idx = i;
+    }
+  }
+  return point_with_range;
+}
+/**
+ * @brief calcSignedArcLength from point to point with search range
+ */
+template <class T>
+double calcSignedArcLengthWithSearchIndex(
+  const T & points, const PointWithSearchRangeIndex & src_point_with_range,
+  const PointWithSearchRangeIndex & dst_point_with_range)
+{
+  tier4_autoware_utils::validateNonEmpty(points);
+  const size_t src_idx = planning_utils::findNearestSegmentIndex(points, src_point_with_range);
+  const size_t dst_idx = planning_utils::findNearestSegmentIndex(points, dst_point_with_range);
+  const double signed_length = tier4_autoware_utils::calcSignedArcLength(points, src_idx, dst_idx);
+  const double signed_length_src_offset = tier4_autoware_utils::calcLongitudinalOffsetToSegment(
+    points, src_idx, src_point_with_range.point);
+  const double signed_length_dst_offset = tier4_autoware_utils::calcLongitudinalOffsetToSegment(
+    points, dst_idx, dst_point_with_range.point);
+  return signed_length - signed_length_src_offset + signed_length_dst_offset;
+}
 Polygon2d toFootprintPolygon(const autoware_auto_perception_msgs::msg::PredictedObject & object);
 bool isAheadOf(const geometry_msgs::msg::Pose & target, const geometry_msgs::msg::Pose & origin);
 Polygon2d generatePathPolygon(

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

@@ -23,12 +23,13 @@ namespace behavior_velocity_planner
 {
 namespace
 {
-std::vector<lanelet::TrafficSignConstPtr> getTrafficSignRegElemsOnPath(
+using TrafficSignsWithLaneId = std::vector<std::pair<lanelet::TrafficSignConstPtr, int64_t>>;
+using StopLineWithLaneId = std::pair<lanelet::ConstLineString3d, int64_t>;
+TrafficSignsWithLaneId getTrafficSignRegElemsOnPath(
   const autoware_auto_planning_msgs::msg::PathWithLaneId & path,
   const lanelet::LaneletMapPtr lanelet_map)
 {
-  std::vector<lanelet::TrafficSignConstPtr> traffic_sign_reg_elems;
-
+  TrafficSignsWithLaneId traffic_signs_reg_elems_with_id;
   std::set<int64_t> unique_lane_ids;
   for (const auto & p : path.points) {
     unique_lane_ids.insert(p.lane_ids.at(0));  // should we iterate ids? keep as it was.
@@ -39,31 +40,34 @@ std::vector<lanelet::TrafficSignConstPtr> getTrafficSignRegElemsOnPath(
 
     const auto tss = ll.regulatoryElementsAs<const lanelet::TrafficSign>();
     for (const auto & ts : tss) {
-      traffic_sign_reg_elems.push_back(ts);
+      traffic_signs_reg_elems_with_id.push_back(std::make_pair(ts, lane_id));
     }
   }
 
-  return traffic_sign_reg_elems;
+  return traffic_signs_reg_elems_with_id;
 }
 
-std::vector<lanelet::ConstLineString3d> getStopLinesOnPath(
+std::vector<StopLineWithLaneId> getStopLinesWithLaneIdOnPath(
   const autoware_auto_planning_msgs::msg::PathWithLaneId & path,
   const lanelet::LaneletMapPtr lanelet_map)
 {
-  std::vector<lanelet::ConstLineString3d> stop_lines;
+  std::vector<StopLineWithLaneId> stop_lines_with_lane_id;
 
-  for (const auto & traffic_sign_reg_elem : getTrafficSignRegElemsOnPath(path, lanelet_map)) {
+  for (const auto & traffic_sign_reg_elem_with_id :
+       getTrafficSignRegElemsOnPath(path, lanelet_map)) {
+    const auto & traffic_sign_reg_elem = traffic_sign_reg_elem_with_id.first;
+    const int64_t lane_id = traffic_sign_reg_elem_with_id.second;
     // Is stop sign?
     if (traffic_sign_reg_elem->type() != "stop_sign") {
       continue;
     }
 
     for (const auto & stop_line : traffic_sign_reg_elem->refLines()) {
-      stop_lines.push_back(stop_line);
+      stop_lines_with_lane_id.push_back(std::make_pair(stop_line, lane_id));
     }
   }
 
-  return stop_lines;
+  return stop_lines_with_lane_id;
 }
 
 std::set<int64_t> getStopLineIdSetOnPath(
@@ -72,8 +76,8 @@ std::set<int64_t> getStopLineIdSetOnPath(
 {
   std::set<int64_t> stop_line_id_set;
 
-  for (const auto & stop_line : getStopLinesOnPath(path, lanelet_map)) {
-    stop_line_id_set.insert(stop_line.id());
+  for (const auto & stop_line_with_lane_id : getStopLinesWithLaneIdOnPath(path, lanelet_map)) {
+    stop_line_id_set.insert(stop_line_with_lane_id.first.id());
   }
 
   return stop_line_id_set;
@@ -94,12 +98,14 @@ StopLineModuleManager::StopLineModuleManager(rclcpp::Node & node)
 void StopLineModuleManager::launchNewModules(
   const autoware_auto_planning_msgs::msg::PathWithLaneId & path)
 {
-  for (const auto & stop_line :
-       getStopLinesOnPath(path, planner_data_->route_handler_->getLaneletMapPtr())) {
-    const auto module_id = stop_line.id();
+  for (const auto & stop_line_with_lane_id :
+       getStopLinesWithLaneIdOnPath(path, planner_data_->route_handler_->getLaneletMapPtr())) {
+    const auto module_id = stop_line_with_lane_id.first.id();
+    const auto lane_id = stop_line_with_lane_id.second;
     if (!isModuleRegistered(module_id)) {
       registerModule(std::make_shared<StopLineModule>(
-        module_id, stop_line, planner_param_, logger_.get_child("stop_line_module"), clock_));
+        module_id, lane_id, stop_line_with_lane_id.first, planner_param_,
+        logger_.get_child("stop_line_module"), clock_));
     }
   }
 }

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

@@ -91,21 +91,25 @@ boost::optional<StopLineModule::SegmentIndexWithOffset> findBackwardOffsetSegmen
 }  // namespace
 
 StopLineModule::StopLineModule(
-  const int64_t module_id, const lanelet::ConstLineString3d & stop_line,
+  const int64_t module_id, const size_t lane_id, const lanelet::ConstLineString3d & stop_line,
   const PlannerParam & planner_param, const rclcpp::Logger logger,
   const rclcpp::Clock::SharedPtr clock)
 : SceneModuleInterface(module_id, logger, clock),
   module_id_(module_id),
   stop_line_(stop_line),
+  lane_id_(lane_id),
   state_(State::APPROACH)
 {
   planner_param_ = planner_param;
 }
 
 boost::optional<StopLineModule::SegmentIndexWithPoint2d> StopLineModule::findCollision(
-  const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const LineString2d & stop_line)
+  const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const LineString2d & stop_line,
+  const SearchRangeIndex & search_index)
 {
-  for (size_t i = 0; i < path.points.size() - 1; ++i) {
+  const size_t min_search_index = std::max(static_cast<size_t>(0), search_index.min_idx);
+  const size_t max_search_index = std::min(search_index.max_idx, path.points.size() - 1);
+  for (size_t i = min_search_index; i < max_search_index; ++i) {
     const auto & p_front = path.points.at(i).point.pose.position;
     const auto & p_back = path.points.at(i + 1).point.pose.position;
 
@@ -190,15 +194,24 @@ bool StopLineModule::modifyPathVelocity(
   tier4_planning_msgs::msg::StopReason * stop_reason)
 {
   debug_data_ = DebugData();
-  debug_data_.base_link2front = planner_data_->vehicle_info_.max_longitudinal_offset_m;
+  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;
   *stop_reason =
     planning_utils::initializeStopReason(tier4_planning_msgs::msg::StopReason::STOP_LINE);
 
   const LineString2d stop_line = planning_utils::extendLine(
     stop_line_[0], stop_line_[1], planner_data_->stop_line_extend_length);
+  const geometry_msgs::msg::Point stop_line_position = getCenterOfStopLine(stop_line_);
+  const auto & current_position = planner_data_->current_pose.pose.position;
+  const PointWithSearchRangeIndex src_point_with_search_range_index =
+    planning_utils::findFirstNearSearchRangeIndex(path->points, current_position);
+  const SearchRangeIndex dst_search_range =
+    planning_utils::getPathIndexRangeIncludeLaneId(*path, lane_id_);
+
   // Find collision
-  const auto collision = findCollision(*path, stop_line);
+  const auto collision = findCollision(*path, stop_line, dst_search_range);
 
   // If no collision found, do nothing
   if (!collision) {
@@ -211,10 +224,19 @@ bool StopLineModule::modifyPathVelocity(
   // Calculate stop pose and insert index
   const auto stop_pose_with_index = calcStopPose(*path, offset_segment);
 
-  // Calc dist to stop pose
-  const double signed_arc_dist_to_stop_point = tier4_autoware_utils::calcSignedArcLength(
-    path->points, planner_data_->current_pose.pose.position, stop_pose_with_index->pose.position);
-
+  const PointWithSearchRangeIndex dst_point_with_search_range_index = {
+    stop_line_position, dst_search_range};
+  const double stop_line_margin = base_link2front + planner_param_.stop_margin;
+  /**
+   * @brief : calculate signed arc length consider stop margin from stop line
+   *
+   * |----------------------------|
+   * s---ego----------x--|--------g
+   */
+  const double signed_arc_dist_to_stop_point =
+    planning_utils::calcSignedArcLengthWithSearchIndex(
+      path->points, src_point_with_search_range_index, dst_point_with_search_range_index) -
+    stop_line_margin;
   if (state_ == State::APPROACH) {
     // Insert stop pose
     *path = insertStopPose(*path, *stop_pose_with_index, stop_reason);