dreamview: use heading to choose lane when start point is car's offset. (#2036)

* dreamview: use heading to choose lane when start point is car's offset.

* dreamview: fix the coding style.

* dreamview: verify routing point when using heading.

* dreamview: fix codes and resolve conflicts.

* dreamview: fix the pointer.

modules/dreamview/backend/map/map_service.cc

@@ -336,6 +336,26 @@ bool MapService::GetNearestLane(const double x, const double y,
   return true;
 }
 
+bool MapService::GetNearestLaneWithHeading(const double x, const double y,
+                                LaneInfoConstPtr *nearest_lane,
+                                double *nearest_s, double *nearest_l,
+                                const double heading) const {
+  boost::shared_lock<boost::shared_mutex> reader_lock(mutex_);
+
+  PointENU point;
+  point.set_x(x);
+  point.set_y(y);
+  static constexpr double kSearchRadius = 1.0;
+  static constexpr double kMaxHeadingDiff = 1.0;
+  if (!MapReady() || HDMap()->GetNearestLaneWithHeading(
+                         point, kSearchRadius, heading, kMaxHeadingDiff,
+                         nearest_lane, nearest_s, nearest_l) < 0) {
+    AERROR << "Failed to get nearest lane with heading.";
+    return false;
+  }
+  return true;
+}
+
 bool MapService::GetPathsFromRouting(const RoutingResponse &routing,
                                      std::vector<Path> *paths) const {
   if (!CreatePathsFromRouting(routing, paths)) {
@@ -365,11 +385,7 @@ bool MapService::ConstructLaneWayPoint(
     return false;
   }
 
-  if (lane->lane().type() != Lane::CITY_DRIVING) {
-    AERROR
-        << "Failed to construct LaneWayPoint for RoutingRequest: Expected lane "
-        << lane->id().id() << " to be CITY_DRIVING, but was "
-        << apollo::hdmap::Lane::LaneType_Name(lane->lane().type());
+  if (!CheckRoutingPointLaneType(lane)) {
     return false;
   }
 
@@ -382,6 +398,39 @@ bool MapService::ConstructLaneWayPoint(
   return true;
 }
 
+bool MapService::ConstructLaneWayPointWithHeading(
+    const double x, const double y, const double heading,
+    routing::LaneWaypoint *laneWayPoint) const {
+  double s, l;
+  LaneInfoConstPtr lane;
+  if (!GetNearestLaneWithHeading(x, y, &lane, &s, &l, heading)) {
+    return false;
+  }
+
+  if (!CheckRoutingPointLaneType(lane)) {
+    return false;
+  }
+
+  laneWayPoint->set_id(lane->id().id());
+  laneWayPoint->set_s(s);
+  auto *pose = laneWayPoint->mutable_pose();
+  pose->set_x(x);
+  pose->set_y(y);
+
+  return true;
+}
+
+bool MapService::CheckRoutingPointLaneType(LaneInfoConstPtr lane) const {
+  if (lane->lane().type() != Lane::CITY_DRIVING) {
+    AERROR
+        << "Failed to construct LaneWayPoint for RoutingRequest: Expected lane "
+        << lane->id().id() << " to be CITY_DRIVING, but was "
+        << apollo::hdmap::Lane::LaneType_Name(lane->lane().type());
+    return false;
+  }
+  return true;
+}
+
 bool MapService::GetStartPoint(apollo::common::PointENU *start_point) const {
   // Start from origin to find a lane from the map.
   double s, l;

modules/dreamview/backend/map/map_service.h

@@ -76,6 +76,12 @@ class MapService {
   bool ConstructLaneWayPoint(const double x, const double y,
                              routing::LaneWaypoint *laneWayPoint) const;
 
+  bool ConstructLaneWayPointWithHeading(
+      const double x, const double y, const double heading,
+      routing::LaneWaypoint *laneWayPoint) const;
+
+  bool CheckRoutingPointLaneType(apollo::hdmap::LaneInfoConstPtr lane) const;
+
   // Reload map from current FLAGS_map_dir.
   bool ReloadMap(bool force_reload);
 
@@ -87,6 +93,11 @@ class MapService {
                       apollo::hdmap::LaneInfoConstPtr *nearest_lane,
                       double *nearest_s, double *nearest_l) const;
 
+  bool GetNearestLaneWithHeading(const double x, const double y,
+                      apollo::hdmap::LaneInfoConstPtr *nearest_lane,
+                      double *nearest_s, double *nearest_l,
+                      const double heading) const;
+
   bool CreatePathsFromRouting(const routing::RoutingResponse &routing,
                               std::vector<apollo::hdmap::Path> *paths) const;
 

modules/dreamview/backend/simulation_world/simulation_world_service.cc

@@ -1163,5 +1163,11 @@ void SimulationWorldService::PublishRoutingRequest(
   routing_request_writer_->Write(routing_request);
 }
 
+void SimulationWorldService::PublishMonitorMessage(
+  apollo::common::monitor::MonitorMessageItem::LogLevel log_level,
+  const std::string &msg) {
+    monitor_logger_buffer_.AddMonitorMsgItem(log_level, msg);
+    monitor_logger_buffer_.Publish();
+}
 }  // namespace dreamview
 }  // namespace apollo

modules/dreamview/backend/simulation_world/simulation_world_service.h

@@ -137,9 +137,7 @@ class SimulationWorldService {
    */
   void PublishMonitorMessage(
       apollo::common::monitor::MonitorMessageItem::LogLevel log_level,
-      const std::string &msg) {
-    monitor_logger_buffer_.AddMonitorMsgItem(log_level, msg);
-  }
+      const std::string &msg);
 
   void PublishNavigationInfo(
       const std::shared_ptr<apollo::relative_map::NavigationInfo> &);

modules/dreamview/backend/simulation_world/simulation_world_updater.cc

@@ -257,11 +257,21 @@ bool SimulationWorldUpdater::ConstructRoutingRequest(
     AERROR << "Failed to prepare a routing request: invalid start point.";
     return false;
   }
-  if (!map_service_->ConstructLaneWayPoint(start["x"], start["y"],
-                                           routing_request->add_waypoint())) {
-    AERROR << "Failed to prepare a routing request:"
-           << " cannot locate start point on map.";
-    return false;
+  if (ContainsKey(start, "heading")) {
+    if (!map_service_->ConstructLaneWayPointWithHeading(
+            start["x"], start["y"], start["heading"],
+            routing_request->add_waypoint())) {
+      AERROR << "Failed to prepare a routing request with heading: "
+             << start["heading"] << " cannot locate start point on map.";
+      return false;
+    }
+  } else {
+    if (!map_service_->ConstructLaneWayPoint(start["x"], start["y"],
+                                             routing_request->add_waypoint())) {
+      AERROR << "Failed to prepare a routing request:"
+             << " cannot locate start point on map.";
+      return false;
+    }
   }
 
   // set way point(s) if any

modules/dreamview/frontend/proto_bundle/sim_world_proto_bundle.json

@@ -2583,6 +2583,13 @@
                   "options": {
                     "default": 6
                   }
+                },
+                "maxToleranceT": {
+                  "type": "double",
+                  "id": 5,
+                  "options": {
+                    "default": 0.5
+                  }
                 }
               }
             },

modules/dreamview/frontend/src/renderer/index.js

@@ -279,6 +279,7 @@ class Renderer {
 
     sendRoutingRequest() {
         return this.routingEditor.sendRoutingRequest(this.adc.mesh.position,
+                                                     this.adc.mesh.rotation.y,
                                                      this.coordinates);
     }
 

modules/dreamview/frontend/src/renderer/routing_editor.js

@@ -73,7 +73,7 @@ export default class RoutingEditor {
         this.routePoints = [];
     }
 
-    sendRoutingRequest(carOffsetPosition, coordinates) {
+    sendRoutingRequest(carOffsetPosition, carHeading, coordinates) {
         if (this.routePoints.length === 0) {
             alert("Please provide at least an end point.");
             return false;
@@ -86,9 +86,10 @@ export default class RoutingEditor {
 
         const start    = (points.length > 1) ? points[0]
                          : coordinates.applyOffset(carOffsetPosition, true);
+        const start_heading  = (points.length > 1) ? null : carHeading;
         const end      = points[points.length-1];
         const waypoint = (points.length > 1) ? points.slice(1,-1) : [];
-        WS.requestRoute(start, waypoint, end, this.parkingSpaceId);
+        WS.requestRoute(start, start_heading, waypoint, end, this.parkingSpaceId);
 
         return true;
     }

modules/dreamview/frontend/src/store/websocket/websocket_realtime.js

@@ -166,7 +166,7 @@ export default class RealtimeWebSocketEndpoint {
         }));
     }
 
-    requestRoute(start, waypoint, end, parkingSpaceId) {
+    requestRoute(start, start_heading, waypoint, end, parkingSpaceId) {
         const request = {
             type: "SendRoutingRequest",
             start: start,
@@ -178,6 +178,9 @@ export default class RealtimeWebSocketEndpoint {
             request.parkingSpaceId = parkingSpaceId;
         }
 
+        if (start_heading) {
+            request.start.heading = start_heading;
+        }
         this.websocket.send(JSON.stringify(request));
     }