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.
|
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>
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... | |
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. 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 ¤t_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 |
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.
|
inline |
Definition at line 63 of file SignalizedIntersectionManager.hpp.
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.
[out] | other | manager |
Definition at line 460 of file SignalizedIntersectionManager.cpp.
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_.
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.
[out] | entry | lane id to lanelet id mappings to return |
[out] | exit | lane id to lanelet id mappings to return |
intersection | MAP.msg that consists all static data portion of the intersection | |
map | lanelet_map to check | |
routing_graph | of the lanelet map to accurately detect lanes |
invalid_argument | if 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.
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().
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.
[out] | intersections | to return |
[out] | traffic_signals | to return |
map_msg | MAP.msg that consists all static data portion of the intersection | |
map | lanelet_map to check | |
routing_graph | of the lanelet map to accurately detect lanes |
Definition at line 349 of file SignalizedIntersectionManager.cpp.
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_.
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.
signal_group_id | of the traffic signal |
exit_llts | of the signal_group |
entry_llts | of the signal_group |
Definition at line 321 of file SignalizedIntersectionManager.cpp.
References signal_group_to_entry_lanelet_ids_, signal_group_to_exit_lanelet_ids_, and signal_group_to_traffic_light_id_.
Referenced by createIntersectionFromMapMsg().
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.
curr_intersection | carma_v2x_msgs::msg::IntersectionState from SPAT |
current_movement_state | carma_v2x_msgs::msg::MovementState from SPAT |
Definition at line 598 of file SignalizedIntersectionManager.cpp.
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().
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.
Referenced by processSpatFromMsg().
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.
References intersection_id_to_regem_id_, and signal_group_to_traffic_light_id_.
Referenced by processSpatFromMsg().
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.
signal_group_id | of the traffic signal |
exit_llts | of the signal_group |
entry_llts | of the signal_group |
Definition at line 423 of file SignalizedIntersectionManager.cpp.
Referenced by createIntersectionFromMapMsg().
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.
entry_llts | of the intersection |
exit_llts | of the intersection |
Definition at line 260 of file SignalizedIntersectionManager.cpp.
Referenced by createIntersectionFromMapMsg().
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
min_end_time | minimum end time of the spat movement event list |
moy_exists | tells weather minute of the year exist or not |
moy | value of the minute of the year |
use_real_time_spat_in_sim | Boolean 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.
References ros1_clock_, simulation_clock_, and carma_cooperative_perception::to_string().
Referenced by extract_signal_states_from_movement_state().
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.
[out] | other | manager |
Definition at line 449 of file SignalizedIntersectionManager.cpp.
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_.
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.
[out] | rhs | manager |
Definition at line 35 of file SignalizedIntersectionManager.cpp.
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_.
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()
spat_msg | Msg to update with |
lanelet_map | to update the regulatory elements according to internal signals recorded |
Definition at line 469 of file SignalizedIntersectionManager.cpp.
References extract_signal_states_from_movement_state(), getTrafficSignal(), getTrafficSignalId(), carma_wm::OFF, and spat_processor_state_.
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.
References max_lane_width_.
void carma_wm::SignalizedIntersectionManager::setTargetFrame | ( | const std::string & | target_frame | ) |
Saves the georeference string to be used for converting MAP.msg coordinates.
target_frame | PROJ string of the map |
Definition at line 25 of file SignalizedIntersectionManager.cpp.
References target_frame_.
std::unordered_map<uint16_t, std::pair<double, double> > carma_wm::SignalizedIntersectionManager::intersection_coord_correction_ |
Definition at line 202 of file SignalizedIntersectionManager.hpp.
Referenced by SignalizedIntersectionManager(), convertLaneToLaneletId(), and operator=().
std::unordered_map<uint16_t, lanelet::Id> carma_wm::SignalizedIntersectionManager::intersection_id_to_regem_id_ |
Definition at line 205 of file SignalizedIntersectionManager.hpp.
Referenced by SignalizedIntersectionManager(), createIntersectionFromMapMsg(), getTrafficSignalId(), boost::serialization::load(), carma_wm::logSignalizedIntersectionManager(), operator=(), operator==(), and boost::serialization::save().
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().
|
private |
Definition at line 231 of file SignalizedIntersectionManager.hpp.
Referenced by convertLaneToLaneletId(), and setMaxLaneWidth().
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().
std::optional<rclcpp::Time> carma_wm::SignalizedIntersectionManager::ros1_clock_ = std::nullopt |
Definition at line 219 of file SignalizedIntersectionManager.hpp.
Referenced by min_end_time_converter_minute_of_year(), and carma_wm::CARMAWorldModel::setRos1Clock().
std::unordered_map<uint8_t, std::unordered_set<lanelet::Id> > carma_wm::SignalizedIntersectionManager::signal_group_to_entry_lanelet_ids_ |
Definition at line 217 of file SignalizedIntersectionManager.hpp.
Referenced by SignalizedIntersectionManager(), convertLaneToLaneletId(), createIntersectionFromMapMsg(), createTrafficSignalUsingSGID(), boost::serialization::load(), carma_wm::logSignalizedIntersectionManager(), operator=(), operator==(), and boost::serialization::save().
std::unordered_map<uint8_t, std::unordered_set<lanelet::Id> > carma_wm::SignalizedIntersectionManager::signal_group_to_exit_lanelet_ids_ |
Definition at line 214 of file SignalizedIntersectionManager.hpp.
Referenced by SignalizedIntersectionManager(), convertLaneToLaneletId(), createIntersectionFromMapMsg(), createTrafficSignalUsingSGID(), boost::serialization::load(), carma_wm::logSignalizedIntersectionManager(), operator=(), operator==(), and boost::serialization::save().
std::unordered_map<uint8_t, lanelet::Id> carma_wm::SignalizedIntersectionManager::signal_group_to_traffic_light_id_ |
Definition at line 211 of file SignalizedIntersectionManager.hpp.
Referenced by SignalizedIntersectionManager(), createIntersectionFromMapMsg(), createTrafficSignalUsingSGID(), getTrafficSignalId(), boost::serialization::load(), carma_wm::logSignalizedIntersectionManager(), operator=(), operator==(), and boost::serialization::save().
std::optional<rclcpp::Time> carma_wm::SignalizedIntersectionManager::simulation_clock_ = std::nullopt |
Definition at line 220 of file SignalizedIntersectionManager.hpp.
Referenced by min_end_time_converter_minute_of_year(), and carma_wm::CARMAWorldModel::setSimulationClock().
SIGNAL_PHASE_PROCESSING carma_wm::SignalizedIntersectionManager::spat_processor_state_ = SIGNAL_PHASE_PROCESSING::OFF |
Definition at line 224 of file SignalizedIntersectionManager.hpp.
Referenced by processSpatFromMsg().
|
private |
Definition at line 228 of file SignalizedIntersectionManager.hpp.
Referenced by convertLaneToLaneletId(), createIntersectionFromMapMsg(), and setTargetFrame().
bool carma_wm::SignalizedIntersectionManager::use_real_time_spat_in_sim_ |
Definition at line 223 of file SignalizedIntersectionManager.hpp.
Referenced by extract_signal_states_from_movement_state().
bool carma_wm::SignalizedIntersectionManager::use_sim_time_ |
Definition at line 222 of file SignalizedIntersectionManager.hpp.
Referenced by extract_signal_states_from_movement_state().