Carma-platform v4.2.0
CARMA Platform is built on robot operating system (ROS) and utilizes open source software (OSS) that enables Cooperative Driving Automation (CDA) features to allow Automated Driving Systems to interact and cooperate with infrastructure and other vehicles through communication.
carma_wm::SignalizedIntersectionManager Class Reference

This class manages and keeps track of all signalized intersections in the map. All of the SPAT and MAP standard's lane ids to lanelet id mapping is recorded here. NOTE: This class functions do not update the map given. More...

#include <SignalizedIntersectionManager.hpp>

Collaboration diagram for carma_wm::SignalizedIntersectionManager:
Collaboration graph

Public Member Functions

 SignalizedIntersectionManager ()
 
 SignalizedIntersectionManager (const SignalizedIntersectionManager &other)
 Copy constructor that copies everything except the traffic signal states. This is to keep the states although the map is updated or a similar event happened NOTE: The function does not update the map with new elements. More...
 
SignalizedIntersectionManageroperator= (SignalizedIntersectionManager other)
 Assignment operator that copies everything except the traffic signal states. This is to keep the states although the map is updated or a similar event happened NOTE: The function does not update the map with new elements. More...
 
bool operator== (const SignalizedIntersectionManager &rhs)
 Equality operator that checks if every mapping are same except the traffic signal states. This is to keep the states although the map is updated or a similar event happened NOTE: The function does not update the map with new elements. More...
 
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.msg and the lanelet_map NOTE: The function does not update the map with new elements. More...
 
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.msg. This function also records signal_group_id_to its own lanelet id, and also signal group to entry and exit lanelets id mappings. More...
 
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 distance to a lanelet as map msg points are guaranteed to apply to a single lane. More...
 
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. More...
 
void setTargetFrame (const std::string &target_frame)
 Saves the georeference string to be used for converting MAP.msg coordinates. More...
 
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) internally. More...
 
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) internally. More...
 
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 individual nodes through wm_listener's setWMSpatProcessingState() More...
 
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 &current_movement_state)
 Extract tuple of 1) vector of pair of traffic signal states' min_end_time and their states, which is mainly used by CARMA to comply with traffic signals. 2) vector of traffic signal states' start_times that match in order. More...
 
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 More...
 
lanelet::Id getTrafficSignalId (uint16_t intersection_id, uint8_t signal_id)
 helper for traffic signal Id More...
 
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 More...
 

Public Attributes

std::unordered_map< uint16_t, std::vector< lanelet::Point3d > > intersection_nodes_
 
std::unordered_map< uint16_t, std::pair< double, double > > intersection_coord_correction_
 
std::unordered_map< uint16_t, lanelet::Id > intersection_id_to_regem_id_
 
std::unordered_map< lanelet::Id, uint16_t > regem_id_to_intersection_id_
 
std::unordered_map< uint8_t, lanelet::Id > signal_group_to_traffic_light_id_
 
std::unordered_map< uint8_t, std::unordered_set< lanelet::Id > > signal_group_to_exit_lanelet_ids_
 
std::unordered_map< uint8_t, std::unordered_set< lanelet::Id > > signal_group_to_entry_lanelet_ids_
 
std::optional< rclcpp::Time > ros1_clock_ = std::nullopt
 
std::optional< rclcpp::Time > simulation_clock_ = std::nullopt
 
bool use_sim_time_
 
bool use_real_time_spat_in_sim_
 
SIGNAL_PHASE_PROCESSING spat_processor_state_ = SIGNAL_PHASE_PROCESSING::OFF
 

Private Attributes

std::string target_frame_ = ""
 
double max_lane_width_ = 4
 

Detailed Description

This class manages and keeps track of all signalized intersections in the map. All of the SPAT and MAP standard's lane ids to lanelet id mapping is recorded here. NOTE: This class functions do not update the map given.

Definition at line 59 of file SignalizedIntersectionManager.hpp.

Constructor & Destructor Documentation

◆ SignalizedIntersectionManager() [1/2]

carma_wm::SignalizedIntersectionManager::SignalizedIntersectionManager ( )
inline

Definition at line 63 of file SignalizedIntersectionManager.hpp.

63{}

◆ SignalizedIntersectionManager() [2/2]

carma_wm::SignalizedIntersectionManager::SignalizedIntersectionManager ( const SignalizedIntersectionManager other)

Copy constructor that copies everything except the traffic signal states. This is to keep the states although the map is updated or a similar event happened NOTE: The function does not update the map with new elements.

Parameters
[out]othermanager

Definition at line 460 of file SignalizedIntersectionManager.cpp.

461 {
462 this->signal_group_to_entry_lanelet_ids_ = other.signal_group_to_entry_lanelet_ids_;
463 this->signal_group_to_exit_lanelet_ids_ = other.signal_group_to_exit_lanelet_ids_;
464 this->intersection_id_to_regem_id_ = other.intersection_id_to_regem_id_;
465 this->signal_group_to_traffic_light_id_ = other.signal_group_to_traffic_light_id_;
466 this->intersection_coord_correction_ = other.intersection_coord_correction_;
467 }
std::unordered_map< uint16_t, std::pair< double, double > > intersection_coord_correction_
std::unordered_map< uint8_t, std::unordered_set< lanelet::Id > > signal_group_to_entry_lanelet_ids_
std::unordered_map< uint16_t, lanelet::Id > intersection_id_to_regem_id_
std::unordered_map< uint8_t, std::unordered_set< lanelet::Id > > signal_group_to_exit_lanelet_ids_
std::unordered_map< uint8_t, lanelet::Id > signal_group_to_traffic_light_id_

References intersection_coord_correction_, intersection_id_to_regem_id_, signal_group_to_entry_lanelet_ids_, signal_group_to_exit_lanelet_ids_, and signal_group_to_traffic_light_id_.

Member Function Documentation

◆ convertLaneToLaneletId()

void carma_wm::SignalizedIntersectionManager::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.msg. This function also records signal_group_id_to its own lanelet id, and also signal group to entry and exit lanelets id mappings.

Parameters
[out]entrylane id to lanelet id mappings to return
[out]exitlane id to lanelet id mappings to return
intersectionMAP.msg that consists all static data portion of the intersection
maplanelet_map to check
routing_graphof the lanelet map to accurately detect lanes
Exceptions
invalid_argumentif given coordinates in the msg doesn't exist in the map TODO: Need to think about error buffer in the future. Map msgs are made from google maps or open streets maps normally so this function might run into some errors from that.

Definition at line 58 of file SignalizedIntersectionManager.cpp.

60 {
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;
63
64 if (target_frame_ == "")
65 {
66 throw std::invalid_argument("Map is not initialized yet as the georeference was not found...");
67 }
68
69 lanelet::projection::LocalFrameProjector local_projector(target_frame_.c_str());
70
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;
75
76 auto ref_node = local_projector.forward(gps_point);
77
78 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm::SignalizedIntersectionManager"), "Reference node in map frame x: " << ref_node.x() << ", y: " << ref_node.y());
79
80
81 for (auto lane : intersection.lane_list)
82 {
83 if (lane.lane_attributes.lane_type.choice != j2735_v2x_msgs::msg::LaneTypeAttributes::VEHICLE)
84 {
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..." );
86 continue;
87 }
88 std::vector<lanelet::Point3d> node_list;
89
90 double curr_x = ref_node.x();
91 double curr_y = ref_node.y();
92
93 if (intersection_coord_correction_.find(intersection.id.id) != intersection_coord_correction_.end())
94 {
95 curr_x += intersection_coord_correction_[intersection.id.id].first;
96 curr_y += intersection_coord_correction_[intersection.id.id].second;
97 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm::SignalizedIntersectionManager"), "Applied reference point correction, delta_x: " << intersection_coord_correction_[intersection.id.id].first <<
98 ", delta_y: " << intersection_coord_correction_[intersection.id.id].second << ", to intersection id: " << (int)lane.lane_id);
99 }
100
101 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm::SignalizedIntersectionManager"), "Processing Lane id: " << (int)lane.lane_id);
102
103 size_t min_number_of_points = 2; // two points minimum are required
104
105 size_t size_of_available_points = lane.node_list.nodes.node_set_xy.size();
106
107 if (size_of_available_points < min_number_of_points)
108 {
109 RCLCPP_WARN_STREAM(rclcpp::get_logger("carma_wm::SignalizedIntersectionManager"), "Not enough points are provided to match a lane. Skipping... ");
110 continue;
111 }
112
113 for (size_t i = 0; i < size_of_available_points; i ++ )
114 {
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};
118
119 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm::SignalizedIntersectionManager"), "Current node x: " << curr_x << ", y: " << curr_y);
120
121 node_list.push_back(curr_node);
122 }
123
124 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm::SignalizedIntersectionManager"), "Lane directions: " << (int)lane.lane_attributes.directional_use.lane_direction);
125
126 if (lane.lane_attributes.directional_use.lane_direction == LANE_DIRECTION::INGRESS)
127 {
128 // flip direction if ingress to pick up correct lanelets
129 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm::SignalizedIntersectionManager"), "Reversed the node list!");
130 std::reverse(node_list.begin(), node_list.end());
131 }
132
133 for (auto node : node_list)
134 {
135 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm::SignalizedIntersectionManager"), "intersection: " << intersection.id.id << ", " << node.x() << ", " << node.y());
136 }
137 intersection_nodes_[intersection.id.id].insert(intersection_nodes_[intersection.id.id].end(), node_list.begin(), node_list.end());
138
139 // save which signal group connect to which exit lanes
140 for (auto connection : lane.connect_to_list)
141 {
142 signal_group_to_exit_lanes[connection.signal_group].emplace(connection.connecting_lane.lane);
143
144 if (lane.lane_attributes.directional_use.lane_direction == LANE_DIRECTION::INGRESS)
145 signal_group_to_entry_lanes[connection.signal_group].emplace(lane.lane_id);
146 }
147
148 // query corresponding lanelet lane from local map
149 auto affected_llts = carma_wm::query::getAffectedLaneletOrAreas(node_list, map, current_routing_graph, max_lane_width_);
150
151 if (affected_llts.empty())
152 {
153 // https://github.com/usdot-fhwa-stol/carma-platform/issues/1593
154 // Open issue TODO on how this error is handled
155 RCLCPP_WARN_STREAM(rclcpp::get_logger("carma_wm::SignalizedIntersectionManager"), "Given offset points are not inside the map for lane: " << (int)lane.lane_id);
156 continue;
157 }
158
159 lanelet::Id corresponding_lanelet_id;
160 if (lane.lane_attributes.directional_use.lane_direction == LANE_DIRECTION::EGRESS)
161 {
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");
164
165 }
166 else //ingress
167 {
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");
170 }
171
172 for (auto llt : affected_llts) // filter out intersection lanelets
173 {
174 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm::SignalizedIntersectionManager"), "Checking if we can get entry/exit from lanelet " << llt.id());
175 //TODO direction of affected_llts may play role, but it should be good
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))
179 {
180 std::vector<lanelet::ConstLanelet> connecting_llts;
181 if (lane.lane_attributes.directional_use.lane_direction == LANE_DIRECTION::EGRESS)
182 {
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());
185 }
186 else
187 {
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());
190 }
191
192 if (!connecting_llts.empty())
193 {
194 corresponding_lanelet_id = connecting_llts[0].id();
195 }
196 else
197 {
198 RCLCPP_WARN_STREAM(rclcpp::get_logger("carma_wm::SignalizedIntersectionManager"), "Interestingly, did not detect here");
199 continue;
200 }
201
202 break;
203 }
204 }
205
206 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm::SignalizedIntersectionManager"), "Found existing lanelet id: " << corresponding_lanelet_id);
207
208 if (lane.lane_attributes.directional_use.lane_direction == LANE_DIRECTION::INGRESS)
209 {
210 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm::SignalizedIntersectionManager"), "Detected INGRESS, " << (int)lane.lane_id);
211 entry[lane.lane_id] = corresponding_lanelet_id;
212 }
213 else if (lane.lane_attributes.directional_use.lane_direction == LANE_DIRECTION::EGRESS)
214 {
215 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm::SignalizedIntersectionManager"), "Detected EGRESS, " << (int)lane.lane_id);
216 exit[lane.lane_id] = corresponding_lanelet_id;
217 }
218 // ignoring types that are neither ingress nor egress
219 }
220
221 // convert and save exit lane ids into lanelet ids with their corresponding signal group ids
222 for (auto iter = signal_group_to_exit_lanes.begin(); iter != signal_group_to_exit_lanes.end(); iter++)
223 {
224 for (auto exit_lane : iter->second)
225 {
226 if (exit.find(exit_lane) != exit.end())
227 {
228 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm::SignalizedIntersectionManager"), "Adding exit_lane id: " << exit_lane);
229 signal_group_to_exit_lanelet_ids_[iter->first].insert(exit[exit_lane]);
230 }
231 else
232 {
233 // https://github.com/usdot-fhwa-stol/carma-platform/issues/1593
234 // Open issue TODO on how this error is handled
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!");
236 }
237 }
238 }
239
240 // convert and save entry lane ids into lanelet ids with their corresponding signal group ids
241 for (auto iter = signal_group_to_entry_lanes.begin(); iter != signal_group_to_entry_lanes.end(); iter++)
242 {
243 for (auto entry_lane : iter->second)
244 {
245 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm::SignalizedIntersectionManager"), "Adding entry_lane id: " << entry_lane);
246 if (entry.find(entry_lane) != entry.end())
247 {
248 signal_group_to_entry_lanelet_ids_[iter->first].insert(entry[entry_lane]);
249 }
250 else
251 {
252 // https://github.com/usdot-fhwa-stol/carma-platform/issues/1593
253 // Open issue TODO on how this error is handled
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!");
255 }
256 }
257 }
258 }
std::unordered_map< uint16_t, std::vector< lanelet::Point3d > > intersection_nodes_
auto to_string(const UtmZone &zone) -> std::string
Definition: utm_zone.cpp:21
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.

References carma_wm::LANE_DIRECTION::EGRESS, carma_wm::query::getAffectedLaneletOrAreas(), process_bag::i, carma_wm::LANE_DIRECTION::INGRESS, intersection_coord_correction_, intersection_nodes_, max_lane_width_, signal_group_to_entry_lanelet_ids_, signal_group_to_exit_lanelet_ids_, target_frame_, and carma_cooperative_perception::to_string().

Referenced by createIntersectionFromMapMsg().

Here is the call graph for this function:
Here is the caller graph for this function:

◆ createIntersectionFromMapMsg()

void carma_wm::SignalizedIntersectionManager::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.msg and the lanelet_map NOTE: The function does not update the map with new elements.

Parameters
[out]intersectionsto return
[out]traffic_signalsto return
map_msgMAP.msg that consists all static data portion of the intersection
maplanelet_map to check
routing_graphof the lanelet map to accurately detect lanes

Definition at line 349 of file SignalizedIntersectionManager.cpp.

351 {
352
353 if (target_frame_ == "")
354 {
355 RCLCPP_WARN_STREAM(rclcpp::get_logger("carma_wm::SignalizedIntersectionManager"), "Georeference is not loaded yet. Returning without processing this MAP msg.");
356 return;
357 }
358
359 for (auto const& intersection : map_msg.intersections)
360 {
361 std::unordered_map<uint8_t, lanelet::Id> entry;
362 std::unordered_map<uint8_t, lanelet::Id> exit;
363
364 convertLaneToLaneletId(entry, exit, intersection, map, routing_graph);
365
366 std::vector<lanelet::Lanelet> entry_llts;
367 std::vector<lanelet::Lanelet> exit_llts;
368
369 for (auto iter = entry.begin(); iter != entry.end(); iter++)
370 {
371 entry_llts.push_back(map->laneletLayer.get(iter->second));
372 }
373 for (auto iter = exit.begin(); iter != exit.end(); iter++)
374 {
375 exit_llts.push_back(map->laneletLayer.get(iter->second));
376 }
377
378 lanelet::Id intersection_id = matchSignalizedIntersection(entry_llts, exit_llts);
379
380 if (intersection_id == lanelet::InvalId)
381 {
382 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm::SignalizedIntersectionManager"), "No existing intersection found. Creating a new one...");
383 intersection_id = lanelet::utils::getId();
384
385 std::vector<lanelet::Lanelet> interior_llts = identifyInteriorLanelets(entry_llts, map);
386
387 std::shared_ptr<lanelet::SignalizedIntersection> sig_inter(new lanelet::SignalizedIntersection
388 (lanelet::SignalizedIntersection::buildData(intersection_id, entry_llts, exit_llts, interior_llts)));
389 intersection_id_to_regem_id_[intersection.id.id] = intersection_id;
390 regem_id_to_intersection_id_[intersection_id] = intersection.id.id;
391 sig_intersections.push_back(sig_inter);
392 }
393 }
394
395
396 // create signal group for each from the message
397 // check if it already exists
398 for (auto sig_grp_pair : signal_group_to_exit_lanelet_ids_)
399 {
400 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm::SignalizedIntersectionManager"), "Creating signal for: " << (int)sig_grp_pair.first);
401 // ignore the traffic signals already inside
402 if (signal_group_to_traffic_light_id_.find(sig_grp_pair.first) != signal_group_to_traffic_light_id_.end() &&
403 map->regulatoryElementLayer.exists(signal_group_to_traffic_light_id_[sig_grp_pair.first]))
404 {
405 continue;
406 }
407
408 std::vector<lanelet::Lanelet> exit_lanelets;
409 for (auto iter = sig_grp_pair.second.begin(); iter != sig_grp_pair.second.end(); iter++)
410 {
411 exit_lanelets.push_back(map->laneletLayer.get(*iter));
412 }
413 std::vector<lanelet::Lanelet> entry_lanelets;
414 for (auto iter = signal_group_to_entry_lanelet_ids_[sig_grp_pair.first].begin(); iter != signal_group_to_entry_lanelet_ids_[sig_grp_pair.first].end(); iter++)
415 {
416 entry_lanelets.push_back(map->laneletLayer.get(*iter));
417 }
418
419 traffic_signals.push_back(createTrafficSignalUsingSGID(sig_grp_pair.first, entry_lanelets, exit_lanelets));
420 }
421 }
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::unordered_map< lanelet::Id, uint16_t > regem_id_to_intersection_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...
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....

References convertLaneToLaneletId(), createTrafficSignalUsingSGID(), identifyInteriorLanelets(), intersection_id_to_regem_id_, matchSignalizedIntersection(), regem_id_to_intersection_id_, signal_group_to_entry_lanelet_ids_, signal_group_to_exit_lanelet_ids_, signal_group_to_traffic_light_id_, and target_frame_.

Here is the call graph for this function:

◆ createTrafficSignalUsingSGID()

std::shared_ptr< lanelet::CarmaTrafficSignal > carma_wm::SignalizedIntersectionManager::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) internally.

Parameters
signal_group_idof the traffic signal
exit_lltsof the signal_group
entry_lltsof the signal_group
Returns
traffic signal corresponding to the signal group

Definition at line 321 of file SignalizedIntersectionManager.cpp.

322 {
323 std::vector<lanelet::LineString3d> stop_lines;
324 for (auto llt : entry_lanelets)
325 {
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));
329
330 lanelet::LineString3d stop_line(lanelet::utils::getId(), points);
331 stop_lines.push_back(stop_line);
332 }
333
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)));
336 signal_group_to_traffic_light_id_[signal_group_id] = traffic_light_id;
337
338 for (auto llt : exit_lanelets)
339 {
340 signal_group_to_exit_lanelet_ids_[signal_group_id].insert(llt.id());
341 }
342 for (auto llt : entry_lanelets)
343 {
344 signal_group_to_entry_lanelet_ids_[signal_group_id].insert(llt.id());
345 }
346 return traffic_light;
347 }

References signal_group_to_entry_lanelet_ids_, signal_group_to_exit_lanelet_ids_, and signal_group_to_traffic_light_id_.

Referenced by createIntersectionFromMapMsg().

Here is the caller graph for this function:

◆ extract_signal_states_from_movement_state()

std::tuple< std::vector< std::pair< boost::posix_time::ptime, lanelet::CarmaTrafficSignalState > >, std::vector< boost::posix_time::ptime > > carma_wm::SignalizedIntersectionManager::extract_signal_states_from_movement_state ( const carma_v2x_msgs::msg::IntersectionState &  curr_intersection,
const carma_v2x_msgs::msg::MovementState &  current_movement_state 
)

Extract tuple of 1) vector of pair of traffic signal states' min_end_time and their states, which is mainly used by CARMA to comply with traffic signals. 2) vector of traffic signal states' start_times that match in order.

Parameters
curr_intersectioncarma_v2x_msgs::msg::IntersectionState from SPAT
current_movement_statecarma_v2x_msgs::msg::MovementState from SPAT
Returns
tuple of 2 vectors. 1) vector of min_end_time and state pairs 2) vector of start_time

Definition at line 598 of file SignalizedIntersectionManager.cpp.

601 {
602 std::vector<std::pair<boost::posix_time::ptime, lanelet::CarmaTrafficSignalState>>
603 min_end_t_and_states;
604
605 std::vector<boost::posix_time::ptime> start_time_and_states;
606
607 for(const auto& current_movement_event:current_movement_state.movement_event_list)
608 {
609 // raw min_end_time in seconds measured from the most recent full hour
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);
614
615 min_end_time = min_end_time_converter_minute_of_year(min_end_time,
616 curr_intersection.moy_exists,curr_intersection.moy,
617 use_sim_time_, use_real_time_spat_in_sim_); // Accounting minute of the year
618
619 start_time = min_end_time_converter_minute_of_year(start_time,
620 curr_intersection.moy_exists,curr_intersection.moy,
621 use_sim_time_, use_real_time_spat_in_sim_); // Accounting minute of the year
622
623 auto received_state = static_cast<lanelet::CarmaTrafficSignalState>(
624 current_movement_event.event_state.movement_phase_state);
625
626 min_end_t_and_states.push_back(std::make_pair(min_end_time, received_state));
627
628 start_time_and_states.push_back(start_time);
629
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);
635 }
636
637 return std::make_tuple(min_end_t_and_states, start_time_and_states);
638 }
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

References min_end_time_converter_minute_of_year(), carma_cooperative_perception::to_string(), use_real_time_spat_in_sim_, and use_sim_time_.

Referenced by processSpatFromMsg().

Here is the call graph for this function:
Here is the caller graph for this function:

◆ getTrafficSignal()

lanelet::CarmaTrafficSignalPtr carma_wm::SignalizedIntersectionManager::getTrafficSignal ( const lanelet::Id &  id,
const std::shared_ptr< lanelet::LaneletMap > &  semantic_map 
) const

helper for getting traffic signal with given lanelet::Id

Definition at line 554 of file SignalizedIntersectionManager.cpp.

556 {
557 auto general_regem = semantic_map->regulatoryElementLayer.get(id);
558
559 auto lanelets_general = semantic_map->laneletLayer.findUsages(general_regem);
560 if (lanelets_general.empty())
561 {
562 RCLCPP_WARN_STREAM(rclcpp::get_logger("carma_wm"),
563 "There was an error querying lanelet for traffic light with id: " << id);
564 }
565
566 if (auto curr_light_list =
567 lanelets_general[0].regulatoryElementsAs<lanelet::CarmaTrafficSignal>();
568 curr_light_list.empty())
569 {
570 RCLCPP_WARN_STREAM(rclcpp::get_logger("carma_wm"),
571 "There was an error querying traffic light with id: " << id);
572 return nullptr;
573 }
574
575 lanelet::CarmaTrafficSignalPtr curr_light;
576
577 for (auto signal : lanelets_general[0].regulatoryElementsAs<lanelet::CarmaTrafficSignal>())
578 {
579 if (signal->id() == id)
580 {
581 curr_light = signal;
582 break;
583 }
584 }
585
586 if (!curr_light)
587 {
588 RCLCPP_WARN_STREAM(rclcpp::get_logger("carma_wm"),
589 "Was not able to find traffic signal with id: " << id << ", ignoring...");
590 return nullptr;
591 }
592
593 return curr_light;
594 }

Referenced by processSpatFromMsg().

Here is the caller graph for this function:

◆ getTrafficSignalId()

lanelet::Id carma_wm::SignalizedIntersectionManager::getTrafficSignalId ( uint16_t  intersection_id,
uint8_t  signal_id 
)

helper for traffic signal Id

Definition at line 529 of file SignalizedIntersectionManager.cpp.

530 {
531 lanelet::Id signal_id = lanelet::InvalId;
532
533 if (intersection_id_to_regem_id_.find(intersection_id) == intersection_id_to_regem_id_.end())
534 {
535 // Currently, platform is not supporting multiple intersections, so if the id exists
536 // signal_group is expected to be for that intersection
537 RCLCPP_WARN_STREAM(rclcpp::get_logger("carma_wm"), "Intersection id: " << intersection_id << " is not found in the map. Returning...");
538 return lanelet::InvalId;
539 }
540
542 {
543 signal_id = signal_group_to_traffic_light_id_[signal_group_id];
544 }
545 else
546 {
547 RCLCPP_WARN_STREAM(rclcpp::get_logger("carma_wm"), "Signal group id: " << signal_group_id << " is not found in the map. Returning...");
548 return signal_id;
549 }
550
551 return signal_id;
552 }

References intersection_id_to_regem_id_, and signal_group_to_traffic_light_id_.

Referenced by processSpatFromMsg().

Here is the caller graph for this function:

◆ identifyInteriorLanelets()

lanelet::Lanelets carma_wm::SignalizedIntersectionManager::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) internally.

Parameters
signal_group_idof the traffic signal
exit_lltsof the signal_group
entry_lltsof the signal_group
Returns
traffic signal corresponding to the signal group

Definition at line 423 of file SignalizedIntersectionManager.cpp.

424 {
425 lanelet::BasicLineString2d polygon_corners;
426
427 if (entry_llts.size() < 2) //at least two lanes (1 ingress and 1 egress) needed to form intersection
428 {
429 return {};
430 }
431
432 for (auto llt : entry_llts)
433 {
434 lanelet::BasicPoint2d pt(llt.centerline2d().back().x(), llt.centerline2d().back().y());
435 polygon_corners.push_back(pt);
436 }
437 lanelet::BasicPolygon2d polygon(polygon_corners);
438 auto interior_llt_pairs = lanelet::geometry::findWithin2d(map->laneletLayer, polygon);
439 lanelet::Lanelets interior_llts;
440
441 for (auto pair : interior_llt_pairs)
442 {
443 if (std::find(entry_llts.begin(),entry_llts.end(), pair.second) == entry_llts.end())
444 interior_llts.push_back(pair.second);
445 }
446 return interior_llts;
447 }

Referenced by createIntersectionFromMapMsg().

Here is the caller graph for this function:

◆ matchSignalizedIntersection()

lanelet::Id carma_wm::SignalizedIntersectionManager::matchSignalizedIntersection ( const lanelet::Lanelets &  entry_llts,
const lanelet::Lanelets &  exit_llts 
)

Returns existing signalized intersection with same entry and exit llts if exists.

Parameters
entry_lltsof the intersection
exit_lltsof the intersection
Returns
id of the matching intersection in the map, or lanelet::InvalId if none exists

Definition at line 260 of file SignalizedIntersectionManager.cpp.

261 {
262 lanelet::Id matching_id = lanelet::InvalId;
263
264 std::vector<lanelet::SignalizedIntersectionPtr> existing_si;
265
266 for (auto llt : entry_llts)
267 {
268 auto intersections = llt.regulatoryElementsAs<lanelet::SignalizedIntersection>();
269
270 if (intersections.empty())
271 {
272 // no match if any of the entry lanelet is not part of any intersection.
273 break;
274 }
275 existing_si.insert(existing_si.end(), intersections.begin(),intersections.end());
276 }
277
278 for (auto intersection : existing_si)
279 {
280 auto existing_entry_llts = intersection->getEntryLanelets();
281 auto existing_exit_llts = intersection->getExitLanelets();
282
283 if (existing_exit_llts.size() != exit_llts.size() || existing_entry_llts.size() != entry_llts.size())
284 continue;
285
286 bool is_different = false;
287
288 for (auto llt: existing_entry_llts)
289 {
290 if (std::find(entry_llts.begin(), entry_llts.end(), llt) == entry_llts.end())
291 {
292 is_different = true;
293
294 break;
295 }
296 }
297
298 for (auto llt: existing_exit_llts)
299 {
300 if (std::find(exit_llts.begin(), exit_llts.end(), llt) == exit_llts.end())
301 {
302 is_different = true;
303
304 break;
305 }
306 }
307
308 if (!is_different)
309 {
310 // found a match
311 matching_id = intersection->id();
312
313 break;
314 }
315
316 }
317
318 return matching_id;
319 }

Referenced by createIntersectionFromMapMsg().

Here is the caller graph for this function:

◆ min_end_time_converter_minute_of_year()

boost::posix_time::ptime carma_wm::SignalizedIntersectionManager::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

Parameters
min_end_timeminimum end time of the spat movement event list
moy_existstells weather minute of the year exist or not
moyvalue of the minute of the year
use_real_time_spat_in_simBoolean to indicate if the incoming spat is based on wall clock. Required in edge cases where deployment in simulation is receiving SPaT messages based on wall clock.

Definition at line 640 of file SignalizedIntersectionManager.cpp.

643 {
644 double simulation_time_difference_in_seconds = 0.0;
645 double wall_time_offset_in_seconds = 0.0;
646 // NOTE: In simulation, ROS1 clock (often coming from CARLA) can have a large time ahead.
647 // the timing calculated here is in Simulation time, which is behind. Therefore, the world model adds the offset to make it meaningful to carma-platform:
648 // https://github.com/usdot-fhwa-stol/carma-platform/issues/2217
650 {
651 simulation_time_difference_in_seconds = ros1_clock_.value().seconds() - simulation_clock_.value().seconds();
652 }
653 else if (ros1_clock_ && use_real_time_spat_in_sim)
654 {
655 // NOTE: If carma-platform is running in simulation with clock synced to sim time but the incoming spat information is based on wall clock
656 // the spat signal phase time must be offset to sim time.
657 wall_time_offset_in_seconds = (std::chrono::duration<double>(std::chrono::system_clock::now().time_since_epoch()).count()) - ros1_clock_.value().seconds();
658 }
659
660 min_end_time += lanelet::time::durationFromSec(simulation_time_difference_in_seconds);
661 min_end_time -= lanelet::time::durationFromSec(wall_time_offset_in_seconds);
662
663 if (moy_exists) //account for minute of the year
664 {
665 auto inception_boost(boost::posix_time::time_from_string("1970-01-01 00:00:00.000")); // inception of epoch
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;
668
669 int curr_year = curr_time_boost.date().year();
670
671 // Force the current year to start of epoch if it is simulation
672 if (is_simulation && !use_real_time_spat_in_sim)
673 curr_year = 1970;
674
675 auto curr_year_start_boost(boost::posix_time::time_from_string(std::to_string(curr_year)+ "-01-01 00:00:00.000"));
676
677 auto curr_minute_stamp_boost = curr_year_start_boost + boost::posix_time::minutes((int)moy);
678
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();
682
683 auto curr_day_boost(boost::posix_time::time_from_string(std::to_string(curr_year) + "/" + std::to_string(curr_month) + "/" + std::to_string(curr_day) +" 00:00:00.000")); // GMT is the standard
684 auto curr_hour_boost = curr_day_boost + boost::posix_time::hours(hours_of_day);
685
686 min_end_time += lanelet::time::durationFromSec(lanelet::time::toSec(curr_hour_boost));
687 return min_end_time;
688 }
689 else
690 {
691 return min_end_time; // return unchanged
692 }
693 }

References ros1_clock_, simulation_clock_, and carma_cooperative_perception::to_string().

Referenced by extract_signal_states_from_movement_state().

Here is the call graph for this function:
Here is the caller graph for this function:

◆ operator=()

SignalizedIntersectionManager & carma_wm::SignalizedIntersectionManager::operator= ( SignalizedIntersectionManager  other)

Assignment operator that copies everything except the traffic signal states. This is to keep the states although the map is updated or a similar event happened NOTE: The function does not update the map with new elements.

Parameters
[out]othermanager

Definition at line 449 of file SignalizedIntersectionManager.cpp.

450 {
451 this->signal_group_to_entry_lanelet_ids_ = other.signal_group_to_entry_lanelet_ids_;
452 this->signal_group_to_exit_lanelet_ids_ = other.signal_group_to_exit_lanelet_ids_;
453 this->intersection_id_to_regem_id_ = other.intersection_id_to_regem_id_;
454 this->signal_group_to_traffic_light_id_ = other.signal_group_to_traffic_light_id_;
455 this->intersection_coord_correction_ = other.intersection_coord_correction_;
456
457 return *this;
458 }

References intersection_coord_correction_, intersection_id_to_regem_id_, signal_group_to_entry_lanelet_ids_, signal_group_to_exit_lanelet_ids_, and signal_group_to_traffic_light_id_.

◆ operator==()

bool carma_wm::SignalizedIntersectionManager::operator== ( const SignalizedIntersectionManager rhs)

Equality operator that checks if every mapping are same except the traffic signal states. This is to keep the states although the map is updated or a similar event happened NOTE: The function does not update the map with new elements.

Parameters
[out]rhsmanager

Definition at line 35 of file SignalizedIntersectionManager.cpp.

36 {
37 bool is_same = true;
38 if (rhs.intersection_id_to_regem_id_ != this->intersection_id_to_regem_id_)
39 {
40 is_same = false;
41 }
42 if (rhs.signal_group_to_entry_lanelet_ids_ != this->signal_group_to_entry_lanelet_ids_)
43 {
44 is_same = false;
45 }
46 if (rhs.signal_group_to_exit_lanelet_ids_ != this->signal_group_to_exit_lanelet_ids_)
47 {
48 is_same = false;
49 }
50 if (rhs.signal_group_to_traffic_light_id_ != this->signal_group_to_traffic_light_id_)
51 {
52 is_same = false;
53 }
54
55 return is_same;
56 }

References intersection_id_to_regem_id_, signal_group_to_entry_lanelet_ids_, signal_group_to_exit_lanelet_ids_, and signal_group_to_traffic_light_id_.

◆ processSpatFromMsg()

void carma_wm::SignalizedIntersectionManager::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 individual nodes through wm_listener's setWMSpatProcessingState()

Parameters
spat_msgMsg to update with
lanelet_mapto update the regulatory elements according to internal signals recorded

Definition at line 469 of file SignalizedIntersectionManager.cpp.

470 {
472 {
473 return;
474 }
475
476 if (!semantic_map)
477 {
478 RCLCPP_INFO_STREAM(rclcpp::get_logger("carma_wm"), "Map is not set yet.");
479 return;
480 }
481
482 if (spat_msg.intersection_state_list.empty())
483 {
484 RCLCPP_WARN_STREAM(rclcpp::get_logger("carma_wm"), "No intersection_state_list in the newly received SPAT msg. Returning...");
485 return;
486 }
487
488 for (const auto& curr_intersection : spat_msg.intersection_state_list)
489 {
490 for (const auto& current_movement_state : curr_intersection.movement_list)
491 {
492 lanelet::Id curr_light_id = getTrafficSignalId(curr_intersection.id.id, current_movement_state.signal_group);
493
494 if (curr_light_id == lanelet::InvalId)
495 {
496 continue;
497 }
498
499 lanelet::CarmaTrafficSignalPtr curr_light =
500 getTrafficSignal(curr_light_id, semantic_map);
501
502 if (curr_light == nullptr)
503 {
504 continue;
505 }
506
507 // all maneuver types in same signal group is currently expected to share signal timing, so only 0th index is used when setting states
508 if (current_movement_state.movement_event_list.empty())
509 {
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);
511 continue;
512 }
513 else
514 {
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);
516 }
517
518 curr_light->revision_ = curr_intersection.revision; // valid SPAT msg
519
520 auto [new_end_times_and_states, new_start_times ]=
521 extract_signal_states_from_movement_state(curr_intersection, current_movement_state);
522
523 curr_light->recorded_time_stamps = new_end_times_and_states;
524 curr_light->recorded_start_time_stamps = new_start_times;
525 }
526 }
527 }
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 &current_movement_state)
Extract tuple of 1) vector of pair of traffic signal states' min_end_time and their states,...
lanelet::Id getTrafficSignalId(uint16_t intersection_id, uint8_t signal_id)
helper for traffic signal Id
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

References extract_signal_states_from_movement_state(), getTrafficSignal(), getTrafficSignalId(), carma_wm::OFF, and spat_processor_state_.

Here is the call graph for this function:

◆ setMaxLaneWidth()

void carma_wm::SignalizedIntersectionManager::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 distance to a lanelet as map msg points are guaranteed to apply to a single lane.

Definition at line 30 of file SignalizedIntersectionManager.cpp.

31 {
32 max_lane_width_ = max_lane_width;
33 }

References max_lane_width_.

◆ setTargetFrame()

void carma_wm::SignalizedIntersectionManager::setTargetFrame ( const std::string &  target_frame)

Saves the georeference string to be used for converting MAP.msg coordinates.

Parameters
target_framePROJ string of the map

Definition at line 25 of file SignalizedIntersectionManager.cpp.

26 {
27 target_frame_ = target_frame;
28 }

References target_frame_.

Member Data Documentation

◆ intersection_coord_correction_

std::unordered_map<uint16_t, std::pair<double, double> > carma_wm::SignalizedIntersectionManager::intersection_coord_correction_

◆ intersection_id_to_regem_id_

std::unordered_map<uint16_t, lanelet::Id> carma_wm::SignalizedIntersectionManager::intersection_id_to_regem_id_

◆ intersection_nodes_

std::unordered_map<uint16_t, std::vector<lanelet::Point3d> > carma_wm::SignalizedIntersectionManager::intersection_nodes_

Definition at line 199 of file SignalizedIntersectionManager.hpp.

Referenced by convertLaneToLaneletId().

◆ max_lane_width_

double carma_wm::SignalizedIntersectionManager::max_lane_width_ = 4
private

Definition at line 231 of file SignalizedIntersectionManager.hpp.

Referenced by convertLaneToLaneletId(), and setMaxLaneWidth().

◆ regem_id_to_intersection_id_

std::unordered_map<lanelet::Id, uint16_t> carma_wm::SignalizedIntersectionManager::regem_id_to_intersection_id_

Definition at line 208 of file SignalizedIntersectionManager.hpp.

Referenced by createIntersectionFromMapMsg().

◆ ros1_clock_

std::optional<rclcpp::Time> carma_wm::SignalizedIntersectionManager::ros1_clock_ = std::nullopt

◆ signal_group_to_entry_lanelet_ids_

std::unordered_map<uint8_t, std::unordered_set<lanelet::Id> > carma_wm::SignalizedIntersectionManager::signal_group_to_entry_lanelet_ids_

◆ signal_group_to_exit_lanelet_ids_

std::unordered_map<uint8_t, std::unordered_set<lanelet::Id> > carma_wm::SignalizedIntersectionManager::signal_group_to_exit_lanelet_ids_

◆ signal_group_to_traffic_light_id_

std::unordered_map<uint8_t, lanelet::Id> carma_wm::SignalizedIntersectionManager::signal_group_to_traffic_light_id_

◆ simulation_clock_

std::optional<rclcpp::Time> carma_wm::SignalizedIntersectionManager::simulation_clock_ = std::nullopt

◆ spat_processor_state_

SIGNAL_PHASE_PROCESSING carma_wm::SignalizedIntersectionManager::spat_processor_state_ = SIGNAL_PHASE_PROCESSING::OFF

Definition at line 224 of file SignalizedIntersectionManager.hpp.

Referenced by processSpatFromMsg().

◆ target_frame_

std::string carma_wm::SignalizedIntersectionManager::target_frame_ = ""
private

◆ use_real_time_spat_in_sim_

bool carma_wm::SignalizedIntersectionManager::use_real_time_spat_in_sim_

◆ use_sim_time_

bool carma_wm::SignalizedIntersectionManager::use_sim_time_

The documentation for this class was generated from the following files: