Dreamview: Add a dummy start point for SimControl (#1364)

modules/dreamview/backend/map/map_service.cc

@@ -233,6 +233,19 @@ Map MapService::RetrieveMapElements(const MapElementIds &ids) const {
   return result;
 }
 
+bool MapService::GetNearestLane(const double x, const double y,
+                                LaneInfoConstPtr *nearest_lane,
+                                double *nearest_s, double *nearest_l) const {
+  PointENU point;
+  point.set_x(x);
+  point.set_y(y);
+  if (BaseMap().GetNearestLane(point, nearest_lane, nearest_s, nearest_l) < 0) {
+    AERROR << "Failed to get nearest lane!";
+    return false;
+  }
+  return true;
+}
+
 bool MapService::GetPointsFromRouting(const RoutingResponse &routing,
                                       std::vector<MapPathPoint> *points) const {
   Path path;
@@ -253,13 +266,9 @@ bool MapService::GetPointsFromRouting(const RoutingResponse &routing,
 
 bool MapService::GetPoseWithRegardToLane(const double x, const double y,
                                          double *theta, double *s) const {
-  apollo::common::PointENU point;
-  point.set_x(x);
-  point.set_y(y);
   double l;
   LaneInfoConstPtr nearest_lane;
-  if (BaseMap().GetNearestLane(point, &nearest_lane, s, &l) < 0) {
-    AERROR << "Failed to get nearest lane!";
+  if (!GetNearestLane(x, y, &nearest_lane, s, &l)) {
     return false;
   }
 
@@ -269,26 +278,33 @@ bool MapService::GetPoseWithRegardToLane(const double x, const double y,
 
 bool MapService::ConstructLaneWayPoint(
     const double x, const double y,
-    RoutingRequest::LaneWaypoint* laneWayPoint) const {
-
-  apollo::common::PointENU point;
-  point.set_x(x);
-  point.set_y(y);
-
+    RoutingRequest::LaneWaypoint *laneWayPoint) const {
   double s, l;
   LaneInfoConstPtr lane;
-  if (BaseMap().GetNearestLane(point, &lane, &s, &l) < 0) {
-    AERROR << "Failed to get nearest lane!";
+  if (!GetNearestLane(x, y, &lane, &s, &l)) {
     return false;
   }
 
   laneWayPoint->set_id(lane->id().id());
   laneWayPoint->set_s(s);
-  auto* pose = laneWayPoint->mutable_pose();
+  auto *pose = laneWayPoint->mutable_pose();
   pose->set_x(x);
   pose->set_y(y);
 
   return true;
 }
+
+bool MapService::GetStartPoint(apollo::common::PointENU *start_point) const {
+  // Start from origin to find a lane from the map.
+  double s, l;
+  LaneInfoConstPtr lane;
+  if (!GetNearestLane(0.0, 0.0, &lane, &s, &l)) {
+    return false;
+  }
+
+  *start_point = lane->GetSmoothPoint(0.0);
+  return true;
+}
+
 }  // namespace dreamview
 }  // namespace apollo

modules/dreamview/backend/map/map_service.h

@@ -80,6 +80,8 @@ class MapService {
   bool GetPoseWithRegardToLane(const double x, const double y, double *theta,
                                double *s) const;
 
+  // Get a point on the map to serve as dummy start point of SimControl
+  bool GetStartPoint(apollo::common::PointENU *start_point) const;
 
   /**
    * @brief The function fills out proper routing lane waypoint
@@ -91,13 +93,17 @@ class MapService {
    */
   bool ConstructLaneWayPoint(
       const double x, const double y,
-      ::apollo::routing::RoutingRequest::LaneWaypoint* laneWayPoint) const;
+      ::apollo::routing::RoutingRequest::LaneWaypoint *laneWayPoint) const;
 
  private:
   const ::apollo::hdmap::HDMap &BaseMap() const {
     return pnc_map_.HDMap();
   }
 
+  bool GetNearestLane(const double x, const double y,
+                      apollo::hdmap::LaneInfoConstPtr *nearest_lane,
+                      double *nearest_s, double *nearest_l) const;
+
   // A wrapper around HDMap to provide some convenient utils dreamview needs.
   const ::apollo::hdmap::PncMap pnc_map_;
   // A downsampled map for dreamview frontend display.

modules/dreamview/backend/map/map_service_test.cc

@@ -89,5 +89,13 @@ TEST_F(MapServiceTest, RetrieveMapElements) {
   EXPECT_EQ("l1", map.lane(0).id().id());
 }
 
+TEST_F(MapServiceTest, GetStartPoint) {
+  PointENU start_point;
+  EXPECT_TRUE(map_service.GetStartPoint(&start_point));
+  EXPECT_DOUBLE_EQ(-1826.4050789145094, start_point.x());
+  EXPECT_DOUBLE_EQ(-3027.5187874953263, start_point.y());
+  EXPECT_DOUBLE_EQ(0.0, start_point.z());
+}
+
 }  // namespace dreamview
 }  // namespace apollo

modules/dreamview/backend/sim_control/sim_control.cc

@@ -61,34 +61,33 @@ SimControl::SimControl(const MapService* map_service)
       initial_start_(true),
       enabled_(FLAGS_enable_sim_control) {
   if (enabled_) {
-    RoutingResponse routing;
-    if (!GetProtoFromFile(FLAGS_routing_response_file, &routing)) {
-      AWARN << "Unable to read start point from file: "
-            << FLAGS_routing_response_file;
+    apollo::common::PointENU start_point;
+    if (!map_service_->GetStartPoint(&start_point)) {
+      AWARN << "Failed to get a dummy start point from map!";
       return;
     }
-    SetStartPoint(routing);
+    SetStartPoint(start_point.x(), start_point.y());
   }
 }
 
-void SimControl::SetStartPoint(const RoutingResponse& routing) {
+void SimControl::SetStartPoint(const double x, const double y) {
   next_point_.set_v(0.0);
   next_point_.set_a(0.0);
 
-  auto* p = next_point_.mutable_path_point();
-
-  p->set_x(routing.routing_request().start().pose().x());
-  p->set_y(routing.routing_request().start().pose().y());
-  p->set_z(0.0);
+  auto* next_point = next_point_.mutable_path_point();
+  next_point->set_x(x);
+  next_point->set_y(y);
+  next_point->set_z(0.0);
 
   double theta = 0.0;
   double s = 0.0;
-  if (!map_service_->GetPoseWithRegardToLane(p->x(), p->y(), &theta, &s)) {
-    AERROR << "Failed to get heading!";
+  if (!map_service_->GetPoseWithRegardToLane(next_point->x(), next_point->y(),
+                                             &theta, &s)) {
+    AERROR << "Failed to get heading from map! Treat theta and s as 0.0!";
   }
-  p->set_theta(theta);
-  p->set_s(s);
-  p->set_kappa(0.0);
+  next_point->set_theta(theta);
+  next_point->set_s(s);
+  next_point->set_kappa(0.0);
 
   prev_point_index_ = next_point_index_ = 0;
   received_planning_ = false;
@@ -98,11 +97,16 @@ void SimControl::SetStartPoint(const RoutingResponse& routing) {
   }
 }
 
+void SimControl::OnRoutingResponse(const RoutingResponse& routing) {
+  const auto& start_pose = routing.routing_request().start().pose();
+  SetStartPoint(start_pose.x(), start_pose.y());
+}
+
 void SimControl::Start() {
   if (initial_start_) {
     // Setup planning and routing result data callback.
     AdapterManager::AddPlanningCallback(&SimControl::OnPlanning, this);
-    AdapterManager::AddRoutingResponseCallback(&SimControl::SetStartPoint,
+    AdapterManager::AddRoutingResponseCallback(&SimControl::OnRoutingResponse,
                                                this);
 
     // Start timer to publish localiztion and chassis messages.

modules/dreamview/backend/sim_control/sim_control.h

@@ -62,10 +62,11 @@ class SimControl {
 
  private:
   void OnPlanning(const apollo::planning::ADCTrajectory &trajectory);
+  void OnRoutingResponse(const apollo::routing::RoutingResponse &routing);
 
-  // Reset the start point according to the RoutingResponse, which can be read
-  // from file or received from a publisher.
-  void SetStartPoint(const apollo::routing::RoutingResponse &routing);
+  // Reset the start point, which can be a dummy point on the map or received
+  // from the routing module.
+  void SetStartPoint(const double x, const double y);
 
   void Freeze();
 

modules/dreamview/backend/sim_control/sim_control_test.cc

@@ -116,13 +116,7 @@ TEST_F(SimControlTest, Test) {
   const double timestamp = 100.0;
   adc_trajectory.mutable_header()->set_timestamp_sec(timestamp);
 
-  RoutingResponse routing;
-  routing.mutable_routing_request()->mutable_start()->mutable_pose()->set_x(
-      1.0);
-  routing.mutable_routing_request()->mutable_start()->mutable_pose()->set_y(
-      1.0);
-
-  sim_control_.SetStartPoint(routing);
+  sim_control_.SetStartPoint(1.0, 1.0);
 
   AdapterManager::AddPlanningCallback(&SimControl::OnPlanning, &sim_control_);
   AdapterManager::GetPlanning()->OnReceive(adc_trajectory);