storytelling: add CloseToCrosswalk

Author: jmtao

modules/storytelling/proto/story.proto

@@ -4,11 +4,11 @@ package apollo.storytelling;
 
 import "modules/common/proto/header.proto";
 
-// TODO(xiaoxq): State triggering conditions.
-message ProceedWithCaution {
+message CloseToCrosswalk {
+  optional string id = 1;
+  optional double distance = 2  [default = nan];
 }
 
-// Triggered when we are close to a junction.
 message CloseToJunction {
   enum JunctionType {
     PNC_JUNCTION = 1;
@@ -31,7 +31,7 @@ message CloseToSignal {
 message Stories {
   optional apollo.common.Header header = 1;
 
-  optional ProceedWithCaution proceed_with_caution = 2;
+  optional CloseToCrosswalk close_to_crosswalk = 2;
   optional CloseToJunction close_to_junction = 3;
   optional CloseToSignal close_to_signal = 4;
 }

modules/storytelling/story_tellers/close_to_junction_teller.cc

@@ -29,6 +29,7 @@ namespace {
 
 using apollo::hdmap::HDMapUtil;
 using apollo::common::PathPoint;
+using apollo::hdmap::CrosswalkInfoConstPtr;
 using apollo::hdmap::JunctionInfoConstPtr;
 using apollo::hdmap::PNCJunctionInfoConstPtr;
 using apollo::hdmap::SignalInfoConstPtr;
@@ -68,6 +69,22 @@ bool GetJunction(const PathPoint& point, std::string* junction_id) {
   return false;
 }
 
+bool GetCrosswalk(const PathPoint& point, std::string* crosswalk_id) {
+  common::PointENU hdmap_point;
+  hdmap_point.set_x(point.x());
+  hdmap_point.set_y(point.y());
+  std::vector<CrosswalkInfoConstPtr> crosswalks;
+  if (HDMapUtil::BaseMap().GetCrosswalks(hdmap_point,
+                                         FLAGS_search_radius,
+                                         &crosswalks) == 0) {
+    if (crosswalks.size() > 0) {
+      *crosswalk_id = crosswalks.front()->id().id();
+      return true;
+    }
+  }
+  return false;
+}
+
 bool GetSignal(const PathPoint& point, std::string* signal_id) {
   common::PointENU hdmap_point;
   hdmap_point.set_x(point.x());
@@ -97,6 +114,8 @@ void CloseToJunctionTeller::GetOverlaps(const ADCTrajectory& adc_trajectory) {
 
   junction_id_.clear();
   junction_distance_ = -1;
+  crosswalk_id_.clear();
+  crosswalk_distance_ = -1;
   signal_id_.clear();
   signal_distance_ = -1;
   for (const auto& point : adc_trajectory.trajectory_point()) {
@@ -130,6 +149,15 @@ void CloseToJunctionTeller::GetOverlaps(const ADCTrajectory& adc_trajectory) {
       }
     }
 
+    // crosswalk
+    if (crosswalk_id_.empty() || crosswalk_distance_ < 0) {
+      std::string crosswalk_id;
+      if (GetCrosswalk(path_point, &crosswalk_id)) {
+        crosswalk_id_ = crosswalk_id;
+        crosswalk_distance_ = path_point.s() - s_start;
+      }
+    }
+
     // signal
     if (signal_id_.empty() || signal_distance_ < 0) {
       std::string signal_id;
@@ -172,6 +200,19 @@ void CloseToJunctionTeller::Update(Stories* stories) {
     stories->clear_close_to_junction();
   }
 
+  // CloseToCrosswalk
+  if (!crosswalk_id_.empty() && crosswalk_distance_ >= 0) {
+    if (!stories->has_close_to_crosswalk()) {
+      AINFO << "Enter CloseToCrosswalk story";
+    }
+    auto* story = stories->mutable_close_to_crosswalk();
+    story->set_id(crosswalk_id_);
+    story->set_distance(crosswalk_distance_);
+  } else if (stories->has_close_to_crosswalk()) {
+    AINFO << "Exit CloseToCrosswalk story";
+    stories->clear_close_to_crosswalk();
+  }
+
   // CloseToSignal
   if (!signal_id_.empty() && signal_distance_ >= 0) {
     if (!stories->has_close_to_signal()) {
@@ -181,7 +222,7 @@ void CloseToJunctionTeller::Update(Stories* stories) {
     story->set_id(signal_id_);
     story->set_distance(signal_distance_);
   } else if (stories->has_close_to_signal()) {
-    AINFO << "Exit CloseToJunction story";
+    AINFO << "Exit CloseToSignal story";
     stories->clear_close_to_signal();
   }
 }

modules/storytelling/story_tellers/close_to_junction_teller.h

@@ -36,6 +36,8 @@ class CloseToJunctionTeller : public BaseTeller {
   std::string junction_id_;
   CloseToJunction::JunctionType junction_type_;
   double junction_distance_;
+  std::string crosswalk_id_;
+  double crosswalk_distance_;
   std::string signal_id_;
   double signal_distance_;
 };