planning: remove move fence behavior

modules/common/data/global_flagfile.txt

@@ -8,3 +8,5 @@
 --vehicle_config_path=modules/common/data/mkz_config.pb.txt
 
 --map_dir=modules/map/data/sunnyvale_loop
+
+--map_dir=modules/map/data/sunnyvale_loop

modules/common/math/qp_solver/active_set_qp_solver.cc

@@ -115,11 +115,11 @@ bool ActiveSetQpSolver::Solve() {
                              lower_bound, upper_bound, constraint_lower_bound,
                              constraint_upper_bound, max_iter);
   if (ret != qpOASES::SUCCESSFUL_RETURN) {
-    AERROR << "Fail to initialze qp problem";
+    AERROR << "Fail to initialize qp problem";
     if (ret == qpOASES::RET_MAX_NWSR_REACHED) {
-      AERROR << "Qp solver failed due to reach max iteration";
+      AERROR << "Qp solver failed due to reached max iteration";
     } else {
-      AERROR << "Qp is failed due to infeasibility or other internal reason";
+      AERROR << "Qp failed due to infeasibility or other internal reasons";
     }
     return false;
   }

modules/planning/common/frame.cc

@@ -20,6 +20,7 @@
 #include "modules/planning/common/frame.h"
 
 #include <cmath>
+#include <functional>
 #include <list>
 #include <string>
 #include <utility>
@@ -31,6 +32,7 @@
 #include "modules/common/log.h"
 #include "modules/common/math/vec2d.h"
 #include "modules/common/vehicle_state/vehicle_state.h"
+#include "modules/map/hdmap/hdmap_util.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"
@@ -112,6 +114,75 @@ bool Frame::InitReferenceLineInfo(
   return true;
 }
 
+const Obstacle *Frame::AddStaticVirtualObstacle(
+    const std::string &id, const common::math::Box2d &box) {
+  const auto *object = obstacles_.Find(id);
+  if (object) {
+    AWARN << "obstacle " << id << " already exist.";
+    return object;
+  }
+  // create a "virtual" perception_obstacle
+  perception::PerceptionObstacle perception_obstacle;
+  // simulator needs a valid integer
+  perception_obstacle.set_id(-(std::hash<std::string>{}(id) >> 1));
+  perception_obstacle.mutable_position()->set_x(box.center().x());
+  perception_obstacle.mutable_position()->set_y(box.center().y());
+  perception_obstacle.set_theta(box.heading());
+  perception_obstacle.mutable_velocity()->set_x(0);
+  perception_obstacle.mutable_velocity()->set_y(0);
+  perception_obstacle.set_length(box.length());
+  perception_obstacle.set_width(box.width());
+  perception_obstacle.set_height(FLAGS_virtual_stop_wall_height);
+  perception_obstacle.set_type(
+      perception::PerceptionObstacle::UNKNOWN_UNMOVABLE);
+  perception_obstacle.set_tracking_time(1.0);
+
+  std::vector<common::math::Vec2d> corner_points;
+  box.GetAllCorners(&corner_points);
+  for (const auto &corner_point : corner_points) {
+    auto *point = perception_obstacle.add_polygon_point();
+    point->set_x(corner_point.x());
+    point->set_y(corner_point.y());
+  }
+
+  auto ptr = std::unique_ptr<Obstacle>(new Obstacle(id, perception_obstacle));
+  auto *obstacle_ptr = ptr.get();
+  obstacles_.Add(id, std::move(ptr));
+  return obstacle_ptr;
+}
+
+const Obstacle *Frame::CreateDestinationObstacle() {
+  if (!routing_response_.routing_request().has_end()) {
+    ADEBUG << "routing_request has no end";
+    return nullptr;
+  }
+  const auto &routing_end = routing_response_.routing_request().end();
+  common::PointENU dest_point = common::util::MakePointENU(
+      routing_end.pose().x(), routing_end.pose().y(), 0.0);
+  const auto lane =
+      pnc_map_->HDMap().GetLaneById(hdmap::MakeMapId(routing_end.id()));
+  if (!lane) {
+    AERROR << "Failed to find lane for destination : "
+           << routing_end.DebugString();
+    return nullptr;
+  }
+  double dest_s = 0.0;
+  double dest_l = 0.0;
+  if (!lane->GetProjection({dest_point.x(), dest_point.y()}, &dest_s,
+                           &dest_l)) {
+    AERROR << "Failed to get projection for " << dest_point.DebugString()
+           << " on lane " << lane->id().id();
+    return nullptr;
+  }
+  // check if destination point is in planning range
+  common::math::Box2d destination_box{{dest_point.x(), dest_point.y()},
+                                      lane->Heading(dest_s),
+                                      FLAGS_virtual_stop_wall_length,
+                                      FLAGS_virtual_stop_wall_width};
+  return AddStaticVirtualObstacle(FLAGS_destination_obstacle_id,
+                                  destination_box);
+}
+
 Status Frame::Init(const PlanningConfig &config,
                    const double current_time_stamp) {
   if (!pnc_map_) {
@@ -141,6 +212,12 @@ Status Frame::Init(const PlanningConfig &config,
   if (FLAGS_enable_prediction) {
     CreatePredictionObstacles(prediction_);
   }
+
+  if (!CreateDestinationObstacle()) {
+    AERROR << "Failed to create the destination obstacle";
+    return Status(ErrorCode::PLANNING_ERROR, "failed to find destination");
+  }
+
   if (CheckCollision()) {
     AERROR << "Found collision with obstacle: " << collision_obstacle_id_;
     return Status(ErrorCode::PLANNING_ERROR,

modules/planning/common/frame.h

@@ -114,6 +114,11 @@ class Frame {
    */
   bool CheckCollision();
 
+  const Obstacle *AddStaticVirtualObstacle(const std::string &id,
+                                           const common::math::Box2d &box);
+
+  const Obstacle *CreateDestinationObstacle();
+
  private:
   common::TrajectoryPoint planning_start_point_;
 

modules/planning/integration_tests/planning_test_base.cc

@@ -145,7 +145,7 @@ bool PlanningTestBase::RunPlanning(const std::string& test_case_name,
         AERROR << "Failed to write to file " << tmp_fname;
       }
       AERROR << "found\ndiff " << tmp_fname << " " << full_golden_path;
-      AERROR << "visualize diff\n python "
+      AERROR << "visualize diff\npython "
                 "modules/tools/plot_trace/plot_planning_result.py "
              << tmp_fname << " " << full_golden_path;
       return false;

modules/planning/tasks/path_decider/path_decider.cc

@@ -175,27 +175,9 @@ bool PathDecider::MakeStaticObstacleDecision(
         // STOP: and break
         ObjectStop *object_stop_ptr = object_decision.mutable_stop();
         object_stop_ptr->set_reason_code(StopReasonCode::STOP_REASON_OBSTACLE);
-
-        const auto &vehicle_param = common::VehicleConfigHelper::instance()
-                                        ->GetConfig()
-                                        .vehicle_param();
-        constexpr double kStopBuffer = 1.0e-6;
-        const double adc_v =
-            common::VehicleState::instance()->linear_velocity();
-        const double distance_to_full_stop =
-            0.5 * (adc_v * adc_v) / std::fabs(vehicle_param.max_deceleration());
         double stop_ref_s =
-            std::fmax(reference_line_info_->AdcSlBoundary().end_s() +
-                          distance_to_full_stop + kStopBuffer,
-                      sl_boundary.start_s() - FLAGS_stop_distance_obstacle);
-
-        constexpr double kMinStopDistanceToObstacle = 1.0;
-        if (stop_ref_s + kMinStopDistanceToObstacle > sl_boundary.start_s()) {
-          stop_ref_s = std::fmax(
-              reference_line_info_->AdcSlBoundary().end_s() + kStopBuffer,
-              sl_boundary.start_s() - kMinStopDistanceToObstacle);
-        }
-        object_stop_ptr->set_distance_s(stop_ref_s - sl_boundary.start_s());
+            sl_boundary.start_s() - FLAGS_stop_distance_obstacle;
+        object_stop_ptr->set_distance_s(-FLAGS_stop_distance_obstacle);
 
         auto stop_ref_point = reference_line_->GetReferencePoint(stop_ref_s);
         object_stop_ptr->mutable_stop_point()->set_x(stop_ref_point.x());

modules/planning/tasks/traffic_decider/traffic_decider.cc

@@ -44,84 +44,10 @@ bool TrafficDecider::Init(const PlanningConfig &config) {
   return true;
 }
 
-const PathObstacle *TrafficDecider::CreateDestinationPathObstacle() {
-  // set destination point
-
-  const auto *destination_obstacle =
-      frame_->FindObstacle(FLAGS_destination_obstacle_id);
-  if (!destination_obstacle) {
-    destination_obstacle = CreateDestinationObstacle();
-    ADEBUG << "Created destination obstacle";
-  }
-  if (!destination_obstacle) {
-    return nullptr;
-  } else {
-    const auto *ptr = reference_line_info_->AddObstacle(destination_obstacle);
-    if (!ptr) {
-      AERROR << "Failed to add destination obstacle's projection";
-      return nullptr;
-    }
-    return ptr;
-  }
-}
-
-const Obstacle *TrafficDecider::CreateDestinationObstacle() {
-  const auto &routing_response = frame_->routing_response();
-  if (!routing_response.routing_request().has_end()) {
-    ADEBUG << "routing_request has no end";
-    return nullptr;
-  }
-  common::math::Vec2d destination;
-  destination.set_x(routing_response.routing_request().end().pose().x());
-  destination.set_y(routing_response.routing_request().end().pose().y());
-
-  // check if destination point is in planning range
-  common::SLPoint destination_sl;
-  const auto &reference_line = reference_line_info_->reference_line();
-  reference_line.XYToSL(destination, &destination_sl);
-  double destination_s = destination_sl.s();
-  double destination_l = destination_sl.l();
-  double left_bound;
-  double right_bound;
-  if (!reference_line.GetLaneWidth(destination_s, &left_bound, &right_bound)) {
-    left_bound = right_bound = FLAGS_default_reference_line_width / 2;
-  }
-  if (destination_s < 0 || destination_s > reference_line.Length() ||
-      destination_l > left_bound || destination_l < -right_bound) {
-    ADEBUG << "destination[s=" << destination_s << "; l=" << destination_l
-           << "] out of planning range. Skip";
-    return nullptr;
-  }
-
-  // adjust destination based on adc front s
-  if (destination_sl.s() <= reference_line_info_->AdcSlBoundary().end_s()) {
-    destination_s = reference_line_info_->AdcSlBoundary().end_s() +
-                    FLAGS_destination_adjust_distance_buffer;
-  }
-
-  std::unique_ptr<Obstacle> obstacle_ptr =
-      reference_line_info_->CreateVirtualObstacle(
-          FLAGS_destination_obstacle_id, {destination.x(), destination.y()},
-          FLAGS_virtual_stop_wall_length, FLAGS_virtual_stop_wall_width,
-          FLAGS_virtual_stop_wall_height);
-  const auto *obstacle = obstacle_ptr.get();
-  if (!frame_->AddObstacle(std::move(obstacle_ptr))) {
-    AERROR << "Failed to add destination obstacle";
-    return nullptr;
-  }
-  return obstacle;
-}
-
 Status TrafficDecider::Execute(Frame *frame,
                                ReferenceLineInfo *reference_line_info) {
   Task::Execute(frame, reference_line_info);
 
-  // 1. add destination stop
-  if (!MakeDestinationStopDecision()) {
-    ADEBUG << "There is no destination stop";
-  } else {
-    ADEBUG << "destination is created";
-  }
   for (const auto rule_config : rule_configs_) {
     auto rule = rule_factory_.CreateObject(rule_config.name());
     if (!rule) {
@@ -134,47 +60,5 @@ Status TrafficDecider::Execute(Frame *frame,
   return Status::OK();
 }
 
-bool TrafficDecider::MakeDestinationStopDecision() {
-  const auto *path_obstacle = CreateDestinationPathObstacle();
-  if (!path_obstacle) {
-    ADEBUG << "The path obstacle is not found";
-    return false;
-  }
-  const auto *obstacle = path_obstacle->obstacle();
-  const auto &reference_line = reference_line_info_->reference_line();
-
-  // check stop_posision on reference line
-  auto stop_position = obstacle->Perception().position();
-  common::SLPoint stop_line_sl;
-  reference_line.XYToSL({stop_position.x(), stop_position.y()}, &stop_line_sl);
-  if (!reference_line.IsOnRoad(stop_line_sl)) {
-    return false;
-  }
-
-  // check stop_line_s vs adc_s. stop_line_s must be ahead of adc_front_s
-  if (stop_line_sl.s() <= reference_line_info_->AdcSlBoundary().end_s()) {
-    ADEBUG << "skip: object:" << obstacle->Id() << " fence route_s["
-           << stop_line_sl.s() << "] behind adc_front_s["
-           << reference_line_info_->AdcSlBoundary().end_s() << "]";
-    return false;
-  }
-
-  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);
-
-  auto stop_ref_point =
-      reference_line.GetReferencePoint(stop_position.x(), stop_position.y());
-  object_stop_ptr->mutable_stop_point()->set_x(stop_ref_point.x());
-  object_stop_ptr->mutable_stop_point()->set_y(stop_ref_point.y());
-  object_stop_ptr->set_stop_heading(stop_ref_point.heading());
-
-  reference_line_info_->path_decision()->AddLongitudinalDecision(
-      "Destination", obstacle->Id(), object_stop);
-
-  return true;
-}
-
 }  // namespace planning
 }  // namespace apollo

modules/planning/tasks/traffic_decider/traffic_decider.h

@@ -56,15 +56,6 @@ class TrafficDecider : public Task {
  private:
   void RegisterRules();
 
-  bool MakeDestinationStopDecision();
-
-  /**
-   * @brief create destination obstacle based on routing end point
-   * @return the created path obstacle
-   */
-  const Obstacle *CreateDestinationObstacle();
-  const PathObstacle *CreateDestinationPathObstacle();
-
   apollo::common::util::Factory<std::string, TrafficRule> rule_factory_;
   google::protobuf::RepeatedPtrField<RuleConfig> rule_configs_;
 };

modules/planning/testdata/garage_test/result_stop_obstacle_0.pb.txt

@@ -2285,5 +2285,13 @@ decision {
         }
       }
     }
+    decision {
+      id: "DEST"
+      perception_id: -1109540454
+      object_decision {
+        ignore {
+        }
+      }
+    }
   }
 }

modules/tools/plot_trace/plot_planning_result.py

@@ -50,9 +50,15 @@ def plot_planning(ax, planning_file):
 
 
 def press_key(event):
-    files = " ".join(g_args.planning_files)
-    if evnet.key == 'f':
-        call(["cp", files])
+    if event.key == 'c':
+        files = g_args.planning_files
+        if len(files) != 2:
+            print "Need more than two files"
+            return
+        command = ["cp"]
+        for f in files:
+            command.append(f)
+        call(command)
 
 
 if __name__ == '__main__':