planning: added the feature of stop at end of reference line when needed.

* handle the case that reference line generation failed or too slow

modules/planning/common/frame.cc

@@ -71,6 +71,10 @@ void Frame::CreatePredictionObstacles(
   }
 }
 
+const common::VehicleState &Frame::vehicle_state() const {
+  return vehicle_state_;
+}
+
 bool Frame::Rerouting() {
   auto *adapter_manager = AdapterManager::instance();
   if (adapter_manager->GetRoutingResponse()->Empty()) {

modules/planning/common/frame.h

@@ -77,6 +77,8 @@ class Frame {
 
   bool Rerouting();
 
+  const common::VehicleState &vehicle_state() const;
+
  private:
   /**
    * @brief create obstacles from prediction input.

modules/planning/common/planning_gflags.cc

@@ -183,6 +183,8 @@ DEFINE_double(virtual_stop_wall_length, 0.1,
               "virtual stop wall length (meters)");
 DEFINE_double(virtual_stop_wall_height, 2.0,
               "virtual stop wall height (meters)");
+DEFINE_string(reference_line_end_obstacle_id, "REF_END",
+              "Obstacle id for the end of reference line obstacle");
 
 // Prediction Part
 DEFINE_double(prediction_total_time, 5.0, "Total prediction time");

modules/planning/common/planning_gflags.h

@@ -104,6 +104,7 @@ DECLARE_double(follow_min_time_sec);
 DECLARE_string(destination_obstacle_id);
 DECLARE_double(virtual_stop_wall_length);
 DECLARE_double(virtual_stop_wall_height);
+DECLARE_string(reference_line_end_obstacle_id);
 
 DECLARE_double(prediction_total_time);
 DECLARE_bool(align_prediction_time);

modules/planning/conf/planning_config.pb.txt

@@ -152,3 +152,6 @@ rule_config : {
 rule_config : {
     rule_id: REROUTING
 }
+rule_config : {
+    rule_id: REFERENCE_LINE_END
+}

modules/planning/proto/planning_config.proto

@@ -25,6 +25,7 @@ message RuleConfig {
         CROSSWALK = 3;
         CLEAR_ZONE = 4;
         REROUTING = 5;
+        REFERENCE_LINE_END = 6;
     }
     optional RuleId rule_id = 1;
 }

modules/planning/tasks/traffic_decider/BUILD

@@ -9,6 +9,7 @@ cc_library(
         "clear_zone.cc",
         "crosswalk.cc",
         "object_priority.cc",
+        "reference_line_end.cc",
         "rerouting.cc",
         "signal_light.cc",
     ],
@@ -17,6 +18,7 @@ cc_library(
         "clear_zone.h",
         "crosswalk.h",
         "object_priority.h",
+        "reference_line_end.h",
         "rerouting.h",
         "signal_light.h",
         "traffic_rule.h",

modules/planning/tasks/traffic_decider/reference_line_end.cc

+/******************************************************************************
+ * Copyright 2017 The Apollo Authors. All Rights Reserved.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *****************************************************************************/
+
+/**
+ * @file
+ **/
+
+#include "modules/planning/tasks/traffic_decider/reference_line_end.h"
+
+#include "modules/common/proto/pnc_point.pb.h"
+
+#include "modules/common/configs/vehicle_config_helper.h"
+#include "modules/planning/common/planning_gflags.h"
+
+namespace apollo {
+namespace planning {
+
+using apollo::perception::TrafficLight;
+using apollo::perception::TrafficLightDetection;
+
+ReferenceLineEnd::ReferenceLineEnd(const RuleConfig& config)
+    : TrafficRule(config) {}
+
+bool ReferenceLineEnd::ApplyRule(Frame* frame,
+                                 ReferenceLineInfo* const reference_line_info) {
+  const auto& reference_line = reference_line_info->reference_line();
+  double remain_s =
+      reference_line.Length() - reference_line_info->AdcSlBoundary().end_s();
+  const double& velocity = frame->vehicle_state().linear_velocity();
+  const double stop_acc = std::fabs(common::VehicleConfigHelper::GetConfig()
+                                        .vehicle_param()
+                                        .max_deceleration() /
+                                    3.0);
+  const double stop_s = velocity * velocity / (2.0 * stop_acc) +
+                        FLAGS_virtual_stop_wall_length +
+                        FLAGS_stop_distance_destination;
+  if (stop_s < remain_s) {
+    ADEBUG << "have enough reference line to drive on";
+    return true;
+  }
+  // Need to create an obstacle at the end of reference line to stop the
+  // vehicle
+  double left_width = 0.0;
+  double right_width = 0.0;
+  double obstacle_start_s =
+      reference_line.Length() - FLAGS_virtual_stop_wall_length;
+  reference_line.GetLaneWidth(obstacle_start_s, &left_width, &right_width);
+
+  auto center_point = reference_line.GetReferencePoint(
+      obstacle_start_s + FLAGS_virtual_stop_wall_length / 2.0);
+
+  common::math::Box2d stop_box{center_point, center_point.heading(),
+                               FLAGS_virtual_stop_wall_length,
+                               left_width + right_width};
+
+  auto* obstacle = frame->AddStaticVirtualObstacle(
+      FLAGS_reference_line_end_obstacle_id + "_" +
+          reference_line_info->Lanes().Id(),
+      stop_box);
+  auto* path_obstacle = reference_line_info->AddObstacle(obstacle);
+  auto* path_decision = reference_line_info->path_decision();
+  auto stop_point = reference_line.GetReferencePoint(
+      obstacle_start_s - FLAGS_stop_distance_destination);
+  ObjectDecisionType stop;
+  stop.mutable_stop();
+  stop.mutable_stop()->set_distance_s(-FLAGS_stop_distance_destination);
+  stop.mutable_stop()->set_stop_heading(stop_point.heading());
+  stop.mutable_stop()->mutable_stop_point()->set_x(stop_point.x());
+  stop.mutable_stop()->mutable_stop_point()->set_y(stop_point.y());
+  stop.mutable_stop()->mutable_stop_point()->set_z(0.0);
+  path_decision->AddLongitudinalDecision(
+      RuleConfig::RuleId_Name(config_.rule_id()), path_obstacle->Id(), stop);
+  return true;
+}
+
+}  // namespace planning
+}  // namespace apollo

modules/planning/tasks/traffic_decider/reference_line_end.h

+/******************************************************************************
+ * Copyright 2017 The Apollo Authors. All Rights Reserved.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *****************************************************************************/
+
+/**
+ * @file
+ **/
+
+#ifndef MODULES_PLANNING_TASKS_TRAFFIC_DECIDER_REFERENCE_LINE_END_H_
+#define MODULES_PLANNING_TASKS_TRAFFIC_DECIDER_REFERENCE_LINE_END_H_
+
+#include <string>
+
+#include "modules/planning/tasks/traffic_decider/traffic_rule.h"
+
+namespace apollo {
+namespace planning {
+
+/**
+ * This class decides whether we should send rerouting request based on traffic
+ * situation.
+ */
+class ReferenceLineEnd : public TrafficRule {
+ public:
+  explicit ReferenceLineEnd(const RuleConfig& config);
+  virtual ~ReferenceLineEnd() = default;
+
+  bool ApplyRule(Frame* frame, ReferenceLineInfo* const reference_line_info);
+};
+
+}  // namespace planning
+}  // namespace apollo
+
+#endif  // MODULES_PLANNING_TASKS_TRAFFIC_DECIDER_REFERENCE_LINE_END_H_

modules/planning/tasks/traffic_decider/traffic_decider.cc

@@ -24,6 +24,7 @@
 #include "modules/planning/common/planning_gflags.h"
 #include "modules/planning/tasks/traffic_decider/backside_vehicle.h"
 #include "modules/planning/tasks/traffic_decider/crosswalk.h"
+#include "modules/planning/tasks/traffic_decider/reference_line_end.h"
 #include "modules/planning/tasks/traffic_decider/rerouting.h"
 #include "modules/planning/tasks/traffic_decider/signal_light.h"
 
@@ -39,7 +40,6 @@ void TrafficDecider::RegisterRules() {
                          [](const RuleConfig &config) -> TrafficRule * {
                            return new BacksideVehicle(config);
                          });
-
   rule_factory_.Register(RuleConfig::SIGNAL_LIGHT,
                          [](const RuleConfig &config) -> TrafficRule * {
                            return new SignalLight(config);
@@ -49,11 +49,14 @@ void TrafficDecider::RegisterRules() {
                          [](const RuleConfig &config) -> TrafficRule * {
                            return new Crosswalk(config);
                          });
-
   rule_factory_.Register(RuleConfig::REROUTING,
                          [](const RuleConfig &config) -> TrafficRule * {
                            return new Rerouting(config);
                          });
+  rule_factory_.Register(RuleConfig::REFERENCE_LINE_END,
+                         [](const RuleConfig &config) -> TrafficRule * {
+                           return new ReferenceLineEnd(config);
+                         });
 }
 
 bool TrafficDecider::Init(const PlanningConfig &config) {