Dreamview: draws navigation path (in navigation view) received from backend (#2983)

* Dreamview: draws navigation path received from backend in navigation view

* Dreamview: extract DownsamplePath method and include radar modules to navigation

modules/dreamview/backend/simulation_world/simulation_world_service.cc

@@ -45,13 +45,13 @@ namespace apollo {
 namespace dreamview {
 
 using apollo::canbus::Chassis;
+using apollo::common::PathPoint;
 using apollo::common::Point3D;
 using apollo::common::TrajectoryPoint;
 using apollo::common::VehicleConfigHelper;
 using apollo::common::adapter::AdapterManager;
 using apollo::common::monitor::MonitorMessage;
 using apollo::common::monitor::MonitorMessageItem;
-using apollo::common::PathPoint;
 using apollo::common::time::Clock;
 using apollo::common::time::ToSecond;
 using apollo::common::time::millis;
@@ -70,6 +70,7 @@ using apollo::planning::StopReasonCode;
 using apollo::planning_internal::PlanningData;
 using apollo::prediction::PredictionObstacle;
 using apollo::prediction::PredictionObstacles;
+using apollo::relative_map::NavigationInfo;
 using apollo::routing::RoutingResponse;
 
 using Json = nlohmann::json;
@@ -257,6 +258,7 @@ void SimulationWorldService::Update() {
   UpdateWithLatestObserved("Planning", AdapterManager::GetPlanning());
   UpdateWithLatestObserved("ControlCommand",
                            AdapterManager::GetControlCommand());
+  UpdateWithLatestObserved("Navigation", AdapterManager::GetNavigation());
   for (const auto &kv : obj_map_) {
     *world_.add_object() = kv.second;
   }
@@ -688,6 +690,18 @@ void SimulationWorldService::UpdateDecision(const DecisionResult &decision_res,
   }
 }
 
+void SimulationWorldService::DownsamplePath(
+    const apollo::common::Path &path, apollo::common::Path *downsampled_path) {
+  auto sampled_indices = DownsampleByAngle(path.path_point(), kAngleThreshold);
+
+  downsampled_path->set_name(path.name());
+  for (int index : sampled_indices) {
+    const auto &path_point = path.path_point()[index];
+    auto *point = downsampled_path->add_path_point();
+    point->CopyFrom(path_point);
+  }
+}
+
 void SimulationWorldService::UpdatePlanningData(const PlanningData &data) {
   auto *planning_data = world_.mutable_planning_data();
 
@@ -756,17 +770,7 @@ void SimulationWorldService::UpdatePlanningData(const PlanningData &data) {
   // Update path
   planning_data->clear_path();
   for (auto &path : data.path()) {
-    // Downsample the path points for frontend display.
-    auto sampled_indices =
-        DownsampleByAngle(path.path_point(), kAngleThreshold);
-
-    auto *downsampled_path = planning_data->add_path();
-    downsampled_path->set_name(path.name());
-    for (int index : sampled_indices) {
-      const auto &path_point = path.path_point()[index];
-      auto *point = downsampled_path->add_path_point();
-      point->CopyFrom(path_point);
-    }
+    DownsamplePath(path, planning_data->add_path());
   }
 }
 
@@ -938,5 +942,16 @@ void SimulationWorldService::UpdateSimulationWorld(
   }
 }
 
+template <>
+void SimulationWorldService::UpdateSimulationWorld(
+    const NavigationInfo &navigation_info) {
+  world_.clear_navigation_path();
+  for (auto &navigation_path : navigation_info.navigation_path()) {
+    if (navigation_path.has_path()) {
+      DownsamplePath(navigation_path.path(), world_.add_navigation_path());
+    }
+  }
+}
+
 }  // namespace dreamview
 }  // namespace apollo

modules/dreamview/backend/simulation_world/simulation_world_service.h

@@ -37,6 +37,7 @@
 #include "modules/common/adapters/adapter_manager.h"
 #include "modules/common/log.h"
 #include "modules/common/monitor_log/monitor_log_buffer.h"
+#include "modules/common/proto/pnc_point.pb.h"
 
 /**
  * @namespace apollo::dreamview
@@ -174,6 +175,10 @@ class SimulationWorldService {
   void CreatePredictionTrajectory(
       const apollo::prediction::PredictionObstacle &obstacle,
       Object *world_object);
+
+  void DownsamplePath(const apollo::common::Path &paths,
+                      apollo::common::Path *downsampled_path);
+
   void UpdatePlanningData(const apollo::planning_internal::PlanningData &data);
 
   void PopulateMapInfo(double radius);

modules/dreamview/frontend/gen_pbjs.sh

@@ -35,11 +35,9 @@ node_modules/protobufjs/bin/pbjs -t json ../proto/simulation_world.proto \
     -o proto_bundle/sim_world_proto_bundle.json
 
 node_modules/protobufjs/bin/pbjs -t json $MAP_PROTOS \
-    $REALTIVE_MAP_PROTOS \
     ../../common/proto/geometry.proto \
     ../../common/proto/header.proto \
     ../../common/proto/error_code.proto \
-    ../../common/proto/pnc_point.proto \
     -o proto_bundle/map_proto_bundle.json
 
 node_modules/protobufjs/bin/pbjs -t json ../proto/point_cloud.proto \

modules/dreamview/frontend/proto_bundle/map_proto_bundle.json

@@ -799,100 +799,6 @@
             }
           }
         },
-        "relative_map": {
-          "nested": {
-            "NavigationPath": {
-              "fields": {
-                "path": {
-                  "type": "apollo.common.Path",
-                  "id": 1
-                },
-                "pathPriority": {
-                  "type": "uint32",
-                  "id": 2
-                }
-              }
-            },
-            "NavigationInfo": {
-              "fields": {
-                "header": {
-                  "type": "apollo.common.Header",
-                  "id": 1
-                },
-                "navigationPath": {
-                  "rule": "repeated",
-                  "type": "NavigationPath",
-                  "id": 2
-                }
-              }
-            },
-            "MapMsg": {
-              "fields": {
-                "header": {
-                  "type": "apollo.common.Header",
-                  "id": 1
-                },
-                "hdmap": {
-                  "type": "apollo.hdmap.Map",
-                  "id": 2
-                },
-                "navigationPath": {
-                  "keyType": "string",
-                  "type": "NavigationPath",
-                  "id": 3
-                }
-              }
-            },
-            "MapGenerationParam": {
-              "fields": {
-                "defaultLeftWidth": {
-                  "type": "double",
-                  "id": 1,
-                  "options": {
-                    "default": 1.75
-                  }
-                },
-                "defaultRightWidth": {
-                  "type": "double",
-                  "id": 2,
-                  "options": {
-                    "default": 1.75
-                  }
-                },
-                "defaultSpeedLimit": {
-                  "type": "double",
-                  "id": 3,
-                  "options": {
-                    "default": 29.0576
-                  }
-                }
-              }
-            },
-            "NavigationLaneConfig": {
-              "fields": {
-                "minLaneMarkerQuality": {
-                  "type": "double",
-                  "id": 1,
-                  "options": {
-                    "default": 0.5
-                  }
-                }
-              }
-            },
-            "RelativeMapConfig": {
-              "fields": {
-                "mapParam": {
-                  "type": "MapGenerationParam",
-                  "id": 1
-                },
-                "navigationLane": {
-                  "type": "NavigationLaneConfig",
-                  "id": 2
-                }
-              }
-            }
-          }
-        },
         "common": {
           "nested": {
             "PointENU": {
@@ -1108,135 +1014,6 @@
                   "id": 2
                 }
               }
-            },
-            "SLPoint": {
-              "fields": {
-                "s": {
-                  "type": "double",
-                  "id": 1
-                },
-                "l": {
-                  "type": "double",
-                  "id": 2
-                }
-              }
-            },
-            "FrenetFramePoint": {
-              "fields": {
-                "s": {
-                  "type": "double",
-                  "id": 1
-                },
-                "l": {
-                  "type": "double",
-                  "id": 2
-                },
-                "dl": {
-                  "type": "double",
-                  "id": 3
-                },
-                "ddl": {
-                  "type": "double",
-                  "id": 4
-                }
-              }
-            },
-            "SpeedPoint": {
-              "fields": {
-                "s": {
-                  "type": "double",
-                  "id": 1
-                },
-                "t": {
-                  "type": "double",
-                  "id": 2
-                },
-                "v": {
-                  "type": "double",
-                  "id": 3
-                },
-                "a": {
-                  "type": "double",
-                  "id": 4
-                },
-                "da": {
-                  "type": "double",
-                  "id": 5
-                }
-              }
-            },
-            "PathPoint": {
-              "fields": {
-                "x": {
-                  "type": "double",
-                  "id": 1
-                },
-                "y": {
-                  "type": "double",
-                  "id": 2
-                },
-                "z": {
-                  "type": "double",
-                  "id": 3
-                },
-                "theta": {
-                  "type": "double",
-                  "id": 4
-                },
-                "kappa": {
-                  "type": "double",
-                  "id": 5
-                },
-                "s": {
-                  "type": "double",
-                  "id": 6
-                },
-                "dkappa": {
-                  "type": "double",
-                  "id": 7
-                },
-                "ddkappa": {
-                  "type": "double",
-                  "id": 8
-                },
-                "laneId": {
-                  "type": "string",
-                  "id": 9
-                }
-              }
-            },
-            "Path": {
-              "fields": {
-                "name": {
-                  "type": "string",
-                  "id": 1
-                },
-                "pathPoint": {
-                  "rule": "repeated",
-                  "type": "PathPoint",
-                  "id": 2
-                }
-              }
-            },
-            "TrajectoryPoint": {
-              "fields": {
-                "pathPoint": {
-                  "type": "PathPoint",
-                  "id": 1
-                },
-                "v": {
-                  "type": "double",
-                  "id": 2
-                },
-                "a": {
-                  "type": "double",
-                  "id": 3
-                },
-                "relativeTime": {
-                  "type": "double",
-                  "id": 4
-                }
-              }
             }
           }
         }

modules/dreamview/frontend/proto_bundle/sim_world_proto_bundle.json

@@ -491,6 +491,11 @@
                 "controlData": {
                   "type": "ControlData",
                   "id": 22
+                },
+                "navigationPath": {
+                  "rule": "repeated",
+                  "type": "apollo.common.Path",
+                  "id": 23
                 }
               }
             }

modules/dreamview/frontend/src/components/Navigation/GmapNavigator.js

 import polyval from "compute-polynomial";
-import protobuf from "protobufjs/light";
 import { UTMToWGS84 } from "utils/coordinate_converter";
 
-const root = protobuf.Root.fromJSON(
-  require("proto_bundle/map_proto_bundle.json")
-);
-const navInfoMessage = root.lookupType("apollo.relative_map.NavigationInfo");
-
 class GmapNavigator {
   constructor() {
     this.WS = null;
@@ -91,11 +85,10 @@ class GmapNavigator {
 
   update(data) {
     const adc = data.autoDrivingCar;
-    if (adc) {
-      this.updateCenterVehicle(adc);
-      this.updateLaneMarkers(adc, data.laneMarker);
-      this.updatePlanningPath(adc, data.planningTrajectory);
-    }
+    this.updateCenterVehicle(adc);
+    this.updateLaneMarkers(adc, data.laneMarker);
+    this.updateNavigationPath(data.navigationPath);
+    this.updatePlanningPath(adc, data.planningTrajectory);
   }
 
   updateCenterVehicle(autoDrivingCar) {
@@ -212,6 +205,38 @@ class GmapNavigator {
     this.plannedPath.setMap(this.map);
   }
 
+  updateNavigationPath(navigationPaths) {
+    if (!navigationPaths) {
+      return;
+    }
+
+    const paths = navigationPaths.map(navigationPath => {
+      return navigationPath.pathPoint.map(point => {
+        const [lon, lat] = UTMToWGS84(point.x, point.y);
+        return { lat: lat, lng: lon };
+      });
+    });
+
+    if (this.routingPath) {
+      this.routingPath.forEach(path => {
+        path.setMap(null);
+      });
+      this.routingPath = [];
+    }
+    paths.forEach(path => {
+      this.routingPath.push(
+        new google.maps.Polyline({
+          path: path,
+          geodesic: true,
+          strokeColor: "#cd5c5c",
+          strokeOpacity: 0.8,
+          strokeWeight: 6,
+          map: this.map
+        })
+      );
+    });
+  }
+
   createCenterControl(controlDiv) {
     const map = this.map;
 
@@ -257,61 +282,26 @@ class GmapNavigator {
       return;
     }
 
-    const url =
-      "http://navi-env.axty8vi3ic.us-west-2.elasticbeanstalk.com" +
-      "/dreamview/navigation" +
-      `?origin=${startLat},${startLng}` +
-      `&destination=${endLat},${endLng}` +
-      "&heading=0";
+    const url = "http://navi-env.axty8vi3ic.us-west-2.elasticbeanstalk.com" +
+        "/dreamview/navigation" +
+        `?origin=${startLat},${startLng}` +
+        `&destination=${endLat},${endLng}` +
+        "&heading=0";
     fetch(url, {
-      method: "GET",
-      mode: "cors"
-    })
-      .then(response => {
+        method: "GET",
+        mode: "cors"
+    }).then(response => {
         return response.arrayBuffer();
-      })
-      .then(response => {
+    }).then(response => {
         if (!response.byteLength) {
           console.warn("No navigation info received.");
           return;
         }
 
         this.WS.publishNavigationInfo(response);
-
-        const navigationInfo = navInfoMessage.toObject(
-          navInfoMessage.decode(new Uint8Array(response)),
-          { enums: String }
-        );
-
-        const paths = navigationInfo.navigationPath.map(naviPath => {
-          return naviPath.path.pathPoint.map(point => {
-            const [lon, lat] = UTMToWGS84(point.x, point.y);
-            return { lat: lat, lng: lon };
-          });
-        });
-
-        if (this.routingPath) {
-          this.routingPath.forEach(path => {
-            path.setMap(null);
-          });
-          this.routingPath = [];
-        }
-        paths.forEach(path => {
-          this.routingPath.push(
-            new google.maps.Polyline({
-              path: paths[0],
-              geodesic: true,
-              strokeColor: "#cd5c5c",
-              strokeOpacity: 0.8,
-              strokeWeight: 6,
-              map: this.map
-            })
-          );
-        });
-      })
-      .catch(error => {
+    }).catch(error => {
         console.error("Failed to retrieve navigation data:", error);
-      });
+    });
   }
 
   createControlElement(parentDiv, backgroundColor, text) {

modules/dreamview/proto/BUILD

@@ -12,6 +12,7 @@ proto_library(
     srcs = ["simulation_world.proto"],
     deps = [
         "//modules/common/monitor_log/proto:monitor_log_proto_lib",
+        "//modules/common/proto:pnc_point_proto_lib",
         "//modules/planning/proto:planning_internal_proto_lib",
         "//modules/perception/proto:perception_proto_lib",
     ],

modules/dreamview/proto/simulation_world.proto

@@ -5,6 +5,7 @@ package apollo.dreamview;
 import "modules/common/monitor_log/proto/monitor_log.proto";
 import "modules/planning/proto/planning_internal.proto";
 import "modules/perception/proto/perception_obstacle.proto";
+import "modules/common/proto/pnc_point.proto";
 
 // Next-id: 4
 message PolygonPoint {
@@ -174,7 +175,7 @@ message ControlData {
   optional double station_error = 2;
 }
 
-// Next-id: 23
+// Next-id: 24
 message SimulationWorld {
   // Timestamp in milliseconds
   optional double timestamp = 1;
@@ -232,4 +233,7 @@ message SimulationWorld {
 
   // Control data
   optional ControlData control_data = 22;
+
+  // Relative Map
+  repeated apollo.common.Path navigation_path = 23;
 }