apply offset in dreamview if meta is present

modules/dreamview/backend/map/map_service.cc

@@ -17,6 +17,7 @@
 #include "modules/dreamview/backend/map/map_service.h"
 
 #include <algorithm>
+#include <fstream>
 
 #include "modules/common/util/json_util.h"
 #include "modules/common/util/string_util.h"
@@ -121,18 +122,67 @@ bool MapService::ReloadMap(bool force_reload) {
   hdmap_ = HDMapUtil::BaseMapPtr();
   sim_map_ = use_sim_map_ ? HDMapUtil::SimMapPtr() : HDMapUtil::BaseMapPtr();
   if (hdmap_ == nullptr || sim_map_ == nullptr) {
-    pending = true;
+    pending_ = true;
     AWARN << "No map data available yet.";
   } else {
-    pending = false;
+    pending_ = false;
   }
+
+  // Update the x,y-offsets if present.
+  UpdateOffsets();
   return ret;
 }
 
+void MapService::UpdateOffsets() {
+  x_offset_ = 0.0;
+  y_offset_ = 0.0;
+  std::ifstream ifs(FLAGS_map_dir + meta_filename_);
+  if (!ifs.is_open()) {
+    AINFO << "Failed to open map meta file: " << meta_filename_;
+  } else {
+    nlohmann::json json;
+    ifs >> json;
+    ifs.close();
+
+    for (auto it = json.begin(); it != json.end(); ++it) {
+      auto val = it.value();
+      if (val.is_object()) {
+        auto x_offset = val.find("xoffset");
+        if (x_offset == val.end()) {
+          AWARN << "Cannot find x_offset for this map " << it.key();
+          continue;
+        }
+
+        if (!x_offset->is_number()) {
+          AWARN << "Expect x_offset with type 'number', but was "
+                << x_offset->type_name();
+          continue;
+        }
+        x_offset_ = x_offset.value();
+
+        auto y_offset = val.find("yoffset");
+        if (y_offset == val.end()) {
+          AWARN << "Cannot find y_offset for this map " << it.key();
+          continue;
+        }
+
+        if (!y_offset->is_number()) {
+          AWARN << "Expect y_offset with type 'number', but was "
+                << y_offset->type_name();
+          continue;
+        }
+        y_offset_ = y_offset.value();
+      }
+    }
+  }
+  AINFO << "Updated with map: x_offset " << x_offset_ << ", y_offset "
+        << y_offset_;
+}
+
 MapElementIds MapService::CollectMapElementIds(const PointENU &point,
                                                double radius) const {
   MapElementIds result;
-  if (pending) {
+  if (pending_) {
     return result;
   }
   boost::shared_lock<boost::shared_mutex> reader_lock(mutex_);
@@ -182,7 +232,7 @@ Map MapService::RetrieveMapElements(const MapElementIds &ids) const {
   boost::shared_lock<boost::shared_mutex> reader_lock(mutex_);
 
   Map result;
-  if (pending) {
+  if (pending_) {
     return result;
   }
   Id map_id;
@@ -254,7 +304,7 @@ bool MapService::GetNearestLane(const double x, const double y,
   PointENU point;
   point.set_x(x);
   point.set_y(y);
-  if (pending
+  if (pending_
       || hdmap_->GetNearestLane(point, nearest_lane, nearest_s, nearest_l)
           < 0) {
     AERROR << "Failed to get nearest lane!";
@@ -328,7 +378,7 @@ bool MapService::CreatePathsFromRouting(const RoutingResponse &routing,
 
 bool MapService::AddPathFromPassageRegion(
     const routing::Passage &passage_region, std::vector<Path> *paths) const {
-  if (pending) {
+  if (pending_) {
     return false;
   }
   boost::shared_lock<boost::shared_mutex> reader_lock(mutex_);

modules/dreamview/backend/map/map_service.h

@@ -67,6 +67,9 @@ class MapService {
  public:
   explicit MapService(bool use_sim_map = true);
 
+  inline double GetXOffset() const { return x_offset_; }
+  inline double GetYOffset() const { return y_offset_; }
+
   MapElementIds CollectMapElementIds(const apollo::common::PointENU &point,
                                      double raidus) const;
 
@@ -99,6 +102,7 @@ class MapService {
   bool ReloadMap(bool force_reload);
 
  private:
+  void UpdateOffsets();
   bool GetNearestLane(const double x, const double y,
                       apollo::hdmap::LaneInfoConstPtr *nearest_lane,
                       double *nearest_s, double *nearest_l) const;
@@ -113,7 +117,10 @@ class MapService {
   const hdmap::HDMap *hdmap_ = nullptr;
   // A downsampled map for dreamview frontend display.
   const hdmap::HDMap *sim_map_ = nullptr;
-  bool pending = true;
+  bool pending_ = true;
+  const std::string meta_filename_ = "/metaInfo.json";
+  double x_offset_ = 0.0;
+  double y_offset_ = 0.0;
 
   // RW lock to protect map data
   mutable boost::shared_mutex mutex_;

modules/dreamview/backend/simulation_world/simulation_world_service.cc

@@ -112,47 +112,6 @@ Object::DisengageType DeduceDisengageType(const Chassis &chassis) {
   }
 }
 
-void SetObstacleInfo(const PerceptionObstacle &obstacle, Object *world_object) {
-  if (world_object == nullptr) {
-    return;
-  }
-
-  world_object->set_id(std::to_string(obstacle.id()));
-  world_object->set_position_x(obstacle.position().x());
-  world_object->set_position_y(obstacle.position().y());
-  world_object->set_heading(obstacle.theta());
-  world_object->set_length(obstacle.length());
-  world_object->set_width(obstacle.width());
-  world_object->set_height(obstacle.height());
-  world_object->set_speed(
-      std::hypot(obstacle.velocity().x(), obstacle.velocity().y()));
-  world_object->set_speed_heading(
-      std::atan2(obstacle.velocity().y(), obstacle.velocity().x()));
-  world_object->set_timestamp_sec(obstacle.timestamp());
-  world_object->set_confidence(obstacle.confidence());
-}
-
-void SetObstaclePolygon(const PerceptionObstacle &obstacle,
-                        Object *world_object) {
-  if (world_object == nullptr) {
-    return;
-  }
-
-  using apollo::common::util::PairHash;
-  std::unordered_set<std::pair<double, double>, PairHash> seen_points;
-  world_object->clear_polygon_point();
-  for (const auto &point : obstacle.polygon_point()) {
-    // Filter out duplicate xy pairs.
-    std::pair<double, double> xy_pair = {point.x(), point.y()};
-    if (seen_points.count(xy_pair) == 0) {
-      PolygonPoint *poly_pt = world_object->add_polygon_point();
-      poly_pt->set_x(point.x());
-      poly_pt->set_y(point.y());
-      seen_points.insert(xy_pair);
-    }
-  }
-}
-
 void SetObstacleType(const PerceptionObstacle &obstacle, Object *world_object) {
   if (world_object == nullptr) {
     return;
@@ -230,69 +189,6 @@ void UpdateTurnSignal(const apollo::common::VehicleSignal &signal,
   }
 }
 
-bool LocateMarker(const apollo::planning::ObjectDecisionType &decision,
-                  Decision *world_decision) {
-  apollo::common::PointENU fence_point;
-  double heading;
-  if (decision.has_stop() && decision.stop().has_stop_point()) {
-    world_decision->set_type(Decision_Type_STOP);
-    fence_point = decision.stop().stop_point();
-    heading = decision.stop().stop_heading();
-  } else if (decision.has_follow() && decision.follow().has_fence_point()) {
-    world_decision->set_type(Decision_Type_FOLLOW);
-    fence_point = decision.follow().fence_point();
-    heading = decision.follow().fence_heading();
-  } else if (decision.has_yield() && decision.yield().has_fence_point()) {
-    world_decision->set_type(Decision_Type_YIELD);
-    fence_point = decision.yield().fence_point();
-    heading = decision.yield().fence_heading();
-  } else if (decision.has_overtake() && decision.overtake().has_fence_point()) {
-    world_decision->set_type(Decision_Type_OVERTAKE);
-    fence_point = decision.overtake().fence_point();
-    heading = decision.overtake().fence_heading();
-  } else {
-    return false;
-  }
-
-  world_decision->set_position_x(fence_point.x());
-  world_decision->set_position_y(fence_point.y());
-  world_decision->set_heading(heading);
-  return true;
-}
-
-void FindNudgeRegion(const apollo::planning::ObjectDecisionType &decision,
-                     const Object &world_obj, Decision *world_decision) {
-  std::vector<apollo::common::math::Vec2d> points;
-  for (auto &polygon_pt : world_obj.polygon_point()) {
-    points.emplace_back(polygon_pt.x(), polygon_pt.y());
-  }
-  const apollo::common::math::Polygon2d obj_polygon(points);
-  const apollo::common::math::Polygon2d &nudge_polygon =
-      obj_polygon.ExpandByDistance(std::fabs(decision.nudge().distance_l()));
-  const std::vector<apollo::common::math::Vec2d> &nudge_points =
-      nudge_polygon.points();
-  for (auto &nudge_pt : nudge_points) {
-    PolygonPoint *poly_pt = world_decision->add_polygon_point();
-    poly_pt->set_x(nudge_pt.x());
-    poly_pt->set_y(nudge_pt.y());
-  }
-  world_decision->set_type(Decision_Type_NUDGE);
-}
-
-void CreatePredictionTrajectory(Object *world_object,
-                                const PredictionObstacle &obstacle) {
-  for (const auto &traj : obstacle.trajectory()) {
-    Prediction *prediction = world_object->add_prediction();
-    prediction->set_probability(traj.probability());
-    for (const auto &point : traj.trajectory_point()) {
-      PolygonPoint *world_point = prediction->add_predicted_trajectory();
-      world_point->set_x(point.path_point().x());
-      world_point->set_y(point.path_point().y());
-      world_point->set_z(point.path_point().z());
-    }
-  }
-}
-
 inline double SecToMs(const double sec) { return sec * 1000.0; }
 
 }  // namespace
@@ -397,8 +293,10 @@ Json SimulationWorldService::GetPlanningData() const {
 Json SimulationWorldService::GetMapElements(double radius) const {
   // Gather required map element ids based on current location.
   apollo::common::PointENU point;
-  point.set_x(world_.auto_driving_car().position_x());
-  point.set_y(world_.auto_driving_car().position_y());
+  point.set_x(
+      world_.auto_driving_car().position_x() + map_service_->GetXOffset());
+  point.set_y(
+      world_.auto_driving_car().position_y() + map_service_->GetYOffset());
 
   MapElementIds map_element_ids =
       map_service_->CollectMapElementIds(point, radius);
@@ -447,8 +345,10 @@ void SimulationWorldService::UpdateSimulationWorld(
   const auto &pose = localization.pose();
 
   // Updates position with the input localization message.
-  auto_driving_car->set_position_x(pose.position().x());
-  auto_driving_car->set_position_y(pose.position().y());
+  auto_driving_car->set_position_x(
+      pose.position().x() + map_service_->GetXOffset());
+  auto_driving_car->set_position_y(
+      pose.position().y() + map_service_->GetYOffset());
   auto_driving_car->set_heading(pose.heading());
 
   // Updates acceleration with the input localization message.
@@ -500,6 +400,50 @@ Object &SimulationWorldService::CreateWorldObjectIfAbsent(
   return obj_map_[id];
 }
 
+void SimulationWorldService::SetObstacleInfo(const PerceptionObstacle &obstacle,
+                                             Object *world_object) {
+  if (world_object == nullptr) {
+    return;
+  }
+
+  world_object->set_id(std::to_string(obstacle.id()));
+  world_object->set_position_x(
+      obstacle.position().x() + map_service_->GetXOffset());
+  world_object->set_position_y(
+      obstacle.position().y() + map_service_->GetYOffset());
+  world_object->set_heading(obstacle.theta());
+  world_object->set_length(obstacle.length());
+  world_object->set_width(obstacle.width());
+  world_object->set_height(obstacle.height());
+  world_object->set_speed(
+      std::hypot(obstacle.velocity().x(), obstacle.velocity().y()));
+  world_object->set_speed_heading(
+      std::atan2(obstacle.velocity().y(), obstacle.velocity().x()));
+  world_object->set_timestamp_sec(obstacle.timestamp());
+  world_object->set_confidence(obstacle.confidence());
+}
+
+void SimulationWorldService::SetObstaclePolygon(
+    const PerceptionObstacle &obstacle, Object *world_object) {
+  if (world_object == nullptr) {
+    return;
+  }
+
+  using apollo::common::util::PairHash;
+  std::unordered_set<std::pair<double, double>, PairHash> seen_points;
+  world_object->clear_polygon_point();
+  for (const auto &point : obstacle.polygon_point()) {
+    // Filter out duplicate xy pairs.
+    std::pair<double, double> xy_pair = {point.x(), point.y()};
+    if (seen_points.count(xy_pair) == 0) {
+      PolygonPoint *poly_pt = world_object->add_polygon_point();
+      poly_pt->set_x(point.x() + map_service_->GetXOffset());
+      poly_pt->set_y(point.y() + map_service_->GetYOffset());
+      seen_points.insert(xy_pair);
+    }
+  }
+}
+
 template <>
 void SimulationWorldService::UpdateSimulationWorld(
     const PerceptionObstacles &obstacles) {
@@ -541,6 +485,11 @@ void SimulationWorldService::UpdatePlanningTrajectory(
       collector.Collect(point);
     }
   }
+  for (int i = 0; i < world_.planning_trajectory_size(); ++i) {
+    auto traj_pt = world_.mutable_planning_trajectory(i);
+    traj_pt->set_position_x(traj_pt->position_x() + map_service_->GetXOffset());
+    traj_pt->set_position_y(traj_pt->position_y() + map_service_->GetYOffset());
+  }
 }
 
 void SimulationWorldService::UpdateMainDecision(
@@ -575,12 +524,64 @@ void SimulationWorldService::UpdateMainDecision(
       SetStopReason(stop.reason_code(), decision);
     }
   }
-  world_main_stop->set_position_x(stop_pt.x());
-  world_main_stop->set_position_y(stop_pt.y());
+  world_main_stop->set_position_x(stop_pt.x() + map_service_->GetXOffset());
+  world_main_stop->set_position_y(stop_pt.y() + map_service_->GetYOffset());
   world_main_stop->set_heading(stop_heading);
   world_main_stop->set_timestamp_sec(update_timestamp_sec);
 }
 
+bool SimulationWorldService::LocateMarker(
+    const apollo::planning::ObjectDecisionType &decision,
+    Decision *world_decision) {
+  apollo::common::PointENU fence_point;
+  double heading;
+  if (decision.has_stop() && decision.stop().has_stop_point()) {
+    world_decision->set_type(Decision_Type_STOP);
+    fence_point = decision.stop().stop_point();
+    heading = decision.stop().stop_heading();
+  } else if (decision.has_follow() && decision.follow().has_fence_point()) {
+    world_decision->set_type(Decision_Type_FOLLOW);
+    fence_point = decision.follow().fence_point();
+    heading = decision.follow().fence_heading();
+  } else if (decision.has_yield() && decision.yield().has_fence_point()) {
+    world_decision->set_type(Decision_Type_YIELD);
+    fence_point = decision.yield().fence_point();
+    heading = decision.yield().fence_heading();
+  } else if (decision.has_overtake() && decision.overtake().has_fence_point()) {
+    world_decision->set_type(Decision_Type_OVERTAKE);
+    fence_point = decision.overtake().fence_point();
+    heading = decision.overtake().fence_heading();
+  } else {
+    return false;
+  }
+
+  world_decision->set_position_x(fence_point.x() + map_service_->GetXOffset());
+  world_decision->set_position_y(fence_point.y() + map_service_->GetYOffset());
+  world_decision->set_heading(heading);
+  return true;
+}
+
+void SimulationWorldService::FindNudgeRegion(
+    const apollo::planning::ObjectDecisionType &decision,
+    const Object &world_obj, Decision *world_decision) {
+  std::vector<apollo::common::math::Vec2d> points;
+  for (auto &polygon_pt : world_obj.polygon_point()) {
+    points.emplace_back(polygon_pt.x() + map_service_->GetXOffset(),
+                        polygon_pt.y() + map_service_->GetYOffset());
+  }
+  const apollo::common::math::Polygon2d obj_polygon(points);
+  const apollo::common::math::Polygon2d &nudge_polygon =
+      obj_polygon.ExpandByDistance(std::fabs(decision.nudge().distance_l()));
+  const std::vector<apollo::common::math::Vec2d> &nudge_points =
+      nudge_polygon.points();
+  for (auto &nudge_pt : nudge_points) {
+    PolygonPoint *poly_pt = world_decision->add_polygon_point();
+    poly_pt->set_x(nudge_pt.x());
+    poly_pt->set_y(nudge_pt.y());
+  }
+  world_decision->set_type(Decision_Type_NUDGE);
+}
+
 void SimulationWorldService::UpdateDecision(const DecisionResult &decision_res,
                                             double header_time) {
   // Update turn signal.
@@ -767,6 +768,21 @@ void SimulationWorldService::UpdateSimulationWorld(
   world_.set_timestamp_sec(std::max(world_.timestamp_sec(), header_time));
 }
 
+
+void SimulationWorldService::CreatePredictionTrajectory(
+    Object *world_object, const PredictionObstacle &obstacle) {
+  for (const auto &traj : obstacle.trajectory()) {
+    Prediction *prediction = world_object->add_prediction();
+    prediction->set_probability(traj.probability());
+    for (const auto &point : traj.trajectory_point()) {
+      PolygonPoint *world_point = prediction->add_predicted_trajectory();
+      world_point->set_x(point.path_point().x() + map_service_->GetXOffset());
+      world_point->set_y(point.path_point().y() + map_service_->GetYOffset());
+      world_point->set_z(point.path_point().z());
+    }
+  }
+}
+
 template <>
 void SimulationWorldService::UpdateSimulationWorld(
     const PredictionObstacles &obstacles) {
@@ -811,8 +827,8 @@ void SimulationWorldService::UpdateSimulationWorld(
     for (int index : sampled_indices) {
       const auto &path_point = path.path_points()[index];
       PolygonPoint *route_point = route_path->add_point();
-      route_point->set_x(path_point.x());
-      route_point->set_y(path_point.y());
+      route_point->set_x(path_point.x() + map_service_->GetXOffset());
+      route_point->set_y(path_point.y() + map_service_->GetYOffset());
     }
   }
 

modules/dreamview/backend/simulation_world/simulation_world_service.h

@@ -143,12 +143,24 @@ class SimulationWorldService {
 
   Object &CreateWorldObjectIfAbsent(
       const apollo::perception::PerceptionObstacle &obstacle);
+  void SetObstacleInfo(const apollo::perception::PerceptionObstacle &obstacle,
+                       Object *world_object);
+  void SetObstaclePolygon(
+      const apollo::perception::PerceptionObstacle &obstacle,
+      Object *world_object);
   void UpdatePlanningTrajectory(
       const apollo::planning::ADCTrajectory &trajectory);
+  bool LocateMarker(const apollo::planning::ObjectDecisionType &decision,
+                    Decision *world_decision);
+  void FindNudgeRegion(const apollo::planning::ObjectDecisionType &decision,
+                       const Object &world_obj, Decision *world_decision);
   void UpdateDecision(const apollo::planning::DecisionResult &decision_res,
                       double header_time);
   void UpdateMainDecision(const apollo::planning::MainDecision &main_decision,
                           double update_timestamp_sec, Object *world_main_stop);
+  void CreatePredictionTrajectory(
+      Object *world_object,
+      const apollo::prediction::PredictionObstacle &obstacle);
   void UpdatePlanningData(const apollo::planning_internal::PlanningData &data);
 
   /**