fix lanechange start_llt_id picking logic

carma_wm/include/carma_wm/CARMAWorldModel.hpp

@@ -32,7 +32,7 @@
 #include "boost/date_time/posix_time/posix_time.hpp"
 #include "carma_wm/SignalizedIntersectionManager.hpp"
 #include <rosgraph_msgs/msg/clock.hpp>
-
+#include <gtest/gtest_prod.h>
 namespace carma_wm
 {
 /*! \brief Class which implements the WorldModel interface. In addition this class provides write access to the world
@@ -267,6 +267,8 @@ class CARMAWorldModel : public WorldModel
 
  std::vector<lanelet::SignalizedIntersectionPtr> getSignalizedIntersectionsAlongRoute(const lanelet::BasicPoint2d &loc) const;
 
+ std::optional<lanelet::ConstLanelet> getFirstLaneletOnShortestPath(const std::vector<lanelet::ConstLanelet>& lanelets_to_filter) const;
+
  std::unordered_map<uint32_t, lanelet::Id> traffic_light_ids_;
 
  carma_wm::SignalizedIntersectionManager sim_; // records SPAT/MAP lane ids to lanelet ids
@@ -322,5 +324,6 @@ class CARMAWorldModel : public WorldModel
  static constexpr double YELLOW_LIGHT_DURATION = 3.0; //in sec
  static constexpr double GREEN_LIGHT_DURATION = 20.0; //in sec
 
+ FRIEND_TEST(CARMAWorldModelTest, getFirstLaneletOnShortestPath);
 };
 } // namespace carma_wm

carma_wm/include/carma_wm/WorldModel.hpp

@@ -123,14 +123,14 @@ class WorldModel
  virtual TrackPos routeTrackPos(const lanelet::BasicPoint2d& point) const = 0;
 
  /*! \brief Returns a list of lanelets which are part of the route and whose downtrack bounds exist within the provided
- * start and end distances. 
+ * start and end distances.
  *
  * \param start The starting downtrack for the query
  * \param end The ending downtrack for the query.
  * \param 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. 
+ * by 0.00001 to get new start and end distances.
  *
  * \throws std::invalid_argument If the route is not yet loaded or if start > end
  *
@@ -140,69 +140,69 @@ class WorldModel
  virtual std::vector<lanelet::ConstLanelet> getLaneletsBetween(double start, double end, bool shortest_path_only = false,
  bool bounds_inclusive = true) const = 0;
 
- /*! \brief Samples the route centerline between the provided downtracks with the provided step size. 
+ /*! \brief 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. 
+ * 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). 
+ * 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.
- * 
+ *
  * \param start_downtrack The starting route downtrack to sample from in meters
  * \param end_downtrack The ending downtrack to stop sampling at in meters
  * \param step_size The sampling step size in meters.
- * 
+ *
  * \return The sampled x,y points
- * 
+ *
  * NOTE: This method really needs clarification of behavior for lane changes.
  */
  virtual std::vector<lanelet::BasicPoint2d> sampleRoutePoints(double start_downtrack, double end_downtrack,
- double step_size) const = 0; 
+ double step_size) const = 0;
 
 
  /*! \brief Converts a route track position into a map frame cartesian point.
  *
  * \param route_pos The TrackPos to convert to and x,y point. This position should be relative to the route
- * 
+ *
  * \return 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. 
+ * 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
  *
  */
- virtual boost::optional<lanelet::BasicPoint2d> pointFromRouteTrackPos(const TrackPos& route_pos) const = 0; 
+ virtual boost::optional<lanelet::BasicPoint2d> pointFromRouteTrackPos(const TrackPos& route_pos) const = 0;
 
  /*! \brief Get a pointer to the current map. If the underlying map has changed the pointer will also need to be
  * reacquired
  *
  * \return Shared pointer to underlying lanelet map. Pointer will return false on boolean check if no map is loaded
  */
- virtual lanelet::LaneletMapConstPtr getMap() const = 0; 
+ virtual lanelet::LaneletMapConstPtr getMap() const = 0;
 
  /*! \brief Get a pointer to the current route. If the underlying route has changed the pointer will also need to be
  * reacquired
  *
  * \return Shared pointer to underlying lanelet route. Pointer will return false on boolean check if no route or map
  * is loaded
  */
- virtual LaneletRouteConstPtr getRoute() const = 0; 
+ virtual LaneletRouteConstPtr getRoute() const = 0;
 
- /*! \brief Get the current route name. 
+ /*! \brief Get the current route name.
  *
  * \return A string that matches the name of the current route.
  */
- virtual std::string getRouteName() const = 0; 
+ virtual std::string getRouteName() const = 0;
 
  /*! \brief Get trackpos of the end of route point relative to the route
  *
  * \return Trackpos
  */
- virtual TrackPos getRouteEndTrackPos() const = 0; 
+ virtual TrackPos getRouteEndTrackPos() const = 0;
 
  /*! \brief 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
@@ -230,7 +230,7 @@ class WorldModel
  getTrafficRules(const std::string& participant) const = 0;
 
  /**
- * @brief Get the Traffic Rules object 
+ * @brief Get the Traffic Rules object
  * @return 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
  */
@@ -266,8 +266,8 @@ class WorldModel
  virtual lanelet::Optional<lanelet::Lanelet> getIntersectingLanelet(const carma_perception_msgs::msg::ExternalObject& object) const = 0;
 
  /**
- * \brief 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, 
+ * \brief 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.
  *
  * \param loc location
@@ -276,7 +276,7 @@ class WorldModel
  * \return list of bus stop along the current route
  */
  virtual std::vector<lanelet::BusStopRulePtr> getBusStopsAlongRoute(const lanelet::BasicPoint2d& loc) const = 0;
- 
+
  /**
  * \brief Gets all roadway objects currently in the same lane as the given lanelet
  *
@@ -358,9 +358,9 @@ class WorldModel
  /**
  * \brief 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.
- * 
+ *
  * \return map version
- */ 
+ */
  virtual size_t getMapVersion() const = 0;
 
  /**
@@ -375,8 +375,8 @@ class WorldModel
  virtual std::vector<lanelet::ConstLanelet> getLaneletsFromPoint(const lanelet::BasicPoint2d& point, const unsigned int n = 10) const = 0;
 
  /**
- * \brief 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, 
+ * \brief 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.
  *
  * \param loc location
@@ -388,7 +388,7 @@ class WorldModel
 
  /**
  * \brief 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. 
+ * both directions in an intersection for example.
  *
  * \param traffic_signal traffic signal of interest
  * \throw std::invalid_argument if nullptr is passed in traffic_signal
@@ -398,8 +398,8 @@ class WorldModel
  virtual boost::optional<std::pair<lanelet::ConstLanelet, lanelet::ConstLanelet>> getEntryExitOfSignalAlongRoute(const lanelet::CarmaTrafficSignalPtr& traffic_signal) const = 0;
 
  /**
- * \brief 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, 
+ * \brief 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.
  *
  * \param loc location
@@ -410,8 +410,8 @@ class WorldModel
  virtual std::vector<std::shared_ptr<lanelet::AllWayStop>> getIntersectionsAlongRoute(const lanelet::BasicPoint2d& loc) const = 0;
 
  /**
- * \brief 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, 
+ * \brief 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.
  *
  * \param loc location
@@ -429,14 +429,24 @@ class WorldModel
  * \param semantic_map Lanelet Map Ptr
  * \param point Cartesian point to check the corressponding lanelet
  * \param n Number of lanelets to return. Default is 10. As there could be many lanelets overlapping.
- * 
+ *
  * \throw 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
  * \return vector of underlying lanelet, empty vector if it is not part of any lanelet
  */
  virtual std::vector<lanelet::ConstLanelet> nonConnectedAdjacentLeft(const lanelet::BasicPoint2d& input_point, const unsigned int n = 10) const = 0;
-
+
+ /**
+ * \brief Given the vector of lanelets, returns the lanelet that's on the shortest path.
+ * Returns earliest lanelet possible on the shortest path.
+ *
+ * \param lanelets_to_filter lanelets to pick from
+ *
+ * \return 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
+ */
+ virtual std::optional<lanelet::ConstLanelet> getFirstLaneletOnShortestPath(const std::vector<lanelet::ConstLanelet>& lanelets_to_filter) const = 0;
+
 };
 // Helpful using declarations for carma_wm classes
 using WorldModelConstPtr = std::shared_ptr<const WorldModel>;

carma_wm/src/CARMAWorldModel.cpp

@@ -1563,4 +1563,24 @@ namespace carma_wm
  }
  }
 
+ std::optional<lanelet::ConstLanelet> CARMAWorldModel::getFirstLaneletOnShortestPath(const std::vector<lanelet::ConstLanelet>& lanelets_to_filter) const
+ {
+ 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;
+ }
+
 } // namespace carma_wm