diff --git a/modules/planning/reference_line/reference_line_provider.cc b/modules/planning/reference_line/reference_line_provider.cc index 7cbed3504ba1069855bbd779af6be31a4119fbcc..0f51d044b572e803a6a46ea02830c945ce02d5c8 100644 --- a/modules/planning/reference_line/reference_line_provider.cc +++ b/modules/planning/reference_line/reference_line_provider.cc @@ -129,23 +129,9 @@ bool ReferenceLineProvider::IsAllowChangeLane( bool ReferenceLineProvider::UpdateRoutingResponse( const routing::RoutingResponse &routing) { - bool is_new_routing = false; - { - std::lock_guard lock(pnc_map_mutex_); - is_new_routing = pnc_map_->IsNewRouting(routing); - if (is_new_routing) { - if (!pnc_map_->UpdateRoutingResponse(routing)) { - AERROR << "Failed to update routing in pnc map"; - return false; - } - has_routing_ = true; - } - } - if (is_new_routing) { - std::lock_guard lock(segment_history_mutex_); - segment_history_.clear(); - segment_history_id_.clear(); - } + std::lock_guard lock(routing_mutex_); + routing_ = routing; + has_routing_ = true; return true; } @@ -331,7 +317,26 @@ bool ReferenceLineProvider::CreateReferenceLine( std::lock_guard lock(vehicle_state_mutex_); vehicle_state = vehicle_state_; } + + routing::RoutingResponse routing; { + std::lock_guard lock(routing_mutex_); + routing = routing_; + } + { + // Update routing in pnc_map + if (pnc_map_->IsNewRouting(routing)) { + if (!pnc_map_->UpdateRoutingResponse(routing)) { + AERROR << "Failed to update routing in pnc map"; + return false; + } + + std::lock_guard lock(segment_history_mutex_); + segment_history_.clear(); + segment_history_id_.clear(); + } + + // Update vehicle state in pnc_map std::lock_guard lock(pnc_map_mutex_); if (!pnc_map_->UpdateVehicleState(vehicle_state)) { AERROR << "Failed to update vehicle state in pnc map"; diff --git a/modules/planning/reference_line/reference_line_provider.h b/modules/planning/reference_line/reference_line_provider.h index f0e1cfd63fbc3b9862fe87dc8fd0e5c49e7a1ffd..2e7bc29918d9cd0e4aefb091d126b94a5f2075fa 100644 --- a/modules/planning/reference_line/reference_line_provider.h +++ b/modules/planning/reference_line/reference_line_provider.h @@ -149,7 +149,10 @@ class ReferenceLineProvider { std::mutex vehicle_state_mutex_; common::VehicleState vehicle_state_; + std::mutex routing_mutex_; + routing::RoutingResponse routing_; bool has_routing_ = false; + struct SegmentHistory { double min_l = 0.0; double accumulate_s = 0.0;