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:

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...

Public Attributes
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< 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::unordered_map< uint16_t, std::unordered_map< uint8_t, std::vector< std::pair< boost::posix_time::ptime, lanelet::CarmaTrafficSignalState > > > >	traffic_signal_states_

std::unordered_map< uint16_t, std::unordered_map< uint8_t, std::vector< boost::posix_time::ptime > > >	traffic_signal_start_times_

std::unordered_map< uint16_t, std::unordered_map< uint8_t, std::pair< boost::posix_time::ptime, lanelet::CarmaTrafficSignalState > > >	last_seen_state_

std::unordered_map< uint16_t, std::unordered_map< uint8_t, int > >	signal_state_counter_

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 52 of file SignalizedIntersectionManager.hpp.

Constructor & Destructor Documentation

◆ SignalizedIntersectionManager() [1/2]

carma_wm::SignalizedIntersectionManager::SignalizedIntersectionManager ( )

inline

Definition at line 56 of file SignalizedIntersectionManager.hpp.

56{}

◆ 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] other manager

Definition at line 458 of file SignalizedIntersectionManager.cpp.

  {
    this->signal_group_to_entry_lanelet_ids_ = other.signal_group_to_entry_lanelet_ids_;
    this->signal_group_to_exit_lanelet_ids_ = other.signal_group_to_exit_lanelet_ids_;
    this->intersection_id_to_regem_id_ = other.intersection_id_to_regem_id_;
    this->signal_group_to_traffic_light_id_ = other.signal_group_to_traffic_light_id_;
    this->intersection_coord_correction_ = other.intersection_coord_correction_;
  }

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]	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

Exceptions

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.

  {
    std::unordered_map<uint8_t, std::unordered_set<uint16_t>> signal_group_to_exit_lanes;
    std::unordered_map<uint8_t, std::unordered_set<uint16_t>> signal_group_to_entry_lanes;
 
    if (target_frame_ == "")
    {
      throw std::invalid_argument("Map is not initialized yet as the georeference was not found...");
    }
 
    lanelet::projection::LocalFrameProjector local_projector(target_frame_.c_str());
 
    lanelet::GPSPoint gps_point;
    gps_point.lat = intersection.ref_point.latitude;
    gps_point.lon = intersection.ref_point.longitude;
    gps_point.ele = intersection.ref_point.elevation;
 
    auto ref_node  = local_projector.forward(gps_point);
 
    RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm::SignalizedIntersectionManager"), "Reference node in map frame x: " << ref_node.x() << ", y: " << ref_node.y());
 
 
    for (auto lane : intersection.lane_list)
    {
      if (lane.lane_attributes.lane_type.choice != j2735_v2x_msgs::msg::LaneTypeAttributes::VEHICLE)
      {
        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..." );
        continue;
      }
      std::vector<lanelet::Point3d> node_list;
 
      double curr_x = ref_node.x();
      double curr_y = ref_node.y();
 
      if (intersection_coord_correction_.find(intersection.id.id) != intersection_coord_correction_.end())
      {
        curr_x += intersection_coord_correction_[intersection.id.id].first;
        curr_y += intersection_coord_correction_[intersection.id.id].second;
        RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm::SignalizedIntersectionManager"), "Applied reference point correction, delta_x: " <<  intersection_coord_correction_[intersection.id.id].first <<
                          ", delta_y: " << intersection_coord_correction_[intersection.id.id].second << ", to intersection id: " << (int)lane.lane_id);
      }
 
      RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm::SignalizedIntersectionManager"), "Processing Lane id: " << (int)lane.lane_id);
 
      size_t min_number_of_points = 2; // two points minimum are required
 
      size_t size_of_available_points = lane.node_list.nodes.node_set_xy.size();
 
      if (size_of_available_points < min_number_of_points)
      {
        RCLCPP_WARN_STREAM(rclcpp::get_logger("carma_wm::SignalizedIntersectionManager"), "Not enough points are provided to match a lane. Skipping... ");
        continue;
      }
 
      for (size_t i = 0; i < size_of_available_points; i ++ )
      {
        curr_x = lane.node_list.nodes.node_set_xy[i].delta.x + curr_x;
        curr_y = lane.node_list.nodes.node_set_xy[i].delta.y + curr_y;
        lanelet::Point3d curr_node{map->pointLayer.uniqueId(), curr_x, curr_y, 0};
 
        RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm::SignalizedIntersectionManager"), "Current node x: " << curr_x << ", y: " << curr_y);
 
        node_list.push_back(curr_node);
      }
 
      RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm::SignalizedIntersectionManager"), "Lane directions: " << (int)lane.lane_attributes.directional_use.lane_direction);
 
      if (lane.lane_attributes.directional_use.lane_direction == LANE_DIRECTION::INGRESS)
      {
        // flip direction if ingress to pick up correct lanelets
        RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm::SignalizedIntersectionManager"), "Reversed the node list!");
        std::reverse(node_list.begin(), node_list.end());
      }
 
      for (auto node : node_list)
      {
        RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm::SignalizedIntersectionManager"), node.x() << ", " << node.y());
      }
 
      // save which signal group connect to which exit lanes
      for (auto connection : lane.connect_to_list)
      {
        signal_group_to_exit_lanes[connection.signal_group].emplace(connection.connecting_lane.lane);
 
        if (lane.lane_attributes.directional_use.lane_direction == LANE_DIRECTION::INGRESS)
          signal_group_to_entry_lanes[connection.signal_group].emplace(lane.lane_id);
      }
 
      // query corresponding lanelet lane from local map
      auto affected_llts = carma_wm::query::getAffectedLaneletOrAreas(node_list, map, current_routing_graph, max_lane_width_);
 
      if (affected_llts.empty())
      {
        // https://github.com/usdot-fhwa-stol/carma-platform/issues/1593
        // Open issue TODO on how this error is handled
        RCLCPP_WARN_STREAM(rclcpp::get_logger("carma_wm::SignalizedIntersectionManager"), "Given offset points are not inside the map for lane: " << (int)lane.lane_id);
        continue;
      }
 
      lanelet::Id corresponding_lanelet_id;
      if (lane.lane_attributes.directional_use.lane_direction == LANE_DIRECTION::EGRESS)
      {
        corresponding_lanelet_id = affected_llts.front().id();
        RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm::SignalizedIntersectionManager"), "Default corresponding_lanelet_id: " << corresponding_lanelet_id <<", in EGRESS");
 
      }
      else //ingress
      {
        corresponding_lanelet_id = affected_llts.back().id();
        RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm::SignalizedIntersectionManager"), "Default corresponding_lanelet_id: " << corresponding_lanelet_id <<", in INGRESS");
      }
 
      for (auto llt : affected_llts) // filter out intersection lanelets
      {
        RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm::SignalizedIntersectionManager"), "Checking if we can get entry/exit from lanelet " << llt.id());
        //TODO direction of affected_llts may play role, but it should be good
        if (llt.lanelet().get().hasAttribute("turn_direction") &&
            (llt.lanelet().get().attribute("turn_direction").value().compare("left") == 0 ||
            llt.lanelet().get().attribute("turn_direction").value().compare("right") == 0))
        {
          std::vector<lanelet::ConstLanelet> connecting_llts;
          if (lane.lane_attributes.directional_use.lane_direction == LANE_DIRECTION::EGRESS)
          {
            RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm::SignalizedIntersectionManager"), "lanelet " << llt.id() << " is actually part of the intersecion. Trying to detect EGRESS...");
            connecting_llts = current_routing_graph->following(llt.lanelet().get());
          }
          else
          {
            RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm::SignalizedIntersectionManager"), "lanelet " << llt.id() << " is actually part of the intersecion. Trying to detect INGRESS...");
            connecting_llts = current_routing_graph->previous(llt.lanelet().get());
          }
 
          if (!connecting_llts.empty())
          {
            corresponding_lanelet_id = connecting_llts[0].id();
          }
          else
          {
            RCLCPP_WARN_STREAM(rclcpp::get_logger("carma_wm::SignalizedIntersectionManager"), "Interestingly, did not detect here");
            continue;
          }
 
          break;
        }
      }
 
      RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm::SignalizedIntersectionManager"), "Found existing lanelet id: " << corresponding_lanelet_id);
 
      if (lane.lane_attributes.directional_use.lane_direction == LANE_DIRECTION::INGRESS)
      {
        RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm::SignalizedIntersectionManager"), "Detected INGRESS, " << (int)lane.lane_id);
        entry[lane.lane_id] = corresponding_lanelet_id;
      }
      else if (lane.lane_attributes.directional_use.lane_direction == LANE_DIRECTION::EGRESS)
      {
        RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm::SignalizedIntersectionManager"), "Detected EGRESS, " << (int)lane.lane_id);
        exit[lane.lane_id] = corresponding_lanelet_id;
      }
      // ignoring types that are neither ingress nor egress
    }
 
    // convert and save exit lane ids into lanelet ids with their corresponding signal group ids
    for (auto iter = signal_group_to_exit_lanes.begin(); iter != signal_group_to_exit_lanes.end(); iter++)
    {
      for (auto exit_lane : iter->second)
      {
        if (exit.find(exit_lane) != exit.end())
        {
          RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm::SignalizedIntersectionManager"), "Adding exit_lane id: " << exit_lane);
          signal_group_to_exit_lanelet_ids_[iter->first].insert(exit[exit_lane]);
        }
        else
        {
          // https://github.com/usdot-fhwa-stol/carma-platform/issues/1593
          // Open issue TODO on how this error is handled
          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!");
        }
      }
    }
 
    // convert and save entry lane ids into lanelet ids with their corresponding signal group ids
    for (auto iter = signal_group_to_entry_lanes.begin(); iter != signal_group_to_entry_lanes.end(); iter++)
    {
      for (auto entry_lane : iter->second)
      {
        RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm::SignalizedIntersectionManager"), "Adding entry_lane id: " << entry_lane);
        if (entry.find(entry_lane) != entry.end())
        {
          signal_group_to_entry_lanelet_ids_[iter->first].insert(entry[entry_lane]);
        }
        else
        {
          // https://github.com/usdot-fhwa-stol/carma-platform/issues/1593
          // Open issue TODO on how this error is handled
          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!");
        }
      }
    }
  }

References carma_wm::LANE_DIRECTION::EGRESS, carma_wm::query::getAffectedLaneletOrAreas(), process_bag::i, carma_wm::LANE_DIRECTION::INGRESS, intersection_coord_correction_, 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]	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 348 of file SignalizedIntersectionManager.cpp.

  {
 
    if (target_frame_ == "")
    {
      RCLCPP_WARN_STREAM(rclcpp::get_logger("carma_wm::SignalizedIntersectionManager"), "Georeference is not loaded yet. Returning without processing this MAP msg.");
      return;
    }
 
    for (auto const& intersection : map_msg.intersections)
    {
      std::unordered_map<uint8_t, lanelet::Id> entry;
      std::unordered_map<uint8_t, lanelet::Id> exit;
 
      convertLaneToLaneletId(entry, exit, intersection, map, routing_graph);
 
      std::vector<lanelet::Lanelet> entry_llts;
      std::vector<lanelet::Lanelet> exit_llts;
 
      for (auto iter = entry.begin(); iter != entry.end(); iter++)
      {
        entry_llts.push_back(map->laneletLayer.get(iter->second));
      }
      for (auto iter = exit.begin(); iter != exit.end(); iter++)
      {
        exit_llts.push_back(map->laneletLayer.get(iter->second));
      }
 
      lanelet::Id intersection_id = matchSignalizedIntersection(entry_llts, exit_llts);
 
      if (intersection_id == lanelet::InvalId)
      {
        RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm::SignalizedIntersectionManager"), "No existing intersection found. Creating a new one...");
        intersection_id = lanelet::utils::getId();
 
        std::vector<lanelet::Lanelet> interior_llts = identifyInteriorLanelets(entry_llts, map);
 
        std::shared_ptr<lanelet::SignalizedIntersection> sig_inter(new lanelet::SignalizedIntersection
                                                          (lanelet::SignalizedIntersection::buildData(intersection_id, entry_llts, exit_llts, interior_llts)));
        intersection_id_to_regem_id_[intersection.id.id] = intersection_id;
        sig_intersections.push_back(sig_inter);
      }
    }
 
 
    // create signal group for each from the message
    // check if it already exists
    for (auto sig_grp_pair : signal_group_to_exit_lanelet_ids_)
    {
      RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm::SignalizedIntersectionManager"), "Creating signal for: " << (int)sig_grp_pair.first);
      // ignore the traffic signals already inside
      if (signal_group_to_traffic_light_id_.find(sig_grp_pair.first) != signal_group_to_traffic_light_id_.end() &&
           map->regulatoryElementLayer.exists(signal_group_to_traffic_light_id_[sig_grp_pair.first]))
      {
        continue;
      }
 
      std::vector<lanelet::Lanelet> exit_lanelets;
      for (auto iter = sig_grp_pair.second.begin(); iter != sig_grp_pair.second.end(); iter++)
      {
        exit_lanelets.push_back(map->laneletLayer.get(*iter));
      }
      std::vector<lanelet::Lanelet> entry_lanelets;
      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++)
      {
        entry_lanelets.push_back(map->laneletLayer.get(*iter));
      }
 
      traffic_signals.push_back(createTrafficSignalUsingSGID(sig_grp_pair.first, entry_lanelets, exit_lanelets));
    }
  }

References convertLaneToLaneletId(), createTrafficSignalUsingSGID(), identifyInteriorLanelets(), intersection_id_to_regem_id_, matchSignalizedIntersection(), 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_id	of the traffic signal
exit_llts	of the signal_group
entry_llts	of the signal_group

Returns: traffic signal corresponding to the signal group

Definition at line 320 of file SignalizedIntersectionManager.cpp.

  {
    std::vector<lanelet::LineString3d> stop_lines;
    for (auto llt : entry_lanelets)
    {
      std::vector<lanelet::Point3d> points;
      points.push_back(lanelet::Point3d(lanelet::utils::getId(), llt.leftBound2d().back().x(), llt.leftBound2d().back().y(), 0));
      points.push_back(lanelet::Point3d(lanelet::utils::getId(), llt.rightBound().back().x(), llt.rightBound().back().y(), 0));
 
      lanelet::LineString3d stop_line(lanelet::utils::getId(), points);
      stop_lines.push_back(stop_line);
    }
 
    lanelet::Id traffic_light_id = lanelet::utils::getId();
    std::shared_ptr<lanelet::CarmaTrafficSignal> traffic_light(new lanelet::CarmaTrafficSignal(lanelet::CarmaTrafficSignal::buildData(traffic_light_id, stop_lines, entry_lanelets, exit_lanelets)));
    signal_group_to_traffic_light_id_[signal_group_id] =  traffic_light_id;
 
    for (auto llt : exit_lanelets)
    {
      signal_group_to_exit_lanelet_ids_[signal_group_id].insert(llt.id());
    }
    for (auto llt : entry_lanelets)
    {
      signal_group_to_entry_lanelet_ids_[signal_group_id].insert(llt.id());
    }
    return traffic_light;
  }

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:

◆ 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_id	of the traffic signal
exit_llts	of the signal_group
entry_llts	of the signal_group

Returns: traffic signal corresponding to the signal group

Definition at line 421 of file SignalizedIntersectionManager.cpp.

  {
    lanelet::BasicLineString2d polygon_corners;
 
    if (entry_llts.size() < 2) //at least two lanes (1 ingress and 1 egress) needed to form intersection
    {
      return {};
    }
 
    for (auto llt : entry_llts)
    {
      lanelet::BasicPoint2d pt(llt.centerline2d().back().x(), llt.centerline2d().back().y());
      polygon_corners.push_back(pt);
    }
    lanelet::BasicPolygon2d polygon(polygon_corners);
    auto interior_llt_pairs = lanelet::geometry::findWithin2d(map->laneletLayer, polygon);
    lanelet::Lanelets interior_llts;
 
    for (auto pair : interior_llt_pairs)
    {
      if (std::find(entry_llts.begin(),entry_llts.end(), pair.second) == entry_llts.end())
        interior_llts.push_back(pair.second);
    }
    return interior_llts;
  }

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_llts	of the intersection
exit_llts	of the intersection

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

Definition at line 259 of file SignalizedIntersectionManager.cpp.

  {
    lanelet::Id matching_id = lanelet::InvalId;
 
    std::vector<lanelet::SignalizedIntersectionPtr> existing_si;
 
    for (auto llt : entry_llts)
    {
      auto intersections = llt.regulatoryElementsAs<lanelet::SignalizedIntersection>();
 
      if (intersections.empty())
      {
        // no match if any of the entry lanelet is not part of any intersection.
        break;
      }
      existing_si.insert(existing_si.end(), intersections.begin(),intersections.end());
    }
 
    for (auto intersection : existing_si)
    {
      auto existing_entry_llts = intersection->getEntryLanelets();
      auto existing_exit_llts = intersection->getExitLanelets();
 
      if (existing_exit_llts.size() != exit_llts.size() || existing_entry_llts.size() != entry_llts.size())
        continue;
 
      bool is_different = false;
 
      for (auto llt: existing_entry_llts)
      {
        if (std::find(entry_llts.begin(), entry_llts.end(), llt) == entry_llts.end())
        {
          is_different = true;
 
          break;
        }
      }
 
      for (auto llt: existing_exit_llts)
      {
        if (std::find(exit_llts.begin(), exit_llts.end(), llt) == exit_llts.end())
        {
          is_different = true;
 
          break;
        }
      }
 
      if (!is_different)
      {
        // found a match
        matching_id = intersection->id();
 
        break;
      }
 
    }
 
    return matching_id;
  }

Referenced by createIntersectionFromMapMsg().

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] other manager

Definition at line 447 of file SignalizedIntersectionManager.cpp.

  {
    this->signal_group_to_entry_lanelet_ids_ = other.signal_group_to_entry_lanelet_ids_;
    this->signal_group_to_exit_lanelet_ids_ = other.signal_group_to_exit_lanelet_ids_;
    this->intersection_id_to_regem_id_ = other.intersection_id_to_regem_id_;
    this->signal_group_to_traffic_light_id_ = other.signal_group_to_traffic_light_id_;
    this->intersection_coord_correction_ = other.intersection_coord_correction_;
 
    return *this;
  }

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] rhs manager

Definition at line 35 of file SignalizedIntersectionManager.cpp.

  {
    bool is_same = true;
    if (rhs.intersection_id_to_regem_id_ != this->intersection_id_to_regem_id_)
    {
      is_same = false;
    }
    if (rhs.signal_group_to_entry_lanelet_ids_ != this->signal_group_to_entry_lanelet_ids_)
    {
      is_same = false;
    }
    if (rhs.signal_group_to_exit_lanelet_ids_ != this->signal_group_to_exit_lanelet_ids_)
    {
      is_same = false;
    }
    if (rhs.signal_group_to_traffic_light_id_ != this->signal_group_to_traffic_light_id_)
    {
      is_same = false;
    }
 
    return is_same;
  }

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_.

◆ 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.

  {
    max_lane_width_ = max_lane_width;
  }

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_frame PROJ string of the map

Definition at line 25 of file SignalizedIntersectionManager.cpp.

  {
    target_frame_ = target_frame;
  }

References target_frame_.

Member Data Documentation

◆ intersection_coord_correction_

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

Definition at line 148 of file SignalizedIntersectionManager.hpp.

Referenced by SignalizedIntersectionManager(), convertLaneToLaneletId(), and operator=().

◆ intersection_id_to_regem_id_

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

Definition at line 151 of file SignalizedIntersectionManager.hpp.

Referenced by SignalizedIntersectionManager(), createIntersectionFromMapMsg(), carma_wm::CARMAWorldModel::getTrafficSignalId(), boost::serialization::load(), carma_wm::logSignalizedIntersectionManager(), operator=(), operator==(), and boost::serialization::save().

◆ last_seen_state_

std::unordered_map<uint16_t, std::unordered_map<uint8_t,std::pair<boost::posix_time::ptime, lanelet::CarmaTrafficSignalState> > > carma_wm::SignalizedIntersectionManager::last_seen_state_

Definition at line 169 of file SignalizedIntersectionManager.hpp.

◆ max_lane_width_

double carma_wm::SignalizedIntersectionManager::max_lane_width_ = 4

private

Definition at line 179 of file SignalizedIntersectionManager.hpp.

Referenced by convertLaneToLaneletId(), and setMaxLaneWidth().

◆ 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_

Definition at line 160 of file SignalizedIntersectionManager.hpp.

Referenced by SignalizedIntersectionManager(), convertLaneToLaneletId(), createIntersectionFromMapMsg(), createTrafficSignalUsingSGID(), boost::serialization::load(), carma_wm::logSignalizedIntersectionManager(), operator=(), operator==(), and boost::serialization::save().

◆ 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_

Definition at line 157 of file SignalizedIntersectionManager.hpp.

Referenced by SignalizedIntersectionManager(), convertLaneToLaneletId(), createIntersectionFromMapMsg(), createTrafficSignalUsingSGID(), boost::serialization::load(), carma_wm::logSignalizedIntersectionManager(), operator=(), operator==(), and boost::serialization::save().

◆ signal_group_to_traffic_light_id_

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

Definition at line 154 of file SignalizedIntersectionManager.hpp.

Referenced by SignalizedIntersectionManager(), createIntersectionFromMapMsg(), createTrafficSignalUsingSGID(), carma_wm::CARMAWorldModel::getTrafficSignalId(), boost::serialization::load(), carma_wm::logSignalizedIntersectionManager(), operator=(), operator==(), and boost::serialization::save().

◆ signal_state_counter_

std::unordered_map<uint16_t, std::unordered_map<uint8_t,int> > carma_wm::SignalizedIntersectionManager::signal_state_counter_

Definition at line 172 of file SignalizedIntersectionManager.hpp.

◆ target_frame_

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

private

Definition at line 176 of file SignalizedIntersectionManager.hpp.

Referenced by convertLaneToLaneletId(), createIntersectionFromMapMsg(), and setTargetFrame().

◆ traffic_signal_start_times_

std::unordered_map<uint16_t, std::unordered_map<uint8_t,std::vector<boost::posix_time::ptime> > > carma_wm::SignalizedIntersectionManager::traffic_signal_start_times_

Definition at line 166 of file SignalizedIntersectionManager.hpp.

Referenced by carma_wm::CARMAWorldModel::processSpatFromMsg().

◆ traffic_signal_states_

std::unordered_map<uint16_t, std::unordered_map<uint8_t,std::vector<std::pair<boost::posix_time::ptime, lanelet::CarmaTrafficSignalState> > > > carma_wm::SignalizedIntersectionManager::traffic_signal_states_

Definition at line 163 of file SignalizedIntersectionManager.hpp.

Referenced by carma_wm::CARMAWorldModel::processSpatFromMsg().

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

carma_wm/include/carma_wm/SignalizedIntersectionManager.hpp
carma_wm/src/SignalizedIntersectionManager.cpp

Public Member Functions

Public Attributes

Private Attributes

Detailed Description

Constructor & Destructor Documentation

◆ SignalizedIntersectionManager() [1/2]

◆ SignalizedIntersectionManager() [2/2]

Member Function Documentation

◆ convertLaneToLaneletId()

◆ createIntersectionFromMapMsg()

◆ createTrafficSignalUsingSGID()

◆ identifyInteriorLanelets()

◆ matchSignalizedIntersection()

◆ operator=()

◆ operator==()

◆ setMaxLaneWidth()

◆ setTargetFrame()

Member Data Documentation

◆ intersection_coord_correction_

◆ intersection_id_to_regem_id_

◆ last_seen_state_

◆ max_lane_width_

◆ signal_group_to_entry_lanelet_ids_

◆ signal_group_to_exit_lanelet_ids_

◆ signal_group_to_traffic_light_id_

◆ signal_state_counter_

◆ target_frame_

◆ traffic_signal_start_times_

◆ traffic_signal_states_