diff --git a/modules/map/pnc_map/pnc_map.cc b/modules/map/pnc_map/pnc_map.cc index 7b443fdbbdf5f5d2e79f91ffa680abd723d09b22..1413bd822dd634eca5a27739ff4175d1490eea61 100644 --- a/modules/map/pnc_map/pnc_map.cc +++ b/modules/map/pnc_map/pnc_map.cc @@ -248,6 +248,27 @@ PncMap::PncMap(const HDMap *hdmap) : hdmap_(hdmap) {} const hdmap::HDMap *PncMap::hdmap() const { return hdmap_; } +bool PncMap::UpdatePosition(const common::PointENU &point) { + if (!GetNearestPointFromRouting(point, ¤t_waypoint_)) { + AERROR << "Failed to get waypoint from routing"; + return false; + } + auto current_route_index = GetWaypointIndex(current_waypoint_); + if (current_route_index.size() != 3 || current_route_index[0] < 0) { + AERROR << "Failed to get routing index from waypoint"; + return false; + } + + // on the same passage as last time, don't update + if (route_index_.size() != 3 || route_index_[0] != current_route_index[0] || + route_index_[1] != current_route_index[1]) { + passage_start_point_ = point; + } + current_point_ = point; + route_index_ = current_route_index; + return true; +} + bool PncMap::UpdateRoutingResponse(const routing::RoutingResponse &routing) { if (routing_.has_header() && routing.has_header() && routing_.header().sequence_num() == routing.header().sequence_num() && @@ -268,7 +289,10 @@ bool PncMap::UpdateRoutingResponse(const routing::RoutingResponse &routing) { } } } - last_waypoint_.reset(nullptr); + current_waypoint_.lane = nullptr; + route_index_.clear(); + current_point_.Clear(); + passage_start_point_.Clear(); routing_ = routing; return true; } @@ -376,21 +400,15 @@ std::vector PncMap::GetNeighborPassages(const routing::RoadSegment &road, } bool PncMap::GetRouteSegments( - const common::PointENU &point, const double backward_length, - const double forward_length, + const double backward_length, const double forward_length, std::vector *const route_segments) const { - LaneWaypoint waypoint; - if (!GetNearestPointFromRouting(point, &waypoint)) { - AERROR << "Failed to get waypoint from routing"; - return false; - } - auto index = GetWaypointIndex(waypoint); - if (index.size() != 3 || index[0] < 0) { - AERROR << "Failed to get routing index from waypoint"; + if (!current_waypoint_.lane || route_index_.size() != 3 || + route_index_[0] < 0) { + AERROR << "Invalid position, use UpdatePosition() function first"; return false; } - int road_index = index[0]; - int passage_index = index[1]; + const int road_index = route_index_[0]; + const int passage_index = route_index_[1]; const auto &road = routing_.road(road_index); // raw filter to find all neighboring passages auto drive_passages = GetNeighborPassages(road, passage_index); @@ -401,9 +419,10 @@ bool PncMap::GetRouteSegments( ADEBUG << "Failed to convert passage to lane segments."; continue; } - auto nearest_point = point; + auto nearest_point = current_point_; if (index == passage_index) { - nearest_point = waypoint.lane->GetSmoothPoint(waypoint.s); + nearest_point = + current_waypoint_.lane->GetSmoothPoint(current_waypoint_.s); } double s = 0.0; double l = 0.0; @@ -414,7 +433,7 @@ bool PncMap::GetRouteSegments( continue; } // check if possible to drive from current waypoint to the passage. - if (index != passage_index && !segments.CanDriveFrom(waypoint)) { + if (index != passage_index && !segments.CanDriveFrom(current_waypoint_)) { ADEBUG << "You cannot drive from current waypoint to passage: " << index; continue; } diff --git a/modules/map/pnc_map/pnc_map.h b/modules/map/pnc_map/pnc_map.h index 09355f2cc9615a35399543a3daaf3e985d4e7655..14ddd22669d635992accd7a4d3e3f8dd6aa6dd3d 100644 --- a/modules/map/pnc_map/pnc_map.h +++ b/modules/map/pnc_map/pnc_map.h @@ -139,14 +139,14 @@ class PncMap { const hdmap::HDMap *hdmap() const; bool UpdateRoutingResponse(const routing::RoutingResponse &routing_response); + bool UpdatePosition(const common::PointENU &point); const routing::RoutingResponse &routing_response() const; static bool CreatePathFromLaneSegments(const RouteSegments &segments, Path *const path); - bool GetRouteSegments(const common::PointENU &point, - const double backward_length, + bool GetRouteSegments(const double backward_length, const double forward_length, std::vector *const route_segments) const; @@ -195,7 +195,10 @@ class PncMap { private: routing::RoutingResponse routing_; std::unordered_set routing_lane_ids_; - std::unique_ptr last_waypoint_; + LaneWaypoint current_waypoint_; + common::PointENU current_point_; + std::vector route_index_; + common::PointENU passage_start_point_; const hdmap::HDMap *hdmap_ = nullptr; }; diff --git a/modules/map/pnc_map/pnc_map_test.cc b/modules/map/pnc_map/pnc_map_test.cc index 343a63b017accacb6ca2e7431b022992103f6297..d732b479fcc9aee1c946ecb19abfd525aabf2000 100644 --- a/modules/map/pnc_map/pnc_map_test.cc +++ b/modules/map/pnc_map/pnc_map_test.cc @@ -136,7 +136,10 @@ TEST_F(PncMapTest, GetRouteSegments) { ASSERT_TRUE(lane); auto point = lane->GetSmoothPoint(0); std::vector segments; - bool result = pnc_map_->GetRouteSegments(point, 10, 30, &segments); + pnc_map_->route_index_.clear(); + EXPECT_FALSE(pnc_map_->GetRouteSegments(10, 30, &segments)); + EXPECT_TRUE(pnc_map_->UpdatePosition(point)); + bool result = pnc_map_->GetRouteSegments(10, 30, &segments); ASSERT_TRUE(result); ASSERT_EQ(2, segments.size()); EXPECT_NEAR(40, RouteLength(segments[0]), 1e-4); @@ -147,6 +150,44 @@ TEST_F(PncMapTest, GetRouteSegments) { EXPECT_FALSE(segments[1].IsOnSegment()); } +TEST_F(PncMapTest, UpdatePosition) { + auto lane1 = hdmap_.GetLaneById(hdmap::MakeMapId("9_1_-1")); + auto first_point = lane1->GetSmoothPoint(5); + pnc_map_->route_index_.clear(); + pnc_map_->UpdatePosition(first_point); + EXPECT_EQ(3, pnc_map_->route_index_.size()); + EXPECT_EQ(0, pnc_map_->route_index_[0]); + EXPECT_EQ(2, pnc_map_->route_index_[1]); + EXPECT_EQ(0, pnc_map_->route_index_[2]); + + EXPECT_EQ(lane1, pnc_map_->current_waypoint_.lane); + EXPECT_NEAR(5, pnc_map_->current_waypoint_.s, 1e-4); + EXPECT_EQ(first_point.x(), pnc_map_->passage_start_point_.x()); + EXPECT_EQ(first_point.y(), pnc_map_->passage_start_point_.y()); + EXPECT_EQ(first_point.z(), pnc_map_->passage_start_point_.z()); + EXPECT_EQ(first_point.x(), pnc_map_->current_point_.x()); + EXPECT_EQ(first_point.y(), pnc_map_->current_point_.y()); + EXPECT_EQ(first_point.z(), pnc_map_->current_point_.z()); + + auto second_point = lane1->GetSmoothPoint(6); + pnc_map_->UpdatePosition(second_point); + EXPECT_EQ(3, pnc_map_->route_index_.size()); + EXPECT_EQ(0, pnc_map_->route_index_[0]); + EXPECT_EQ(2, pnc_map_->route_index_[1]); + EXPECT_EQ(0, pnc_map_->route_index_[2]); + + EXPECT_EQ(lane1, pnc_map_->current_waypoint_.lane); + EXPECT_NEAR(6, pnc_map_->current_waypoint_.s, 1e-4); + EXPECT_EQ(first_point.x(), pnc_map_->passage_start_point_.x()); + EXPECT_EQ(first_point.y(), pnc_map_->passage_start_point_.y()); + EXPECT_EQ(first_point.z(), pnc_map_->passage_start_point_.z()); + EXPECT_EQ(second_point.x(), pnc_map_->current_point_.x()); + EXPECT_EQ(second_point.y(), pnc_map_->current_point_.y()); + EXPECT_EQ(second_point.z(), pnc_map_->current_point_.z()); + + pnc_map_->route_index_.clear(); +} + TEST_F(PncMapTest, GetNeighborPassages) { const auto& road0 = routing_.road(0); { diff --git a/modules/planning/common/frame.cc b/modules/planning/common/frame.cc index 6d5aa772f76863e3e08fcb26380a7f8bef0b8d73..d169774695c12fb2db14ec795315ae5f2750e414 100644 --- a/modules/planning/common/frame.cc +++ b/modules/planning/common/frame.cc @@ -284,7 +284,12 @@ bool Frame::CreateReferenceLineFromRouting( ? FLAGS_look_forward_distance : FLAGS_look_forward_min_distance; - if (!pnc_map_->GetRouteSegments(position, FLAGS_look_backward_distance, + if (!pnc_map_->UpdatePosition(position)) { + AERROR << "Failed to update position: " << position.ShortDebugString() + << " in pnc map."; + return false; + } + if (!pnc_map_->GetRouteSegments(FLAGS_look_backward_distance, look_forward_distance, &route_segments)) { AERROR << "Failed to extract segments from routing"; return false; diff --git a/modules/planning/reference_line/reference_line_provider.cc b/modules/planning/reference_line/reference_line_provider.cc index 1208dfd7537098e9ab0e330c180e36158704c7e3..0208ba74db3f419120fbc1fab6035485e1a5974d 100644 --- a/modules/planning/reference_line/reference_line_provider.cc +++ b/modules/planning/reference_line/reference_line_provider.cc @@ -138,7 +138,12 @@ bool ReferenceLineProvider::CreateReferenceLineFromRouting( : FLAGS_look_forward_min_distance; { std::lock_guard lock(pnc_map_mutex_); - if (!pnc_map_->GetRouteSegments(position, FLAGS_look_backward_distance, + if (!pnc_map_->UpdatePosition(position)) { + AERROR << "Failed to update pnc_map position: " + << position.ShortDebugString(); + return false; + } + if (!pnc_map_->GetRouteSegments(FLAGS_look_backward_distance, look_forward_distance, &route_segments)) { AERROR << "Failed to extract segments from routing"; return false; @@ -161,8 +166,7 @@ bool ReferenceLineProvider::CreateReferenceLineFromRouting( ReferenceLine raw_reference_line(hdmap_path); ReferenceLine reference_line; if (FLAGS_enable_spiral_reference_line) { - if (!spiral_smoother.Smooth(raw_reference_line, - &reference_line)) { + if (!spiral_smoother.Smooth(raw_reference_line, &reference_line)) { AERROR << "Failed to smooth reference_line with spiral smoother"; } } else {