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
@@ -265,6 +265,8 @@ class CARMAWorldModel : public WorldModel
 
  std::vector<lanelet::SignalizedIntersectionPtr> getSignalizedIntersectionsAlongRoute(const lanelet::BasicPoint2d &loc) const;
 
+ std::optional<lanelet::ConstLanelet> getLaneletOnShortestPath(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
@@ -320,5 +322,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, getLaneletOnShortestPath);
 };
 } // namespace carma_wm

carma_wm/include/carma_wm/WorldModel.hpp

@@ -436,7 +436,16 @@ class WorldModel
  * \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
+ *
+ * \param lanelets_to_filter lanelets to pick from
+ * 
+ * \return 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> getLaneletOnShortestPath(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

@@ -1556,4 +1556,26 @@ namespace carma_wm
  }
  }
 
+ std::optional<lanelet::ConstLanelet> CARMAWorldModel::getLaneletOnShortestPath(const std::vector<lanelet::ConstLanelet>& lanelets_to_filter) const
+ {
+ if (!route_ || lanelets_to_filter.empty())
+ {
+ return std::nullopt;
+ }
+
+ std::optional<lanelet::ConstLanelet> lanelet_on_the_route = 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())
+ {
+ lanelet_on_the_route = llt;
+ break;
+ }
+ }
+
+ return lanelet_on_the_route;
+ }
+
 } // namespace carma_wm

carma_wm/test/CARMAWorldModelTest.cpp

@@ -33,7 +33,7 @@ namespace carma_wm
 void createTestingWorld(std::vector<lanelet::Lanelet>& llts, lanelet::LaneletMapPtr& map, std::vector<carma_perception_msgs::msg::ExternalObject>& obstacles)
 {
  //Note: all params are outputs provided by this function.
- 
+
  /*
  * Create 2x2 lanelets map by hand
  */
@@ -165,7 +165,7 @@ TEST(CARMAWorldModelTest, getLane)
  ASSERT_THROW(cmw.getLane(llts[0]), std::invalid_argument);
 
  // Set map
- cmw.setMap(map); 
+ cmw.setMap(map);
 
  // Convert to RoadwayObstacle format
  std::vector<carma_perception_msgs::msg::RoadwayObstacle> roadway_objects;
@@ -208,7 +208,7 @@ TEST(CARMAWorldModelTest, getNearestObjInLane)
  ASSERT_THROW(cmw.getNearestObjInLane({5,4}), std::invalid_argument);
 
  // Set map
- cmw.setMap(map); 
+ cmw.setMap(map);
 
  // Convert to RoadwayObstacle format
  std::vector<carma_perception_msgs::msg::RoadwayObstacle> roadway_objects;
@@ -246,7 +246,7 @@ TEST(CARMAWorldModelTest, getNearestObjInLane)
  tp = cmw.getNearestObjInLane({15,17}, LANE_FULL).get();
  ASSERT_NEAR(std::get<0>(tp).downtrack, 5.242, 0.001);
  ASSERT_EQ(std::get<1>(tp).lanelet_id, roadway_objects[4].lanelet_id);
- 
+
 }
 
 TEST(CARMAWorldModelTest, nearestObjectBehindInLane)
@@ -268,7 +268,7 @@ TEST(CARMAWorldModelTest, nearestObjectBehindInLane)
  ASSERT_THROW(cmw.nearestObjectBehindInLane({5,4}), std::invalid_argument);
 
  // Set map
- cmw.setMap(map); 
+ cmw.setMap(map);
 
  // Convert to RoadwayObstacle format
  std::vector<carma_perception_msgs::msg::RoadwayObstacle> roadway_objects;
@@ -314,7 +314,7 @@ TEST(CARMAWorldModelTest, nearestObjectAheadInLane)
  ASSERT_THROW(cmw.nearestObjectAheadInLane({5,4}), std::invalid_argument);
 
  // Set map
- cmw.setMap(map); 
+ cmw.setMap(map);
 
  // Convert to RoadwayObstacle format
  std::vector<carma_perception_msgs::msg::RoadwayObstacle> roadway_objects;
@@ -381,7 +381,7 @@ TEST(CARMAWorldModelTest, getInLaneObjects)
 
  // Set roadway objects
  cmw.setRoadwayObjects(roadway_objects);
- 
+
  // Test with lanelet that is not on the map
  auto p1 = getPoint(25, 0, 0);
  auto p2 = getPoint(25, 25, 0);
@@ -445,13 +445,13 @@ TEST(CARMAWorldModelTest, distToNearestObjInLane)
 
  // Test with no roadway objects set
  ASSERT_FALSE(!!cmw.distToNearestObjInLane(car_pose));
- 
+
  // Set roadway objects
  cmw.setRoadwayObjects(roadway_objects);
- 
+
  // Test with pose that is not on any lane
  ASSERT_THROW(cmw.distToNearestObjInLane({20, 1}), std::invalid_argument);
- 
+
  // Test closest object
  double result = cmw.distToNearestObjInLane(car_pose).get();
  ASSERT_EQ(result, 4);
@@ -462,7 +462,7 @@ TEST(CARMAWorldModelTest, distToNearestObjInLane)
  ASSERT_NEAR(result, 4.5, 0.00001);
 
  // Check an object on a different lane
- car_pose = {12.6, 7}; 
+ car_pose = {12.6, 7};
  result = cmw.distToNearestObjInLane(car_pose).get();
  ASSERT_EQ(result, 2);
 
@@ -501,7 +501,7 @@ TEST(CARMAWorldModelTest, getIntersectingLanelet)
 
  // very wide car that is little off the road to right
  geometry_msgs::msg::Vector3 size;
- size.x = 4; 
+ size.x = 4;
  size.y = 1.8; // Right on the edge, touching the linestring
  size.z = 1;
 
@@ -542,7 +542,7 @@ TEST(CARMAWorldModelTest, getIntersectingLanelet)
  result = cmw.getIntersectingLanelet(obj);
 
  ASSERT_FALSE(!!result);
- 
+
 }
 
 TEST(CARMAWorldModelTest, getSetMap)
@@ -831,7 +831,7 @@ TEST(CARMAWorldModelTest, getLaneletsBetween) // HERE
 
  result = cmw.getLaneletsBetween(1.0, 1.0, true, false);
  ASSERT_EQ(0u, result.size());
- 
+
  result = cmw.getLaneletsBetween(0.1, 0.2, false, false); // FAIL This fails when the bounds are not inclusive
  ASSERT_EQ(1u, result.size());
  ASSERT_EQ(result[0].id(), cmw.getRoute()->shortestPath().begin()->id());
@@ -1048,7 +1048,7 @@ TEST(CARMAWorldModelTest, getLaneletsFromPoint)
  ASSERT_EQ(underlyings.size(), 1u);
  ASSERT_EQ(underlyings.front().id(), 1200);
 
- auto ll_1500 = test::getLanelet(1500, {getPoint(0.0,0.1, 0),getPoint(0.0,1.1, 0)}, 
+ auto ll_1500 = test::getLanelet(1500, {getPoint(0.0,0.1, 0),getPoint(0.0,1.1, 0)},
  {getPoint(1.0,0.1, 0),getPoint(1.0,1.1, 0)}); // another lanelet the point is in
  cmw_ptr->getMutableMap()->add(ll_1500);
  underlyings = cmw_ptr->getLaneletsFromPoint({0.5,0.5});
@@ -1135,7 +1135,7 @@ TEST(CARMAWorldModelTest, pointFromRouteTrackPos)
 
  ASSERT_NEAR(alt_point.downtrack, pos.downtrack, 0.001);
  ASSERT_NEAR(alt_point.crosstrack, pos.crosstrack, 0.001);
- 
+
  pos.downtrack = 20.0;
  point = wm->pointFromRouteTrackPos(pos); // Test lanelet connection point
  if (!point) {
@@ -1184,7 +1184,7 @@ TEST(CARMAWorldModelTest, pointFromRouteTrackPos)
  lanelet = map->laneletLayer.get(1200);
  ASSERT_NEAR((*point).x(), lanelet.centerline().back().x() + 1.0, 0.001);
  ASSERT_NEAR((*point).y(), lanelet.centerline().back().y(), 0.001);
- 
+
  pos.downtrack = 20.0;
  point = wm->pointFromRouteTrackPos(pos); // Test lanelet connection point
  if (!point) {
@@ -1233,7 +1233,7 @@ TEST(CARMAWorldModelTest, pointFromRouteTrackPos)
  lanelet = map->laneletLayer.get(1200);
  ASSERT_NEAR((*point).x(), lanelet.centerline().back().x() - 1.0, 0.001);
  ASSERT_NEAR((*point).y(), lanelet.centerline().back().y(), 0.001);
- 
+
  pos.downtrack = 20.0;
  point = wm->pointFromRouteTrackPos(pos); // Test lanelet connection point
  if (!point) {
@@ -1274,12 +1274,12 @@ TEST(CARMAWorldModelTest, sampleRoutePoints)
  for (auto p : points) {
  ASSERT_NEAR(p.x(), 1.85, 0.001);
  if (i != points.size() - 1) {
- ASSERT_NEAR(p.y(), (double)i, 0.001); 
+ ASSERT_NEAR(p.y(), (double)i, 0.001);
  }
  else {
- ASSERT_NEAR(p.y(), 10.5, 0.001); 
+ ASSERT_NEAR(p.y(), 10.5, 0.001);
  }
- i++; 
+ i++;
  }
 }
 
@@ -1291,7 +1291,7 @@ TEST(CARMAWorldModelTest, getTrafficSignalId)
  cmw.sim_.intersection_id_to_regem_id_[intersection_id] = 1001;
  cmw.sim_.signal_group_to_traffic_light_id_[signal_group_id] = 1000;
 
- EXPECT_EQ(cmw.getTrafficSignalId(intersection_id, signal_group_id), 1000); 
+ EXPECT_EQ(cmw.getTrafficSignalId(intersection_id, signal_group_id), 1000);
 }
 
 TEST(CARMAWorldModelTest, processSpatFromMsg)
@@ -1372,7 +1372,7 @@ TEST(CARMAWorldModelTest, processSpatFromMsg)
 
  // Multiple states
  // first state
- spat.intersection_state_list[0] = state; 
+ spat.intersection_state_list[0] = state;
  // second state
  event.event_state.movement_phase_state = 3;
  event.timing.min_end_time = 40;
@@ -1398,7 +1398,7 @@ TEST(CARMAWorldModelTest, getSignalsAlongRoute)
  // Create a complete map
  test::MapOptions mp(1,1);
  auto cmw_ptr = test::getGuidanceTestMap(mp);
- 
+
  auto pl2 = carma_wm::getPoint(0, 1, 0);
  auto pl3 = carma_wm::getPoint(0, 2, 0);
  auto pr2 = carma_wm::getPoint(1, 1, 0);
@@ -1416,13 +1416,66 @@ TEST(CARMAWorldModelTest, getSignalsAlongRoute)
  carma_wm::test::setRouteByIds({ 1200, 1201, 1202}, cmw_ptr);
 
  auto lights = cmw_ptr->getSignalsAlongRoute({0.5, 0});
- 
+
  EXPECT_EQ(lights.size(), 2u);
  EXPECT_EQ(lights[0]->id(), traffic_light_id1);
  EXPECT_EQ(lights[1]->id(), traffic_light_id2);
 
 }
 
+TEST(CARMAWorldModelTest, getLaneletOnShortestPath)
+{
+ // Create a complete map
+ test::MapOptions mp(1,1);
+ auto cmw_ptr = test::getGuidanceTestMap(mp);
+
+ std::vector<lanelet::ConstLanelet> random_lanelets;
+ random_lanelets.push_back(cmw_ptr->getMap()->laneletLayer.get(1200));
+ random_lanelets.push_back(cmw_ptr->getMap()->laneletLayer.get(1221));
+ random_lanelets.push_back(cmw_ptr->getMap()->laneletLayer.get(1222));
+
+ // No route
+ cmw_ptr->route_ = nullptr;
+ auto lanelet_on_the_route = cmw_ptr->getLaneletOnShortestPath(random_lanelets);
+ EXPECT_EQ(lanelet_on_the_route, std::nullopt);
+
+ // No lanelet on route
+ random_lanelets = {};
+ random_lanelets.push_back(cmw_ptr->getMap()->laneletLayer.get(1220));
+ random_lanelets.push_back(cmw_ptr->getMap()->laneletLayer.get(1221));
+ random_lanelets.push_back(cmw_ptr->getMap()->laneletLayer.get(1222));
+ carma_wm::test::setRouteByIds({ 1200, 1201, 1202}, cmw_ptr);
+ lanelet_on_the_route = cmw_ptr->getLaneletOnShortestPath(random_lanelets);
+ EXPECT_EQ(lanelet_on_the_route, std::nullopt);
+
+ // 1 lanelet on the route
+ random_lanelets = {};
+ random_lanelets.push_back(cmw_ptr->getMap()->laneletLayer.get(1200));
+ random_lanelets.push_back(cmw_ptr->getMap()->laneletLayer.get(1221));
+ random_lanelets.push_back(cmw_ptr->getMap()->laneletLayer.get(1222));
+
+ lanelet_on_the_route = cmw_ptr->getLaneletOnShortestPath(random_lanelets);
+ EXPECT_EQ(lanelet_on_the_route, cmw_ptr->getMap()->laneletLayer.get(1200));
+
+ // 1 lanelet on the route
+ random_lanelets = {};
+ random_lanelets.push_back(cmw_ptr->getMap()->laneletLayer.get(1220));
+ random_lanelets.push_back(cmw_ptr->getMap()->laneletLayer.get(1221));
+ random_lanelets.push_back(cmw_ptr->getMap()->laneletLayer.get(1202));
+
+ lanelet_on_the_route = cmw_ptr->getLaneletOnShortestPath(random_lanelets);
+ EXPECT_EQ(lanelet_on_the_route, cmw_ptr->getMap()->laneletLayer.get(1202));
+
+ // 2 lanelets on the route, return earliest
+ random_lanelets = {};
+ random_lanelets.push_back(cmw_ptr->getMap()->laneletLayer.get(1200));
+ random_lanelets.push_back(cmw_ptr->getMap()->laneletLayer.get(1221));
+ random_lanelets.push_back(cmw_ptr->getMap()->laneletLayer.get(1202));
+
+ lanelet_on_the_route = cmw_ptr->getLaneletOnShortestPath(random_lanelets);
+ EXPECT_EQ(lanelet_on_the_route, cmw_ptr->getMap()->laneletLayer.get(1200));
+}
+
 TEST(CARMAWorldModelTest, getIntersectionAlongRoute)
 {
  lanelet::Id id{1200};
@@ -1445,7 +1498,7 @@ TEST(CARMAWorldModelTest, getIntersectionAlongRoute)
  ll1 = lanelet::Lanelet(++id, ls1, ls2);
  ll2 = lanelet::Lanelet(++id, ls2, ls3);
 
- 
+
  carma_wm::CARMAWorldModel cmw;
  lanelet::LaneletMapPtr map;
  // Create a complete map
@@ -1458,7 +1511,7 @@ TEST(CARMAWorldModelTest, getIntersectionAlongRoute)
  carma_wm::test::setRouteByIds({ 1200, 1201, 1202}, cmw_ptr);
 
  auto ints = cmw_ptr->getIntersectionsAlongRoute({0.5, 0});
- 
+
  EXPECT_EQ(ints.size(), 1u);
  EXPECT_EQ(ints[0]->id(), int_id);
 

lci_strategic_plugin/src/lci_strategic_plugin.cpp

@@ -1321,15 +1321,8 @@ void LCIStrategicPlugin::plan_maneuvers_callback(
  }
 
  // get the lanelet that is on the route in case overlapping ones found
- for (auto llt : current_lanelets)
- {
- auto route = wm_->getRoute()->shortestPath();
- if (std::find(route.begin(), route.end(), llt) != route.end())
- {
- current_lanelet = llt;
- break;
- }
- }
+ auto llt_on_route_optional = wm_->getLaneletOnShortestPath(current_lanelets);
+ current_lanelet = llt_on_route_optional ? llt_on_route_optional.value() : current_lanelets[0];
 
  lanelet::CarmaTrafficSignalPtr nearest_traffic_signal = nullptr;
 

light_controlled_intersection_tactical_plugin/src/light_controlled_intersection_tactical_plugin.cpp

@@ -71,15 +71,8 @@ namespace light_controlled_intersection_tactical_plugin
  }
 
  // get the lanelet that is on the route in case overlapping ones found
- for (auto llt : current_lanelets)
- {
- auto route = wm_->getRoute()->shortestPath();
- if (std::find(route.begin(), route.end(), llt) != route.end())
- {
- current_lanelet = llt;
- break;
- }
- }
+ auto llt_on_route_optional = wm_->getLaneletOnShortestPath(current_lanelets);
+ current_lanelet = llt_on_route_optional ? llt_on_route_optional.value() : current_lanelets[0];
 
  RCLCPP_DEBUG_STREAM(rclcpp::get_logger("light_controlled_intersection_tactical_plugin"), "Current_lanelet: " << current_lanelet.id());