fix(behavior_velocity_planner): do not consider stop_line_margin when…

… a stop line is defined at map in crosswalk module (autowarefoundation#1978) Signed-off-by: tomoya.kimura <tomoya.kimura@tier4.jp> Signed-off-by: tomoya.kimura <tomoya.kimura@tier4.jp>
boyali · Oct 3, 2022 · 2f70f50 · 2f70f50
1 parent d7d27cd
commit 2f70f50
Show file tree

Hide file tree

Showing 2 changed files with 11 additions and 6 deletions.
diff --git a/planning/behavior_velocity_planner/include/scene_module/crosswalk/scene_crosswalk.hpp b/planning/behavior_velocity_planner/include/scene_module/crosswalk/scene_crosswalk.hpp
@@ -111,7 +111,7 @@ class CrosswalkModule : public SceneModuleInterface
     const PathWithLaneId & ego_path, StopFactor & stop_factor);
 
   boost::optional<std::pair<double, geometry_msgs::msg::Point>> getStopLine(
-    const PathWithLaneId & ego_path) const;
+    const PathWithLaneId & ego_path, bool & exist_stopline_in_map) const;
 
   std::vector<CollisionPoint> getCollisionPoints(
     const PathWithLaneId & ego_path, const PredictedObject & object,

diff --git a/planning/behavior_velocity_planner/src/scene_module/crosswalk/scene_crosswalk.cpp b/planning/behavior_velocity_planner/src/scene_module/crosswalk/scene_crosswalk.cpp
@@ -230,11 +230,12 @@ bool CrosswalkModule::modifyPathVelocity(PathWithLaneId * path, StopReason * sto
 }
 
 boost::optional<std::pair<double, geometry_msgs::msg::Point>> CrosswalkModule::getStopLine(
-  const PathWithLaneId & ego_path) const
+  const PathWithLaneId & ego_path, bool & exist_stopline_in_map) const
 {
   const auto & ego_pos = planner_data_->current_pose.pose.position;
 
   const auto stop_line = getStopLineFromMap(module_id_, planner_data_, "crosswalk_id");
+  exist_stopline_in_map = static_cast<bool>(stop_line);
   if (stop_line) {
     const auto intersects = getLinestringIntersects(
       ego_path, lanelet::utils::to2D(stop_line.get()).basicLineString(), ego_pos, 2);
@@ -263,13 +264,15 @@ boost::optional<geometry_msgs::msg::Point> CrosswalkModule::findRTCStopPoint(
 {
   const auto & base_link2front = planner_data_->vehicle_info_.max_longitudinal_offset_m;
 
-  const auto p_stop_line = getStopLine(ego_path);
+  bool exist_stopline_in_map;
+  const auto p_stop_line = getStopLine(ego_path, exist_stopline_in_map);
   if (!p_stop_line) {
     return {};
   }
 
   const auto & p_stop = p_stop_line.get().second;
-  const auto margin = planner_param_.stop_line_distance + base_link2front;
+  const auto stop_line_distance = exist_stopline_in_map ? 0.0 : planner_param_.stop_line_distance;
+  const auto margin = stop_line_distance + base_link2front;
   const auto stop_pose = calcLongitudinalOffsetPose(ego_path.points, p_stop, -margin);
 
   if (!stop_pose) {
@@ -292,7 +295,8 @@ boost::optional<geometry_msgs::msg::Point> CrosswalkModule::findNearestStopPoint
   const auto & objects_ptr = planner_data_->predicted_objects;
   const auto & base_link2front = planner_data_->vehicle_info_.max_longitudinal_offset_m;
 
-  const auto p_stop_line = getStopLine(ego_path);
+  bool exist_stopline_in_map;
+  const auto p_stop_line = getStopLine(ego_path, exist_stopline_in_map);
   if (!p_stop_line) {
     return {};
   }
@@ -397,7 +401,8 @@ boost::optional<geometry_msgs::msg::Point> CrosswalkModule::findNearestStopPoint
   const auto stop_at_stop_line = !found_pedestrians || within_stop_line_margin;
 
   const auto & p_stop = stop_at_stop_line ? p_stop_line.get().second : first_stop_point;
-  const auto margin = stop_at_stop_line ? planner_param_.stop_line_distance + base_link2front
+  const auto stop_line_distance = exist_stopline_in_map ? 0.0 : planner_param_.stop_line_distance;
+  const auto margin = stop_at_stop_line ? stop_line_distance + base_link2front
                                         : planner_param_.stop_margin + base_link2front;
   const auto stop_pose = calcLongitudinalOffsetPose(ego_path.points, p_stop, -margin);