Dreamview: fix sim control with rerouting

modules/dreamview/backend/sim_control/sim_control.cc

@@ -65,6 +65,7 @@ SimControl::SimControl(const MapService* map_service)
       next_point_index_(0),
       received_planning_(false),
       planning_count_(-1),
+      re_routing_triggered_(false),
       enabled_(FLAGS_enable_sim_control) {}
 
 void SimControl::Init(bool set_start_point) {
@@ -122,9 +123,12 @@ void SimControl::OnRoutingResponse(const RoutingResponse& routing) {
   CHECK_LE(2, routing.routing_request().waypoint_size());
   const auto& start_pose = routing.routing_request().waypoint(0).pose();
 
+  current_routing_header_ = routing.header();
+
   // If this is from a planning re-routing request, don't reset car's location.
-  if (routing.routing_request().header().module_name() != "planning") {
-    current_routing_header_ = routing.header();
+  re_routing_triggered_ =
+      routing.routing_request().header().module_name() == "planning";
+  if (!re_routing_triggered_) {
     ClearPlanning();
     SetStartPoint(start_pose.x(), start_pose.y());
   }
@@ -143,7 +147,8 @@ void SimControl::Stop() {
 void SimControl::OnPlanning(const apollo::planning::ADCTrajectory& trajectory) {
   // Reset current trajectory and the indices upon receiving a new trajectory.
   // The routing SimControl owns must match with the one Planning has.
-  if (CompareHeader(trajectory.routing_header(), current_routing_header_)) {
+  if (re_routing_triggered_ ||
+      CompareHeader(trajectory.routing_header(), current_routing_header_)) {
     // Hold a few cycles until the position information is fully refreshed on
     // planning side. Don't wait for the very first planning received.
     ++planning_count_;

modules/dreamview/backend/sim_control/sim_control.h

@@ -120,6 +120,8 @@ class SimControl {
   // Number of planning received in terms of one RoutingResponse.
   int planning_count_;
 
+  bool re_routing_triggered_;
+
   // Whether the sim control is enabled.
   bool enabled_;
 

modules/map/hdmap/hdmap.cc

@@ -21,6 +21,7 @@ namespace apollo {
 namespace hdmap {
 
 int HDMap::LoadMapFromFile(const std::string& map_filename) {
+  AINFO << "Loading HDMap: " << map_filename << "...";
   return impl_.LoadMapFromFile(map_filename);
 }
 
@@ -113,8 +114,8 @@ int HDMap::GetRoads(const apollo::common::PointENU& point, double distance,
 }
 
 int HDMap::GetNearestLane(const common::PointENU& point,
-                          LaneInfoConstPtr* nearest_lane,
-                          double* nearest_s, double* nearest_l) const {
+                          LaneInfoConstPtr* nearest_lane, double* nearest_s,
+                          double* nearest_l) const {
   return impl_.GetNearestLane(point, nearest_lane, nearest_s, nearest_l);
 }
 
@@ -125,9 +126,9 @@ int HDMap::GetNearestLaneWithHeading(const apollo::common::PointENU& point,
                                      LaneInfoConstPtr* nearest_lane,
                                      double* nearest_s,
                                      double* nearest_l) const {
-  return impl_.GetNearestLaneWithHeading(
-      point, distance, central_heading, max_heading_difference, nearest_lane,
-      nearest_s, nearest_l);
+  return impl_.GetNearestLaneWithHeading(point, distance, central_heading,
+                                         max_heading_difference, nearest_lane,
+                                         nearest_s, nearest_l);
 }
 
 int HDMap::GetLanesWithHeading(const apollo::common::PointENU& point,
@@ -135,8 +136,8 @@ int HDMap::GetLanesWithHeading(const apollo::common::PointENU& point,
                                const double central_heading,
                                const double max_heading_difference,
                                std::vector<LaneInfoConstPtr>* lanes) const {
-  return impl_.GetLanesWithHeading(
-      point, distance, central_heading, max_heading_difference, lanes);
+  return impl_.GetLanesWithHeading(point, distance, central_heading,
+                                   max_heading_difference, lanes);
 }
 
 int HDMap::GetRoadBoundaries(