carma_wm::CARMAWorldModel Class Reference

Class which implements the WorldModel interface. In addition this class provides write access to the world model. Write access is achieved through setters for the Map and Route and getMutableMap(). NOTE: This class should NOT be used in runtime code by users and is exposed solely for use in unit tests where the WMListener class cannot be instantiated. More...

#include <CARMAWorldModel.hpp>

Inheritance diagram for carma_wm::CARMAWorldModel:

Collaboration diagram for carma_wm::CARMAWorldModel:

Public Member Functions
	CARMAWorldModel ()=default
	Constructor. More...
	~CARMAWorldModel ()=default
	Destructor as required by interface. More...
void	setMap (lanelet::LaneletMapPtr map, size_t map_version=0, bool recompute_routing_graph=true)
	Set the current map. More...
void	setRoutingGraph (LaneletRoutingGraphPtr graph)
	Set the routing graph for the participant type. This function may serve as an optimization to recomputing the routing graph when it is already available. More...
void	setRoute (LaneletRoutePtr route)
	Set the current route. This route must match the current map for this class to function properly. More...
void	setTrafficLightIds (uint32_t id, lanelet::Id lanelet_id)
	Sets the id mapping between intersection/signal group and lanelet::Id for traffic lights in the map. More...
lanelet::LaneletMapPtr	getMutableMap () const
	Get a mutable version of the current map. More...
void	setRoadwayObjects (const std::vector< carma_perception_msgs::msg::RoadwayObstacle > &rw_objs)
	Update internal records of roadway objects. These objects MUST be guaranteed to be on the road. More...
lanelet::Optional< std::tuple< TrackPos, carma_perception_msgs::msg::RoadwayObstacle > >	getNearestObjInLane (const lanelet::BasicPoint2d &object_center, const LaneSection &section=LANE_AHEAD) const
	This function is called by distanceToObjectBehindInLane or distanceToObjectAheadInLane. Gets Downtrack distance to AND copy of the closest object on the same lane as the given point. Also returns crosstrack distance relative to that object. Plus downtrack if the object is ahead along the lane, and also plus crosstrack if the object is to the right relative to the reference line that crosses given object_center and is parallel to the centerline of the lane. More...
void	setConfigSpeedLimit (double config_lim)
void	setVehicleParticipationType (const std::string &participant)
	Set vehicle participation type. More...
std::string	getVehicleParticipationType ()
	Get vehicle participation type. More...
void	setRouteEndPoint (const lanelet::BasicPoint3d &end_point)
	Set endpoint of the route. More...
void	setRouteName (const std::string &route_name)
	Set the name of the route. More...
void	setRos1Clock (const rclcpp::Time &time_now)
	Set current Ros1 clock (only used in simulation runs) More...
void	setSimulationClock (const rclcpp::Time &time_now)
	Set simulation clock clock (only used in simulation runs) More...
std::vector< lanelet::Lanelet >	getLaneletsFromPoint (const lanelet::BasicPoint2d &point, const unsigned int n)
	(non-const version) Gets the underlying lanelet, given the cartesian point on the map More...
std::vector< lanelet::Lanelet >	nonConnectedAdjacentLeft (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...
std::pair< TrackPos, TrackPos >	routeTrackPos (const lanelet::ConstArea &area) const override
	Returns a pair of TrackPos, computed in 2d, of the provided area relative to the current route. The distance is based on the Area vertex with the smallest and largest downtrack distance This method overload is the most expensive of the routeTrackPos methods. More...
TrackPos	routeTrackPos (const lanelet::ConstLanelet &lanelet) const override
	Returns the TrackPos, computed in 2d, of the provided lanelet relative to the current route. The distance is based on the first point in the lanelet centerline. More...
TrackPos	routeTrackPos (const lanelet::BasicPoint2d &point) const override
	Returns the TrackPos, computed in 2d, of the provided point relative to the current route. More...
std::vector< lanelet::ConstLanelet >	getLaneletsBetween (double start, double end, bool shortest_path_only=false, bool bounds_inclusive=true) const override
	Returns a list of lanelets which are part of the route and whose downtrack bounds exist within the provided start and end distances. More...
std::vector< lanelet::BasicPoint2d >	sampleRoutePoints (double start_downtrack, double end_downtrack, double step_size) const override
	Samples the route centerline between the provided downtracks with the provided step size. At lane changes the points should exhibit a discontinuous jump at the end of the initial lanelet to the end of the lane change lanelet as expected by the route definition. Returned points are always inclusive of provided bounds, so the last step might be less than step_size. More...
boost::optional< lanelet::BasicPoint2d >	pointFromRouteTrackPos (const TrackPos &route_pos) const override
	Converts a route track position into a map frame cartesian point. More...
lanelet::LaneletMapConstPtr	getMap () const override
	Get a pointer to the current map. If the underlying map has changed the pointer will also need to be reacquired. More...
LaneletRouteConstPtr	getRoute () const override
	Get a pointer to the current route. If the underlying route has changed the pointer will also need to be reacquired. More...
std::string	getRouteName () const override
	Get the current route name. More...
TrackPos	getRouteEndTrackPos () const override
	Get trackpos of the end of route point relative to the route. More...
LaneletRoutingGraphConstPtr	getMapRoutingGraph () const override
	Get a pointer to the routing graph for the current map. If the underlying map has changed the pointer will also need to be reacquired. More...
lanelet::Optional< TrafficRulesConstPtr >	getTrafficRules (const std::string &participant) const override
	Get a pointer to the traffic rules object used internally by the world model and considered the carma system default. More...
lanelet::Optional< TrafficRulesConstPtr >	getTrafficRules () const override
	Get the Traffic Rules object. More...
std::vector< carma_perception_msgs::msg::RoadwayObstacle >	getRoadwayObjects () const override
	Get most recent roadway objects - all objects on the road detected by perception stack. More...
std::vector< carma_perception_msgs::msg::RoadwayObstacle >	getInLaneObjects (const lanelet::ConstLanelet &lanelet, const LaneSection &section=LANE_AHEAD) const override
	Gets all roadway objects currently in the same lane as the given lanelet. More...
lanelet::Optional< lanelet::Lanelet >	getIntersectingLanelet (const carma_perception_msgs::msg::ExternalObject &object) const override
	Gets the a lanelet the object is currently on determined by its position on the semantic map. If it's across multiple lanelets, get the closest one. More...
lanelet::Optional< carma_perception_msgs::msg::RoadwayObstacle >	toRoadwayObstacle (const carma_perception_msgs::msg::ExternalObject &object) const override
	Converts an ExternalObject in a RoadwayObstacle by mapping its position onto the semantic map. Can also be used to determine if the object is on the roadway. More...
lanelet::Optional< double >	distToNearestObjInLane (const lanelet::BasicPoint2d &object_center) const override
	Gets Cartesian distance to the closest object on the same lane as the given point. More...
lanelet::Optional< std::tuple< TrackPos, carma_perception_msgs::msg::RoadwayObstacle > >	nearestObjectAheadInLane (const lanelet::BasicPoint2d &object_center) const override
	Gets Downtrack distance to AND copy of the closest object AHEAD on the same lane as the given point. Also returns crosstrack distance relative to that object. Plus downtrack if the object is ahead along the lane, and also plus crosstrack if the object is to the right relative to the reference line that crosses given object_center and is parallel to the centerline of the lane. More...
lanelet::Optional< std::tuple< TrackPos, carma_perception_msgs::msg::RoadwayObstacle > >	nearestObjectBehindInLane (const lanelet::BasicPoint2d &object_center) const override
	Gets Downtrack distance to AND copy of the closest object BEHIND on the same lane as the given point. Also returns crosstrack distance relative to that object. Plus downtrack if the object is ahead along the lane, and also plus crosstrack if the object is to the right relative to the reference line that crosses given object_center and is parallel to the centerline of the lane. More...
std::vector< lanelet::ConstLanelet >	getLane (const lanelet::ConstLanelet &lanelet, const LaneSection &section=LANE_AHEAD) const override
	Gets the specified lane section achievable without lane change, sorted from the start, that includes the given lanelet. More...
size_t	getMapVersion () const override
	Returns a monotonically increasing version number which represents the version stamp of the map geometry data It is possible for the non-geometric aspects of the map to change without this value increasing. More...
std::vector< lanelet::ConstLanelet >	getLaneletsFromPoint (const lanelet::BasicPoint2d &point, const unsigned int n=10) const override
	Gets the underlying lanelet, given the cartesian point on the map. More...
std::vector< lanelet::ConstLanelet >	nonConnectedAdjacentLeft (const lanelet::BasicPoint2d &input_point, const unsigned int n=10) const override
	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::CarmaTrafficSignalPtr >	getSignalsAlongRoute (const lanelet::BasicPoint2d &loc) const override
	Return a list of traffic lights/intersections along the current route. The traffic lights along a route and the next traffic light ahead of us on the route specifically, so a sorted list (by downtrack distance) of traffic lights on the route ahead of us thus eliminating those behind the vehicle. More...
std::vector< lanelet::BusStopRulePtr >	getBusStopsAlongRoute (const lanelet::BasicPoint2d &loc) const override
	Return a list of bus stop along the current route. The bus stop along a route and the next bus stop ahead of us on the route specifically, so a sorted list (by downtrack distance) of bus stop on the route ahead of us thus eliminating those behind the vehicle. More...
boost::optional< std::pair< lanelet::ConstLanelet, lanelet::ConstLanelet > >	getEntryExitOfSignalAlongRoute (const lanelet::CarmaTrafficSignalPtr &traffic_signal) const override
	Returns the entry and exit lanelet of the signal along the SHORTEST PATH of route. This is useful if traffic_signal controls both directions in an intersection for example. More...
std::vector< std::shared_ptr< lanelet::AllWayStop > >	getIntersectionsAlongRoute (const lanelet::BasicPoint2d &loc) const override
	Return a list of all way stop intersections along the current route. The tall way stop intersections along a route and the next all way stop intersections ahead of us on the route specifically, so a sorted list (by downtrack distance) of all way stop intersections on the route ahead of us thus eliminating those behind the vehicle. More...
std::vector< lanelet::SignalizedIntersectionPtr >	getSignalizedIntersectionsAlongRoute (const lanelet::BasicPoint2d &loc) const
	Return a list of signalized intersections along the current route. The signalized intersections along a route and the next signalized intersections ahead of us on the route specifically, so a sorted list (by downtrack distance) of signalized intersections on the route ahead of us thus eliminating those behind the vehicle. More...
std::optional< lanelet::ConstLanelet >	getFirstLaneletOnShortestPath (const std::vector< lanelet::ConstLanelet > &lanelets_to_filter) const
	Given the vector of lanelets, returns the lanelet that's on the shortest path. Returns earliest lanelet possible on the shortest path. More...
Public Member Functions inherited from carma_wm::WorldModel
virtual	~WorldModel ()
	Virtual destructor to ensure delete safety for pointers to implementing classes. More...
virtual std::pair< TrackPos, TrackPos >	routeTrackPos (const lanelet::ConstArea &area) const =0
	Returns a pair of TrackPos, computed in 2d, of the provided area relative to the current route. The distance is based on the Area vertex with the smallest and largest downtrack distance This method overload is the most expensive of the routeTrackPos methods. More...
virtual TrackPos	routeTrackPos (const lanelet::ConstLanelet &lanelet) const =0
	Returns the TrackPos, computed in 2d, of the provided lanelet relative to the current route. The distance is based on the first point in the lanelet centerline. More...
virtual TrackPos	routeTrackPos (const lanelet::BasicPoint2d &point) const =0
	Returns the TrackPos, computed in 2d, of the provided point relative to the current route. More...
virtual std::vector< lanelet::ConstLanelet >	getLaneletsBetween (double start, double end, bool shortest_path_only=false, bool bounds_inclusive=true) const =0
	Returns a list of lanelets which are part of the route and whose downtrack bounds exist within the provided start and end distances. More...
virtual std::vector< lanelet::BasicPoint2d >	sampleRoutePoints (double start_downtrack, double end_downtrack, double step_size) const =0
	Samples the route centerline between the provided downtracks with the provided step size. At lane changes the points should exhibit a discontinuous jump at the end of the initial lanelet to the end of the lane change lanelet as expected by the route definition. Returned points are always inclusive of provided bounds, so the last step might be less than step_size. More...
virtual boost::optional< lanelet::BasicPoint2d >	pointFromRouteTrackPos (const TrackPos &route_pos) const =0
	Converts a route track position into a map frame cartesian point. More...
virtual lanelet::LaneletMapConstPtr	getMap () const =0
	Get a pointer to the current map. If the underlying map has changed the pointer will also need to be reacquired. More...
virtual LaneletRouteConstPtr	getRoute () const =0
	Get a pointer to the current route. If the underlying route has changed the pointer will also need to be reacquired. More...
virtual std::string	getRouteName () const =0
	Get the current route name. More...
virtual TrackPos	getRouteEndTrackPos () const =0
	Get trackpos of the end of route point relative to the route. More...
virtual LaneletRoutingGraphConstPtr	getMapRoutingGraph () const =0
	Get a pointer to the routing graph for the current map. If the underlying map has changed the pointer will also need to be reacquired. More...
virtual std::vector< carma_perception_msgs::msg::RoadwayObstacle >	getRoadwayObjects () const =0
	Get most recent roadway objects - all objects on the road detected by perception stack. More...
virtual lanelet::Optional< TrafficRulesConstPtr >	getTrafficRules (const std::string &participant) const =0
	Get a pointer to the traffic rules object used internally by the world model and considered the carma system default. More...
virtual lanelet::Optional< TrafficRulesConstPtr >	getTrafficRules () const =0
	Get the Traffic Rules object. More...
virtual lanelet::Optional< carma_perception_msgs::msg::RoadwayObstacle >	toRoadwayObstacle (const carma_perception_msgs::msg::ExternalObject &object) const =0
	Converts an ExternalObject in a RoadwayObstacle by mapping its position onto the semantic map. Can also be used to determine if the object is on the roadway. More...
virtual lanelet::Optional< lanelet::Lanelet >	getIntersectingLanelet (const carma_perception_msgs::msg::ExternalObject &object) const =0
	Gets the a lanelet the object is currently on determined by its position on the semantic map. If it's across multiple lanelets, get the closest one. More...
virtual std::vector< lanelet::BusStopRulePtr >	getBusStopsAlongRoute (const lanelet::BasicPoint2d &loc) const =0
	Return a list of bus stop along the current route. The bus stop along a route and the next bus stop ahead of us on the route specifically, so a sorted list (by downtrack distance) of bus stop on the route ahead of us thus eliminating those behind the vehicle. More...
virtual std::vector< carma_perception_msgs::msg::RoadwayObstacle >	getInLaneObjects (const lanelet::ConstLanelet &lanelet, const LaneSection &section=LANE_AHEAD) const =0
	Gets all roadway objects currently in the same lane as the given lanelet. More...
virtual lanelet::Optional< double >	distToNearestObjInLane (const lanelet::BasicPoint2d &object_center) const =0
	Gets Cartesian distance to the closest object on the same lane as the given point. More...
virtual lanelet::Optional< std::tuple< TrackPos, carma_perception_msgs::msg::RoadwayObstacle > >	nearestObjectAheadInLane (const lanelet::BasicPoint2d &object_center) const =0
	Gets Downtrack distance to AND copy of the closest object AHEAD on the same lane as the given point. Also returns crosstrack distance relative to that object. Plus downtrack if the object is ahead along the lane, and also plus crosstrack if the object is to the right relative to the reference line that crosses given object_center and is parallel to the centerline of the lane. More...
virtual lanelet::Optional< std::tuple< TrackPos, carma_perception_msgs::msg::RoadwayObstacle > >	nearestObjectBehindInLane (const lanelet::BasicPoint2d &object_center) const =0
	Gets Downtrack distance to AND copy of the closest object BEHIND on the same lane as the given point. Also returns crosstrack distance relative to that object. Plus downtrack if the object is ahead along the lane, and also plus crosstrack if the object is to the right relative to the reference line that crosses given object_center and is parallel to the centerline of the lane. More...
virtual std::vector< lanelet::ConstLanelet >	getLane (const lanelet::ConstLanelet &lanelet, const LaneSection &section=LANE_AHEAD) const =0
	Gets the specified lane section achievable without lane change, sorted from the start, that includes the given lanelet. More...
virtual size_t	getMapVersion () const =0
	Returns a monotonically increasing version number which represents the version stamp of the map geometry data It is possible for the non-geometric aspects of the map to change without this value increasing. More...
virtual std::vector< lanelet::ConstLanelet >	getLaneletsFromPoint (const lanelet::BasicPoint2d &point, const unsigned int n=10) const =0
	Gets the underlying lanelet, given the cartesian point on the map. More...
virtual std::vector< lanelet::CarmaTrafficSignalPtr >	getSignalsAlongRoute (const lanelet::BasicPoint2d &loc) const =0
	Return a list of traffic lights/intersections along the current route. The traffic lights along a route and the next traffic light ahead of us on the route specifically, so a sorted list (by downtrack distance) of traffic lights on the route ahead of us thus eliminating those behind the vehicle. More...
virtual boost::optional< std::pair< lanelet::ConstLanelet, lanelet::ConstLanelet > >	getEntryExitOfSignalAlongRoute (const lanelet::CarmaTrafficSignalPtr &traffic_signal) const =0
	Returns the entry and exit lanelet of the signal along the SHORTEST PATH of route. This is useful if traffic_signal controls both directions in an intersection for example. More...
virtual std::vector< std::shared_ptr< lanelet::AllWayStop > >	getIntersectionsAlongRoute (const lanelet::BasicPoint2d &loc) const =0
	Return a list of all way stop intersections along the current route. The tall way stop intersections along a route and the next all way stop intersections ahead of us on the route specifically, so a sorted list (by downtrack distance) of all way stop intersections on the route ahead of us thus eliminating those behind the vehicle. More...
virtual std::vector< lanelet::SignalizedIntersectionPtr >	getSignalizedIntersectionsAlongRoute (const lanelet::BasicPoint2d &loc) const =0
	Return a list of signalized intersections along the current route. The signalized intersections along a route and the next signalized intersections ahead of us on the route specifically, so a sorted list (by downtrack distance) of signalized intersections on the route ahead of us thus eliminating those behind the vehicle. More...
virtual std::vector< lanelet::ConstLanelet >	nonConnectedAdjacentLeft (const lanelet::BasicPoint2d &input_point, const unsigned int n=10) const =0
	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...
virtual std::optional< lanelet::ConstLanelet >	getFirstLaneletOnShortestPath (const std::vector< lanelet::ConstLanelet > &lanelets_to_filter) const =0
	Given the vector of lanelets, returns the lanelet that's on the shortest path. Returns earliest lanelet possible on the shortest path. More...

Public Attributes
std::unordered_map< uint32_t, lanelet::Id >	traffic_light_ids_
carma_wm::SignalizedIntersectionManager	sim_

Private Member Functions
void	computeDowntrackReferenceLine ()
	Helper function to compute the geometry of the route downtrack/crosstrack reference line This function should generally only be called from inside the setRoute function as it uses member variables set in that function. More...
lanelet::LineString3d	copyConstructLineString (const lanelet::ConstLineString3d &line) const
	Helper function to perform a deep copy of a LineString and assign new ids to all the elements. Used during route centerline construction. More...
	FRIEND_TEST (CARMAWorldModelTest, getFirstLaneletOnShortestPath)

Private Attributes
double	config_speed_limit_
std::string	participant_type_ = lanelet::Participants::Vehicle
std::shared_ptr< lanelet::LaneletMap >	semantic_map_
LaneletRoutePtr	route_
LaneletRoutingGraphPtr	map_routing_graph_
double	route_length_ = 0
lanelet::LaneletSubmapConstUPtr	shortest_path_view_
std::vector< lanelet::LineString3d >	shortest_path_centerlines_
IndexedDistanceMap	shortest_path_distance_map_
lanelet::LaneletMapUPtr	shortest_path_filtered_centerline_view_
std::vector< carma_perception_msgs::msg::RoadwayObstacle >	roadway_objects_
size_t	map_version_ = 0
std::string	route_name_

Static Private Attributes
static constexpr double	RED_LIGHT_DURATION = 20.0
static constexpr double	YELLOW_LIGHT_DURATION = 3.0
static constexpr double	GREEN_LIGHT_DURATION = 20.0

Detailed Description

Class which implements the WorldModel interface. In addition this class provides write access to the world model. Write access is achieved through setters for the Map and Route and getMutableMap(). NOTE: This class should NOT be used in runtime code by users and is exposed solely for use in unit tests where the WMListener class cannot be instantiated.

Proper usage of this class dictates that the Map and Route object be kept in sync. For this reason normal WorldModel users should not try to construct this class directly unless in unit tests.

NOTE: This class uses the CarmaUSTrafficRules class internally to interpret routes. So routes which are set on this model should use the getTrafficRules() method to build using the correct rule set

Definition at line 47 of file CARMAWorldModel.hpp.

Constructor & Destructor Documentation

CARMAWorldModel()

carma_wm::CARMAWorldModel::CARMAWorldModel ( )

default

Constructor.

~CARMAWorldModel()

carma_wm::CARMAWorldModel::~CARMAWorldModel ( )

default

Destructor as required by interface.

Member Function Documentation

computeDowntrackReferenceLine()

void carma_wm::CARMAWorldModel::computeDowntrackReferenceLine ( )

private

Helper function to compute the geometry of the route downtrack/crosstrack reference line This function should generally only be called from inside the setRoute function as it uses member variables set in that function.

Sets the shortest_path_centerlines_, shortest_path_centerlines_lengths_, and shortest_path_filtered_centerline_view_ member variables

Definition at line 657 of file CARMAWorldModel.cpp.

  {
    IndexedDistanceMap distance_map;
 
    lanelet::routing::LaneletPath shortest_path = route_->shortestPath();
    // Build shortest path routing graph
    TrafficRulesConstPtr traffic_rules = *(getTrafficRules(participant_type_ ));
 
    lanelet::routing::RoutingGraphUPtr shortest_path_graph =
        lanelet::routing::RoutingGraph::build(*shortest_path_view_, *traffic_rules);
 
    std::vector<lanelet::LineString3d> lineStrings;  // List of continuos line strings representing segments of the route
                                                    // reference line
 
    bool first = true;
    size_t next_index = 0;
    // Iterate over each lanelet in the shortest path this loop works by looking one lanelet ahead to detect lane changes
    for (lanelet::ConstLanelet ll : shortest_path)
    {
      next_index++;
      if (first)
      {  // For the first lanelet store its centerline and length
        lineStrings.push_back(copyConstructLineString(ll.centerline()));
        first = false;
      }
      if (next_index < shortest_path.size())
      {  // Check for remaining lanelets
        auto nextLanelet = shortest_path[next_index];
        lanelet::LineString3d nextCenterline = copyConstructLineString(nextLanelet.centerline());
        size_t connectionCount = shortest_path_graph->possiblePaths(ll, (uint32_t)2, false).size();
 
        if (connectionCount == 1)
        {  // Get list of connected lanelets without lanechanges. On the shortest path this should only return 1 or 0
          // No lane change
          // Append distance to current centerline
          size_t offset =
              lineStrings.size() == 0 || lineStrings.back().size() == 0 ?
                0 :
                1;  // Prevent duplicate points when concatenating. Not clear if causes an issue at lane changes
          if ((nextCenterline.size() <= 1) || (nextCenterline.size() <= 2 && offset == 1))
          {
            throw std::invalid_argument("Cannot process route with lanelet containing very short centerline");
          }
          lineStrings.back().insert(lineStrings.back().end(), nextCenterline.begin() + offset, nextCenterline.end());
        }
        else if (connectionCount == 0)
        {
          // Lane change required
          // Break the point chain when a lanechange occurs
          if (lineStrings.back().size() == 0)
            continue;  // we don't have to create empty_linestring if we already have one
                      // occurs when route is changing lanes multiple times in sequence
          lanelet::LineString3d empty_linestring;
          empty_linestring.setId(lanelet::utils::getId());
          distance_map.pushBack(lanelet::utils::to2D(lineStrings.back()));
          lineStrings.push_back(empty_linestring);
        }
        else
        {
          assert(false);  // It should not be possable to reach this point. Doing so demonstrates a bug
        }
      }
    }
    // Copy values to member variables
    while (lineStrings.back().size() == 0)
      lineStrings.pop_back();  // clear empty linestrings that was never used in the end
    shortest_path_centerlines_ = lineStrings;
    shortest_path_distance_map_ = distance_map;
 
    // Add length of final sections
    if (shortest_path_centerlines_.size() > shortest_path_distance_map_.size())
    {
      shortest_path_distance_map_.pushBack(lanelet::utils::to2D(lineStrings.back()));  // Record length of last continuous
                                                                                      // segment
    }
 
    // Since our copy constructed linestrings do not contain references to lanelets they can be added to a full map
    // instead of a submap
    shortest_path_filtered_centerline_view_ = lanelet::utils::createMap(shortest_path_centerlines_);
  }

References copyConstructLineString(), getTrafficRules(), participant_type_, carma_wm::IndexedDistanceMap::pushBack(), route_, shortest_path_centerlines_, shortest_path_distance_map_, shortest_path_filtered_centerline_view_, shortest_path_view_, and carma_wm::IndexedDistanceMap::size().

Referenced by setRoute().

Here is the call graph for this function:

Here is the caller graph for this function:

copyConstructLineString()

lanelet::LineString3d carma_wm::CARMAWorldModel::copyConstructLineString ( const lanelet::ConstLineString3d &  line ) const

private

Helper function to perform a deep copy of a LineString and assign new ids to all the elements. Used during route centerline construction.

Parameters

line	The linestring to be coppied

Returns

A new linestring containing unique ids for all points and the linestring itself

Definition at line 644 of file CARMAWorldModel.cpp.

  {
    std::vector<lanelet::Point3d> coppied_points;
    coppied_points.reserve(line.size());
 
    for (auto basic_p : line.basicLineString())
    {
      coppied_points.push_back(lanelet::Point3d(lanelet::utils::getId(), basic_p));
    }
 
    return lanelet::LineString3d(lanelet::utils::getId(), coppied_points);
  }

Referenced by computeDowntrackReferenceLine().

Here is the caller graph for this function:

distToNearestObjInLane()

lanelet::Optional< double > carma_wm::CARMAWorldModel::distToNearestObjInLane ( const lanelet::BasicPoint2d &  object_center ) const

overridevirtual

Gets Cartesian distance to the closest object on the same lane as the given point.

Parameters

object_center

the point to measure the distance from

Exceptions

std::invalid_argument

if the map is not set, contains no lanelets, or the given point is not on the current semantic map

Returns

An optional Cartesian distance in double to the closest in lane object. Return empty if there is no objects on current lane or the road

Implements carma_wm::WorldModel.

Definition at line 932 of file CARMAWorldModel.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");
    }
 
    // return empty if there is no object nearby
    if (roadway_objects_.size() == 0)
      return boost::none;
 
    // Get the lanelet of this point
    auto curr_lanelet = semantic_map_->laneletLayer.nearest(object_center, 1)[0];
 
    // Check if this point at least is actually within this lanelet; otherwise, it wouldn't be "in-lane"
    if (!boost::geometry::within(object_center, curr_lanelet.polygon2d().basicPolygon()))
      throw std::invalid_argument("Given point is not within any lanelet");
 
    std::vector<carma_perception_msgs::msg::RoadwayObstacle> lane_objects = getInLaneObjects(curr_lanelet);
 
    // return empty if there is no object in the lane
    if (lane_objects.size() == 0)
      return boost::none;
 
    // Record the closest distance out of all polygons, 4 points each
    double min_dist = INFINITY;
    for (auto obj : roadway_objects_)
    {
      lanelet::BasicPolygon2d object_polygon = geometry::objectToMapPolygon(obj.object.pose.pose, obj.object.size);
 
      // Point to closest edge on polygon distance by boost library
      double curr_dist = lanelet::geometry::distance(object_center, object_polygon);
      if (min_dist > curr_dist)
        min_dist = curr_dist;
    }
 
    // Return the closest distance out of all polygons
    return min_dist;
  }

References getInLaneObjects(), carma_wm::geometry::objectToMapPolygon(), roadway_objects_, and semantic_map_.

Here is the call graph for this function:

FRIEND_TEST()

carma_wm::CARMAWorldModel::FRIEND_TEST ( CARMAWorldModelTest  , getFirstLaneletOnShortestPath    )

private

getBusStopsAlongRoute()

std::vector< lanelet::BusStopRulePtr > carma_wm::CARMAWorldModel::getBusStopsAlongRoute ( const lanelet::BasicPoint2d &  loc ) const

overridevirtual

Return a list of bus stop along the current route. The bus stop along a route and the next bus stop ahead of us on the route specifically, so a sorted list (by downtrack distance) of bus stop on the route ahead of us thus eliminating those behind the vehicle.

Parameters

loc

location

Exceptions

std::invalid_argument

if the map is not set, contains no lanelets, or route is not set

Returns

list of bus stop along the current route

Implements carma_wm::WorldModel.

Definition at line 101 of file CARMAWorldModel.cpp.

  {
    // Check if the map is loaded yet
    if (!semantic_map_ || semantic_map_->laneletLayer.empty())
    {
      RCLCPP_ERROR_STREAM(rclcpp::get_logger("carma_wm"), "Map is not set or does not contain lanelets");
      return {};
    }
    // Check if the route was loaded yet
    if (!route_)
    {
      RCLCPP_ERROR_STREAM(rclcpp::get_logger("carma_wm"), "Route has not yet been loaded");
      return {};
    }
    std::vector<lanelet::BusStopRulePtr> bus_stop_list;
    auto curr_downtrack = routeTrackPos(loc).downtrack;
    // shortpath is already sorted by distance
 
    for (const auto &ll : route_->shortestPath())
    {
      auto bus_stops = semantic_map_->laneletLayer.get(ll.id()).regulatoryElementsAs<lanelet::BusStopRule>();
      if (bus_stops.empty())
      {
        continue;
      }
 
      for (auto bus_stop : bus_stops)
      {
        auto stop_line = bus_stop->stopAndWaitLine();
        if (stop_line.empty())
        {
          RCLCPP_ERROR_STREAM(rclcpp::get_logger("carma_wm"), "No stop line");
          continue;
        }
        else
        {
          double bus_stop_downtrack = routeTrackPos(stop_line.front().front().basicPoint2d()).downtrack;
          double distance_remaining_to_bus_stop = bus_stop_downtrack - curr_downtrack;
 
          if (distance_remaining_to_bus_stop < 0)
          {
            continue;
          }
          bus_stop_list.push_back(bus_stop);
        }
      }
    }
    return bus_stop_list;
  }

References route_, routeTrackPos(), and semantic_map_.

Here is the call graph for this function:

getEntryExitOfSignalAlongRoute()

boost::optional< std::pair< lanelet::ConstLanelet, lanelet::ConstLanelet > > carma_wm::CARMAWorldModel::getEntryExitOfSignalAlongRoute ( const lanelet::CarmaTrafficSignalPtr &  traffic_signal ) const

overridevirtual

Returns the entry and exit lanelet of the signal along the SHORTEST PATH of route. This is useful if traffic_signal controls both directions in an intersection for example.

Parameters

traffic_signal

traffic signal of interest

Exceptions

std::invalid_argument

if nullptr is passed in traffic_signal

Returns

pair of entry and exit lanelet of the signal along the route. boost::none if the given signal doesn't have both entry/exit pair along the shortest path route.

Implements carma_wm::WorldModel.

Definition at line 1250 of file CARMAWorldModel.cpp.

  {
    if (!traffic_signal)
    {
      throw std::invalid_argument("Empty traffic signal pointer has been passed!");
    }
 
    std::pair<lanelet::ConstLanelet, lanelet::ConstLanelet> entry_exit;
    bool found_entry = false;
    bool found_exit = false;
    auto entry_lanelets = traffic_signal->getControlStartLanelets();
    auto exit_lanelets = traffic_signal->getControlEndLanelets();
 
    // get entry and exit lane along route for the nearest given signal
    for (const auto& ll: route_->shortestPath())
    {
      if (!found_entry)
      {
        for (const auto& entry: entry_lanelets)
        {
          if (ll.id() == entry.id())
          {
            entry_exit.first = entry;
            found_entry = true;
            break;
          }
        }
      }
 
      if (!found_exit)
      {
        for (const auto& exit: exit_lanelets)
        {
          if (ll.id() == exit.id())
          {
            entry_exit.second = exit;
            found_exit = true;
            break;
          }
        }
      }
 
      if (found_entry && found_exit)
        return entry_exit;
    }
 
    // was not able to find entry and exit for this signal along route
    return boost::none;
  }

References route_.

getFirstLaneletOnShortestPath()

std::optional< lanelet::ConstLanelet > carma_wm::CARMAWorldModel::getFirstLaneletOnShortestPath ( const std::vector< lanelet::ConstLanelet > &  lanelets_to_filter ) const

virtual

Given the vector of lanelets, returns the lanelet that's on the shortest path. Returns earliest lanelet possible on the shortest path.

Parameters

lanelets_to_filter

lanelets to pick from

Returns

the first lanelet on the shortest path from the input list. std::nullopt if no route is available or no lanelet is on the route

Implements carma_wm::WorldModel.

Definition at line 1374 of file CARMAWorldModel.cpp.

  {
    if (!route_ || lanelets_to_filter.empty())
    {
      return std::nullopt;
    }
 
    // pick a lanelet on the shortest path
    for (const auto& llt : lanelets_to_filter)
    {
      auto shortest_path = route_->shortestPath();
      if (std::find(shortest_path.begin(), shortest_path.end(), llt) != shortest_path.end())
      {
        return llt;
      }
    }
 
    return std::nullopt;
  }

References route_.

getInLaneObjects()

std::vector< carma_perception_msgs::msg::RoadwayObstacle > carma_wm::CARMAWorldModel::getInLaneObjects ( const lanelet::ConstLanelet &  lanelet, const LaneSection &  section = LANE_AHEAD  ) const

overridevirtual

Gets all roadway objects currently in the same lane as the given lanelet.

Parameters

lanelet	the lanelet that is part of the continuous lane
section	either of LANE_AHEAD, LANE_BEHIND, LANE_FULL each including the current lanelet

Exceptions

std::invalid_argument

if the map is not set, contains no lanelets, or if the given lanelet is not on the current semantic map, or lane section input is not of the three

Returns

A vector of RoadwayObstacle objects that is on the current lane. Return empty vector if there is no objects on current lane or the road

Implements carma_wm::WorldModel.

Definition at line 838 of file CARMAWorldModel.cpp.

  {
    // Get all lanelets on current lane section
    std::vector<lanelet::ConstLanelet> lane = getLane(lanelet, section);
 
    // Check if any roadway object is registered
    if (roadway_objects_.size() == 0)
    {
      return std::vector<carma_perception_msgs::msg::RoadwayObstacle>{};
    }
 
    // Initialize useful variables
    std::vector<carma_perception_msgs::msg::RoadwayObstacle> lane_objects, roadway_objects_copy = roadway_objects_;
 
    /*
   * Get all in lane objects
   * For each lane, we check if each object is on it by matching lanelet_id
   * Complexity N*K, where N: num of lanelets, K: num of objects
   */
    int curr_idx;
    std::queue<int> obj_idxs_queue;
 
    // Create an index queue for roadway objects to quickly pop the idx if associated
    // lanelet is found. This is to reduce number of objects to check as we check new lanelets
    for (size_t i = 0; i < roadway_objects_copy.size(); i++)
    {
      obj_idxs_queue.push((int)i);
    }
 
    // check each lanelets
    for (auto llt : lane)
    {
      int checked_queue_items = 0, to_check = obj_idxs_queue.size();
      // check each objects
      while (checked_queue_items < to_check)
      {
        curr_idx = obj_idxs_queue.front();
        obj_idxs_queue.pop();
        checked_queue_items++;
 
        // Check if the object is in the lanelet
        auto curr_obj = roadway_objects_copy[curr_idx];
        if (curr_obj.lanelet_id == llt.id())
        {
          // found intersecting lanelet for this object
          lane_objects.push_back(curr_obj);
        }
        // handle a case where an object might be lane-changing, so check adjacent ids
        // a bit faster than checking intersection solely as && is left-to-right evaluation
        else if (((map_routing_graph_->left(llt) && curr_obj.lanelet_id == map_routing_graph_->left(llt).get().id()) ||
                  (map_routing_graph_->right(llt) && curr_obj.lanelet_id == map_routing_graph_->right(llt).get().id())) &&
                 boost::geometry::intersects(
                     llt.polygon2d().basicPolygon(),
                     geometry::objectToMapPolygon(curr_obj.object.pose.pose, curr_obj.object.size)))
        {
          // found intersecting lanelet for this object
          lane_objects.push_back(curr_obj);
        }
        else
        {
          // did not find suitable lanelet, so it will be processed again for next lanelet
          obj_idxs_queue.push(curr_idx);
        }
      }
    }
 
    return lane_objects;
  }

References getLane(), process_bag::i, map_routing_graph_, carma_wm::geometry::objectToMapPolygon(), and roadway_objects_.

Referenced by distToNearestObjInLane(), and getNearestObjInLane().

Here is the call graph for this function:

Here is the caller graph for this function:

getIntersectingLanelet()

lanelet::Optional< lanelet::Lanelet > carma_wm::CARMAWorldModel::getIntersectingLanelet ( const carma_perception_msgs::msg::ExternalObject &  object ) const

overridevirtual

Gets the a lanelet the object is currently on determined by its position on the semantic map. If it's across multiple lanelets, get the closest one.

Parameters

object

the external object to get the lanelet of

Exceptions

std::invalid_argument

if the map is not set or contains no lanelets

Returns

An optional lanelet primitive that is on the semantic map. If the external object is not on the roadway then the optional will be empty.

Implements carma_wm::WorldModel.

Definition at line 909 of file CARMAWorldModel.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");
    }
 
    lanelet::BasicPoint2d object_center(object.pose.pose.position.x, object.pose.pose.position.y);
    lanelet::BasicPolygon2d object_polygon = geometry::objectToMapPolygon(object.pose.pose, object.size);
 
    auto nearestLanelet = semantic_map_->laneletLayer.nearest(
        object_center, 1)[0];  // Since the map contains lanelets there should always be at least 1 element
 
    // Check if the object is inside or intersecting this lanelet
    // If no intersection then the object can be considered off the road and does not need to processed
    if (!boost::geometry::intersects(nearestLanelet.polygon2d().basicPolygon(), object_polygon))
    {
      return boost::none;
    }
    return nearestLanelet;
  }

References carma_wm::geometry::objectToMapPolygon(), and semantic_map_.

Referenced by toRoadwayObstacle().

Here is the call graph for this function:

Here is the caller graph for this function:

getIntersectionsAlongRoute()

std::vector< std::shared_ptr< lanelet::AllWayStop > > carma_wm::CARMAWorldModel::getIntersectionsAlongRoute ( const lanelet::BasicPoint2d &  loc ) const

overridevirtual

Return a list of all way stop intersections along the current route. The tall way stop intersections along a route and the next all way stop intersections ahead of us on the route specifically, so a sorted list (by downtrack distance) of all way stop intersections on the route ahead of us thus eliminating those behind the vehicle.

Parameters

loc

location

Exceptions

std::invalid_argument

if the map is not set, contains no lanelets, or route is not set

Returns

list of all way stop intersections along the current route

Implements carma_wm::WorldModel.

Definition at line 1300 of file CARMAWorldModel.cpp.

  {
    // Check if the map is loaded yet
    if (!semantic_map_ || semantic_map_->laneletLayer.empty())
    {
      RCLCPP_ERROR_STREAM(rclcpp::get_logger("carma_wm"), "Map is not set or does not contain lanelets");
      return {};
    }
    // Check if the route was loaded yet
    if (!route_)
    {
      RCLCPP_ERROR_STREAM(rclcpp::get_logger("carma_wm"), "Route has not yet been loaded");
      return {};
    }
    std::vector<std::shared_ptr<lanelet::AllWayStop>> intersection_list;
    auto curr_downtrack = routeTrackPos(loc).downtrack;
    // shortpath is already sorted by distance
    for(const auto& ll : route_->shortestPath())
    {
      auto intersections = semantic_map_->laneletLayer.get(ll.id()).regulatoryElementsAs<lanelet::AllWayStop>();
      if (intersections.empty())
      {
        continue;
      }
      for (auto intersection : intersections)
      {
        double intersection_downtrack = routeTrackPos(intersection->stopLines().front().front().basicPoint2d()).downtrack;
        if (intersection_downtrack < curr_downtrack)
        {
          continue;
        }
        intersection_list.push_back(intersection);
      }
    }
    return intersection_list;
  }

References route_, routeTrackPos(), and semantic_map_.

Here is the call graph for this function:

getLane()

std::vector< lanelet::ConstLanelet > carma_wm::CARMAWorldModel::getLane ( const lanelet::ConstLanelet &  lanelet, const LaneSection &  section = LANE_AHEAD  ) const

overridevirtual

Gets the specified lane section achievable without lane change, sorted from the start, that includes the given lanelet.

Parameters

lanelet	the lanelet to get the full lane of
section	either of LANE_AHEAD, LANE_BEHIND, LANE_FULL each including the current lanelet

Exceptions

std::invalid_argument

if the map is not set, contains no lanelets, or the given lanelet is not on the current semantic map, or lane section input is not of the three

Returns

An optional vector of ConstLanalet. Returns at least the vector of given lanelet if no other is found

Implements carma_wm::WorldModel.

Definition at line 1098 of file CARMAWorldModel.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");
    }
 
    // Check if the lanelet is in map
    if (semantic_map_->laneletLayer.find(lanelet.id()) == semantic_map_->laneletLayer.end())
    {
      throw std::invalid_argument("Lanelet is not on the map");
    }
 
    // Check if the lane section input is correct
    if (section != LANE_FULL && section != LANE_BEHIND && section != LANE_AHEAD)
    {
      throw std::invalid_argument("Undefined lane section is requested");
    }
 
    std::vector<lanelet::ConstLanelet> following_lane = {lanelet};
    std::stack<lanelet::ConstLanelet> prev_lane_helper;
    std::vector<lanelet::ConstLanelet> prev_lane;
    std::vector<lanelet::ConstLanelet> connecting_lanelet = map_routing_graph_->following(lanelet, false);
 
    // if only interested in following lanelets, as it is the most case
    while (connecting_lanelet.size() != 0)
    {
      following_lane.push_back(connecting_lanelet[0]);
      connecting_lanelet = map_routing_graph_->following(connecting_lanelet[0], false);
    }
    if (section == LANE_AHEAD)
      return following_lane;
 
    // if interested in lanelets behind
    connecting_lanelet = map_routing_graph_->previous(lanelet, false);
    while (connecting_lanelet.size() != 0)
    {
      prev_lane_helper.push(connecting_lanelet[0]);
      connecting_lanelet = map_routing_graph_->previous(connecting_lanelet[0], false);
    }
 
    // gather all lanelets with correct start order
    while (prev_lane_helper.size() != 0)
    {
      prev_lane.push_back(prev_lane_helper.top());
      prev_lane_helper.pop();
    }
 
    // if only interested in lane behind
    if (section == LANE_BEHIND)
    {
      prev_lane.push_back(lanelet);
      return prev_lane;
    }
 
    // if interested in full lane
    prev_lane.insert(prev_lane.end(), following_lane.begin(), following_lane.end());
    return prev_lane;
  }

References carma_wm::LANE_AHEAD, carma_wm::LANE_BEHIND, carma_wm::LANE_FULL, map_routing_graph_, and semantic_map_.

Referenced by getInLaneObjects(), and getNearestObjInLane().

Here is the caller graph for this function:

getLaneletsBetween()

std::vector< lanelet::ConstLanelet > carma_wm::CARMAWorldModel::getLaneletsBetween ( double  start, double  end, bool  shortest_path_only = false, bool  bounds_inclusive = true  ) const

overridevirtual

Returns a list of lanelets which are part of the route and whose downtrack bounds exist within the provided start and end distances.

Parameters

start	The starting downtrack for the query
end	The ending downtrack for the query.
shortest_path_only	If true the lanelets returned will be part of the route shortest path param bounds_inclusive If true, the bounds are included so areas which end exactly at start or start exactly at end are included. NOTE: Non-inclusive behavior toggled by !bounds_inclusive is not equivalent to a != check as it merely shrinks bounds by 0.00001 to get new start and end distances.

Exceptions

std::invalid_argument

If the route is not yet loaded or if start > end

Returns

A list of lanelets which contain regions that lie between start and end along the route. This function will not return lanelets which are not part of the route

Implements carma_wm::WorldModel.

Definition at line 287 of file CARMAWorldModel.cpp.

  {
    // Check if the route was loaded yet
    if (!route_)
    {
      throw std::invalid_argument("Route has not yet been loaded");
    }
    if (start > end)
    {
      throw std::invalid_argument("Start distance is greater than end distance");
    }
 
    std::vector<lanelet::ConstLanelet> output;
    std::priority_queue<LaneletDowntrackPair, std::vector<LaneletDowntrackPair>, std::greater<LaneletDowntrackPair>>
        prioritized_lanelets;
 
    auto lanelet_map = route_->laneletMap();
    for (lanelet::ConstLanelet lanelet : lanelet_map->laneletLayer)
    {
      if (shortest_path_only && !shortest_path_view_->laneletLayer.exists(lanelet.id()))
      {
        continue;  // Continue if we are only evaluating the shortest path and this lanelet is not part of it
      }
      lanelet::ConstLineString2d centerline = lanelet::utils::to2D(lanelet.centerline());
 
      auto front = centerline.front();
      auto back = centerline.back();
      TrackPos min = routeTrackPos(front);
      TrackPos max = routeTrackPos(back);
 
      if (!bounds_inclusive) // reduce bounds slightly to avoid including exact bounds
      {
        if (std::max(min.downtrack, start + 0.00001) > std::min(max.downtrack, end - 0.00001)
          || (start == end && (min.downtrack >= start || max.downtrack <= end)))
        {  // Check for 1d intersection
          // No intersection so continue
          continue;
        }
      }
      else
      {
        if (std::max(min.downtrack, start) > std::min(max.downtrack, end)
          || (start == end && (min.downtrack > start || max.downtrack < end)))
        {  // Check for 1d intersection
          // No intersection so continue
          continue;
        }
      }
      // Intersection has occurred so add lanelet to list
      LaneletDowntrackPair pair(lanelet, min.downtrack);
      prioritized_lanelets.push(pair);
    }
 
    output.reserve(prioritized_lanelets.size());
    while (!prioritized_lanelets.empty())
    {
      auto pair = prioritized_lanelets.top();
      prioritized_lanelets.pop();
      output.push_back(pair.lanelet_);
    }
 
    if (!shortest_path_only)
    {
      return output;
    }
 
    //Sort lanelets according to shortest path if using shortest path
    std::vector<lanelet::ConstLanelet> sorted_output;
    for (auto llt : route_->shortestPath())
    {
      for (size_t i = 0; i < output.size(); i++)
      {
        if (llt.id() == output[i].id())
        {
          sorted_output.push_back(llt);
          break;
        }
      }
    }
 
    return sorted_output;
  }

References carma_wm::TrackPos::downtrack, process_bag::i, route_, routeTrackPos(), and shortest_path_view_.

Here is the call graph for this function:

getLaneletsFromPoint() [1/2]

std::vector< lanelet::Lanelet > carma_wm::CARMAWorldModel::getLaneletsFromPoint	(	const lanelet::BasicPoint2d &	point,
		const unsigned int	n
	)

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

Parameters

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 1180 of file CARMAWorldModel.cpp.

  {
    return carma_wm::query::getLaneletsFromPoint(semantic_map_, point, n);
  }

References carma_wm::query::getLaneletsFromPoint(), process_traj_logs::point, and semantic_map_.

Here is the call graph for this function:

getLaneletsFromPoint() [2/2]

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

overridevirtual

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

Parameters

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

Implements carma_wm::WorldModel.

Definition at line 1185 of file CARMAWorldModel.cpp.

  {
    return carma_wm::query::getLaneletsFromPoint(getMap(), point, n);
  }

References carma_wm::query::getLaneletsFromPoint(), getMap(), and process_traj_logs::point.

Here is the call graph for this function:

getMap()

lanelet::LaneletMapConstPtr carma_wm::CARMAWorldModel::getMap ( ) const

overridevirtual

Get a pointer to the current map. If the underlying map has changed the pointer will also need to be reacquired.

Returns

Shared pointer to underlying lanelet map. Pointer will return false on boolean check if no map is loaded

Implements carma_wm::WorldModel.

Definition at line 529 of file CARMAWorldModel.cpp.

  {
    return std::static_pointer_cast<lanelet::LaneletMap const>(semantic_map_);  // Cast pointer to const variant
  }

References semantic_map_.

Referenced by getLaneletsFromPoint(), and nonConnectedAdjacentLeft().

Here is the caller graph for this function:

getMapRoutingGraph()

LaneletRoutingGraphConstPtr carma_wm::CARMAWorldModel::getMapRoutingGraph ( ) const

overridevirtual

Get a pointer to the routing graph for the current map. If the underlying map has changed the pointer will also need to be reacquired.

Returns

Shared pointer to underlying lanelet route graph. Pointer will return false on boolean check if no map is loaded

Implements carma_wm::WorldModel.

Definition at line 738 of file CARMAWorldModel.cpp.

  {
    return std::static_pointer_cast<const lanelet::routing::RoutingGraph>(map_routing_graph_);  // Cast pointer to const
                                                                                               // variant
  }

References map_routing_graph_.

getMapVersion()

size_t carma_wm::CARMAWorldModel::getMapVersion ( ) const

overridevirtual

Returns a monotonically increasing version number which represents the version stamp of the map geometry data It is possible for the non-geometric aspects of the map to change without this value increasing.

Returns

map version

Implements carma_wm::WorldModel.

Definition at line 588 of file CARMAWorldModel.cpp.

  {
    return map_version_;
  }

References map_version_.

getMutableMap()

lanelet::LaneletMapPtr carma_wm::CARMAWorldModel::getMutableMap

(

)

const

Get a mutable version of the current map.

NOTE: the user must make sure to setMap() after any edit to the map and to set a valid route

Definition at line 593 of file CARMAWorldModel.cpp.

  {
    return semantic_map_;
  }

References semantic_map_.

getNearestObjInLane()

lanelet::Optional< std::tuple< TrackPos, carma_perception_msgs::msg::RoadwayObstacle > > carma_wm::CARMAWorldModel::getNearestObjInLane	(	const lanelet::BasicPoint2d &	object_center,
		const LaneSection &	section = LANE_AHEAD
	)		const

This function is called by distanceToObjectBehindInLane or distanceToObjectAheadInLane. Gets Downtrack distance to AND copy of the closest object on the same lane as the given point. Also returns crosstrack distance relative to that object. Plus downtrack if the object is ahead along the lane, and also plus crosstrack if the object is to the right relative to the reference line that crosses given object_center and is parallel to the centerline of the lane.

Parameters

object_center	the point to measure the distance from
section	either of LANE_AHEAD, LANE_BEHIND, LANE_FULL each including the current lanelet

Exceptions

std::invalid_argument

if the map is not set, contains no lanelets, or the given point is not on the current semantic map

Returns

An optional tuple of <TrackPos, carma_perception_msgs::msg::RoadwayObstacle> to the closest in lane object. Return empty if there is no objects on current lane or the road

Definition at line 974 of file CARMAWorldModel.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");
    }
 
    // return empty if there is no object nearby
    if (roadway_objects_.size() == 0)
      return boost::none;
 
    // Get the lanelet of this point
    auto curr_lanelet = semantic_map_->laneletLayer.nearest(object_center, 1)[0];
 
    // Check if this point at least is actually within this lanelet; otherwise, it wouldn't be "in-lane"
    if (!boost::geometry::within(object_center, curr_lanelet.polygon2d().basicPolygon()))
      throw std::invalid_argument("Given point is not within any lanelet");
 
    // Get objects that are in the lane
    std::vector<carma_perception_msgs::msg::RoadwayObstacle> lane_objects = getInLaneObjects(curr_lanelet, section);
 
    // return empty if there is no object in the lane
    if (lane_objects.size() == 0)
      return boost::none;
 
    // Get the lane that is including this lanelet
    std::vector<lanelet::ConstLanelet> lane_section = getLane(curr_lanelet, section);
 
    std::vector<double> object_downtracks, object_crosstracks;
    std::vector<int> object_idxs;
    std::queue<int> obj_idxs_queue;
    double base_downtrack = 0;
    double input_obj_downtrack = 0;
    int curr_idx = 0;
 
    // Create an index queue for in lane objects to quickly pop the idx if associated
    // lanelet is found. This is to reduce number of objects to check as we check new lanelets
    for (size_t i = 0; i < lane_objects.size(); i++)
    {
      obj_idxs_queue.push((int)i);
    }
 
    // For each lanelet, check if each object is inside it. if so, calculate downtrack
    for (auto llt : lane_section)
    {
      int checked_queue_items = 0, to_check = obj_idxs_queue.size();
 
      // check each remaining objects
      while (checked_queue_items < to_check)
      {
        curr_idx = obj_idxs_queue.front();
        obj_idxs_queue.pop();
        checked_queue_items++;
 
        // if the object is on it, store its total downtrack distance
        if (lane_objects[curr_idx].lanelet_id == llt.id())
        {
          object_downtracks.push_back(base_downtrack + lane_objects[curr_idx].down_track);
          object_crosstracks.push_back(lane_objects[curr_idx].cross_track);
          object_idxs.push_back(curr_idx);
        }
        // if it's not on it, try adjacent lanelets because the object could be lane changing
        else if ((map_routing_graph_->left(llt) &&
                  lane_objects[curr_idx].lanelet_id == map_routing_graph_->left(llt).get().id()) ||
                 (map_routing_graph_->right(llt) &&
                  lane_objects[curr_idx].lanelet_id == map_routing_graph_->right(llt).get().id()))
        {
          // no need to check intersection as the objects are guaranteed to be intersecting this lane
          lanelet::BasicPoint2d obj_center(lane_objects[curr_idx].object.pose.pose.position.x,
                                           lane_objects[curr_idx].object.pose.pose.position.y);
          TrackPos new_tp = geometry::trackPos(llt, obj_center);
          object_downtracks.push_back(base_downtrack + new_tp.downtrack);
          object_crosstracks.push_back(lane_objects[curr_idx].cross_track);
          object_idxs.push_back(curr_idx);
        }
        else
        {
          // the object is not in the lanelet if above conditions do not meet
          obj_idxs_queue.push(curr_idx);
        }
      }
      // try to update object_center's downtrack
      if (curr_lanelet.id() == llt.id())
        input_obj_downtrack = base_downtrack + geometry::trackPos(llt, object_center).downtrack;
 
      // this lanelet's entire centerline as downtrack
      base_downtrack +=
          geometry::trackPos(llt.centerline().back().basicPoint2d(), llt.centerline().front().basicPoint2d(),
                             llt.centerline().back().basicPoint2d())
              .downtrack;
    }
 
    // compare with input's downtrack and return the min_dist
    size_t min_idx = 0;
    double min_dist = INFINITY;
    for (size_t idx = 0; idx < object_downtracks.size(); idx++)
    {
      if (min_dist > std::fabs(object_downtracks[idx] - input_obj_downtrack))
      {
        min_dist = std::fabs(object_downtracks[idx] - input_obj_downtrack);
        min_idx = idx;
      }
    }
 
    // if before the parallel line with the start of the llt that crosses given object_center, neg downtrack.
    // if left to the parallel line with the centerline of the llt that crosses given object_center, pos crosstrack
    return std::tuple<TrackPos, carma_perception_msgs::msg::RoadwayObstacle>(
        TrackPos(object_downtracks[min_idx] - input_obj_downtrack,
                 object_crosstracks[min_idx] - geometry::trackPos(curr_lanelet, object_center).crosstrack),
        lane_objects[object_idxs[min_idx]]);
  }

References carma_wm::TrackPos::downtrack, getInLaneObjects(), getLane(), process_bag::i, map_routing_graph_, roadway_objects_, semantic_map_, and carma_wm::geometry::trackPos().

Referenced by nearestObjectAheadInLane(), and nearestObjectBehindInLane().

Here is the call graph for this function:

Here is the caller graph for this function:

getRoadwayObjects()

std::vector< carma_perception_msgs::msg::RoadwayObstacle > carma_wm::CARMAWorldModel::getRoadwayObjects ( ) const

overridevirtual

Get most recent roadway objects - all objects on the road detected by perception stack.

Returns

Vector list of RoadwayObstacle which are lanelet compatible. Empty vector if no object found.

Implements carma_wm::WorldModel.

Definition at line 833 of file CARMAWorldModel.cpp.

  {
    return roadway_objects_;
  }

References roadway_objects_.

getRoute()

LaneletRouteConstPtr carma_wm::CARMAWorldModel::getRoute ( ) const

overridevirtual

Get a pointer to the current route. If the underlying route has changed the pointer will also need to be reacquired.

Returns

Shared pointer to underlying lanelet route. Pointer will return false on boolean check if no route or map is loaded

Implements carma_wm::WorldModel.

Definition at line 534 of file CARMAWorldModel.cpp.

  {
    return std::static_pointer_cast<const lanelet::routing::Route>(route_);  // Cast pointer to const variant
  }

References route_.

getRouteEndTrackPos()

TrackPos carma_wm::CARMAWorldModel::getRouteEndTrackPos ( ) const

overridevirtual

Get trackpos of the end of route point relative to the route.

Returns

Trackpos

Implements carma_wm::WorldModel.

Definition at line 539 of file CARMAWorldModel.cpp.

  {
    TrackPos p(route_length_, 0);
    return p;
  }

References route_length_.

Referenced by pointFromRouteTrackPos(), and sampleRoutePoints().

Here is the caller graph for this function:

getRouteName()

std::string carma_wm::CARMAWorldModel::getRouteName ( ) const

overridevirtual

Get the current route name.

Returns

A string that matches the name of the current route.

Implements carma_wm::WorldModel.

Definition at line 639 of file CARMAWorldModel.cpp.

  {
    return route_name_;
  }

References route_name_.

getSignalizedIntersectionsAlongRoute()

std::vector< lanelet::SignalizedIntersectionPtr > carma_wm::CARMAWorldModel::getSignalizedIntersectionsAlongRoute ( const lanelet::BasicPoint2d &  loc ) const

virtual

Return a list of signalized intersections along the current route. The signalized intersections along a route and the next signalized intersections ahead of us on the route specifically, so a sorted list (by downtrack distance) of signalized intersections on the route ahead of us thus eliminating those behind the vehicle.

Parameters

loc

location

Exceptions

std::invalid_argument

if the map is not set, contains no lanelets, or route is not set

Returns

list of signalized intersections along the current route

Implements carma_wm::WorldModel.

Definition at line 1337 of file CARMAWorldModel.cpp.

  {
    // Check if the map is loaded yet
    if (!semantic_map_ || semantic_map_->laneletLayer.empty())
    {
      RCLCPP_ERROR_STREAM(rclcpp::get_logger("carma_wm"), "Map is not set or does not contain lanelets");
      return {};
    }
    // Check if the route was loaded yet
    if (!route_)
    {
      RCLCPP_ERROR_STREAM(rclcpp::get_logger("carma_wm"), "Route has not yet been loaded");
      return {};
    }
    std::vector<lanelet::SignalizedIntersectionPtr> intersection_list;
    auto curr_downtrack = routeTrackPos(loc).downtrack;
    // shortpath is already sorted by distance
    for (const auto &ll : route_->shortestPath())
    {
      auto intersections = semantic_map_->laneletLayer.get(ll.id()).regulatoryElementsAs<lanelet::SignalizedIntersection>();
      if (intersections.empty())
      {
        continue;
      }
      for (auto intersection : intersections)
      {
        double intersection_downtrack = routeTrackPos(ll.centerline().back().basicPoint2d()).downtrack;
        if (intersection_downtrack < curr_downtrack)
        {
          continue;
        }
        intersection_list.push_back(intersection);
      }
    }
    return intersection_list;
  }

References route_, routeTrackPos(), and semantic_map_.

Here is the call graph for this function:

getSignalsAlongRoute()

std::vector< lanelet::CarmaTrafficSignalPtr > carma_wm::CARMAWorldModel::getSignalsAlongRoute ( const lanelet::BasicPoint2d &  loc ) const

overridevirtual

Return a list of traffic lights/intersections along the current route. The traffic lights along a route and the next traffic light ahead of us on the route specifically, so a sorted list (by downtrack distance) of traffic lights on the route ahead of us thus eliminating those behind the vehicle.

Parameters

loc

location

Exceptions

std::invalid_argument

if the map is not set, contains no lanelets, or route is not set

Returns

list of traffic lights along the current route

Implements carma_wm::WorldModel.

Definition at line 1200 of file CARMAWorldModel.cpp.

  {
    // Check if the map is loaded yet
    if (!semantic_map_ || semantic_map_->laneletLayer.empty())
    {
      RCLCPP_ERROR_STREAM(rclcpp::get_logger("carma_wm"), "Map is not set or does not contain lanelets");
      return {};
    }
    // Check if the route was loaded yet
    if (!route_)
    {
      RCLCPP_ERROR_STREAM(rclcpp::get_logger("carma_wm"), "Route has not yet been loaded");
      return {};
    }
    std::vector<lanelet::CarmaTrafficSignalPtr> light_list;
    auto curr_downtrack = routeTrackPos(loc).downtrack;
    // shortpath is already sorted by distance
 
    for (const auto &ll : route_->shortestPath())
    {
      auto lights = semantic_map_->laneletLayer.get(ll.id()).regulatoryElementsAs<lanelet::CarmaTrafficSignal>();
      if (lights.empty())
      {
        continue;
      }
 
      for (auto light : lights)
      {
        auto stop_line = light->getStopLine(ll);
        if (!stop_line)
        {
          RCLCPP_ERROR_STREAM(rclcpp::get_logger("carma_wm"), "No stop line");
          continue;
        }
        else
        {
          double light_downtrack = routeTrackPos(stop_line.get().front().basicPoint2d()).downtrack;
          double distance_remaining_to_traffic_light = light_downtrack - curr_downtrack;
 
          if (distance_remaining_to_traffic_light < 0)
          {
            continue;
          }
          light_list.push_back(light);
        }
      }
    }
    return light_list;
  }

References route_, routeTrackPos(), and semantic_map_.

Here is the call graph for this function:

getTrafficRules() [1/2]

lanelet::Optional< TrafficRulesConstPtr > carma_wm::CARMAWorldModel::getTrafficRules ( ) const

overridevirtual

Get the Traffic Rules object.

Returns

Optional Shared pointer to an intialized traffic rules object which is used by carma. A default participant value will be used in case setVehicleParticipationType is not called. Acceptable participants are Vehicle, VehicleCar, and VehicleTruck

Implements carma_wm::WorldModel.

Definition at line 769 of file CARMAWorldModel.cpp.

  {
 
    return getTrafficRules(participant_type_);
  }

References getTrafficRules(), and participant_type_.

Referenced by computeDowntrackReferenceLine(), getTrafficRules(), and setMap().

Here is the call graph for this function:

Here is the caller graph for this function:

getTrafficRules() [2/2]

lanelet::Optional< TrafficRulesConstPtr > carma_wm::CARMAWorldModel::getTrafficRules ( const std::string &  participant ) const

overridevirtual

Get a pointer to the traffic rules object used internally by the world model and considered the carma system default.

Parameters

participant

The lanelet participant to return the traffic rules object for. Defaults to a generic vehicle

Returns

Optional Shared pointer to an intialized traffic rules object which is used by carma. Optional is false if no rule set is available for the requested participant.

Implements carma_wm::WorldModel.

Definition at line 744 of file CARMAWorldModel.cpp.

  {
    lanelet::Optional<TrafficRulesConstPtr> optional_ptr;
    // Create carma traffic rules object
    try
    {
      lanelet::traffic_rules::TrafficRulesUPtr traffic_rules = lanelet::traffic_rules::TrafficRulesFactory::create(
          lanelet::traffic_rules::CarmaUSTrafficRules::Location, participant);
 
      auto carma_traffic_rules = std::make_shared<lanelet::traffic_rules::CarmaUSTrafficRules>();
 
      carma_traffic_rules = std::static_pointer_cast<lanelet::traffic_rules::CarmaUSTrafficRules>(
          lanelet::traffic_rules::TrafficRulesPtr(std::move(traffic_rules)));
      carma_traffic_rules->setConfigSpeedLimit(config_speed_limit_);
 
      optional_ptr = std::static_pointer_cast<const lanelet::traffic_rules::CarmaUSTrafficRules>(carma_traffic_rules);
    }
    catch (const lanelet::InvalidInputError& e)
    {
      return optional_ptr;
    }
 
    return optional_ptr;
  }

References config_speed_limit_.

getVehicleParticipationType()

std::string carma_wm::CARMAWorldModel::getVehicleParticipationType

(

)

Get vehicle participation type.

Definition at line 1175 of file CARMAWorldModel.cpp.

  {
    return participant_type_;
  }

References participant_type_.

nearestObjectAheadInLane()

lanelet::Optional< std::tuple< TrackPos, carma_perception_msgs::msg::RoadwayObstacle > > carma_wm::CARMAWorldModel::nearestObjectAheadInLane ( const lanelet::BasicPoint2d &  object_center ) const

overridevirtual

Gets Downtrack distance to AND copy of the closest object AHEAD on the same lane as the given point. Also returns crosstrack distance relative to that object. Plus downtrack if the object is ahead along the lane, and also plus crosstrack if the object is to the right relative to the reference line that crosses given object_center and is parallel to the centerline of the lane.

Parameters

object_center

the point to measure the distance from

Exceptions

std::invalid_argument

if the map is not set, contains no lanelets, or the given point is not on the current semantic map

Returns

An optional tuple of <TrackPos, carma_perception_msgs::msg::RoadwayObstacle> to the closest in lane object AHEAD. Return empty if there is no objects on current lane or the road

Implements carma_wm::WorldModel.

Definition at line 1088 of file CARMAWorldModel.cpp.

  {
    return getNearestObjInLane(object_center, LANE_AHEAD);
  }

References getNearestObjInLane(), and carma_wm::LANE_AHEAD.

Here is the call graph for this function:

nearestObjectBehindInLane()

lanelet::Optional< std::tuple< TrackPos, carma_perception_msgs::msg::RoadwayObstacle > > carma_wm::CARMAWorldModel::nearestObjectBehindInLane ( const lanelet::BasicPoint2d &  object_center ) const

overridevirtual

Gets Downtrack distance to AND copy of the closest object BEHIND on the same lane as the given point. Also returns crosstrack distance relative to that object. Plus downtrack if the object is ahead along the lane, and also plus crosstrack if the object is to the right relative to the reference line that crosses given object_center and is parallel to the centerline of the lane.

Parameters

object_center

the point to measure the distance from

Exceptions

std::invalid_argument

if the map is not set, contains no lanelets, or the given point is not on the current semantic map

Returns

An optional tuple of <TrackPos, carma_perception_msgs::msg::RoadwayObstacle> to the closest in lane object BEHIND. Return empty if there is no objects on current lane or the road

Implements carma_wm::WorldModel.

Definition at line 1094 of file CARMAWorldModel.cpp.

  {
    return getNearestObjInLane(object_center, LANE_BEHIND);
  }

References getNearestObjInLane(), and carma_wm::LANE_BEHIND.

Here is the call graph for this function:

nonConnectedAdjacentLeft() [1/2]

std::vector< lanelet::Lanelet > carma_wm::CARMAWorldModel::nonConnectedAdjacentLeft	(	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

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

Returns

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

Definition at line 1190 of file CARMAWorldModel.cpp.

  {
    return carma_wm::query::getLaneletsFromPoint(semantic_map_, input_point, n);
  }

References carma_wm::query::getLaneletsFromPoint(), and semantic_map_.

Here is the call graph for this function:

nonConnectedAdjacentLeft() [2/2]

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

overridevirtual

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

Returns

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

Implements carma_wm::WorldModel.

Definition at line 1195 of file CARMAWorldModel.cpp.

  {
    return carma_wm::query::getLaneletsFromPoint(getMap(), input_point, n);
  }

References carma_wm::query::getLaneletsFromPoint(), and getMap().

Here is the call graph for this function:

pointFromRouteTrackPos()

boost::optional< lanelet::BasicPoint2d > carma_wm::CARMAWorldModel::pointFromRouteTrackPos ( const TrackPos &  route_pos ) const

overridevirtual

Converts a route track position into a map frame cartesian point.

Parameters

route_pos

The TrackPos to convert to and x,y point. This position should be relative to the route

Returns

If the provided downtrack position is not within the route bounds or the route is not set then boost::none is returned. Otherwise the point is converted to x,y.

NOTE: This method will run faster if the crosstrack is unset or set to 0. The default implementation lookup has complexity O(log n) where n is the number of lane changes is a route + 1

Implements carma_wm::WorldModel.

Definition at line 412 of file CARMAWorldModel.cpp.

  {
    double downtrack = route_pos.downtrack;
    double crosstrack = route_pos.crosstrack;
 
    if (!route_)
    {
      RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm"), "Route has not yet been loaded");
      return boost::none;
    }
 
    if (downtrack < 0 || downtrack > getRouteEndTrackPos().downtrack)
    {
      RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm"), "Tried to convert a downtrack of: " << downtrack
                                                           << " to map point, but it did not fit in route bounds of "
                                                           << getRouteEndTrackPos().downtrack);
      return boost::none;
    }
 
    // Use fast lookup to identify the points before and after the provided downtrack on the route
    auto indices = shortest_path_distance_map_.getElementIndexByDistance(downtrack, true); // Get the linestring matching the provided downtrack
    size_t ls_i = std::get<0>(indices);
    size_t pt_i = std::get<1>(indices);
 
    auto linestring = shortest_path_centerlines_[ls_i];
 
    if (pt_i >= linestring.size())
    {
      throw std::invalid_argument("Impossible index: pt: " + std::to_string(pt_i) + " linestring: " + std::to_string(ls_i));
    }
 
    double ls_downtrack = shortest_path_distance_map_.distanceToElement(ls_i);
 
    double relative_downtrack = downtrack - ls_downtrack;
 
    size_t centerline_size = linestring.size();
 
    size_t prior_idx = std::min(pt_i, centerline_size - 1);
 
    size_t next_idx = std::min(pt_i + 1, centerline_size - 1);
 
    // This if block handles the edge case where the downtrack distance has landed exactly on an existing point
    if (prior_idx == next_idx)
    {  // If both indexes are the same we are on the point
 
      if (crosstrack == 0)
      {  // Crosstrack not provided so we can return the point directly
        auto prior_point = linestring[prior_idx];
 
        return lanelet::BasicPoint2d(prior_point.x(), prior_point.y());
      }
 
      lanelet::BasicPoint2d prior_point, next_point;
      double x, y;
      if (prior_idx < centerline_size - 1)
      {  // If this is not the end point compute the crosstrack based on the next point
        prior_point = linestring[prior_idx].basicPoint2d();
        next_point = linestring[prior_idx + 1].basicPoint2d(); // No need to check bounds as this class will reject routes with fewer than 2 points in a centerline
        x = prior_point.x();
        y = prior_point.y();
      }
      else
      {  // If this is the end point compute the crosstrack based on previous point
        prior_point = linestring[prior_idx - 1].basicPoint2d();
        next_point = linestring[prior_idx].basicPoint2d();
        x = next_point.x();
        y = next_point.y();
      }
 
      // Compute the crosstrack
      double sigma = geometry::point_to_point_yaw(prior_point, next_point); // Angle between route segment and x-axis
      double theta = sigma + M_PI_2;                                        // M_PI_2 is 90 deg. Theta is the angle to the vector from the route projected point
                                                                            // to the target point
      double delta_x = cos(theta) * -crosstrack;
      double delta_y = sin(theta) * -crosstrack;
 
      return lanelet::BasicPoint2d(x + delta_x, y + delta_y);
    }
 
    double prior_downtrack = shortest_path_distance_map_.distanceToPointAlongElement(ls_i, prior_idx);
    double next_downtrack = shortest_path_distance_map_.distanceToPointAlongElement(ls_i, next_idx);
 
    double prior_to_next_dist = next_downtrack - prior_downtrack;
    double prior_to_target_dist = relative_downtrack - prior_downtrack;
    double interpolation_percentage = 0;
 
    if (prior_to_next_dist < 0.000001)
    {
      interpolation_percentage = 0;
    }
    else
    {
      interpolation_percentage = prior_to_target_dist / prior_to_next_dist; // Use the percentage progress between both points to compute the downtrack point
    }
 
    auto prior_point = linestring[prior_idx].basicPoint2d();
    auto next_point = linestring[next_idx].basicPoint2d();
    auto delta_vec = next_point - prior_point;
 
    double x = prior_point.x() + interpolation_percentage * delta_vec.x();
    double y = prior_point.y() + interpolation_percentage * delta_vec.y();
 
    if (crosstrack != 0) // If the crosstrack is not set no need to do the costly computation.
    {
      double sigma = geometry::point_to_point_yaw(prior_point, next_point); // Angle between route segment and x-axis
      double theta = sigma + M_PI_2;                                        // M_PI_2 is 90 deg. Theta is the angle to the vector from the route projected point
                                                                            // to the target point
      double delta_x = cos(theta) * -crosstrack;
      double delta_y = sin(theta) * -crosstrack;
 
      x += delta_x; // Adjust x and y of target point to account for crosstrack
      y += delta_y;
    }
 
    return lanelet::BasicPoint2d(x, y);
  }

References carma_wm::TrackPos::crosstrack, carma_wm::IndexedDistanceMap::distanceToElement(), carma_wm::IndexedDistanceMap::distanceToPointAlongElement(), carma_wm::TrackPos::downtrack, carma_wm::IndexedDistanceMap::getElementIndexByDistance(), getRouteEndTrackPos(), carma_wm::geometry::point_to_point_yaw(), route_, shortest_path_centerlines_, shortest_path_distance_map_, carma_cooperative_perception::to_string(), process_traj_logs::x, and process_traj_logs::y.

Referenced by sampleRoutePoints().

Here is the call graph for this function:

Here is the caller graph for this function:

routeTrackPos() [1/3]

TrackPos carma_wm::CARMAWorldModel::routeTrackPos ( const lanelet::BasicPoint2d &  point ) const

overridevirtual

Returns the TrackPos, computed in 2d, of the provided point relative to the current route.

NOTE: The route definition used in this class contains discontinuities in the reference line at lane changes. It is important to consider that when using route related functions.

Parameters

point

The lanelet2 point which will have its distance computed

Exceptions

std::invalid_argument

If the route is not yet loaded

Returns

The TrackPos of the point

Implements carma_wm::WorldModel.

Definition at line 151 of file CARMAWorldModel.cpp.

  {
    // Check if the route was loaded yet
    if (!route_)
    {
      throw std::invalid_argument("Route has not yet been loaded");
    }
 
    // Find the nearest continuos shortest path centerline segment using fast map nearest search
    lanelet::Points3d near_points =
        shortest_path_filtered_centerline_view_->pointLayer.nearest(point, 1);  // Find the nearest points
 
    // Match point with linestring using fast map lookup
    auto lineString_1 =
        lanelet::utils::to2D(shortest_path_filtered_centerline_view_->lineStringLayer.findUsages(near_points[0])
                                 .front());  // Only need the first index due to nature of route centerline
 
    if (lineString_1.size() == 0)
    {
      throw std::invalid_argument("Invalid route loaded. Shortest path does not have proper references");
    }
 
    // New approach
    // 1. Find nearest point
    // 2. Find linestring associated with nearest point
    // 3. If the nearest point is the first point on the linestring then we need to check the downtrack value
    // 4. -- If donwtrack is negative then iterate over route segments to find preceeding segment
    // 5. -- If downtrack is positive then we are on the correct segment
    // 6. If the nearest point is the last point on the linestring then we need to check downtrack value
    // 7. -- If downtrack is less then seg length then we are on the correct segment
    // 8. -- If downtrack is greater then seg length then iterate over route segments to find succeeding segment
    // 9. With correct segment identified compute segment downtrack distance
    // 10. Accumulate previos segment distances if needed.
 
    // Find best route segment
    lanelet::Id bestRouteSegId;
    TrackPos tp(0, 0);
    // Check for end cases
 
    auto indexes = shortest_path_distance_map_.getIndexFromId(near_points[0].id());
    size_t ls_i = indexes.first;
    size_t p_i = indexes.second;
 
    if (near_points[0].id() == lineString_1.front().id())
    {  // Nearest point is at the start of a line string
      // Get start point of cur segment and add 1
      auto next_point = lineString_1[1];
      TrackPos tp_next = geometry::trackPos(point, lineString_1.front().basicPoint(), next_point.basicPoint());
 
      if (tp_next.downtrack >= 0 || ls_i == 0)
      {
        bestRouteSegId = lineString_1.id();
        tp = tp_next;
        // If downtrack is positive then we are on the correct segment
      }
      else
      {
        // If downtrack is negative then iterate over route segments to find preceeding segment
        const size_t prev_ls_i = ls_i - 1;
 
        auto prev_centerline = lanelet::utils::to2D(shortest_path_centerlines_[prev_ls_i]);  // Get prev centerline
        tp = geometry::trackPos(point, prev_centerline[prev_centerline.size() - 2].basicPoint(),
                                prev_centerline[prev_centerline.size() - 1].basicPoint());
        tp.downtrack += shortest_path_distance_map_.distanceToPointAlongElement(prev_ls_i, prev_centerline.size() - 2);
        bestRouteSegId = prev_centerline.id();
      }
    }
    else if (near_points[0].id() == lineString_1.back().id())
    {  // Nearest point is the end of a line string
 
      // Get end point of cur segment and subtract 1
      auto prev_prev_point = lineString_1[lineString_1.size() - 2];
      auto prev_point = lineString_1[lineString_1.size() - 1];
 
      TrackPos tp_prev = geometry::trackPos(point, prev_prev_point.basicPoint(), prev_point.basicPoint());
 
      double last_seg_length =
          shortest_path_distance_map_.distanceBetween(ls_i, lineString_1.size() - 2, lineString_1.size() - 1);
 
      if (tp_prev.downtrack < last_seg_length || ls_i == shortest_path_centerlines_.size() - 1)
      {
        // If downtrack is less then seg length then we are on the correct segment
        bestRouteSegId = lineString_1.id();
        tp = tp_prev;
        tp.downtrack += shortest_path_distance_map_.distanceToPointAlongElement(ls_i, lineString_1.size() - 2);
      }
      else
      {
        // If downtrack is greater then seg length then we need to find the succeeding segment
        auto next_centerline = lanelet::utils::to2D(shortest_path_centerlines_[ls_i + 1]);  // Get prev centerline
        tp = geometry::trackPos(point, next_centerline[0].basicPoint(), next_centerline[1].basicPoint());
        bestRouteSegId = next_centerline.id();
      }
    }
    else
    {  // The nearest point is in the middle of a line string
      // Graph the two bounding points on the line string and call matchSegment using a 3 element segment
      // There is a guarantee from the earlier if statements that near_points[0] will always be located at an index within
      // the exclusive range (0,lineString_1.size() - 1) so no need for range checks
 
      lanelet::BasicLineString2d subSegment = lanelet::BasicLineString2d(
          { lineString_1[p_i - 1].basicPoint(), lineString_1[p_i].basicPoint(), lineString_1[p_i + 1].basicPoint() });
 
      tp = std::get<0>(geometry::matchSegment(point, subSegment));  // Get track pos along centerline
 
      tp.downtrack += shortest_path_distance_map_.distanceToPointAlongElement(ls_i, p_i - 1);
 
      bestRouteSegId = lineString_1.id();
    }
 
    // Accumulate distance
    auto bestRouteSegIndex = shortest_path_distance_map_.getIndexFromId(bestRouteSegId);
    tp.downtrack += shortest_path_distance_map_.distanceToElement(bestRouteSegIndex.first);
 
    return tp;
  }

References carma_wm::IndexedDistanceMap::distanceBetween(), carma_wm::IndexedDistanceMap::distanceToElement(), carma_wm::IndexedDistanceMap::distanceToPointAlongElement(), carma_wm::TrackPos::downtrack, carma_wm::IndexedDistanceMap::getIndexFromId(), carma_wm::geometry::matchSegment(), process_traj_logs::point, route_, shortest_path_centerlines_, shortest_path_distance_map_, shortest_path_filtered_centerline_view_, and carma_wm::geometry::trackPos().

Here is the call graph for this function:

routeTrackPos() [2/3]

std::pair< TrackPos, TrackPos > carma_wm::CARMAWorldModel::routeTrackPos ( const lanelet::ConstArea &  area ) const

overridevirtual

Returns a pair of TrackPos, computed in 2d, of the provided area relative to the current route. The distance is based on the Area vertex with the smallest and largest downtrack distance This method overload is the most expensive of the routeTrackPos methods.

NOTE: The route definition used in this class contains discontinuities in the reference line at lane changes. It is important to consider that when using route related functions.

Parameters

area	The lanelet2 area which will have its distance computed

Exceptions

std::invalid_argument

If the route is not yet loaded or the area does not contain vertices

Returns

A pair where the first element contains the TrackPos area's earliest downtrack vertex and the second element contains the area's latest downtrack point

Implements carma_wm::WorldModel.

Definition at line 40 of file CARMAWorldModel.cpp.

  {
    // Check if the route was loaded yet
    if (!route_)
    {
      throw std::invalid_argument("Route has not yet been loaded");
    }
 
    lanelet::ConstLineStrings3d outer_bound = area.outerBound();
 
    if (outer_bound.empty())
    {
      throw std::invalid_argument("Provided area outer bound is invalid as it contains no points");
    }
 
    TrackPos minPos(0, 0);
    TrackPos maxPos(0, 0);
    bool first = true;
    for (lanelet::ConstLineString3d sub_bound3d : outer_bound)
    {
      auto sub_bound = lanelet::utils::to2D(sub_bound3d);
      for (lanelet::ConstPoint2d point : sub_bound)
      {
        TrackPos tp = routeTrackPos(point);
        if (first)
        {
          minPos = maxPos = tp;
          first = false;
        }
        else if (tp.downtrack < minPos.downtrack)
        {
          minPos.downtrack = tp.downtrack;
          minPos.crosstrack = tp.crosstrack;
        }
        else if (tp.downtrack > maxPos.downtrack)
        {
          maxPos.downtrack = tp.downtrack;
          maxPos.crosstrack = tp.crosstrack;
        }
      }
    }
    return std::make_pair(minPos, maxPos);
  }

References carma_wm::TrackPos::crosstrack, carma_wm::TrackPos::downtrack, process_traj_logs::point, route_, and routeTrackPos().

Referenced by getBusStopsAlongRoute(), getIntersectionsAlongRoute(), getLaneletsBetween(), getSignalizedIntersectionsAlongRoute(), getSignalsAlongRoute(), routeTrackPos(), setRoute(), and setRouteEndPoint().

Here is the call graph for this function:

Here is the caller graph for this function:

routeTrackPos() [3/3]

TrackPos carma_wm::CARMAWorldModel::routeTrackPos ( const lanelet::ConstLanelet &  lanelet ) const

overridevirtual

Returns the TrackPos, computed in 2d, of the provided lanelet relative to the current route. The distance is based on the first point in the lanelet centerline.

NOTE: The route definition used in this class contains discontinuities in the reference line at lane changes. It is important to consider that when using route related functions.

Parameters

lanelet

The lanelet2 lanelet which will have its distance computed

Exceptions

std::invalid_argument

If the route is not yet loaded or the lanelet centerline cannot be found

Returns

The TrackPos containing the downtrack and crosstrack location of the lanelet's first centerline point

Implements carma_wm::WorldModel.

Definition at line 84 of file CARMAWorldModel.cpp.

  {
    // Check if the route was loaded yet
    if (!route_)
    {
      throw std::invalid_argument("Route has not yet been loaded");
    }
 
    lanelet::ConstLineString2d centerline = lanelet::utils::to2D(lanelet.centerline());
    if (centerline.empty())
    {
      throw std::invalid_argument("Provided lanelet has invalid centerline containing no points");
    }
    auto front = centerline.front();
    return routeTrackPos(front);
  }

References route_, and routeTrackPos().

Here is the call graph for this function:

sampleRoutePoints()

std::vector< lanelet::BasicPoint2d > carma_wm::CARMAWorldModel::sampleRoutePoints ( double  start_downtrack, double  end_downtrack, double  step_size  ) const

overridevirtual

Samples the route centerline between the provided downtracks with the provided step size. At lane changes the points should exhibit a discontinuous jump at the end of the initial lanelet to the end of the lane change lanelet as expected by the route definition. Returned points are always inclusive of provided bounds, so the last step might be less than step_size.

NOTE: If start_downtrack == end_downtrack a single point is returned. If the route is not set or the bounds lie outside the route an empty vector is returned.

In the default implementation, this method has m * O(log n) complexity where n is the number of lane changes in the route + 1 and m is the number of sampled points which is nominally 1 + ((start_downtrack - end_downtrack) / step_size). Since the route is fixed for the duration of method execution this can generally be thought of as a linear complexity call dominated by m.

Parameters

start_downtrack	The starting route downtrack to sample from in meters
end_downtrack	The ending downtrack to stop sampling at in meters
step_size	The sampling step size in meters.

Returns

The sampled x,y points

NOTE: This method really needs clarification of behavior for lane changes.

Implements carma_wm::WorldModel.

Definition at line 371 of file CARMAWorldModel.cpp.

  {
    std::vector<lanelet::BasicPoint2d> output;
    if (!route_)
    {
      RCLCPP_WARN_STREAM(rclcpp::get_logger("carma_wm"), "Route has not yet been loaded");
      return output;
    }
 
    double route_end = getRouteEndTrackPos().downtrack;
 
    if (start_downtrack < 0 || start_downtrack > route_end || end_downtrack < 0 || end_downtrack > route_end ||
        start_downtrack > end_downtrack)
    {
      RCLCPP_WARN_STREAM(rclcpp::get_logger("carma_wm"), "Invalid input downtracks");
      return output;
    }
 
    TrackPos route_pos(start_downtrack, 0);
 
    if (end_downtrack == start_downtrack)
    {
      output.emplace_back(*(pointFromRouteTrackPos(route_pos)));  // If a single point was provided return that point
      return output;
    }
 
    output.reserve(2 + (end_downtrack - start_downtrack) / step_size);
    double downtrack = start_downtrack;
    while (downtrack < end_downtrack)
    {
      route_pos.downtrack = downtrack;
      output.emplace_back(*(pointFromRouteTrackPos(route_pos)));
      downtrack += step_size;
    }
 
    route_pos.downtrack = end_downtrack;
    output.emplace_back(*(pointFromRouteTrackPos(route_pos)));
    return output;
  }

References carma_wm::TrackPos::downtrack, getRouteEndTrackPos(), pointFromRouteTrackPos(), and route_.

Here is the call graph for this function:

setConfigSpeedLimit()

void carma_wm::CARMAWorldModel::setConfigSpeedLimit

(

double

config_lim

)

Parameters

config_lim

the configurable speed limit value populated from WMListener using the config_speed_limit parameter in VehicleConfigParams.yaml

Definition at line 1165 of file CARMAWorldModel.cpp.

  {
    config_speed_limit_ = config_lim;
  }

References config_speed_limit_.

setMap()

void carma_wm::CARMAWorldModel::setMap	(	lanelet::LaneletMapPtr	map,
		size_t	map_version = 0,
		bool	recompute_routing_graph = true
	)

Set the current map.

Parameters

map	A shared pointer to the map which will share ownership to this object
map_version	Optional field to set the map version. While this is technically optional its uses is highly advised to manage synchronization.
recompute_routing_graph	Optional field which if true will result in the routing graph being recomputed. NOTE: If this map is the first map set the graph will always be recomputed

Definition at line 545 of file CARMAWorldModel.cpp.

  {
    // If this is the first time the map has been set, then recompute the routing graph
    if (!semantic_map_)
    {
 
      RCLCPP_INFO_STREAM(rclcpp::get_logger("carma_wm"), "First time map is set in carma_wm. Routing graph will be recomputed reguardless of method inputs.");
 
      recompute_routing_graph = true;
    }
 
    semantic_map_ = map;
    map_version_ = map_version;
 
    // If the routing graph should be updated then recompute it
    if (recompute_routing_graph)
    {
 
      RCLCPP_INFO_STREAM(rclcpp::get_logger("carma_wm"), "Building routing graph");
 
      auto tr = getTrafficRules(participant_type_);
 
      if (!tr)
      {
        throw std::invalid_argument("Could not construct traffic rules for participant");
      }
 
      TrafficRulesConstPtr traffic_rules = *tr;
 
      lanelet::routing::RoutingGraphUPtr map_graph = lanelet::routing::RoutingGraph::build(*semantic_map_, *traffic_rules);
      map_routing_graph_ = std::move(map_graph);
 
      RCLCPP_INFO_STREAM(rclcpp::get_logger("carma_wm"), "Done building routing graph");
    }
  }

References getTrafficRules(), map_routing_graph_, map_version_, participant_type_, and semantic_map_.

Here is the call graph for this function:

setRoadwayObjects()

void carma_wm::CARMAWorldModel::setRoadwayObjects

(

const std::vector< carma_perception_msgs::msg::RoadwayObstacle > &

rw_objs

)

Update internal records of roadway objects. These objects MUST be guaranteed to be on the road.

These are detected by the sensor fusion node and are passed as objects compatible with lanelet

Definition at line 828 of file CARMAWorldModel.cpp.

  {
    roadway_objects_ = rw_objs;
  }

References roadway_objects_.

setRos1Clock()

void carma_wm::CARMAWorldModel::setRos1Clock

(

const rclcpp::Time &

time_now

)

Set current Ros1 clock (only used in simulation runs)

Definition at line 598 of file CARMAWorldModel.cpp.

  {
    sim_.ros1_clock_ = time_now;
  }

References carma_wm::SignalizedIntersectionManager::ros1_clock_, and sim_.

setRoute()

void carma_wm::CARMAWorldModel::setRoute

(

LaneletRoutePtr

route

)

Set the current route. This route must match the current map for this class to function properly.

Parameters

route

A shared pointer to the route which will share ownership to this object

Definition at line 608 of file CARMAWorldModel.cpp.

  {
    route_ = route;
    lanelet::ConstLanelets path_lanelets(route_->shortestPath().begin(), route_->shortestPath().end());
    shortest_path_view_ = lanelet::utils::createConstSubmap(path_lanelets, {});
    computeDowntrackReferenceLine();
    // NOTE: Setting the route_length_ field here will likely result in the final lanelets final point being used. Call setRouteEndPoint to use the destination point value
    route_length_ = routeTrackPos(route_->getEndPoint().basicPoint2d()).downtrack;  // Cache the route length with
                                                                                   // consideration for endpoint
  }

References computeDowntrackReferenceLine(), route_, route_length_, routeTrackPos(), and shortest_path_view_.

Here is the call graph for this function:

setRouteEndPoint()

void carma_wm::CARMAWorldModel::setRouteEndPoint

(

const lanelet::BasicPoint3d &

end_point

)

Set endpoint of the route.

Definition at line 619 of file CARMAWorldModel.cpp.

  {
    if (!route_)
    {
      throw std::invalid_argument("Route endpoint set before route was available.");
    }
 
    lanelet::ConstPoint3d const_end_point{ lanelet::utils::getId(), end_point.x(), end_point.y(), end_point.z() };
 
    route_->setEndPoint(const_end_point);
 
    route_length_ = routeTrackPos(route_->getEndPoint().basicPoint2d()).downtrack;  // Cache the route length with
                                                                                   // consideration for endpoint
  }

References route_, route_length_, and routeTrackPos().

Here is the call graph for this function:

setRouteName()

void carma_wm::CARMAWorldModel::setRouteName

(

const std::string &

route_name

)

Set the name of the route.

Definition at line 634 of file CARMAWorldModel.cpp.

  {
    route_name_ = route_name;
  }

References route_name_.

setRoutingGraph()

void carma_wm::CARMAWorldModel::setRoutingGraph

(

LaneletRoutingGraphPtr

graph

)

Set the routing graph for the participant type. This function may serve as an optimization to recomputing the routing graph when it is already available.

NOTE: The set graph will be overwritten if setMap(recompute_routing_graph=true) is called. It will not, be overwritten if the map is set with recompute_routing_graph=false

Parameters

graph

The graph to set. NOTE: This graph must be for the participant type specified getVehicleParticipationType(). There is no way to validate this from the object so the user must ensure consistency.

Definition at line 581 of file CARMAWorldModel.cpp.

                                                                    {
 
    RCLCPP_INFO_STREAM(rclcpp::get_logger("carma_wm"), "Setting the routing graph with user or listener provided graph");
 
    map_routing_graph_ = graph;
  }

References map_routing_graph_.

setSimulationClock()

void carma_wm::CARMAWorldModel::setSimulationClock

(

const rclcpp::Time &

time_now

)

Set simulation clock clock (only used in simulation runs)

Definition at line 603 of file CARMAWorldModel.cpp.

  {
    sim_.simulation_clock_ = time_now;
  }

References sim_, and carma_wm::SignalizedIntersectionManager::simulation_clock_.

setTrafficLightIds()

void carma_wm::CARMAWorldModel::setTrafficLightIds	(	uint32_t	id,
		lanelet::Id	lanelet_id
	)

Sets the id mapping between intersection/signal group and lanelet::Id for traffic lights in the map.

Parameters

id	intersection_id (16bit) and signal_group_id (8bit) concatenated in that order and saved in 32bit
lanelet_id	lanelet_id

Definition at line 1160 of file CARMAWorldModel.cpp.

  {
    traffic_light_ids_[id] = lanelet_id;
  }

References traffic_light_ids_.

setVehicleParticipationType()

void carma_wm::CARMAWorldModel::setVehicleParticipationType

(

const std::string &

participant

)

Set vehicle participation type.

Definition at line 1170 of file CARMAWorldModel.cpp.

  {
    participant_type_ = participant;
  }

References participant_type_.

toRoadwayObstacle()

lanelet::Optional< carma_perception_msgs::msg::RoadwayObstacle > carma_wm::CARMAWorldModel::toRoadwayObstacle ( const carma_perception_msgs::msg::ExternalObject &  object ) const

overridevirtual

Converts an ExternalObject in a RoadwayObstacle by mapping its position onto the semantic map. Can also be used to determine if the object is on the roadway.

Parameters

object

the external object to convert

Exceptions

std::invalid_argument

if the map is not set or contains no lanelets

Returns

An optional RoadwayObstacle message created from the provided object. If the external object is not on the roadway then the optional will be empty.

Implements carma_wm::WorldModel.

Definition at line 776 of file CARMAWorldModel.cpp.

  {
    if (!semantic_map_ || semantic_map_->laneletLayer.size() == 0)
    {
      throw std::invalid_argument("Map is not set or does not contain lanelets");
    }
 
    lanelet::BasicPoint2d object_center(object.pose.pose.position.x, object.pose.pose.position.y);
 
    auto nearestLaneletBoost = getIntersectingLanelet(object);
 
    if (!nearestLaneletBoost)
      return boost::none;
 
    lanelet::Lanelet nearestLanelet = nearestLaneletBoost.get();
 
    carma_perception_msgs::msg::RoadwayObstacle obs;
    obs.object = object;
    obs.connected_vehicle_type.type =
        carma_perception_msgs::msg::ConnectedVehicleType::NOT_CONNECTED;  // TODO No clear way to determine automation state at this time
    obs.lanelet_id = nearestLanelet.id();
 
    carma_wm::TrackPos obj_track_pos = geometry::trackPos(nearestLanelet, object_center);
    obs.down_track = obj_track_pos.downtrack;
    obs.cross_track = obj_track_pos.crosstrack;
 
    for (auto prediction : object.predictions)
    {
      // Compute prediction polygon
      lanelet::BasicPolygon2d prediction_polygon =
          geometry::objectToMapPolygon(prediction.predicted_position, object.size);
      lanelet::BasicPoint2d prediction_center(prediction.predicted_position.position.x,
                                              prediction.predicted_position.position.y);
 
      auto predNearestLanelet = semantic_map_->laneletLayer.nearest(prediction_center, 1)[0];
 
      carma_wm::TrackPos pred_track_pos = geometry::trackPos(predNearestLanelet, prediction_center);
 
      obs.predicted_lanelet_ids.emplace_back(predNearestLanelet.id());
      obs.predicted_cross_tracks.emplace_back(pred_track_pos.crosstrack);
      obs.predicted_down_tracks.emplace_back(pred_track_pos.downtrack);
 
      // Since the predictions are having their lanelet ids matched based on the nearest nounding box search rather than
      // checking for intersection The id confidence will be set to 90% of the position confidence
      obs.predicted_lanelet_id_confidences.emplace_back(0.9 * prediction.predicted_position_confidence);
      obs.predicted_cross_track_confidences.emplace_back(0.9 * prediction.predicted_position_confidence);
      obs.predicted_down_track_confidences.emplace_back(0.9 * prediction.predicted_position_confidence);
    }
 
    return obs;
  }

References carma_wm::TrackPos::crosstrack, carma_wm::TrackPos::downtrack, getIntersectingLanelet(), carma_wm::geometry::objectToMapPolygon(), semantic_map_, and carma_wm::geometry::trackPos().

Here is the call graph for this function:

Member Data Documentation

config_speed_limit_

double carma_wm::CARMAWorldModel::config_speed_limit_

private

Definition at line 252 of file CARMAWorldModel.hpp.

Referenced by getTrafficRules(), and setConfigSpeedLimit().

GREEN_LIGHT_DURATION

constexpr double carma_wm::CARMAWorldModel::GREEN_LIGHT_DURATION = 20.0

staticconstexprprivate

Definition at line 296 of file CARMAWorldModel.hpp.

map_routing_graph_

LaneletRoutingGraphPtr carma_wm::CARMAWorldModel::map_routing_graph_

private

Definition at line 276 of file CARMAWorldModel.hpp.

Referenced by getInLaneObjects(), getLane(), getMapRoutingGraph(), getNearestObjInLane(), setMap(), and setRoutingGraph().

map_version_

size_t carma_wm::CARMAWorldModel::map_version_ = 0

private

Definition at line 287 of file CARMAWorldModel.hpp.

Referenced by getMapVersion(), and setMap().

participant_type_

std::string carma_wm::CARMAWorldModel::participant_type_ = lanelet::Participants::Vehicle

private

Definition at line 254 of file CARMAWorldModel.hpp.

Referenced by computeDowntrackReferenceLine(), getTrafficRules(), getVehicleParticipationType(), setMap(), and setVehicleParticipationType().

RED_LIGHT_DURATION

constexpr double carma_wm::CARMAWorldModel::RED_LIGHT_DURATION = 20.0

staticconstexprprivate

Definition at line 294 of file CARMAWorldModel.hpp.

roadway_objects_

std::vector<carma_perception_msgs::msg::RoadwayObstacle> carma_wm::CARMAWorldModel::roadway_objects_

private

Definition at line 285 of file CARMAWorldModel.hpp.

Referenced by distToNearestObjInLane(), getInLaneObjects(), getNearestObjInLane(), getRoadwayObjects(), and setRoadwayObjects().

route_

LaneletRoutePtr carma_wm::CARMAWorldModel::route_

private

Definition at line 275 of file CARMAWorldModel.hpp.

Referenced by computeDowntrackReferenceLine(), getBusStopsAlongRoute(), getEntryExitOfSignalAlongRoute(), getFirstLaneletOnShortestPath(), getIntersectionsAlongRoute(), getLaneletsBetween(), getRoute(), getSignalizedIntersectionsAlongRoute(), getSignalsAlongRoute(), pointFromRouteTrackPos(), routeTrackPos(), sampleRoutePoints(), setRoute(), and setRouteEndPoint().

route_length_

double carma_wm::CARMAWorldModel::route_length_ = 0

private

Definition at line 277 of file CARMAWorldModel.hpp.

Referenced by getRouteEndTrackPos(), setRoute(), and setRouteEndPoint().

route_name_

std::string carma_wm::CARMAWorldModel::route_name_

private

Definition at line 289 of file CARMAWorldModel.hpp.

Referenced by getRouteName(), and setRouteName().

semantic_map_

std::shared_ptr<lanelet::LaneletMap> carma_wm::CARMAWorldModel::semantic_map_

private

Definition at line 274 of file CARMAWorldModel.hpp.

Referenced by distToNearestObjInLane(), getBusStopsAlongRoute(), getIntersectingLanelet(), getIntersectionsAlongRoute(), getLane(), getLaneletsFromPoint(), getMap(), getMutableMap(), getNearestObjInLane(), getSignalizedIntersectionsAlongRoute(), getSignalsAlongRoute(), nonConnectedAdjacentLeft(), setMap(), and toRoadwayObstacle().

shortest_path_centerlines_

std::vector<lanelet::LineString3d> carma_wm::CARMAWorldModel::shortest_path_centerlines_

private

Definition at line 280 of file CARMAWorldModel.hpp.

Referenced by computeDowntrackReferenceLine(), pointFromRouteTrackPos(), and routeTrackPos().

shortest_path_distance_map_

IndexedDistanceMap carma_wm::CARMAWorldModel::shortest_path_distance_map_

private

Definition at line 282 of file CARMAWorldModel.hpp.

Referenced by computeDowntrackReferenceLine(), pointFromRouteTrackPos(), and routeTrackPos().

shortest_path_filtered_centerline_view_

lanelet::LaneletMapUPtr carma_wm::CARMAWorldModel::shortest_path_filtered_centerline_view_

private

Definition at line 283 of file CARMAWorldModel.hpp.

Referenced by computeDowntrackReferenceLine(), and routeTrackPos().

shortest_path_view_

lanelet::LaneletSubmapConstUPtr carma_wm::CARMAWorldModel::shortest_path_view_

private

Definition at line 278 of file CARMAWorldModel.hpp.

Referenced by computeDowntrackReferenceLine(), getLaneletsBetween(), and setRoute().

sim_

carma_wm::SignalizedIntersectionManager carma_wm::CARMAWorldModel::sim_

Definition at line 248 of file CARMAWorldModel.hpp.

Referenced by setRos1Clock(), and setSimulationClock().

traffic_light_ids_

std::unordered_map<uint32_t, lanelet::Id> carma_wm::CARMAWorldModel::traffic_light_ids_

Definition at line 246 of file CARMAWorldModel.hpp.

Referenced by setTrafficLightIds().

YELLOW_LIGHT_DURATION

constexpr double carma_wm::CARMAWorldModel::YELLOW_LIGHT_DURATION = 3.0

staticconstexprprivate

Definition at line 295 of file CARMAWorldModel.hpp.

---

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

• carma_wm/include/carma_wm/CARMAWorldModel.hpp
• carma_wm/src/CARMAWorldModel.cpp