17#include <boost/archive/binary_iarchive.hpp>
18#include <boost/archive/binary_oarchive.hpp>
21#include <j2735_v2x_msgs/msg/lane_type_attributes.hpp>
59 const std::shared_ptr<lanelet::LaneletMap>& map, std::shared_ptr<const lanelet::routing::RoutingGraph> current_routing_graph)
61 std::unordered_map<uint8_t, std::unordered_set<uint16_t>> signal_group_to_exit_lanes;
62 std::unordered_map<uint8_t, std::unordered_set<uint16_t>> signal_group_to_entry_lanes;
66 throw std::invalid_argument(
"Map is not initialized yet as the georeference was not found...");
69 lanelet::projection::LocalFrameProjector local_projector(
target_frame_.c_str());
71 lanelet::GPSPoint gps_point;
72 gps_point.lat = intersection.ref_point.latitude;
73 gps_point.lon = intersection.ref_point.longitude;
74 gps_point.ele = intersection.ref_point.elevation;
76 auto ref_node = local_projector.forward(gps_point);
78 RCLCPP_DEBUG_STREAM(rclcpp::get_logger(
"carma_wm::SignalizedIntersectionManager"),
"Reference node in map frame x: " << ref_node.x() <<
", y: " << ref_node.y());
81 for (
auto lane : intersection.lane_list)
83 if (lane.lane_attributes.lane_type.choice != j2735_v2x_msgs::msg::LaneTypeAttributes::VEHICLE)
85 RCLCPP_DEBUG_STREAM(rclcpp::get_logger(
"carma_wm::SignalizedIntersectionManager"),
"Lane id: " << (
int)lane.lane_id <<
", is not a lane for vehicle. Only vehicle road is currently supported. Skipping..." );
88 std::vector<lanelet::Point3d> node_list;
90 double curr_x = ref_node.x();
91 double curr_y = ref_node.y();
97 RCLCPP_DEBUG_STREAM(rclcpp::get_logger(
"carma_wm::SignalizedIntersectionManager"),
"Applied reference point correction, delta_x: " <<
intersection_coord_correction_[intersection.id.id].first <<
101 RCLCPP_DEBUG_STREAM(rclcpp::get_logger(
"carma_wm::SignalizedIntersectionManager"),
"Processing Lane id: " << (
int)lane.lane_id);
103 size_t min_number_of_points = 2;
105 size_t size_of_available_points = lane.node_list.nodes.node_set_xy.size();
107 if (size_of_available_points < min_number_of_points)
109 RCLCPP_WARN_STREAM(rclcpp::get_logger(
"carma_wm::SignalizedIntersectionManager"),
"Not enough points are provided to match a lane. Skipping... ");
113 for (
size_t i = 0;
i < size_of_available_points;
i ++ )
115 curr_x = lane.node_list.nodes.node_set_xy[
i].delta.x + curr_x;
116 curr_y = lane.node_list.nodes.node_set_xy[
i].delta.y + curr_y;
117 lanelet::Point3d curr_node{map->pointLayer.uniqueId(), curr_x, curr_y, 0};
119 RCLCPP_DEBUG_STREAM(rclcpp::get_logger(
"carma_wm::SignalizedIntersectionManager"),
"Current node x: " << curr_x <<
", y: " << curr_y);
121 node_list.push_back(curr_node);
124 RCLCPP_DEBUG_STREAM(rclcpp::get_logger(
"carma_wm::SignalizedIntersectionManager"),
"Lane directions: " << (
int)lane.lane_attributes.directional_use.lane_direction);
129 RCLCPP_DEBUG_STREAM(rclcpp::get_logger(
"carma_wm::SignalizedIntersectionManager"),
"Reversed the node list!");
130 std::reverse(node_list.begin(), node_list.end());
133 for (
auto node : node_list)
135 RCLCPP_DEBUG_STREAM(rclcpp::get_logger(
"carma_wm::SignalizedIntersectionManager"),
"intersection: " << intersection.id.id <<
", " << node.x() <<
", " << node.y());
140 for (
auto connection : lane.connect_to_list)
142 signal_group_to_exit_lanes[connection.signal_group].emplace(connection.connecting_lane.lane);
145 signal_group_to_entry_lanes[connection.signal_group].emplace(lane.lane_id);
151 if (affected_llts.empty())
155 RCLCPP_WARN_STREAM(rclcpp::get_logger(
"carma_wm::SignalizedIntersectionManager"),
"Given offset points are not inside the map for lane: " << (
int)lane.lane_id);
159 lanelet::Id corresponding_lanelet_id;
162 corresponding_lanelet_id = affected_llts.front().id();
163 RCLCPP_DEBUG_STREAM(rclcpp::get_logger(
"carma_wm::SignalizedIntersectionManager"),
"Default corresponding_lanelet_id: " << corresponding_lanelet_id <<
", in EGRESS");
168 corresponding_lanelet_id = affected_llts.back().id();
169 RCLCPP_DEBUG_STREAM(rclcpp::get_logger(
"carma_wm::SignalizedIntersectionManager"),
"Default corresponding_lanelet_id: " << corresponding_lanelet_id <<
", in INGRESS");
172 for (
auto llt : affected_llts)
174 RCLCPP_DEBUG_STREAM(rclcpp::get_logger(
"carma_wm::SignalizedIntersectionManager"),
"Checking if we can get entry/exit from lanelet " << llt.id());
176 if (llt.lanelet().get().hasAttribute(
"turn_direction") &&
177 (llt.lanelet().get().attribute(
"turn_direction").value().compare(
"left") == 0 ||
178 llt.lanelet().get().attribute(
"turn_direction").value().compare(
"right") == 0))
180 std::vector<lanelet::ConstLanelet> connecting_llts;
183 RCLCPP_DEBUG_STREAM(rclcpp::get_logger(
"carma_wm::SignalizedIntersectionManager"),
"lanelet " << llt.id() <<
" is actually part of the intersecion. Trying to detect EGRESS...");
184 connecting_llts = current_routing_graph->following(llt.lanelet().get());
188 RCLCPP_DEBUG_STREAM(rclcpp::get_logger(
"carma_wm::SignalizedIntersectionManager"),
"lanelet " << llt.id() <<
" is actually part of the intersecion. Trying to detect INGRESS...");
189 connecting_llts = current_routing_graph->previous(llt.lanelet().get());
192 if (!connecting_llts.empty())
194 corresponding_lanelet_id = connecting_llts[0].id();
198 RCLCPP_WARN_STREAM(rclcpp::get_logger(
"carma_wm::SignalizedIntersectionManager"),
"Interestingly, did not detect here");
206 RCLCPP_DEBUG_STREAM(rclcpp::get_logger(
"carma_wm::SignalizedIntersectionManager"),
"Found existing lanelet id: " << corresponding_lanelet_id);
210 RCLCPP_DEBUG_STREAM(rclcpp::get_logger(
"carma_wm::SignalizedIntersectionManager"),
"Detected INGRESS, " << (
int)lane.lane_id);
211 entry[lane.lane_id] = corresponding_lanelet_id;
215 RCLCPP_DEBUG_STREAM(rclcpp::get_logger(
"carma_wm::SignalizedIntersectionManager"),
"Detected EGRESS, " << (
int)lane.lane_id);
216 exit[lane.lane_id] = corresponding_lanelet_id;
222 for (
auto iter = signal_group_to_exit_lanes.begin(); iter != signal_group_to_exit_lanes.end(); iter++)
224 for (
auto exit_lane : iter->second)
226 if (exit.find(exit_lane) != exit.end())
228 RCLCPP_DEBUG_STREAM(rclcpp::get_logger(
"carma_wm::SignalizedIntersectionManager"),
"Adding exit_lane id: " << exit_lane);
235 RCLCPP_WARN_STREAM(rclcpp::get_logger(
"carma_wm::SignalizedIntersectionManager"),
"Unable to convert exit lane Id: " +
std::to_string((
int)exit_lane) +
", to lanelet id using the given MAP.msg!");
241 for (
auto iter = signal_group_to_entry_lanes.begin(); iter != signal_group_to_entry_lanes.end(); iter++)
243 for (
auto entry_lane : iter->second)
245 RCLCPP_DEBUG_STREAM(rclcpp::get_logger(
"carma_wm::SignalizedIntersectionManager"),
"Adding entry_lane id: " << entry_lane);
246 if (entry.find(entry_lane) != entry.end())
254 RCLCPP_WARN_STREAM(rclcpp::get_logger(
"carma_wm::SignalizedIntersectionManager"),
"Unable to convert entry lane Id: " +
std::to_string((
int)entry_lane) +
", to lanelet id using the given MAP.msg!");
262 lanelet::Id matching_id = lanelet::InvalId;
264 std::vector<lanelet::SignalizedIntersectionPtr> existing_si;
266 for (
auto llt : entry_llts)
268 auto intersections = llt.regulatoryElementsAs<lanelet::SignalizedIntersection>();
270 if (intersections.empty())
275 existing_si.insert(existing_si.end(), intersections.begin(),intersections.end());
278 for (
auto intersection : existing_si)
280 auto existing_entry_llts = intersection->getEntryLanelets();
281 auto existing_exit_llts = intersection->getExitLanelets();
283 if (existing_exit_llts.size() != exit_llts.size() || existing_entry_llts.size() != entry_llts.size())
286 bool is_different =
false;
288 for (
auto llt: existing_entry_llts)
290 if (std::find(entry_llts.begin(), entry_llts.end(), llt) == entry_llts.end())
298 for (
auto llt: existing_exit_llts)
300 if (std::find(exit_llts.begin(), exit_llts.end(), llt) == exit_llts.end())
311 matching_id = intersection->id();
323 std::vector<lanelet::LineString3d> stop_lines;
324 for (
auto llt : entry_lanelets)
326 std::vector<lanelet::Point3d> points;
327 points.push_back(lanelet::Point3d(lanelet::utils::getId(), llt.leftBound2d().back().x(), llt.leftBound2d().back().y(), 0));
328 points.push_back(lanelet::Point3d(lanelet::utils::getId(), llt.rightBound().back().x(), llt.rightBound().back().y(), 0));
330 lanelet::LineString3d stop_line(lanelet::utils::getId(), points);
331 stop_lines.push_back(stop_line);
334 lanelet::Id traffic_light_id = lanelet::utils::getId();
335 std::shared_ptr<lanelet::CarmaTrafficSignal> traffic_light(
new lanelet::CarmaTrafficSignal(lanelet::CarmaTrafficSignal::buildData(traffic_light_id, stop_lines, entry_lanelets, exit_lanelets)));
338 for (
auto llt : exit_lanelets)
342 for (
auto llt : entry_lanelets)
346 return traffic_light;
350 const std::shared_ptr<lanelet::LaneletMap>& map, std::shared_ptr<const lanelet::routing::RoutingGraph> routing_graph)
355 RCLCPP_WARN_STREAM(rclcpp::get_logger(
"carma_wm::SignalizedIntersectionManager"),
"Georeference is not loaded yet. Returning without processing this MAP msg.");
359 for (
auto const& intersection : map_msg.intersections)
361 std::unordered_map<uint8_t, lanelet::Id> entry;
362 std::unordered_map<uint8_t, lanelet::Id> exit;
366 std::vector<lanelet::Lanelet> entry_llts;
367 std::vector<lanelet::Lanelet> exit_llts;
369 for (
auto iter = entry.begin(); iter != entry.end(); iter++)
371 entry_llts.push_back(map->laneletLayer.get(iter->second));
373 for (
auto iter = exit.begin(); iter != exit.end(); iter++)
375 exit_llts.push_back(map->laneletLayer.get(iter->second));
380 if (intersection_id == lanelet::InvalId)
382 RCLCPP_DEBUG_STREAM(rclcpp::get_logger(
"carma_wm::SignalizedIntersectionManager"),
"No existing intersection found. Creating a new one...");
383 intersection_id = lanelet::utils::getId();
387 std::shared_ptr<lanelet::SignalizedIntersection> sig_inter(
new lanelet::SignalizedIntersection
388 (lanelet::SignalizedIntersection::buildData(intersection_id, entry_llts, exit_llts, interior_llts)));
391 sig_intersections.push_back(sig_inter);
400 RCLCPP_DEBUG_STREAM(rclcpp::get_logger(
"carma_wm::SignalizedIntersectionManager"),
"Creating signal for: " << (
int)sig_grp_pair.first);
408 std::vector<lanelet::Lanelet> exit_lanelets;
409 for (
auto iter = sig_grp_pair.second.begin(); iter != sig_grp_pair.second.end(); iter++)
411 exit_lanelets.push_back(map->laneletLayer.get(*iter));
413 std::vector<lanelet::Lanelet> entry_lanelets;
416 entry_lanelets.push_back(map->laneletLayer.get(*iter));
425 lanelet::BasicLineString2d polygon_corners;
427 if (entry_llts.size() < 2)
432 for (
auto llt : entry_llts)
434 lanelet::BasicPoint2d pt(llt.centerline2d().back().x(), llt.centerline2d().back().y());
435 polygon_corners.push_back(pt);
437 lanelet::BasicPolygon2d polygon(polygon_corners);
438 auto interior_llt_pairs = lanelet::geometry::findWithin2d(map->laneletLayer, polygon);
439 lanelet::Lanelets interior_llts;
441 for (
auto pair : interior_llt_pairs)
443 if (std::find(entry_llts.begin(),entry_llts.end(), pair.second) == entry_llts.end())
444 interior_llts.push_back(pair.second);
446 return interior_llts;
478 RCLCPP_INFO_STREAM(rclcpp::get_logger(
"carma_wm"),
"Map is not set yet.");
482 if (spat_msg.intersection_state_list.empty())
484 RCLCPP_WARN_STREAM(rclcpp::get_logger(
"carma_wm"),
"No intersection_state_list in the newly received SPAT msg. Returning...");
488 for (
const auto& curr_intersection : spat_msg.intersection_state_list)
490 for (
const auto& current_movement_state : curr_intersection.movement_list)
492 lanelet::Id curr_light_id =
getTrafficSignalId(curr_intersection.id.id, current_movement_state.signal_group);
494 if (curr_light_id == lanelet::InvalId)
499 lanelet::CarmaTrafficSignalPtr curr_light =
502 if (curr_light ==
nullptr)
508 if (current_movement_state.movement_event_list.empty())
510 RCLCPP_DEBUG_STREAM(rclcpp::get_logger(
"carma_wm"),
"Movement_event_list is empty . intersection_id: " << (
int)curr_intersection.id.id <<
", and signal_group_id: " << (
int)current_movement_state.signal_group);
515 RCLCPP_DEBUG_STREAM(rclcpp::get_logger(
"carma_wm"),
"Movement_event_list size: " << current_movement_state.movement_event_list.size() <<
" . intersection_id: " << (
int)curr_intersection.id.id <<
", and signal_group_id: " << (
int)current_movement_state.signal_group);
518 curr_light->revision_ = curr_intersection.revision;
520 auto [new_end_times_and_states, new_start_times ]=
523 curr_light->recorded_time_stamps = new_end_times_and_states;
524 curr_light->recorded_start_time_stamps = new_start_times;
531 lanelet::Id signal_id = lanelet::InvalId;
537 RCLCPP_WARN_STREAM(rclcpp::get_logger(
"carma_wm"),
"Intersection id: " << intersection_id <<
" is not found in the map. Returning...");
538 return lanelet::InvalId;
547 RCLCPP_WARN_STREAM(rclcpp::get_logger(
"carma_wm"),
"Signal group id: " << signal_group_id <<
" is not found in the map. Returning...");
555 lanelet::Id&
id,
const std::shared_ptr<lanelet::LaneletMap>& semantic_map)
const
557 auto general_regem = semantic_map->regulatoryElementLayer.get(
id);
559 auto lanelets_general = semantic_map->laneletLayer.findUsages(general_regem);
560 if (lanelets_general.empty())
562 RCLCPP_WARN_STREAM(rclcpp::get_logger(
"carma_wm"),
563 "There was an error querying lanelet for traffic light with id: " <<
id);
566 if (
auto curr_light_list =
567 lanelets_general[0].regulatoryElementsAs<lanelet::CarmaTrafficSignal>();
568 curr_light_list.empty())
570 RCLCPP_WARN_STREAM(rclcpp::get_logger(
"carma_wm"),
571 "There was an error querying traffic light with id: " <<
id);
575 lanelet::CarmaTrafficSignalPtr curr_light;
577 for (
auto signal : lanelets_general[0].regulatoryElementsAs<lanelet::CarmaTrafficSignal>())
579 if (signal->id() ==
id)
588 RCLCPP_WARN_STREAM(rclcpp::get_logger(
"carma_wm"),
589 "Was not able to find traffic signal with id: " <<
id <<
", ignoring...");
596 std::tuple<std::vector<std::pair<boost::posix_time::ptime, lanelet::CarmaTrafficSignalState>>,
597 std::vector<boost::posix_time::ptime>>
599 const carma_v2x_msgs::msg::IntersectionState& curr_intersection,
600 const carma_v2x_msgs::msg::MovementState& current_movement_state)
602 std::vector<std::pair<boost::posix_time::ptime, lanelet::CarmaTrafficSignalState>>
603 min_end_t_and_states;
605 std::vector<boost::posix_time::ptime> start_time_and_states;
607 for(
const auto& current_movement_event:current_movement_state.movement_event_list)
610 boost::posix_time::ptime min_end_time = lanelet::time::timeFromSec(
611 current_movement_event.timing.min_end_time);
612 boost::posix_time::ptime start_time = lanelet::time::timeFromSec(
613 current_movement_event.timing.start_time);
616 curr_intersection.moy_exists,curr_intersection.moy,
620 curr_intersection.moy_exists,curr_intersection.moy,
623 auto received_state =
static_cast<lanelet::CarmaTrafficSignalState
>(
624 current_movement_event.event_state.movement_phase_state);
626 min_end_t_and_states.push_back(std::make_pair(min_end_time, received_state));
628 start_time_and_states.push_back(start_time);
630 RCLCPP_DEBUG_STREAM(rclcpp::get_logger(
"carma_wm"),
"intersection id: "
631 << (
int)curr_intersection.id.id <<
", signal: " << (
int)current_movement_state.signal_group
632 <<
", start_time: " <<
std::to_string(lanelet::time::toSec(start_time))
633 <<
", end_time: " <<
std::to_string(lanelet::time::toSec(min_end_time))
634 <<
", state: " << received_state);
637 return std::make_tuple(min_end_t_and_states, start_time_and_states);
641 boost::posix_time::ptime min_end_time,
bool moy_exists,uint32_t moy,
bool is_simulation,
642 bool use_real_time_spat_in_sim)
644 double simulation_time_difference_in_seconds = 0.0;
645 double wall_time_offset_in_seconds = 0.0;
657 wall_time_offset_in_seconds = (std::chrono::duration<double>(std::chrono::system_clock::now().time_since_epoch()).count()) -
ros1_clock_.value().seconds();
660 min_end_time += lanelet::time::durationFromSec(simulation_time_difference_in_seconds);
661 min_end_time -= lanelet::time::durationFromSec(wall_time_offset_in_seconds);
665 auto inception_boost(boost::posix_time::time_from_string(
"1970-01-01 00:00:00.000"));
666 auto duration_since_inception(lanelet::time::durationFromSec(std::chrono::duration<double>(std::chrono::system_clock::now().time_since_epoch()).count()));
667 auto curr_time_boost = inception_boost + duration_since_inception;
669 int curr_year = curr_time_boost.date().year();
672 if (is_simulation && !use_real_time_spat_in_sim)
675 auto curr_year_start_boost(boost::posix_time::time_from_string(
std::to_string(curr_year)+
"-01-01 00:00:00.000"));
677 auto curr_minute_stamp_boost = curr_year_start_boost + boost::posix_time::minutes((
int)moy);
679 long hours_of_day = curr_minute_stamp_boost.time_of_day().hours();
680 long curr_month = curr_minute_stamp_boost.date().month();
681 long curr_day = curr_minute_stamp_boost.date().day();
684 auto curr_hour_boost = curr_day_boost + boost::posix_time::hours(hours_of_day);
686 min_end_time += lanelet::time::durationFromSec(lanelet::time::toSec(curr_hour_boost));
This class manages and keeps track of all signalized intersections in the map. All of the SPAT and MA...
lanelet::Lanelets identifyInteriorLanelets(const lanelet::Lanelets &entry_llts, const std::shared_ptr< lanelet::LaneletMap > &map)
Returns carma traffic signal and saves all id relevant id mappings (signal group to lanelet id) inter...
lanelet::Id matchSignalizedIntersection(const lanelet::Lanelets &entry_llts, const lanelet::Lanelets &exit_llts)
Returns existing signalized intersection with same entry and exit llts if exists.
std::optional< rclcpp::Time > simulation_clock_
std::unordered_map< uint16_t, std::vector< lanelet::Point3d > > intersection_nodes_
std::tuple< std::vector< std::pair< boost::posix_time::ptime, lanelet::CarmaTrafficSignalState > >, std::vector< boost::posix_time::ptime > > extract_signal_states_from_movement_state(const carma_v2x_msgs::msg::IntersectionState &curr_intersection, const carma_v2x_msgs::msg::MovementState ¤t_movement_state)
Extract tuple of 1) vector of pair of traffic signal states' min_end_time and their states,...
std::unordered_map< uint16_t, std::pair< double, double > > intersection_coord_correction_
void setTargetFrame(const std::string &target_frame)
Saves the georeference string to be used for converting MAP.msg coordinates.
std::unordered_map< uint8_t, std::unordered_set< lanelet::Id > > signal_group_to_entry_lanelet_ids_
std::optional< rclcpp::Time > ros1_clock_
std::string target_frame_
std::unordered_map< lanelet::Id, uint16_t > regem_id_to_intersection_id_
std::unordered_map< uint16_t, lanelet::Id > intersection_id_to_regem_id_
lanelet::Id getTrafficSignalId(uint16_t intersection_id, uint8_t signal_id)
helper for traffic signal Id
std::shared_ptr< lanelet::CarmaTrafficSignal > createTrafficSignalUsingSGID(uint8_t signal_group_id, const lanelet::Lanelets &entry_lanelets, const lanelet::Lanelets &exit_lanelets)
Returns carma traffic signal and saves all id relevant id mappings (signal group to lanelet id) inter...
SIGNAL_PHASE_PROCESSING spat_processor_state_
lanelet::CarmaTrafficSignalPtr getTrafficSignal(const lanelet::Id &id, const std::shared_ptr< lanelet::LaneletMap > &semantic_map) const
helper for getting traffic signal with given lanelet::Id
bool use_real_time_spat_in_sim_
SignalizedIntersectionManager & operator=(SignalizedIntersectionManager other)
Assignment operator that copies everything except the traffic signal states. This is to keep the stat...
bool operator==(const SignalizedIntersectionManager &rhs)
Equality operator that checks if every mapping are same except the traffic signal states....
std::unordered_map< uint8_t, std::unordered_set< lanelet::Id > > signal_group_to_exit_lanelet_ids_
void processSpatFromMsg(const carma_v2x_msgs::msg::SPAT &spat_msg, const std::shared_ptr< lanelet::LaneletMap > &semantic_map)
processSpatFromMsg update map's traffic light states with SPAT msg NOTE: This is enabled in the indiv...
std::unordered_map< uint8_t, lanelet::Id > signal_group_to_traffic_light_id_
void convertLaneToLaneletId(std::unordered_map< uint8_t, lanelet::Id > &entry, std::unordered_map< uint8_t, lanelet::Id > &exit, const carma_v2x_msgs::msg::IntersectionGeometry &intersection, const std::shared_ptr< lanelet::LaneletMap > &map, std::shared_ptr< const lanelet::routing::RoutingGraph > current_routing_graph)
Returns mapping of MAP lane id to lanelet id for the given map and intersection.msg in the MAP....
boost::posix_time::ptime min_end_time_converter_minute_of_year(boost::posix_time::ptime min_end_time, bool moy_exists, uint32_t moy=0, bool is_simulation=true, bool use_real_time_spat_in_sim=false)
update minimum end time to account for minute of the year
void setMaxLaneWidth(double max_lane_width)
Sets the max lane width in meters. Map msg points are associated to a lanelet if they are within this...
void createIntersectionFromMapMsg(std::vector< lanelet::SignalizedIntersectionPtr > &intersections, std::vector< lanelet::CarmaTrafficSignalPtr > &traffic_signals, const carma_v2x_msgs::msg::MapData &map_msg, const std::shared_ptr< lanelet::LaneletMap > &map, std::shared_ptr< const lanelet::routing::RoutingGraph > routing_graph)
Create relevant signalized intersection and carma traffic signals based on the MAP....
SignalizedIntersectionManager()
auto to_string(const UtmZone &zone) -> std::string
lanelet::ConstLaneletOrAreas getAffectedLaneletOrAreas(const lanelet::Points3d &gf_pts, const lanelet::LaneletMapPtr &lanelet_map, std::shared_ptr< const lanelet::routing::RoutingGraph > routing_graph, double max_lane_width)
Gets the affected lanelet or areas based on the points in the given map's frame.
static const uint8_t INGRESS
static const uint8_t EGRESS