planning: added traffic-light right-turn-unprotected scenario

liupzone · Jan 14, 2019 · 982b48d · 982b48d
1 parent ef2957b
commit 982b48d
Show file tree

Hide file tree

Showing 21 changed files with 281 additions and 151 deletions.
diff --git a/modules/planning/common/planning_context.h b/modules/planning/common/planning_context.h
@@ -28,6 +28,7 @@
 #include "modules/common/proto/drive_state.pb.h"
 #include "modules/common/proto/pnc_point.pb.h"
 #include "modules/map/pnc_map/path.h"
+#include "modules/perception/proto/traffic_light_detection.pb.h"
 #include "modules/planning/proto/planning_status.pb.h"
 #include "modules/routing/proto/routing.pb.h"
 
@@ -49,6 +50,7 @@ class PlanningContext {
   struct ScenarioInfo {
     apollo::hdmap::PathOverlap next_stop_sign_overlap;
     apollo::hdmap::PathOverlap next_traffic_light_overlap;
+    apollo::perception::TrafficLight_Color traffic_light_color;
     apollo::hdmap::PathOverlap next_crosswalk_overlap;
     // still in the scenario for this overlap, but stop already done
     // => no stop fence from decider_rule_based_stop task

diff --git a/modules/planning/common/planning_gflags.h b/modules/planning/common/planning_gflags.h
@@ -28,10 +28,10 @@ DECLARE_string(scenario_lane_follow_config_file);
 DECLARE_string(scenario_side_pass_config_file);
 DECLARE_string(scenario_stop_sign_unprotected_config_file);
 DECLARE_string(scenario_traffic_light_right_turn_unprotected_config_file);
+
 DECLARE_bool(enable_scenario_side_pass);
 DECLARE_double(side_pass_min_signal_intersection_distance);
 DECLARE_bool(enable_scenario_side_pass_multiple_parked_obstacles);
-
 DECLARE_bool(enable_scenario_stop_sign_unprotected);
 DECLARE_bool(enable_scenario_traffic_light_right_turn_unprotected);
 

diff --git a/modules/planning/conf/scenario/traffic_light_right_turn_unprotected_config.pb.txt b/modules/planning/conf/scenario/traffic_light_right_turn_unprotected_config.pb.txt
@@ -1,9 +1,9 @@
 scenario_type: TRAFFIC_LIGHT_RIGHT_TURN_UNPROTECTED
 traffic_light_right_turn_unprotected_config: {
-  start_traffic_light_scenario_timer: 20
   max_valid_stop_distance: 3.5
   max_adc_stop_speed: 0.3
   min_pass_s_distance: 3.0
+  red_light_right_turn_stop_duration: 3.0
   creep_timeout: 10.0
 }
 stage_type: TRAFFIC_LIGHT_RIGHT_TURN_UNPROTECTED_STOP

diff --git a/modules/planning/proto/planning_config.proto b/modules/planning/proto/planning_config.proto
@@ -86,18 +86,18 @@ message ScenarioStopSignUnprotectedConfig {
   optional double watch_vehicle_max_valid_stop_distance = 2 [default = 5.0];  // meter
   optional double max_valid_stop_distance = 3 [default = 3.5];  // meter
   optional double max_adc_stop_speed = 4 [default = 0.3];  // m/s
-  optional float stop_duration = 5 [default = 1.0];
+  optional float stop_duration = 5 [default = 1.0];  // sec
   optional double min_pass_s_distance = 6 [default = 3.0];  // meter
   optional float stop_timeout = 7 [default = 8.0];  // secoond
   optional float creep_timeout = 8 [default = 10.0];  // secoond
 }
 
 message ScenarioTrafficLightRightTurnUnprotectedConfig {
-  optional uint32 start_traffic_light_scenario_timer = 1 [default = 20];  // sec
-  optional double max_valid_stop_distance = 2 [default = 3.5];  // meter
-  optional double max_adc_stop_speed = 3 [default = 0.3];  // m/s
-  optional double min_pass_s_distance = 4 [default = 3.0];  // meter
-  optional float creep_timeout = 5 [default = 10.0];  // secoond  
+  optional double max_valid_stop_distance = 1 [default = 3.5];  // meter
+  optional double max_adc_stop_speed = 2 [default = 0.3];  // m/s
+  optional double min_pass_s_distance = 3 [default = 3.0];  // meter
+  optional float red_light_right_turn_stop_duration = 4 [default = 3.0];  // sec
+  optional float creep_timeout = 5 [default = 10.0];  // secoond
 }
 
 // scenario configs

diff --git a/modules/planning/scenarios/stop_sign/stop_sign_unprotected/stage_creep.cc b/modules/planning/scenarios/stop_sign/stop_sign_unprotected/stage_creep.cc
@@ -17,6 +17,8 @@
 /**
  * @file
  **/
+#include <string>
+#include <vector>
 
 #include "modules/planning/scenarios/stop_sign/stop_sign_unprotected/stage_creep.h"
 
@@ -50,6 +52,11 @@ Stage::StageStatus StageCreep::Process(
     return FinishStage();
   }
 
+  bool plan_ok = ExecuteTaskOnReferenceLine(planning_init_point, frame);
+  if (!plan_ok) {
+    AERROR << "StageCreep planning error";
+  }
+
   const auto& reference_line_info = frame->reference_line_info().front();
 
   // check if the stop_sign is still along referenceline
@@ -72,11 +79,6 @@ Stage::StageStatus StageCreep::Process(
   if (dynamic_cast<DeciderCreep*>(FindTask(TaskConfig::DECIDER_CREEP))
           ->CheckCreepDone(*frame, reference_line_info,
                            stop_sign_overlap_it->end_s, wait_time, timeout)) {
-    bool plan_ok = ExecuteTaskOnReferenceLine(planning_init_point, frame);
-    if (!plan_ok) {
-      AERROR << "StageCreep planning error";
-    }
-
     return FinishStage();
   }
 
@@ -85,7 +87,7 @@ Stage::StageStatus StageCreep::Process(
       ->SetProceedWithCautionSpeedParam(*frame, reference_line_info,
                                         stop_sign_overlap_it->end_s);
 
-  bool plan_ok = ExecuteTaskOnReferenceLine(planning_init_point, frame);
+  plan_ok = ExecuteTaskOnReferenceLine(planning_init_point, frame);
   if (!plan_ok) {
     AERROR << "StageCreep planning error";
   }

diff --git a/modules/planning/scenarios/stop_sign/stop_sign_unprotected/stage_creep.h b/modules/planning/scenarios/stop_sign/stop_sign_unprotected/stage_creep.h
@@ -20,9 +20,6 @@
 
 #pragma once
 
-#include <string>
-#include <vector>
-
 #include "modules/planning/proto/planning_config.pb.h"
 
 #include "modules/planning/scenarios/stage.h"

diff --git a/modules/planning/scenarios/stop_sign/stop_sign_unprotected/stage_intersection_cruise.cc b/modules/planning/scenarios/stop_sign/stop_sign_unprotected/stage_intersection_cruise.cc
@@ -18,6 +18,9 @@
  * @file
  **/
 
+#include <string>
+#include <vector>
+
 #include "modules/planning/scenarios/stop_sign/stop_sign_unprotected/stage_intersection_cruise.h"
 
 #include "modules/perception/proto/perception_obstacle.pb.h"

diff --git a/modules/planning/scenarios/stop_sign/stop_sign_unprotected/stage_intersection_cruise.h b/modules/planning/scenarios/stop_sign/stop_sign_unprotected/stage_intersection_cruise.h
@@ -20,9 +20,6 @@
 
 #pragma once
 
-#include <string>
-#include <vector>
-
 #include "modules/planning/proto/planning_config.pb.h"
 
 #include "modules/planning/scenarios/stage.h"

diff --git a/modules/planning/scenarios/stop_sign/stop_sign_unprotected/stop_sign_unprotected_scenario.cc b/modules/planning/scenarios/stop_sign/stop_sign_unprotected/stop_sign_unprotected_scenario.cc
@@ -127,6 +127,11 @@ std::unique_ptr<Stage> StopSignUnprotectedScenario::CreateStage(
 bool StopSignUnprotectedScenario::IsTransferable(
     const Scenario& current_scenario, const TrajectoryPoint& ego_point,
     const Frame& frame) {
+  if (PlanningContext::GetScenarioInfo()
+          ->next_stop_sign_overlap.object_id.empty()) {
+    return false;
+  }
+
   const auto& reference_line_info = frame.reference_line_info().front();
   const double adc_front_edge_s = reference_line_info.AdcSlBoundary().end_s();
   const double stop_sign_overlap_start_s =
@@ -141,10 +146,6 @@ bool StopSignUnprotectedScenario::IsTransferable(
     case ScenarioConfig::CHANGE_LANE:
     case ScenarioConfig::SIDE_PASS:
     case ScenarioConfig::APPROACH:
-      if (PlanningContext::GetScenarioInfo()
-              ->next_stop_sign_overlap.object_id.empty()) {
-        return false;
-      }
       return (adc_distance_to_stop_sign > 0 &&
               adc_distance_to_stop_sign <=
                   config_.stop_sign_unprotected_config()

diff --git a/modules/planning/scenarios/traffic_light/right_turn_unprotected/BUILD b/modules/planning/scenarios/traffic_light/right_turn_unprotected/BUILD
@@ -5,16 +5,16 @@ package(default_visibility = ["//visibility:public"])
 cc_library(
     name = "traffic_light_right_turn_unprotected_scenario",
     srcs = [
-        "traffic_light_right_turn_unprotected_creep.cc",
-        "traffic_light_right_turn_unprotected_intersection_cruise.cc",
+        "stage_creep.cc",
+        "stage_intersection_cruise.cc",
+        "stage_stop.cc",
         "traffic_light_right_turn_unprotected_scenario.cc",
-        "traffic_light_right_turn_unprotected_stop.cc",
     ],
     hdrs = [
-        "traffic_light_right_turn_unprotected_creep.h",
-        "traffic_light_right_turn_unprotected_intersection_cruise.h",
+        "stage_creep.h",
+        "stage_intersection_cruise.h",
+        "stage_stop.h",
         "traffic_light_right_turn_unprotected_scenario.h",
-        "traffic_light_right_turn_unprotected_stop.h",
     ],
     copts = ["-DMODULE_NAME=\\\"planning\\\""],
     deps = [

diff --git a/...fic_light_right_turn_unprotected_creep.cc → ...ght/right_turn_unprotected/stage_creep.cc b/...fic_light_right_turn_unprotected_creep.cc → ...ght/right_turn_unprotected/stage_creep.cc
@@ -18,9 +18,13 @@
  * @file
  **/
 
-#include "modules/planning/scenarios/traffic_light/right_turn_unprotected/traffic_light_right_turn_unprotected_creep.h"
+#include <string>
+#include <vector>
+
+#include "modules/planning/scenarios/traffic_light/right_turn_unprotected/stage_creep.h"
 
 #include "modules/perception/proto/perception_obstacle.pb.h"
+#include "modules/perception/proto/traffic_light_detection.pb.h"
 
 #include "cyber/common/log.h"
 #include "modules/common/time/time.h"
@@ -35,47 +39,75 @@ namespace scenario {
 namespace traffic_light {
 
 using common::time::Clock;
+using common::TrajectoryPoint;
+using hdmap::PathOverlap;
+using perception::TrafficLight;
 
-Stage::StageStatus TrafficLightRightTurnUnprotectedCreep::Process(
-    const common::TrajectoryPoint& planning_init_point, Frame* frame) {
+Stage::StageStatus StageCreep::Process(
+    const TrajectoryPoint& planning_init_point, Frame* frame) {
   ADEBUG << "stage: Creep";
   CHECK_NOTNULL(frame);
 
   scenario_config_.CopyFrom(GetContext()->scenario_config);
 
   if (!config_.enabled()) {
-    next_stage_ = ScenarioConfig::TRAFFIC_LIGHT_RIGHT_TURN_UNPROTECTED_STOP;
+    return FinishStage();
+  }
+
+  bool plan_ok = ExecuteTaskOnReferenceLine(planning_init_point, frame);
+  if (!plan_ok) {
+    AERROR << "StageCreep planning error";
+  }
+
+  const auto& reference_line_info = frame->reference_line_info().front();
+
+  // check if the traffic_light is still along referenceline
+  std::string traffic_light_overlap_id = GetContext()->traffic_light_id;
+  const std::vector<PathOverlap>& traffic_light_overlaps =
+      reference_line_info.reference_line().map_path().signal_overlaps();
+  auto traffic_light_overlap_it =
+      std::find_if(traffic_light_overlaps.begin(), traffic_light_overlaps.end(),
+                   [&traffic_light_overlap_id](const PathOverlap& overlap) {
+                     return overlap.object_id == traffic_light_overlap_id;
+                   });
+  if (traffic_light_overlap_it == traffic_light_overlaps.end()) {
+    next_stage_ = ScenarioConfig::NO_STAGE;
     return Stage::FINISHED;
   }
 
-  auto& reference_line_info = frame->mutable_reference_line_info()->front();
-  const double traffic_light_overlap_end_s =
-      PlanningContext::GetScenarioInfo()->next_traffic_light_overlap.end_s;
+  // check on traffic light color
+  if (PlanningContext::GetScenarioInfo()->traffic_light_color ==
+      TrafficLight::GREEN) {
+    return FinishStage();
+  }
 
-  const double wait_time = Clock::NowInSeconds() -
-      GetContext()->creep_start_time;
+  const double wait_time =
+      Clock::NowInSeconds() - GetContext()->creep_start_time;
   const double timeout = scenario_config_.creep_timeout();
   if (dynamic_cast<DeciderCreep*>(FindTask(TaskConfig::DECIDER_CREEP))
           ->CheckCreepDone(*frame, reference_line_info,
-                           traffic_light_overlap_end_s,
+                           traffic_light_overlap_it->end_s,
                            wait_time, timeout)) {
-    next_stage_ = ScenarioConfig::
-        TRAFFIC_LIGHT_RIGHT_TURN_UNPROTECTED_INTERSECTION_CRUISE;
-    return Stage::FINISHED;
+    return FinishStage();
   }
 
   // set param for PROCEED_WITH_CAUTION_SPEED
   dynamic_cast<DeciderCreep*>(FindTask(TaskConfig::DECIDER_CREEP))
       ->SetProceedWithCautionSpeedParam(*frame, reference_line_info,
-                                        traffic_light_overlap_end_s);
+                                        traffic_light_overlap_it->end_s);
 
-  bool plan_ok = ExecuteTaskOnReferenceLine(planning_init_point, frame);
+  plan_ok = ExecuteTaskOnReferenceLine(planning_init_point, frame);
   if (!plan_ok) {
-    AERROR << "TrafficLightRightTurnUnprotectedCreep planning error";
+    AERROR << "StageCreep planning error";
   }
   return Stage::RUNNING;
 }
 
+Stage::StageStatus StageCreep::FinishStage() {
+  next_stage_ = ScenarioConfig::STOP_SIGN_UNPROTECTED_INTERSECTION_CRUISE;
+  return Stage::FINISHED;
+}
+
 }  // namespace traffic_light
 }  // namespace scenario
 }  // namespace planning

diff --git a/...ffic_light_right_turn_unprotected_creep.h → ...ight/right_turn_unprotected/stage_creep.h b/...ffic_light_right_turn_unprotected_creep.h → ...ight/right_turn_unprotected/stage_creep.h
@@ -32,9 +32,9 @@ namespace traffic_light {
 
 struct TrafficLightRightTurnUnprotectedContext;
 
-class TrafficLightRightTurnUnprotectedCreep : public Stage {
+class StageCreep : public Stage {
  public:
-  explicit TrafficLightRightTurnUnprotectedCreep(
+  explicit StageCreep(
       const ScenarioConfig::StageConfig& config) : Stage(config) {}
 
  private:
@@ -45,6 +45,9 @@ class TrafficLightRightTurnUnprotectedCreep : public Stage {
     return Stage::GetContextAs<TrafficLightRightTurnUnprotectedContext>();
   }
 
+ private:
+  Stage::StageStatus FinishStage();
+
  private:
   ScenarioTrafficLightRightTurnUnprotectedConfig scenario_config_;
 };

diff --git a/...t_turn_unprotected_intersection_cruise.cc → ..._unprotected/stage_intersection_cruise.cc b/...t_turn_unprotected_intersection_cruise.cc → ..._unprotected/stage_intersection_cruise.cc
@@ -18,7 +18,10 @@
  * @file
  **/
 
-#include "modules/planning/scenarios/traffic_light/right_turn_unprotected/traffic_light_right_turn_unprotected_intersection_cruise.h"
+#include <string>
+#include <vector>
+
+#include "modules/planning/scenarios/traffic_light/right_turn_unprotected/stage_intersection_cruise.h"
 
 #include "modules/perception/proto/perception_obstacle.pb.h"
 
@@ -34,25 +37,50 @@ namespace planning {
 namespace scenario {
 namespace traffic_light {
 
-Stage::StageStatus TrafficLightRightTurnUnprotectedIntersectionCruise::Process(
-    const common::TrajectoryPoint& planning_init_point, Frame* frame) {
+using common::TrajectoryPoint;
+using hdmap::PathOverlap;
+
+Stage::StageStatus StageIntersectionCruise::Process(
+    const TrajectoryPoint& planning_init_point, Frame* frame) {
   ADEBUG << "stage: IntersectionCruise";
   CHECK_NOTNULL(frame);
 
-  if (GetContext()->traffic_light_id !=
-      PlanningContext::GetScenarioInfo()
-          ->next_traffic_light_overlap.object_id) {
-    next_stage_ = ScenarioConfig::NO_STAGE;
-    return Stage::FINISHED;
-  }
-
   bool plan_ok = ExecuteTaskOnReferenceLine(planning_init_point, frame);
   if (!plan_ok) {
-    AERROR << "TrafficLightRightTurnUnprotectedIntersectionCruise plan error";
+    AERROR << "StageIntersectionCruise plan error";
+  }
+
+  const auto& reference_line_info = frame->reference_line_info().front();
+
+  // check if the traffic_light is still along referenceline
+  std::string traffic_light_overlap_id = GetContext()->traffic_light_id;
+  const std::vector<PathOverlap>& traffic_light_overlaps =
+      reference_line_info.reference_line().map_path().signal_overlaps();
+  auto traffic_light_overlap_it =
+      std::find_if(traffic_light_overlaps.begin(), traffic_light_overlaps.end(),
+                   [&traffic_light_overlap_id](const PathOverlap& overlap) {
+                     return overlap.object_id == traffic_light_overlap_id;
+                   });
+  if (traffic_light_overlap_it == traffic_light_overlaps.end()) {
+    return FinishStage();
+  }
+
+  // check pass intersection
+  // TODO(all): update when pnc-junction is ready
+  constexpr double kIntersectionLength = 10.0;  // unit: m
+  const double adc_back_edge_s = reference_line_info.AdcSlBoundary().start_s();
+  if (adc_back_edge_s - traffic_light_overlap_it->end_s > kIntersectionLength) {
+    return FinishStage();
   }
+
   return Stage::RUNNING;
 }
 
+Stage::StageStatus StageIntersectionCruise::FinishStage() {
+  next_stage_ = ScenarioConfig::NO_STAGE;
+  return Stage::FINISHED;
+}
+
 }  // namespace traffic_light
 }  // namespace scenario
 }  // namespace planning