navi: added lidar based perception into navigation mode

modules/dreamview/conf/hmi.conf

@@ -509,19 +509,21 @@ modes {
 modes {
   key: "Navigation"
   value: {
+    live_modules: "navigation_camera"
+    live_modules: "navigation_perception"
+    live_modules: "mobileye"
+    live_modules: "third_party_perception"
+    live_modules: "velodyne"
+    live_modules: "perception"
     live_modules: "canbus"
     live_modules: "GPS"
-    live_modules: "mobileye"
     live_modules: "radar"
-    live_modules: "navigation_camera"
     live_modules: "navigation_localization"
     live_modules: "relative_map"
     live_modules: "navigation_planning"
     live_modules: "navigation_prediction"
     live_modules: "navigation_control"
     live_modules: "guardian"
-    live_modules: "navigation_perception"
-    live_modules: "third_party_perception"
     live_modules: "record_bag"
     live_hardware: "GPS"
     live_hardware: "CAN"

modules/map/relative_map/navigation_lane.cc

@@ -668,8 +668,27 @@ bool NavigationLane::CreateMap(const MapGenerationParam &map_config,
     }
   }
 
-  // Set neighbor information for each lane
   int lane_num = hdmap->lane_size();
+
+  // Set road boundary
+  auto *road = hdmap->add_road();
+  road->mutable_id()->set_id("road_" + hdmap->lane(0).id().id());
+  auto *section = road->add_section();
+  for (int i = 0; i < lane_num; ++i) {
+    auto *lane_id = section->add_lane_id();
+    lane_id->CopyFrom(hdmap->lane(0).id());
+  }
+  auto *outer_polygon = section->mutable_boundary()->mutable_outer_polygon();
+  auto *left_edge = outer_polygon->add_edge();
+  left_edge->set_type(apollo::hdmap::BoundaryEdge::LEFT_BOUNDARY);
+  left_edge->mutable_curve()->CopyFrom(hdmap->lane(0).left_boundary().curve());
+
+  auto *right_edge = outer_polygon->add_edge();
+  right_edge->set_type(apollo::hdmap::BoundaryEdge::RIGHT_BOUNDARY);
+  right_edge->mutable_curve()->
+      CopyFrom(hdmap->lane(lane_num - 1).right_boundary().curve());
+
+  // Set neighbor information for each lane
   if (lane_num < 2) {
     return true;
   }
@@ -684,7 +703,6 @@ bool NavigationLane::CreateMap(const MapGenerationParam &map_config,
           hdmap->lane(i + 1).id());
     }
   }
-
   return true;
 }
 

modules/perception/conf/adapter.conf

@@ -61,5 +61,9 @@ config {
   mode: PUBLISH_ONLY
   message_history_limit: 50
 }
-
+config {
+  type: RELATIVE_MAP
+  mode: RECEIVE_ONLY
+  message_history_limit: 1
+}
 is_ros: true

modules/perception/obstacle/fusion/probabilistic_fusion/probabilistic_fusion.cc

@@ -284,7 +284,7 @@ void ProbabilisticFusion::FuseForegroundObjects(
                          sensor_type, sensor_id, timestamp);
 
   if (FLAGS_use_navigation_mode) {
-    if (is_camera(sensor_type)) {
+    if (is_camera(sensor_type) || is_lidar(sensor_type)) {
       CreateNewTracks(*foreground_objects, unassigned_objects);
     }
   } else {

modules/perception/obstacle/onboard/lidar_process_subnode.cc

@@ -125,6 +125,9 @@ void LidarProcessSubnode::OnPointCloud(
   // error_code_ = common::OK;
 
   /// call hdmap to get ROI
+  if (FLAGS_use_navigation_mode) {
+    AdapterManager::Observe();
+  }
   HdmapStructPtr hdmap = nullptr;
   if (hdmap_input_) {
     PointD velodyne_pose = {0.0, 0.0, 0.0, 0};  // (0,0,0)

modules/perception/onboard/transform_input.cc

@@ -83,8 +83,11 @@ bool GetVelodyneTrans(const double query_time, Eigen::Matrix4d* trans) {
   Eigen::Affine3d affine_localization_3d;
   tf::transformMsgToEigen(transform_stamped.transform, affine_localization_3d);
   Eigen::Matrix4d novatel2world_trans = affine_localization_3d.matrix();
-
-  *trans = novatel2world_trans * lidar2novatel_trans;
+  if (FLAGS_use_navigation_mode) {
+    *trans = lidar2novatel_trans;
+  } else {
+    *trans = novatel2world_trans * lidar2novatel_trans;
+  }
   ADEBUG << "get " << FLAGS_lidar_tf2_frame_id << " to "
          << FLAGS_localization_tf2_child_frame_id << " trans: " << *trans;
   return true;