adjust destination stop_line to make sure it's always ahead of adc_front_s

modules/planning/common/BUILD

@@ -212,6 +212,7 @@ cc_library(
         ":path_obstacle",
         ":planning_data",
         ":reference_line_info",
+        "//modules/common/configs:vehicle_config_helper",
         "//modules/common:log",
         "//modules/common/adapters:adapter_manager",
         "//modules/common/vehicle_state",

modules/planning/common/frame.cc

@@ -25,8 +25,10 @@
 #include <utility>
 
 #include "modules/common/adapters/adapter_manager.h"
+#include "modules/common/configs/vehicle_config_helper.h"
 #include "modules/common/log.h"
 #include "modules/common/math/vec2d.h"
+#include "modules/common/vehicle_state/vehicle_state.h"
 #include "modules/map/pnc_map/pnc_map.h"
 #include "modules/planning/common/planning_gflags.h"
 #include "modules/planning/reference_line/reference_line_smoother.h"
@@ -84,6 +86,10 @@ void Frame::CreatePredictionObstacles(
 
 bool Frame::CreateDestinationObstacle() {
   // set destination point
+  if (!routing_response_.routing_request().has_end()) {
+    ADEBUG << "routing_request has no end";
+    return false;
+  }
   common::math::Vec2d destination;
   destination.set_x(routing_response_.routing_request().end().pose().x());
   destination.set_y(routing_response_.routing_request().end().pose().y());
@@ -98,9 +104,23 @@ bool Frame::CreateDestinationObstacle() {
     return true;
   }
 
+  // adjust destination based on adc_front_s
+  common::SLPoint adc_sl;
+  auto& adc_position = common::VehicleState::instance()->pose().position();
+  reference_line_.get_point_in_frenet_frame(
+      {adc_position.x(), adc_position.y()}, &adc_sl);
+  const auto& vehicle_param =
+      common::VehicleConfigHelper::instance()->GetConfig().vehicle_param();
+  double adc_front_s = adc_sl.s() + vehicle_param.front_edge_to_center();
+  if (destination_sl.s() <= adc_front_s) {
+    destination_s = adc_front_s + FLAGS_destination_adjust_distance_buffer;
+  }
+
   std::unique_ptr<Obstacle> obstacle_ptr = CreateVirtualObstacle(
-      FLAGS_destination_obstacle_id, destination,
-      FLAGS_virtual_stop_wall_length, FLAGS_virtual_stop_wall_width,
+      FLAGS_destination_obstacle_id,
+      destination_s,
+      FLAGS_virtual_stop_wall_length,
+      FLAGS_virtual_stop_wall_width,
       FLAGS_virtual_stop_wall_height);
 
   obstacles_.Add(FLAGS_destination_obstacle_id, std::move(obstacle_ptr));
@@ -155,6 +175,8 @@ bool Frame::Init(const PlanningConfig &config) {
     CreatePredictionObstacles(prediction_);
   }
 
+  CreateDestinationObstacle();
+
   InitReferenceLineInfo(reference_lines);
   reference_line_ = reference_lines.front();
 
@@ -177,20 +199,44 @@ PlanningData *Frame::mutable_planning_data() { return &_planning_data; }
 
 const ReferenceLine &Frame::reference_line() const { return reference_line_; }
 
-bool Frame::MakeTrafficDecision(const routing::RoutingResponse &,
-                                const ReferenceLine &) {
+bool Frame::MakeTrafficDecision(const routing::RoutingResponse &routing,
+                                const ReferenceLine &reference_line) {
   const auto &path_obstacles = path_decision_->path_obstacles();
   for (const auto path_obstacle : path_obstacles.Items()) {
     const auto &obstacle = path_obstacle->Obstacle();
 
     // destination stop
     if (obstacle->Id() == FLAGS_destination_obstacle_id) {
+      // check stop_posision on reference line
+      auto stop_position = obstacle->Perception().position();
+      common::SLPoint stop_line_sl;
+      reference_line.get_point_in_frenet_frame(
+          { stop_position.x(),  stop_position.x()}, &stop_line_sl);
+      if (!reference_line.is_on_road(stop_line_sl)) {
+        continue;
+      }
+
+      // check stop_line_s vs adc_s. stop_line_s must be ahead of adc_front_s
+      common::SLPoint adc_sl;
+      auto& adc_position = common::VehicleState::instance()->pose().position();
+      reference_line.get_point_in_frenet_frame(
+          {adc_position.x(), adc_position.y()}, &adc_sl);
+      const auto& vehicle_param =
+          common::VehicleConfigHelper::instance()->GetConfig().vehicle_param();
+      double adc_front_s = adc_sl.s() + vehicle_param.front_edge_to_center();
+      if (stop_line_sl.s() <= adc_front_s) {
+        ADEBUG << "skip: object:" << obstacle->Id() << " fence route_s["
+               << stop_line_sl.s() << "] behind adc_front_s["
+               << adc_front_s << "]";
+        continue;
+      }
+
       ObjectDecisionType object_stop;
       ObjectStop *object_stop_ptr = object_stop.mutable_stop();
       object_stop_ptr->set_distance_s(FLAGS_stop_line_min_distance);
-      object_stop_ptr->set_reason_code(StopReasonCode::STOP_REASON_DESTINATION);
+      object_stop_ptr->set_reason_code(
+          StopReasonCode::STOP_REASON_DESTINATION);
 
-      auto stop_position = obstacle->Perception().position();
       auto stop_ref_point = reference_line_.get_reference_point(
           stop_position.x(), stop_position.y());
       object_stop_ptr->mutable_stop_point()->set_x(stop_ref_point.x());
@@ -275,16 +321,18 @@ void Frame::AlignPredictionTime(const double trajectory_header_time) {
 }
 
 std::unique_ptr<Obstacle> Frame::CreateVirtualObstacle(
-    const std::string &obstacle_id, const common::math::Vec2d &position,
+    const std::string &obstacle_id,
+    const double route_s,
     const double length, const double width, const double height) {
   std::unique_ptr<Obstacle> obstacle_ptr(nullptr);
 
   // create a "virtual" perception_obstacle
   perception::PerceptionObstacle perception_obstacle;
   perception_obstacle.set_id(-1);  // simulator needs a valid integer
-  perception_obstacle.mutable_position()->set_y(position.y());
   auto dest_ref_point =
-      reference_line_.get_reference_point(position.x(), position.y());
+      reference_line_.get_reference_point(route_s);
+  perception_obstacle.mutable_position()->set_x(dest_ref_point.x());
+  perception_obstacle.mutable_position()->set_y(dest_ref_point.y());
   perception_obstacle.set_theta(dest_ref_point.heading());
   perception_obstacle.mutable_velocity()->set_x(0);
   perception_obstacle.mutable_velocity()->set_y(0);

modules/planning/common/frame.h

@@ -121,7 +121,8 @@ class Frame {
   bool CreateDestinationObstacle();
 
   std::unique_ptr<Obstacle> CreateVirtualObstacle(
-      const std::string &obstacle_id, const common::math::Vec2d &position,
+      const std::string &obstacle_id,
+      const double route_s,
       const double length, const double width, const double height);
 
   /**

modules/planning/common/planning_gflags.cc

@@ -116,6 +116,8 @@ DEFINE_double(stop_distance_pedestrian, 5.0,
               "stop distance from in-lane pedestrian (meters)");
 DEFINE_double(stop_distance_obstacle, 5.0,
               "stop distance from in-lane obstacle (meters)");
+DEFINE_double(destination_adjust_distance_buffer, 1.0,
+              "distance buffer when adjusting destination stop line");
 DEFINE_double(nudge_distance_vehicle, 0.3,
               "minimum distance to nudge a vehicle");
 DEFINE_double(nudge_distance_bicycle, 0.9144,

modules/planning/common/planning_gflags.h

@@ -80,6 +80,7 @@ DECLARE_double(static_decision_stop_buffer);
 DECLARE_double(dynamic_decision_follow_range);
 DECLARE_double(stop_distance_pedestrian);
 DECLARE_double(stop_distance_obstacle);
+DECLARE_double(destination_adjust_distance_buffer);
 DECLARE_double(nudge_distance_vehicle);
 DECLARE_double(nudge_distance_bicycle);
 DECLARE_double(nudge_distance_obstacle);