Relative Map: added processing for navigation line.

modules/map/relative_map/navigation_lane.cc

@@ -22,7 +22,13 @@
 
 namespace apollo {
 namespace relative_map {
+namespace {
 
+double GetDistance(const common::VehicleState& adc_state,
+                   const common::PathPoint& p) {
+  return std::hypot(adc_state.x() - p.x(), adc_state.y() - p.y());
+}
+}
 using apollo::perception::PerceptionObstacles;
 using apollo::common::VehicleStateProvider;
 
@@ -37,11 +43,19 @@ bool NavigationLane::Update(const PerceptionObstacles& perception_obstacles) {
   // update adc_state_ from VehicleStateProvider
   adc_state_ = VehicleStateProvider::instance()->vehicle_state();
 
-  // TODO(All): consider lane_marker qualities when converting lane_marker to
-  // navigation path
   navigation_path_.Clear();
   auto* path = navigation_path_.mutable_path();
-  ConvertLaneMarkerToPath(perception_obstacles_.lane_marker(), path);
+  const auto& lane_marker = perception_obstacles_.lane_marker();
+
+  const double kQualityThreshold = 0.5;
+  if (navigation_info_.navigation_path_size() > 0 &&
+      std::fmax(lane_marker.left_lane_marker().quality(),
+                lane_marker.right_lane_marker().quality()) <
+          kQualityThreshold) {
+    ConvertNavigationLineToPath();
+  } else {
+    ConvertLaneMarkerToPath(perception_obstacles_.lane_marker(), path);
+  }
   return true;
 }
 
@@ -51,6 +65,51 @@ double NavigationLane::EvaluateCubicPolynomial(const double c0, const double c1,
   return c3 * std::pow(z, 3) + c2 * std::pow(z, 2) + c1 * z + c0;
 }
 
+void NavigationLane::ConvertNavigationLineToPath() {
+  if (navigation_info_.navigation_path_size() == 0 ||
+      !navigation_info_.navigation_path(0).has_path() ||
+      navigation_info_.navigation_path(0).path().path_point_size() == 0) {
+    // path is empty
+    return;
+  }
+  navigation_path_.Clear();
+  auto* curr_path = navigation_path_.mutable_path();
+  UpdateProjectionIndex();
+
+  // TODO(All): support multiple navigation path
+  // only support 1 navigation path
+  const auto& path = navigation_info_.navigation_path(0).path();
+  int curr_project_index = last_project_index_;
+  for (int i = curr_project_index; i < path.path_point_size(); ++i) {
+    auto* point = curr_path->add_path_point();
+    point->CopyFrom(path.path_point(i));
+    const double accumulated_s =
+        path.path_point(i).s() - path.path_point(curr_project_index).s();
+    point->set_s(accumulated_s);
+  }
+}
+
+// project adc_state_ onto path
+void NavigationLane::UpdateProjectionIndex() {
+  // TODO(All): support multiple navigation path
+  // only support 1 navigation path
+  const auto& path = navigation_info_.navigation_path(0).path();
+  int index = 0;
+  double min_d = std::numeric_limits<double>::max();
+  for (int i = last_project_index_; i + 1 < path.path_point_size(); ++i) {
+    const double d = GetDistance(adc_state_, path.path_point(i));
+    if (d < min_d) {
+      min_d = d;
+      index = i;
+    }
+    const double kMaxDistance = 50.0;
+    if (d > kMaxDistance) {
+      break;
+    }
+  }
+  last_project_index_ = index;
+}
+
 void NavigationLane::ConvertLaneMarkerToPath(
     const perception::LaneMarkers& lane_marker, common::Path* path) {
   const auto& left_lane = lane_marker.left_lane_marker();

modules/map/relative_map/navigation_lane.h

@@ -30,6 +30,9 @@ class NavigationLane {
   ~NavigationLane() = default;
 
   bool Update(const perception::PerceptionObstacles& perception_obstacles);
+  void UpdateNavigationInfo(const NavigationInfo& navigation_info) {
+    navigation_info_ = navigation_info;
+  }
 
   const NavigationPath& Path() { return navigation_path_; }
 
@@ -40,10 +43,21 @@ class NavigationLane {
   void ConvertLaneMarkerToPath(const perception::LaneMarkers& lane_marker,
                                common::Path* path);
 
+  void ConvertNavigationLineToPath();
+
+  void UpdateProjectionIndex();
+
+  // received from topic: /apollo/perception_obstacles
   perception::PerceptionObstacles perception_obstacles_;
 
+  // received from topic: /apollo/navigation
+  NavigationInfo navigation_info_;
+
+  // navigation_path_ is the combined results from perception and navigation
   NavigationPath navigation_path_;
 
+  int last_project_index_ = 0;
+
   common::VehicleState adc_state_;
 };
 

modules/map/relative_map/relative_map.cc

@@ -91,6 +91,7 @@ apollo::common::Status RelativeMap::Start() {
   buffer.INFO("RelativeMap started");
 
   AdapterManager::AddPerceptionObstaclesCallback(&RelativeMap::RunOnce, this);
+  AdapterManager::AddNavigationCallback(&RelativeMap::RunOnce, this);
 
   if (AdapterManager::GetPerceptionObstacles()->Empty()) {
     AWARN << "Perception is not ready.";
@@ -107,6 +108,11 @@ void RelativeMap::RunOnce(const PerceptionObstacles& perception_obstacles) {
   Publish(&map_msg);
 }
 
+void RelativeMap::RunOnce(const NavigationInfo& navigation_info) {
+  AdapterManager::Observe();
+  navigation_lane_.UpdateNavigationInfo(navigation_info);
+}
+
 void RelativeMap::CreateMapFromPerception(
     const PerceptionObstacles& perception_obstacles, MapMsg* map_msg) {
   // update vehicle state from localization and chassis

modules/map/relative_map/relative_map.h

@@ -70,6 +70,13 @@ class RelativeMap : public RelativeMapInterface {
   void RunOnce(
       const perception::PerceptionObstacles& perception_obstacles) override;
 
+  /**
+   * @brief Data callback upon receiving a navigation message.
+   * This function is used to update the navigation path.
+   * @param navigation_info received message.
+   */
+  void RunOnce(const NavigationInfo& navigation_info);
+
  private:
   void CreateMapFromPerception(
       const apollo::perception::PerceptionObstacles& perception_obstacles,