50using google::protobuf::RepeatedPtrField;
54template <
typename MapElementInfoConstPtr>
55void ExtractIds(
const std::vector<MapElementInfoConstPtr> &items,
56 RepeatedPtrField<std::string> *ids) {
57 ids->Reserve(
static_cast<unsigned int>(items.size()));
58 for (
const auto &item : items) {
59 ids->Add()->assign(item->id().id());
63 std::sort(ids->begin(), ids->end());
66void ExtractRoadAndLaneIds(
const std::vector<LaneInfoConstPtr> &lanes,
67 RepeatedPtrField<std::string> *lane_ids,
68 RepeatedPtrField<std::string> *road_ids) {
69 lane_ids->Reserve(
static_cast<unsigned int>(lanes.size()));
70 road_ids->Reserve(
static_cast<unsigned int>(lanes.size()));
72 for (
const auto &lane : lanes) {
73 lane_ids->Add()->assign(lane->id().id());
74 if (!lane->road_id().id().empty()) {
75 road_ids->Add()->assign(lane->road_id().id());
80 std::sort(lane_ids->begin(), lane_ids->end());
81 std::sort(road_ids->begin(), road_ids->end());
86const char MapService::kMetaFileName[] =
"/metaInfo.json";
93 boost::unique_lock<boost::shared_mutex> writer_lock(mutex_);
104void MapService::UpdateOffsets() {
107 std::ifstream ifs(FLAGS_map_dir + kMetaFileName);
108 if (!ifs.is_open()) {
109 AINFO <<
"Failed to open map meta file: " << kMetaFileName;
115 for (
auto it = json.begin(); it != json.end(); ++it) {
116 auto val = it.value();
117 if (val.is_object()) {
118 auto x_offset = val.find(
"xoffset");
119 if (x_offset == val.end()) {
120 AWARN <<
"Cannot find x_offset for this map " << it.key();
124 if (!x_offset->is_number()) {
125 AWARN <<
"Expect x_offset with type 'number', but was "
126 << x_offset->type_name();
129 x_offset_ = x_offset.value();
131 auto y_offset = val.find(
"yoffset");
132 if (y_offset == val.end()) {
133 AWARN <<
"Cannot find y_offset for this map " << it.key();
137 if (!y_offset->is_number()) {
138 AWARN <<
"Expect y_offset with type 'number', but was "
139 << y_offset->type_name();
142 y_offset_ = y_offset.value();
146 AINFO <<
"Updated with map: x_offset " << x_offset_ <<
", y_offset "
150const hdmap::HDMap *MapService::HDMap()
const {
154const hdmap::HDMap *MapService::SimMap()
const {
158bool MapService::MapReady()
const {
return HDMap() && SimMap(); }
166 return (ids.ByteSizeLong() != 0);
174 boost::shared_lock<boost::shared_mutex> reader_lock(mutex_);
176 std::vector<LaneInfoConstPtr> lanes;
177 if (SimMap()->GetLanes(point, radius, &lanes) != 0) {
178 AERROR <<
"Fail to get lanes from sim_map.";
180 ExtractRoadAndLaneIds(lanes, ids->mutable_lane(), ids->mutable_road());
182 std::vector<ClearAreaInfoConstPtr> clear_areas;
183 if (SimMap()->GetClearAreas(point, radius, &clear_areas) != 0) {
184 AERROR <<
"Fail to get clear areas from sim_map.";
186 ExtractIds(clear_areas, ids->mutable_clear_area());
188 std::vector<CrosswalkInfoConstPtr> crosswalks;
189 if (SimMap()->GetCrosswalks(point, radius, &crosswalks) != 0) {
190 AERROR <<
"Fail to get crosswalks from sim_map.";
192 ExtractIds(crosswalks, ids->mutable_crosswalk());
194 std::vector<JunctionInfoConstPtr> junctions;
195 if (SimMap()->GetJunctions(point, radius, &junctions) != 0) {
196 AERROR <<
"Fail to get junctions from sim_map.";
198 ExtractIds(junctions, ids->mutable_junction());
200 std::vector<PNCJunctionInfoConstPtr> pnc_junctions;
201 if (SimMap()->GetPNCJunctions(point, radius, &pnc_junctions) != 0) {
202 AERROR <<
"Fail to get pnc junctions from sim_map.";
204 ExtractIds(pnc_junctions, ids->mutable_pnc_junction());
206 std::vector<ParkingSpaceInfoConstPtr> parking_spaces;
207 if (SimMap()->GetParkingSpaces(point, radius, &parking_spaces) != 0) {
208 AERROR <<
"Fail to get parking space from sim_map.";
210 ExtractIds(parking_spaces, ids->mutable_parking_space());
212 std::vector<SpeedBumpInfoConstPtr> speed_bumps;
213 if (SimMap()->GetSpeedBumps(point, radius, &speed_bumps) != 0) {
214 AERROR <<
"Fail to get speed bump from sim_map.";
216 ExtractIds(speed_bumps, ids->mutable_speed_bump());
218 std::vector<SignalInfoConstPtr> signals;
219 if (SimMap()->GetSignals(point, radius, &signals) != 0) {
220 AERROR <<
"Failed to get signals from sim_map.";
223 ExtractIds(signals, ids->mutable_signal());
225 std::vector<StopSignInfoConstPtr> stop_signs;
226 if (SimMap()->GetStopSigns(point, radius, &stop_signs) != 0) {
227 AERROR <<
"Failed to get stop signs from sim_map.";
229 ExtractIds(stop_signs, ids->mutable_stop_sign());
231 std::vector<YieldSignInfoConstPtr> yield_signs;
232 if (SimMap()->GetYieldSigns(point, radius, &yield_signs) != 0) {
233 AERROR <<
"Failed to get yield signs from sim_map.";
235 ExtractIds(yield_signs, ids->mutable_yield());
237 std::vector<AreaInfoConstPtr> ad_areas;
238 if (SimMap()->GetAreas(point, radius, &ad_areas) != 0) {
239 AERROR <<
"Failed to get ad areas from sim_map.";
241 ExtractIds(ad_areas, ids->mutable_ad_area());
243 std::vector<BarrierGateInfoConstPtr> barrier_gates;
244 if (SimMap()->GetBarrierGates(point, radius, &barrier_gates) != 0) {
245 AERROR <<
"Failed to get barrier gate from sim_map.";
247 ExtractIds(barrier_gates, ids->mutable_barrier_gate());
251 boost::shared_lock<boost::shared_mutex> reader_lock(mutex_);
259 for (
const auto &
id : ids.
lane()) {
263 auto lane = element->lane();
264 lane.clear_left_sample();
265 lane.clear_right_sample();
266 lane.clear_left_road_sample();
267 lane.clear_right_road_sample();
268 *result.add_lane() = lane;
276 *result.add_clear_area() = element->
clear_area();
284 *result.add_crosswalk() = element->
crosswalk();
288 for (
const auto &
id : ids.
junction()) {
292 *result.add_junction() = element->
junction();
296 for (
const auto &
id : ids.
signal()) {
300 *result.add_signal() = element->
signal();
308 *result.add_stop_sign() = element->
stop_sign();
312 for (
const auto &
id : ids.
yield()) {
316 *result.add_yield() = element->yield_sign();
320 for (
const auto &
id : ids.
road()) {
324 *result.add_road() = element->
road();
340 *result.add_speed_bump() = element->
speed_bump();
352 for (
const auto &
id : ids.
ad_area()) {
356 *result.add_ad_area() = element->area();
369 if (SimMap()->GetMapHeader(&map_header)) {
370 result.mutable_header()->CopyFrom(map_header);
376bool MapService::GetNearestLaneWithDistance(
377 const double x,
const double y,
379 double *nearest_l)
const {
380 boost::shared_lock<boost::shared_mutex> reader_lock(mutex_);
385 static constexpr double kSearchRadius = 3.0;
387 HDMap()->GetNearestLaneWithDistance(point, kSearchRadius, nearest_lane,
388 nearest_s, nearest_l) < 0) {
389 AERROR <<
"Failed to get nearest lane!";
395bool MapService::GetNearestLane(
const double x,
const double y,
396 LaneInfoConstPtr *nearest_lane,
397 double *nearest_s,
double *nearest_l)
const {
398 boost::shared_lock<boost::shared_mutex> reader_lock(mutex_);
404 HDMap()->GetNearestLane(point, nearest_lane, nearest_s, nearest_l) < 0) {
405 AERROR <<
"Failed to get nearest lane!";
411bool MapService::GetNearestLaneWithHeading(
const double x,
const double y,
412 LaneInfoConstPtr *nearest_lane,
413 double *nearest_s,
double *nearest_l,
414 const double heading)
const {
415 boost::shared_lock<boost::shared_mutex> reader_lock(mutex_);
420 static constexpr double kSearchRadius = 3.0;
421 static constexpr double kMaxHeadingDiff = 1.0;
422 if (!MapReady() || HDMap()->GetNearestLaneWithHeading(
423 point, kSearchRadius, heading, kMaxHeadingDiff,
424 nearest_lane, nearest_s, nearest_l) < 0) {
425 AERROR <<
"Failed to get nearest lane with heading.";
432 std::vector<Path> *paths)
const {
433 if (!CreatePathsFromRouting(routing, paths)) {
434 AERROR <<
"Unable to get paths from routing!";
441 double *theta,
double *s)
const {
443 LaneInfoConstPtr nearest_lane;
444 if (!GetNearestLane(x, y, &nearest_lane, s, &l)) {
448 *theta = nearest_lane->Heading(*s);
455 LaneInfoConstPtr lane;
456 if (!GetNearestLane(x, y, &lane, &s, &l)) {
464 laneWayPoint->set_id(lane->id().id());
465 laneWayPoint->set_s(s);
466 auto *pose = laneWayPoint->mutable_pose();
474 const double x,
const double y,
const double heading,
477 LaneInfoConstPtr lane;
478 if (!GetNearestLaneWithHeading(x, y, &lane, &s, &l, heading)) {
486 laneWayPoint->set_id(lane->id().id());
487 laneWayPoint->set_s(s);
488 auto *pose = laneWayPoint->mutable_pose();
496 const double x,
const double y,
const std::string
id,
512 if (!lane->GetProjection({point.x(), point.y()}, &s, &l)) {
518 if (s > lane->lane().length()) {
519 s = lane->lane().length();
522 laneWayPoint->set_id(
id);
523 laneWayPoint->set_s(s);
524 auto *pose = laneWayPoint->mutable_pose();
533 LaneInfoConstPtr lane;
534 if (!GetNearestLaneWithDistance(x, y, &lane, &s, &l)) {
547 const double heading)
const {
549 LaneInfoConstPtr lane;
550 if (!GetNearestLaneWithHeading(x, y, &lane, &s, &l, heading)) {
565 <<
"Failed to construct LaneWayPoint for RoutingRequest: Expected lane "
566 << lane->id().id() <<
" to be CITY_DRIVING, but was "
567 << apollo::hdmap::Lane::LaneType_Name(lane->lane().type());
576 LaneInfoConstPtr lane;
577 if (!GetNearestLane(0.0, 0.0, &lane, &s, &l)) {
581 *start_point = lane->GetSmoothPoint(0.0);
586 std::vector<Path> *paths)
const {
587 if (routing.
road().empty()) {
591 for (
const auto &road : routing.road()) {
592 for (
const auto &passage_region : road.passage()) {
594 if (!AddPathFromPassageRegion(passage_region, paths)) {
602bool MapService::AddPathFromPassageRegion(
603 const routing::Passage &passage_region, std::vector<Path> *paths)
const {
607 boost::shared_lock<boost::shared_mutex> reader_lock(mutex_);
609 std::vector<MapPathPoint> path_points;
610 for (
const auto &segment : passage_region.segment()) {
613 AERROR <<
"Failed to find lane: " << segment.id();
616 if (segment.start_s() >= segment.end_s()) {
621 path_points.insert(path_points.end(), points.begin(), points.end());
624 if (path_points.size() < 2) {
627 paths->emplace_back(Path(std::move(path_points)));
633 static std::hash<std::string> hash_function;
634 return hash_function(ids.DebugString());
638 auto *hdmap = HDMap();
639 CHECK(hdmap) <<
"Failed to get hdmap";
643 LaneInfoConstPtr lane_ptr = hdmap->GetLaneById(
id);
644 if (lane_ptr !=
nullptr) {
645 return lane_ptr->Heading(s);
651 const double y)
const {
655 static constexpr double kSearchRadius = 3.0;
656 std::vector<ParkingSpaceInfoConstPtr> parking_spaces;
659 if (HDMap()->GetParkingSpaces(point, kSearchRadius, &parking_spaces) != 0) {
660 AERROR <<
"Fail to get parking space from sim_map.";
665 for (
const auto &parking_sapce : parking_spaces) {
666 if (!parking_sapce->polygon().is_convex()) {
667 AERROR <<
"The parking space information is incorrect and is not a "
672 if (parking_sapce->polygon().IsPointIn(checked_point)) {
673 return parking_sapce->id().id();
Implements a class of 2-dimensional vectors.
bool CheckRoutingPoint(const double x, const double y) const
bool CheckRoutingPointWithHeading(const double x, const double y, const double heading) const
bool PointIsValid(const double x, const double y) const
double GetLaneHeading(const std::string &id_str, double s)
void CollectMapElementIds(const apollo::common::PointENU &point, double raidus, MapElementIds *ids) const
hdmap::Map RetrieveMapElements(const MapElementIds &ids) const
bool ReloadMap(bool force_reload)
MapService(bool use_sim_map=true)
bool GetStartPoint(apollo::common::PointENU *start_point) const
bool ConstructLaneWayPointWithLaneId(const double x, const double y, const std::string id, 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
bool GetPoseWithRegardToLane(const double x, const double y, double *theta, double *s) const
size_t CalculateMapHash(const MapElementIds &ids) const
bool GetPathsFromRouting(const apollo::routing::RoutingResponse &routing, std::vector< apollo::hdmap::Path > *paths) const
bool ConstructLaneWayPoint(const double x, const double y, routing::LaneWaypoint *laneWayPoint) const
The function fills out proper routing lane waypoint from the given (x,y) position.
std::string GetParkingSpaceId(const double x, const double y) const
static const HDMap * BaseMapPtr()
static const HDMap * SimMapPtr()
StopSignInfoConstPtr GetStopSignById(const Id &id) const
PNCJunctionInfoConstPtr GetPNCJunctionById(const Id &id) const
YieldSignInfoConstPtr GetYieldSignById(const Id &id) const
BarrierGateInfoConstPtr GetBarrierGateById(const Id &id) const
ClearAreaInfoConstPtr GetClearAreaById(const Id &id) const
LaneInfoConstPtr GetLaneById(const Id &id) const
AreaInfoConstPtr GetAreaById(const Id &id) const
RoadInfoConstPtr GetRoadById(const Id &id) const
JunctionInfoConstPtr GetJunctionById(const Id &id) const
ParkingSpaceInfoConstPtr GetParkingSpaceById(const Id &id) const
CrosswalkInfoConstPtr GetCrosswalkById(const Id &id) const
SignalInfoConstPtr GetSignalById(const Id &id) const
SpeedBumpInfoConstPtr GetSpeedBumpById(const Id &id) const
static std::vector< MapPathPoint > GetPointsFromLane(LaneInfoConstPtr lane, const double start_s, const double end_s)
std::shared_ptr< const PNCJunctionInfo > PNCJunctionInfoConstPtr
std::shared_ptr< const JunctionInfo > JunctionInfoConstPtr
apollo::hdmap::Id MakeMapId(const std::string &id)
create a Map ID given a string.
std::shared_ptr< const StopSignInfo > StopSignInfoConstPtr
std::shared_ptr< const AreaInfo > AreaInfoConstPtr
std::shared_ptr< const LaneInfo > LaneInfoConstPtr
std::shared_ptr< const ClearAreaInfo > ClearAreaInfoConstPtr
std::shared_ptr< const BarrierGateInfo > BarrierGateInfoConstPtr
std::shared_ptr< const SignalInfo > SignalInfoConstPtr
std::shared_ptr< const SpeedBumpInfo > SpeedBumpInfoConstPtr
std::shared_ptr< const CrosswalkInfo > CrosswalkInfoConstPtr
std::shared_ptr< const YieldSignInfo > YieldSignInfoConstPtr
std::shared_ptr< const ParkingSpaceInfo > ParkingSpaceInfoConstPtr
Some string util functions.
repeated string pnc_junction
repeated string crosswalk
repeated string clear_area
repeated string stop_sign
repeated string speed_bump
repeated string parking_space
repeated string barrier_gate
repeated Junction junction
repeated StopSign stop_sign
repeated Crosswalk crosswalk
repeated ParkingSpace parking_space
repeated SpeedBump speed_bump
repeated BarrierGate barrier_gate
repeated PNCJunction pnc_junction
repeated ClearArea clear_area
repeated apollo::routing::RoadSegment road