From cb3f30af69614aae68faa1ef6fcfecb1b5a29285 Mon Sep 17 00:00:00 2001 From: Kevin Ge Date: Tue, 30 Apr 2024 18:56:06 -0500 Subject: [PATCH] fix: warn but don't exit when not on campus.xodr map --- src/mapping/map_management/src/MapManager.cpp | 22 +++++++++++++------ 1 file changed, 15 insertions(+), 7 deletions(-) diff --git a/src/mapping/map_management/src/MapManager.cpp b/src/mapping/map_management/src/MapManager.cpp index 16ef4f5e..bd846966 100755 --- a/src/mapping/map_management/src/MapManager.cpp +++ b/src/mapping/map_management/src/MapManager.cpp @@ -606,15 +606,15 @@ void MapManagementNode::publishGrids(int top_dist, int bottom_dist, int side_dis std::vector lane_shapes_in_range; map_wide_tree_.query(bgi::intersects(search_region), std::back_inserter(lane_shapes_in_range)); - if (lane_shapes_in_range.size() < 1) + + bool has_nearby_lanes = lane_shapes_in_range.size() > 0: + // Warn if there are no nearby lanes. + // This may simply because we are testing in an area that is not on the campus.xodr map. + if (!has_nearby_lanes) { - // RCLCPP_WARN(get_logger(), "There are no lane shapes nearby."); - std::printf("(%f, %f)\n", vehicle_pos.x, vehicle_pos.y); - return; + RCLCPP_WARN(get_logger(), "There are no lane shapes nearby."); } - // std::printf("There are %i shapes in range.\n", lane_shapes_in_range.size()); - int idx = 0; // This was commented out... @@ -725,7 +725,15 @@ void MapManagementNode::publishGrids(int top_dist, int bottom_dist, int side_dis } } - drivable_grid_data.push_back(cell_is_drivable ? 0 : 100); + // If no lanes are are nearby, then the vehicle is assumed to be testing on a route + // area which is not on the campus.xodr map. + // In this case, make the entire drivable grid 0 cost. + if (!has_nearby_lanes) { + drivable_grid_data.push_back(0); + } else { + drivable_grid_data.push_back(cell_is_drivable ? 0 : 100); + } + junction_grid_data.push_back(cell_is_in_junction ? 100 : 0); /* Taking route distance out of the pipeline