diff --git a/modules/dreamview/conf/hmi.conf b/modules/dreamview/conf/hmi.conf index 98a7167ce8c670340d956d158465893e354cc254..0a97a4c0e2d302f59bd34a5116c940e076fb5a98 100644 --- a/modules/dreamview/conf/hmi.conf +++ b/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" diff --git a/modules/map/relative_map/navigation_lane.cc b/modules/map/relative_map/navigation_lane.cc index 28b24a520d929f63aa686576bf1aaa60e437f73a..0dc87db44c471e4b09004a62f7ffb274c2d49e98 100644 --- a/modules/map/relative_map/navigation_lane.cc +++ b/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; } diff --git a/modules/perception/conf/adapter.conf b/modules/perception/conf/adapter.conf index 1296d1bee7ffe461b987eaa207c5353247d3412e..4d772a4f83f47073e10fe2e43209a964fb065840 100644 --- a/modules/perception/conf/adapter.conf +++ b/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 diff --git a/modules/perception/obstacle/fusion/probabilistic_fusion/probabilistic_fusion.cc b/modules/perception/obstacle/fusion/probabilistic_fusion/probabilistic_fusion.cc index e074ff1a1d097ce405394df9025a2f22e2ddd736..b54a3523926cfaba8cd8d13d486241e55b7e4115 100644 --- a/modules/perception/obstacle/fusion/probabilistic_fusion/probabilistic_fusion.cc +++ b/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 { diff --git a/modules/perception/obstacle/onboard/lidar_process_subnode.cc b/modules/perception/obstacle/onboard/lidar_process_subnode.cc index f30c2d7e28d3b2f054f5bbd56dbed01b44461593..97d77a588ab5bf3e96688555d7c5ff9b61beedfa 100644 --- a/modules/perception/obstacle/onboard/lidar_process_subnode.cc +++ b/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) diff --git a/modules/perception/onboard/transform_input.cc b/modules/perception/onboard/transform_input.cc index f42e445fc737ba5cd6947a39c0f709aa1da9d0e8..937b72e74398b839b95b32360bef7ca473985ab9 100644 --- a/modules/perception/onboard/transform_input.cc +++ b/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;