From a3c0cbf40f63f19fb7a452b05d9407e3b4240750 Mon Sep 17 00:00:00 2001 From: unacao Date: Sun, 24 Dec 2017 16:14:02 -0800 Subject: [PATCH] apply offset in dreamview if meta is present --- modules/dreamview/backend/map/map_service.cc | 62 ++++- modules/dreamview/backend/map/map_service.h | 9 +- .../simulation_world_service.cc | 240 ++++++++++-------- .../simulation_world_service.h | 12 + 4 files changed, 204 insertions(+), 119 deletions(-) diff --git a/modules/dreamview/backend/map/map_service.cc b/modules/dreamview/backend/map/map_service.cc index 101812f0cb..ab9d8858cf 100644 --- a/modules/dreamview/backend/map/map_service.cc +++ b/modules/dreamview/backend/map/map_service.cc @@ -17,6 +17,7 @@ #include "modules/dreamview/backend/map/map_service.h" #include +#include #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 reader_lock(mutex_); @@ -182,7 +232,7 @@ Map MapService::RetrieveMapElements(const MapElementIds &ids) const { boost::shared_lock 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 *paths) const { - if (pending) { + if (pending_) { return false; } boost::shared_lock reader_lock(mutex_); diff --git a/modules/dreamview/backend/map/map_service.h b/modules/dreamview/backend/map/map_service.h index 5ccd297028..b89e023795 100644 --- a/modules/dreamview/backend/map/map_service.h +++ b/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_; diff --git a/modules/dreamview/backend/simulation_world/simulation_world_service.cc b/modules/dreamview/backend/simulation_world/simulation_world_service.cc index c697d42a00..8d4c329954 100644 --- a/modules/dreamview/backend/simulation_world/simulation_world_service.cc +++ b/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, PairHash> seen_points; - world_object->clear_polygon_point(); - for (const auto &point : obstacle.polygon_point()) { - // Filter out duplicate xy pairs. - std::pair 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 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 &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, PairHash> seen_points; + world_object->clear_polygon_point(); + for (const auto &point : obstacle.polygon_point()) { + // Filter out duplicate xy pairs. + std::pair 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 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 &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()); } } diff --git a/modules/dreamview/backend/simulation_world/simulation_world_service.h b/modules/dreamview/backend/simulation_world/simulation_world_service.h index 4d57e74163..f6b75d9b5e 100644 --- a/modules/dreamview/backend/simulation_world/simulation_world_service.h +++ b/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); /** -- GitLab