carma_wm::query Namespace Reference

Functions
std::vector< lanelet::ConstLanelet >	getLaneletsFromPoint (const lanelet::LaneletMapConstPtr &semantic_map, const lanelet::BasicPoint2d &point, const unsigned int n=10)
	carma_wm::query namespace contains implementations for query functions (input and output read or write-able) for stand-alone lanelet map without rest of the CARMAWorldModel features. Currently mainly WMBroadcaster (carma_wm_ctrl) is using this to manipulate its own map without creating instance of carma_wm More...
std::vector< lanelet::Lanelet >	getLaneletsFromPoint (const lanelet::LaneletMapPtr &semantic_map, const lanelet::BasicPoint2d &point, const unsigned int n=10)
	(non-const version) Gets the underlying lanelet, given the cartesian point on the map More...
std::vector< lanelet::ConstLanelet >	nonConnectedAdjacentLeft (const lanelet::LaneletMapConstPtr &semantic_map, const lanelet::BasicPoint2d &input_point, const unsigned int n=10)
	Given the cartesian point on the map, tries to get the opposite direction lanelet on the left This function is intended to find "adjacentLeft lanelets" that doesn't share points between lanelets where adjacentLeft of lanelet library fails. More...
std::vector< lanelet::Lanelet >	nonConnectedAdjacentLeft (const lanelet::LaneletMapPtr &semantic_map, const lanelet::BasicPoint2d &input_point, const unsigned int n=10)
	(non-const version) Given the cartesian point on the map, tries to get the opposite direction lanelet on the left This function is intended to find "adjacentLeft lanelets" that doesn't share points between lanelets where adjacentLeft of lanelet library fails More...
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. More...
std::unordered_set< lanelet::Lanelet >	filterSuccessorLanelets (const std::unordered_set< lanelet::Lanelet > &possible_lanelets, const std::unordered_set< lanelet::Lanelet > &root_lanelets, const lanelet::LaneletMapPtr &lanelet_map, std::shared_ptr< const lanelet::routing::RoutingGraph > routing_graph)
	A function that filters successor lanelets of root_lanelets from possible_lanelets. More...

Function Documentation

filterSuccessorLanelets()

std::unordered_set< lanelet::Lanelet > carma_wm::query::filterSuccessorLanelets	(	const std::unordered_set< lanelet::Lanelet > &	possible_lanelets,
		const std::unordered_set< lanelet::Lanelet > &	root_lanelets,
		const lanelet::LaneletMapPtr &	lanelet_map,
		std::shared_ptr< const lanelet::routing::RoutingGraph >	routing_graph
	)

A function that filters successor lanelets of root_lanelets from possible_lanelets.

Parameters

possible_lanelets	all possible lanelets to check
root_lanelets	lanelets to filter from
lanelet_map	Lanelet Map Ptr
routing_graph	Routing graph of the lanelet map

NOTE:Mainly used as a helper function for getAffectedLaneletOrAreas

Definition at line 270 of file WorldModelUtils.cpp.

{
  if (!routing_graph) {
    throw std::invalid_argument("No routing graph available");
  }
  
  std::unordered_set<lanelet::Lanelet> filtered_lanelets;
  // we utilize routes to filter llts that are overlapping but not connected
  // as this is the last lanelet 
  // we have to filter the llts that are only geometrically overlapping yet not connected to prev llts
  for (auto recorded_llt: root_lanelets)
  {
    for (auto following_llt: routing_graph->following(recorded_llt, false))
    {
      auto mutable_llt = lanelet_map->laneletLayer.get(following_llt.id());
      auto it = possible_lanelets.find(mutable_llt);
      if (it != possible_lanelets.end())
      {
        filtered_lanelets.insert(mutable_llt);
      }
    }
  }
  return filtered_lanelets;
}

Referenced by getAffectedLaneletOrAreas().

Here is the caller graph for this function:

getAffectedLaneletOrAreas()

lanelet::ConstLaneletOrAreas carma_wm::query::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.

Parameters

geofence_msg	lanelet::Points3d in local frame
lanelet_map	Lanelet Map Ptr
routing_graph	Routing graph of the lanelet map
max_lane_width	max lane width of the lanes in the map

NOTE:Currently this function only checks lanelets and will be expanded to areas in the future.

Definition at line 117 of file WorldModelUtils.cpp.

{
  // Logic to detect which part is affected
  RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm::query"), "Get affected lanelets loop");
  std::unordered_set<lanelet::Lanelet> affected_lanelets;
  for (size_t idx = 0; idx < gf_pts.size(); idx ++)
  {
    RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm::query"), "Index: " << idx << " Point: " << gf_pts[idx].x() << ", " << gf_pts[idx].y());
    std::unordered_set<lanelet::Lanelet> possible_lanelets;
 
    // This loop identifes the lanelets which this point lies within that could be impacted by the geofence
    // This loop somewhat inefficiently calls the findNearest method iteratively until all the possible lanelets are identified. 
    // The reason findNearest is used instead of nearestUntil is because that method orders results by bounding box which
    // can give invalid sequences when dealing with large curved lanelets.  
    bool continue_search = true; 
    size_t nearest_count = 0;
    while (continue_search) {
      
      nearest_count += 10; // Increase the index search radius by 10 each loop until all nearby lanelets are found
 
      for (const auto& ll_pair : lanelet::geometry::findNearest(lanelet_map->laneletLayer, gf_pts[idx].basicPoint2d(), nearest_count)) { // Get the nearest lanelets and iterate over them
        auto ll = std::get<1>(ll_pair);
 
        if (possible_lanelets.find(ll) != possible_lanelets.end()) { // Skip if already found
          continue;
        }
 
        double dist = std::get<0>(ll_pair);
        RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm::query"), "Distance to lanelet " << ll.id() << ": " << dist << " max_lane_width: " << max_lane_width);
        
        if (dist > max_lane_width) { // Only save values closer than max_lane_width. Since we are iterating in distance order when we reach this distance the search can stop
          continue_search = false;
          break;
        }
 
        // Check if the point is inside this lanelet
        if(dist == 0.0) { // boost geometry uses a distance of 0 to indicate a point is within a polygon
          possible_lanelets.insert(ll);
        }
 
      }
 
      if (nearest_count >= lanelet_map->laneletLayer.size()) { // if we are out of lanelets to evaluate then end the search
        continue_search = false;
      }
    }
 
    // among these llts, filter the ones that are on same direction as the geofence using routing
    if (idx + 1 == gf_pts.size()) // we only check this for the last gf_pt after saving everything
    {
      RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm::query"), "Last point");
      std::unordered_set<lanelet::Lanelet> filtered = filterSuccessorLanelets(possible_lanelets, affected_lanelets, lanelet_map, routing_graph);
      RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm::query"), "Got successor lanelets of size: " << filtered.size());
      affected_lanelets.insert(filtered.begin(), filtered.end());
 
 
      if (affected_lanelets.empty() && !possible_lanelets.empty() && idx !=0 )
      {
        RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm::query"), "Checking if it is the edge case where only last point falls on a valid (correct direction) lanelet");
        for (auto llt: possible_lanelets)
        {
          RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm::query"), "Evaluating lanelet: " << llt.id());
          lanelet::BasicLineString2d gf_dir_line({gf_pts[idx - 1].basicPoint2d(), gf_pts[idx].basicPoint2d()});
          lanelet::BasicLineString2d llt_boundary({(llt.leftBound2d().begin())->basicPoint2d(), (llt.rightBound2d().begin())->basicPoint2d()});
 
          // record the llts that are on the same dir
          if (boost::geometry::intersects(llt_boundary, gf_dir_line))
          {
            RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm::query"), "Overlaps starting line... Picking llt: " << llt.id());
            affected_lanelets.insert(llt);
          }  
        }
      }
 
      break;
    } 
 
    RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm::query"), "Checking possible lanelets");
    // check if each lines connecting end points of the llt is crossing with the line connecting current and next gf_pts
    for (auto llt: possible_lanelets)
    {
      RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm::query"), "Evaluating lanelet: " << llt.id());
      lanelet::BasicLineString2d gf_dir_line({gf_pts[idx].basicPoint2d(), gf_pts[idx+1].basicPoint2d()});
      lanelet::BasicLineString2d llt_boundary({(llt.leftBound2d().end() -1)->basicPoint2d(), (llt.rightBound2d().end() - 1)->basicPoint2d()});
      
      // record the llts that are on the same dir
      if (boost::geometry::intersects(llt_boundary, gf_dir_line))
      {
        RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm::query"), "Overlaps end line");
        affected_lanelets.insert(llt);
      }
      // check condition if two geofence points are in one lanelet then check matching direction and record it also
      else if (boost::geometry::within(gf_pts[idx+1].basicPoint2d(), llt.polygon2d()) && 
              affected_lanelets.find(llt) == affected_lanelets.end())
      { 
        RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm::query"), "Within new lanelet");
        lanelet::BasicPoint2d median({((llt.leftBound2d().end() - 1)->basicPoint2d().x() + (llt.rightBound2d().end() - 1)->basicPoint2d().x())/2 , 
                                      ((llt.leftBound2d().end() - 1)->basicPoint2d().y() + (llt.rightBound2d().end() - 1)->basicPoint2d().y())/2});
        // turn into vectors
        Eigen::Vector2d vec_to_median(median);
        Eigen::Vector2d vec_to_gf_start(gf_pts[idx].basicPoint2d());
        Eigen::Vector2d vec_to_gf_end(gf_pts[idx + 1].basicPoint2d());
 
        // Get vector from start to external point
        Eigen::Vector2d start_to_median = vec_to_median - vec_to_gf_start;
 
        // Get vector from start to end point
        Eigen::Vector2d start_to_end = vec_to_gf_end - vec_to_gf_start;
 
        // Get angle between both vectors
        double interior_angle = carma_wm::geometry::getAngleBetweenVectors(start_to_median, start_to_end);
 
        RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm::query"), "vec_to_median: " << vec_to_median.x() << ", " << vec_to_median.y());
        RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm::query"), "vec_to_gf_start: " << vec_to_gf_start.x() << ", " << vec_to_gf_start.y());
        RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm::query"), "vec_to_gf_end: " << vec_to_gf_end.x() << ", " << vec_to_gf_end.y());
        RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm::query"), "start_to_median: " << start_to_median.x() << ", " << start_to_median.y());
        RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm::query"), "start_to_end: " << start_to_end.x() << ", " << start_to_end.y());
        RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm::query"), "interior_angle: " << interior_angle);
        // Save the lanelet if the direction of two points inside aligns with that of the lanelet
 
        if (interior_angle < M_PI_2 && interior_angle >= 0) 
          affected_lanelets.insert(llt); 
      }
      else
      {
        RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm::query"), "------ Did not record anything...");
      }
 
    }
  }
  
  RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm::query"), "affected_lanelets size: " << affected_lanelets.size());
  // Currently only returning lanelet, but this could be expanded to LanelerOrArea compound object 
  // by implementing non-const version of that LaneletOrArea
  lanelet::ConstLaneletOrAreas affected_parts;
  // return results in ascending downtrack order from the first point of geofence
  std::vector<std::pair<double, lanelet::Lanelet>> sorted_parts;
 
  for (auto llt : affected_lanelets)
  {
    sorted_parts.push_back(std::make_pair(carma_wm::geometry::trackPos(llt, gf_pts.front().basicPoint2d()).downtrack, llt));
  }
  std::sort(sorted_parts.begin(), sorted_parts.end(), [](const auto& x, const auto& y){return x.first > y.first;});
 
  for (auto pair : sorted_parts)
  {
    affected_parts.push_back(pair.second);
  }
  
  return affected_parts;
}

References carma_wm::TrackPos::downtrack, filterSuccessorLanelets(), carma_wm::geometry::getAngleBetweenVectors(), carma_wm::geometry::trackPos(), process_traj_logs::x, and process_traj_logs::y.

Referenced by carma_wm::SignalizedIntersectionManager::convertLaneToLaneletId(), and carma_wm_ctrl::WMBroadcaster::getAffectedLaneletOrAreas().

Here is the call graph for this function:

Here is the caller graph for this function:

getLaneletsFromPoint() [1/2]

std::vector< lanelet::ConstLanelet > carma_wm::query::getLaneletsFromPoint	(	const lanelet::LaneletMapConstPtr &	semantic_map,
		const lanelet::BasicPoint2d &	point,
		const unsigned int	n = 10
	)

carma_wm::query namespace contains implementations for query functions (input and output read or write-able) for stand-alone lanelet map without rest of the CARMAWorldModel features. Currently mainly WMBroadcaster (carma_wm_ctrl) is using this to manipulate its own map without creating instance of carma_wm

Gets the underlying lanelet, given the cartesian point on the map

Parameters

semantic_map	Lanelet Map Ptr
point	Cartesian point to check the corressponding lanelet
n	Number of lanelets to return. Default is 10. As there could be many lanelets overlapping.

Exceptions

std::invalid_argument

if the map is not set, contains no lanelets

Returns

vector of underlying lanelet, empty vector if it is not part of any lanelet

Definition at line 24 of file WorldModelUtils.cpp.

{
  // Check if the map is loaded yet
  if (!semantic_map || semantic_map->laneletLayer.size() == 0)
  {
    throw std::invalid_argument("Map is not set or does not contain lanelets");
  }
  std::vector<lanelet::ConstLanelet> possible_lanelets;
  auto nearestLanelets = lanelet::geometry::findNearest(semantic_map->laneletLayer, point, n);
  if (nearestLanelets.size() == 0)
    return {};
 
  size_t id = 0;  // closest ones are in the back
  // loop through until the point is no longer geometrically in the lanelet
 
  while (boost::geometry::within(point, nearestLanelets[id].second.polygon2d()))
  {
    possible_lanelets.push_back(nearestLanelets[id].second);
    id++;
    if (id >= nearestLanelets.size())
      break;
  }
  return possible_lanelets;
}

References process_traj_logs::point.

Referenced by carma_wm::CARMAWorldModel::getLaneletsFromPoint(), getLaneletsFromPoint(), carma_wm::CARMAWorldModel::nonConnectedAdjacentLeft(), nonConnectedAdjacentLeft(), and carma_wm_ctrl::WMBroadcaster::preprocessWorkzoneGeometry().

Here is the caller graph for this function:

getLaneletsFromPoint() [2/2]

std::vector< lanelet::Lanelet > carma_wm::query::getLaneletsFromPoint	(	const lanelet::LaneletMapPtr &	semantic_map,
		const lanelet::BasicPoint2d &	point,
		const unsigned int	n = 10
	)

(non-const version) Gets the underlying lanelet, given the cartesian point on the map

Parameters

semantic_map	Lanelet Map Ptr
point	Cartesian point to check the corressponding lanelet
n	Number of lanelets to return. Default is 10. As there could be many lanelets overlapping.

Exceptions

std::invalid_argument

if the map is not set, contains no lanelets

Returns

vector of underlying lanelet, empty vector if it is not part of any lanelet

Definition at line 50 of file WorldModelUtils.cpp.

{
  lanelet::LaneletMapConstPtr const_ptr = semantic_map;
  auto possible_lanelets = getLaneletsFromPoint(const_ptr, point, n);
  std::vector<lanelet::Lanelet> return_lanelets;
  for (auto llt : possible_lanelets)
  {
    return_lanelets.push_back(semantic_map->laneletLayer.get(llt.id()));
  }
  return return_lanelets;
}

References getLaneletsFromPoint(), and process_traj_logs::point.

Here is the call graph for this function:

nonConnectedAdjacentLeft() [1/2]

std::vector< lanelet::ConstLanelet > carma_wm::query::nonConnectedAdjacentLeft	(	const lanelet::LaneletMapConstPtr &	semantic_map,
		const lanelet::BasicPoint2d &	input_point,
		const unsigned int	n = 10
	)

Given the cartesian point on the map, tries to get the opposite direction lanelet on the left This function is intended to find "adjacentLeft lanelets" that doesn't share points between lanelets where adjacentLeft of lanelet library fails.

Parameters

semantic_map	Lanelet Map Ptr
point	Cartesian point to check the corressponding lanelet
n	Number of lanelets to return. Default is 10. As there could be many lanelets overlapping.

Exceptions

std::invalid_argument

if the map is not set, contains no lanelets, or if adjacent lanelet is not opposite direction NOTE: Only to be used on 2 lane, opposite direction road. Number of points in all linestrings are assumed to be roughly the same. The point is assumed to be on roughly similar shape of overlapping lanelets if any Enhancement issue for protection against checking if the laneis opposite direction here: https://github.com/usdot-fhwa-stol/carma-platform/issues/1381

Returns

vector of underlying lanelet, empty vector if it is not part of any lanelet

Definition at line 79 of file WorldModelUtils.cpp.

{
  
  // Check if the map is loaded yet
  if (!semantic_map || semantic_map->laneletLayer.size() == 0)
  {
    throw std::invalid_argument("Map is not set or does not contain lanelets");
  }
  
  auto input_lanelets = getLaneletsFromPoint(semantic_map, input_point);
  
  if (input_lanelets.empty())
  {
    throw std::invalid_argument("Input point x: " + std::to_string(input_point.x()) + ", y: " + std::to_string(input_point.y()) + " is not in the map");
  }
  
  auto input_lanelet = input_lanelets[0]; 
  
  auto point_downtrack = carma_wm::geometry::trackPos(input_lanelet, input_point).downtrack;
  
  auto point_downtrack_ratio = point_downtrack / carma_wm::geometry::trackPos(input_lanelet, input_lanelet.centerline().back().basicPoint2d()).downtrack;
 
  auto point_on_ls = input_lanelet.leftBound2d()[std::round((input_lanelet.leftBound2d().size()- 1) * point_downtrack_ratio) ];
 
  // point_on_opposite_lane coord is acquired by extrapolating the line from input_point to point_on_ls with same distance.
  // threfore point_on_opposite_lane.x() = input_point.x() + 2dx, where dx = point_on_ls.x() - input_point.x(). Here, one of input_point.x() cancels out, results in:
  auto point_on_opposite_lane = lanelet::BasicPoint2d{2 * point_on_ls.x() - input_point.x(), 2 * point_on_ls.y() - input_point.y()};
 
  auto opposite_lanelets = getLaneletsFromPoint(semantic_map, point_on_opposite_lane, n);
 
  // TODO: create opposite direction protection throw
  // currently the function doesn't recognize if adjacent lane is opposite direction, but assumes it
 
  return opposite_lanelets;
}

References carma_wm::TrackPos::downtrack, getLaneletsFromPoint(), carma_cooperative_perception::to_string(), and carma_wm::geometry::trackPos().

Referenced by nonConnectedAdjacentLeft(), and carma_wm_ctrl::WMBroadcaster::splitOppositeLaneletWithPoint().

Here is the call graph for this function:

Here is the caller graph for this function:

nonConnectedAdjacentLeft() [2/2]

std::vector< lanelet::Lanelet > carma_wm::query::nonConnectedAdjacentLeft	(	const lanelet::LaneletMapPtr &	semantic_map,
		const lanelet::BasicPoint2d &	input_point,
		const unsigned int	n = 10
	)

(non-const version) Given the cartesian point on the map, tries to get the opposite direction lanelet on the left This function is intended to find "adjacentLeft lanelets" that doesn't share points between lanelets where adjacentLeft of lanelet library fails

Parameters

semantic_map	Lanelet Map Ptr
point	Cartesian point to check the corressponding lanelet
n	Number of lanelets to return. Default is 10. As there could be many lanelets overlapping.

Exceptions

std::invalid_argument

if the map is not set, contains no lanelets, or if adjacent lanelet is not opposite direction NOTE: Only to be used on 2 lane, opposite direction road. Number of points in all linestrings are assumed to be roughly the same. The point is assumed to be on roughly similar shape of overlapping lanelets if any Enhancement issue for protection against checking if the laneis opposite direction here: https://github.com/usdot-fhwa-stol/carma-platform/issues/1381

Returns

vector of underlying lanelet, empty vector if it is not part of any lanelet

Definition at line 63 of file WorldModelUtils.cpp.

{
  
  lanelet::LaneletMapConstPtr const_ptr = semantic_map;
  auto possible_lanelets = nonConnectedAdjacentLeft(const_ptr, point, n);
  
  
  std::vector<lanelet::Lanelet> return_lanelets;
  for (auto llt : possible_lanelets)
  {
    return_lanelets.push_back(semantic_map->laneletLayer.get(llt.id()));
  }
  return return_lanelets;
}

References nonConnectedAdjacentLeft(), and process_traj_logs::point.

Here is the call graph for this function: