From 05902d423d5ccc160f456f0d38af74e4b6e4f804 Mon Sep 17 00:00:00 2001 From: Misheel Bayartsengel Date: Mon, 17 Jun 2024 11:57:18 -0400 Subject: [PATCH] fix lanechange start_llt_id picking logic (#2397) * fix lanechange start_llt_id picking logic * refactor to use laneletOnShortestPath * fix syntax error * fix unit test * fix syntax error * add FRIEND_TEST * address pr * address PR comments * add unit test to plan_delegator * fix lci_strategic sonar config * fix other problematic configs --- .sonarqube/sonar-scanner.properties | 19 ++- carma_wm/include/carma_wm/CARMAWorldModel.hpp | 5 +- carma_wm/include/carma_wm/WorldModel.hpp | 86 ++++++------ carma_wm/src/CARMAWorldModel.cpp | 20 +++ carma_wm/test/CARMAWorldModelTest.cpp | 107 +++++++++++---- .../src/lci_strategic_plugin.cpp | 18 +-- ...ontrolled_intersection_tactical_plugin.cpp | 18 +-- plan_delegator/include/plan_delegator.hpp | 29 +++-- plan_delegator/src/plan_delegator.cpp | 38 ++++-- plan_delegator/test/test_plan_delegator.cpp | 123 ++++++++++++++++++ 10 files changed, 344 insertions(+), 119 deletions(-) diff --git a/.sonarqube/sonar-scanner.properties b/.sonarqube/sonar-scanner.properties index 2858ced7bd..16e9aa1733 100644 --- a/.sonarqube/sonar-scanner.properties +++ b/.sonarqube/sonar-scanner.properties @@ -248,16 +248,15 @@ sci_strategic_plugin.sonar.tests = test stop_controlled_intersection_tactical_plugin.sonar.tests = test -system_controller.sonar.sources = test -subsystem_controllers.sonar.sources = test -frame_transformer.sonar.sources = test -object_visualizer.sonar.sources = test -points_map_filter.sonar.sources = test +system_controller.sonar.tests = test +subsystem_controllers.sonar.tests = test +frame_transformer.sonar.tests = test +object_visualizer.sonar.tests = test +points_map_filter.sonar.tests = test light_controlled_intersection_tactical_plugin.sonar.tests = test -lci_strategic_plugin.sonar.sources = src -lci_strategic_plugin.sonar.sources = test -carma_guidance_plugins.sonar.sources = test -carma_cloud_client.sonar.sources = test -approaching_emergency_vehicle_plugin.sonar.sources = test +lci_strategic_plugin.sonar.tests = test +carma_guidance_plugins.sonar.tests = test +carma_cloud_client.sonar.tests = test +approaching_emergency_vehicle_plugin.sonar.tests = test carma_cooperative_perception.tests = test trajectory_follower_wrapper.tests = test diff --git a/carma_wm/include/carma_wm/CARMAWorldModel.hpp b/carma_wm/include/carma_wm/CARMAWorldModel.hpp index 483d66b4e9..3f98d0d348 100644 --- a/carma_wm/include/carma_wm/CARMAWorldModel.hpp +++ b/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 - +#include 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 getSignalizedIntersectionsAlongRoute(const lanelet::BasicPoint2d &loc) const; + std::optional getFirstLaneletOnShortestPath(const std::vector& lanelets_to_filter) const; + std::unordered_map 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 diff --git a/carma_wm/include/carma_wm/WorldModel.hpp b/carma_wm/include/carma_wm/WorldModel.hpp index c0b9dacbb5..8b84b31d7b 100644 --- a/carma_wm/include/carma_wm/WorldModel.hpp +++ b/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,49 +140,49 @@ class WorldModel virtual std::vector 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 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 pointFromRouteTrackPos(const TrackPos& route_pos) const = 0; + virtual boost::optional 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 @@ -190,19 +190,19 @@ class WorldModel * \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 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 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 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> 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> 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 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 getFirstLaneletOnShortestPath(const std::vector& lanelets_to_filter) const = 0; + }; // Helpful using declarations for carma_wm classes using WorldModelConstPtr = std::shared_ptr; diff --git a/carma_wm/src/CARMAWorldModel.cpp b/carma_wm/src/CARMAWorldModel.cpp index 003b519791..6c37ea8e69 100755 --- a/carma_wm/src/CARMAWorldModel.cpp +++ b/carma_wm/src/CARMAWorldModel.cpp @@ -1563,4 +1563,24 @@ namespace carma_wm } } + std::optional CARMAWorldModel::getFirstLaneletOnShortestPath(const std::vector& 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 \ No newline at end of file diff --git a/carma_wm/test/CARMAWorldModelTest.cpp b/carma_wm/test/CARMAWorldModelTest.cpp index 294a9e284d..1b09e6f493 100755 --- a/carma_wm/test/CARMAWorldModelTest.cpp +++ b/carma_wm/test/CARMAWorldModelTest.cpp @@ -33,7 +33,7 @@ namespace carma_wm void createTestingWorld(std::vector& llts, lanelet::LaneletMapPtr& map, std::vector& 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 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 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 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 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, getFirstLaneletOnShortestPath) +{ + // Create a complete map + test::MapOptions mp(1,1); + auto cmw_ptr = test::getGuidanceTestMap(mp); + + std::vector 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->getFirstLaneletOnShortestPath(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->getFirstLaneletOnShortestPath(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->getFirstLaneletOnShortestPath(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->getFirstLaneletOnShortestPath(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->getFirstLaneletOnShortestPath(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); diff --git a/lci_strategic_plugin/src/lci_strategic_plugin.cpp b/lci_strategic_plugin/src/lci_strategic_plugin.cpp index f622d0e92a..ef720feff4 100755 --- a/lci_strategic_plugin/src/lci_strategic_plugin.cpp +++ b/lci_strategic_plugin/src/lci_strategic_plugin.cpp @@ -1321,14 +1321,16 @@ 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_->getFirstLaneletOnShortestPath(current_lanelets); + + if (llt_on_route_optional){ + current_lanelet = llt_on_route_optional.value(); + } + else{ + RCLCPP_WARN_STREAM(rclcpp::get_logger("lci_strategic_plugin"), "When identifying the corresponding lanelet for requested maneuever's state, x: " + << req->veh_x << ", y: " << req->veh_y << ", no possible lanelet was found to be on the shortest path." + << "Picking arbitrary lanelet: " << current_lanelets[0].id() << ", instead"); + current_lanelet = current_lanelets[0]; } lanelet::CarmaTrafficSignalPtr nearest_traffic_signal = nullptr; diff --git a/light_controlled_intersection_tactical_plugin/src/light_controlled_intersection_tactical_plugin.cpp b/light_controlled_intersection_tactical_plugin/src/light_controlled_intersection_tactical_plugin.cpp index fc67ef8f6d..7dbbe68dab 100644 --- a/light_controlled_intersection_tactical_plugin/src/light_controlled_intersection_tactical_plugin.cpp +++ b/light_controlled_intersection_tactical_plugin/src/light_controlled_intersection_tactical_plugin.cpp @@ -71,14 +71,16 @@ 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_->getFirstLaneletOnShortestPath(current_lanelets); + + if (llt_on_route_optional){ + current_lanelet = llt_on_route_optional.value(); + } + else{ + RCLCPP_WARN_STREAM(rclcpp::get_logger("light_controlled_intersection_tactical_plugin"), "When identifying the corresponding lanelet for requested trajectory plan's state, x: " + << req->vehicle_state.x_pos_global << ", y: " << req->vehicle_state.y_pos_global << ", no possible lanelet was found to be on the shortest path." + << "Picking arbitrary lanelet: " << current_lanelets[0].id() << ", instead"); + current_lanelet = current_lanelets[0]; } RCLCPP_DEBUG_STREAM(rclcpp::get_logger("light_controlled_intersection_tactical_plugin"), "Current_lanelet: " << current_lanelet.id()); diff --git a/plan_delegator/include/plan_delegator.hpp b/plan_delegator/include/plan_delegator.hpp index 6d80e95308..1803612700 100644 --- a/plan_delegator/include/plan_delegator.hpp +++ b/plan_delegator/include/plan_delegator.hpp @@ -76,9 +76,9 @@ namespace plan_delegator double max_trajectory_duration = 6.0; double min_crawl_speed = 2.2352; // Min crawl speed in m/s double duration_to_signal_before_lane_change = 2.5; // (Seconds) If an upcoming lane change will begin in under this time threshold, a turn signal activation command will be published. - int tactical_plugin_service_call_timeout = 100; // (Milliseconds) The maximum duration that Plan Delegator will wait after calling a tactical plugin's trajectory planning service; if trajectory + int tactical_plugin_service_call_timeout = 100; // (Milliseconds) The maximum duration that Plan Delegator will wait after calling a tactical plugin's trajectory planning service; if trajectory // generation takes longer than this, then planning will immediately end for the current trajectory planning iteration. - + // Stream operator for this config friend std::ostream &operator<<(std::ostream &output, const Config &c) { @@ -102,7 +102,7 @@ namespace plan_delegator double starting_downtrack; // The starting downtrack of the lane change bool is_right_lane_change; // Flag to indicate whether lane change is a right lane change; false if it is a left lane change }; - + class PlanDelegator : public carma_ros2_utils::CarmaLifecycleNode { public: @@ -111,7 +111,7 @@ namespace plan_delegator static const constexpr double MILLISECOND_TO_SECOND = 0.001; /** - * \brief PlanDelegator constructor + * \brief PlanDelegator constructor */ explicit PlanDelegator(const rclcpp::NodeOptions &); @@ -158,8 +158,8 @@ namespace plan_delegator void lookupFrontBumperTransform(); /** - * \brief Update the starting downtrack, ending downtrack, and maneuver-specific Lanelet ID parameters associated - * with a given maneuver. These updates are required since the starting and ending downtrack values of each maneuver + * \brief Update the starting downtrack, ending downtrack, and maneuver-specific Lanelet ID parameters associated + * with a given maneuver. These updates are required since the starting and ending downtrack values of each maneuver * are shifted based on the distance between the base_link frame and the vehicle_front frame. * \param maneuver The maneuver to be updated. */ @@ -174,7 +174,7 @@ namespace plan_delegator protected: // Node configuration Config config_; - + // map to store service clients std::unordered_map> trajectory_planners_; // local storage of incoming messages @@ -257,8 +257,8 @@ namespace plan_delegator LaneChangeInformation getLaneChangeInformation(const carma_planning_msgs::msg::Maneuver& lane_change_maneuver); /** - * \brief Function for processing an optional LaneChangeInformation object pertaining to an upcoming lane change. If not empty, - * an UpcomingLaneChangeStatus message is created and published based on the contents of the LaneChangeInformation. The published + * \brief Function for processing an optional LaneChangeInformation object pertaining to an upcoming lane change. If not empty, + * an UpcomingLaneChangeStatus message is created and published based on the contents of the LaneChangeInformation. The published * UpcomingLaneChangeStatus message is stored in upcoming_lane_change_status_. * \param upcoming_lane_change_information An optional LaneChangeInformation object. Empty if no upcoming lane change exists. */ @@ -267,13 +267,13 @@ namespace plan_delegator /** * \brief Function for processing an optional LaneChangeInformation object pertaining to the currently-occurring lane change * and an UpcomingLaneChangeStatus message. If the optional object pertaining to the currently-occurring lane change is not empty, - * then a turn signal command is published based on the current lane change direction. Otherwise, a turn signal command in the direction + * then a turn signal command is published based on the current lane change direction. Otherwise, a turn signal command in the direction * of the UpcomingLaneChangeStatus message is published if the vehicle is estimated to begin that lane change in under the time - * threshold defined by config_.duration_to_signal_before_lane_change. The published TurnSignalComand message is stored in + * threshold defined by config_.duration_to_signal_before_lane_change. The published TurnSignalComand message is stored in * latest_turn_signal_command_. - * \param current_lane_change_information An optional LaneChangeInformation object pertaining to the current lane + * \param current_lane_change_information An optional LaneChangeInformation object pertaining to the current lane * change. Empty if vehicle is not currently changing lanes. - * \param upcoming_lane_change_status An UpcomingLaneChangeStatus message containing the lane change direction of an upcoming lane change, along + * \param upcoming_lane_change_status An UpcomingLaneChangeStatus message containing the lane change direction of an upcoming lane change, along * with the downtrack distance to that lane change. */ void publishTurnSignalCommand(const boost::optional& current_lane_change_information, const carma_planning_msgs::msg::UpcomingLaneChangeStatus& upcoming_lane_change_status); @@ -283,5 +283,6 @@ namespace plan_delegator FRIEND_TEST(TestPlanDelegator, TestPlanDelegator); FRIEND_TEST(TestPlanDelegator, TestLaneChangeInformation); FRIEND_TEST(TestPlanDelegator, TestUpcomingLaneChangeAndTurnSignals); + FRIEND_TEST(TestPlanDelegator, TestUpdateManeuverParameters); }; -} \ No newline at end of file +} diff --git a/plan_delegator/src/plan_delegator.cpp b/plan_delegator/src/plan_delegator.cpp index ed01c3edf1..adbf5514d7 100644 --- a/plan_delegator/src/plan_delegator.cpp +++ b/plan_delegator/src/plan_delegator.cpp @@ -495,20 +495,22 @@ namespace plan_delegator if(adjusted_start_dist < original_starting_lanelet_centerline_start_point_dt){ auto previous_lanelets = wm_->getMapRoutingGraph()->previous(original_starting_lanelet, false); - auto previous_lanelet_to_add = previous_lanelets[0]; - // pick a lanelet on the shortest path - for (const auto& llt : previous_lanelets) - { - auto route = wm_->getRoute()->shortestPath(); - if (std::find(route.begin(), route.end(), llt) != route.end()) - { - previous_lanelet_to_add = llt; - break; + if(!previous_lanelets.empty()){ + + auto llt_on_route_optional = wm_->getFirstLaneletOnShortestPath(previous_lanelets); + + lanelet::ConstLanelet previous_lanelet_to_add; + + if (llt_on_route_optional){ + previous_lanelet_to_add = llt_on_route_optional.value(); + } + else{ + RCLCPP_WARN_STREAM(rclcpp::get_logger("plan_delegator"), "When adjusting maneuver for lane follow, no previous lanelet found on the shortest path for lanelet " + << original_starting_lanelet.id() << ". Picking arbitrary lanelet: " << previous_lanelets[0].id() << ", instead"); + previous_lanelet_to_add = previous_lanelets[0]; } - } - if(!previous_lanelets.empty()){ // lane_ids array is ordered by increasing downtrack, so this new starting lanelet is inserted at the front maneuver.lane_following_maneuver.lane_ids.insert(maneuver.lane_following_maneuver.lane_ids.begin(), std::to_string(previous_lanelet_to_add.id())); @@ -547,9 +549,19 @@ namespace plan_delegator if(adjusted_start_dist < original_starting_lanelet_centerline_start_point_dt){ auto previous_lanelets = wm_->getMapRoutingGraph()->previous(original_starting_lanelet, false); - if(!previous_lanelets.empty()){ - setManeuverStartingLaneletId(maneuver, previous_lanelets[0].id()); + auto llt_on_route_optional = wm_->getFirstLaneletOnShortestPath(previous_lanelets); + lanelet::ConstLanelet previous_lanelet_to_add; + + if (llt_on_route_optional){ + previous_lanelet_to_add = llt_on_route_optional.value(); + } + else{ + RCLCPP_WARN_STREAM(rclcpp::get_logger("plan_delegator"), "When adjusting non-lane follow maneuver, no previous lanelet found on the shortest path for lanelet " + << original_starting_lanelet.id() << ". Picking arbitrary lanelet: " << previous_lanelets[0].id() << ", instead"); + previous_lanelet_to_add = previous_lanelets[0]; + } + setManeuverStartingLaneletId(maneuver, previous_lanelet_to_add.id()); } else{ RCLCPP_WARN_STREAM(rclcpp::get_logger("plan_delegator"), "No previous lanelet was found for lanelet " << original_starting_lanelet.id()); diff --git a/plan_delegator/test/test_plan_delegator.cpp b/plan_delegator/test/test_plan_delegator.cpp index 0a2840e86b..f2c6e175ed 100644 --- a/plan_delegator/test/test_plan_delegator.cpp +++ b/plan_delegator/test/test_plan_delegator.cpp @@ -380,6 +380,129 @@ namespace plan_delegator{ } */ + /** + * Total route length should be 100m + * + * |1203|1213|1223| + * | _ _ _ _ _| + * |1202|1212|1222| + * | _ _ _ _ _| + * |1201|1211|1221| num = lanelet id hardcoded for easier testing + * | _ _ _ _ _| | = lane lines + * |1200|1210|1220| - - - = Lanelet boundary + * | 12100 | + * **************** + * START_LINE + */ + TEST(TestPlanDelegator, TestUpdateManeuverParameters) + { + rclcpp::NodeOptions node_options; + auto pd = std::make_shared(node_options); + pd->configure(); + pd->activate(); + + // Use Guidance Lib to create map without an obstacle + carma_wm::test::MapOptions options; + options.obstacle_ = carma_wm::test::MapOptions::Obstacle::NONE; + std::shared_ptr cmw = carma_wm::test::getGuidanceTestMap(options); + + // Introduce overlapping lanelet not on the route + lanelet::Lanelet start_lanelet = cmw->getMutableMap()->laneletLayer.get(1210); + lanelet::Lanelet start_lanelet_overlapping = carma_wm::test::getLanelet(12100, start_lanelet.leftBound(), start_lanelet.rightBound()); + auto overlapping_llt_id = std::to_string(start_lanelet_overlapping.id()); + cmw->getMutableMap()->add(start_lanelet_overlapping); + cmw->setMap(cmw->getMutableMap()); // re-trigger routing graph + carma_wm::test::setRouteByIds({1210, 1213}, cmw); + // Set PlanDelegator's world model object + pd->wm_ = cmw; + + // Verify that start end dist are correct and lanelet on the route is prioritized when picking lanelet + carma_planning_msgs::msg::Maneuver maneuver_1; + maneuver_1.type = carma_planning_msgs::msg::Maneuver::LANE_FOLLOWING; + maneuver_1.lane_following_maneuver.start_dist = 25.0; + maneuver_1.lane_following_maneuver.end_dist = 50.0; + maneuver_1.lane_following_maneuver.lane_ids.push_back("1211"); + pd->length_to_front_bumper_ = 4.0; // 4.0 meter vehicle length + pd->updateManeuverParameters(maneuver_1); + + EXPECT_NEAR(maneuver_1.lane_following_maneuver.start_dist, 21.0, 0.01); + EXPECT_NEAR(maneuver_1.lane_following_maneuver.end_dist, 46.0, 0.01); + EXPECT_EQ(maneuver_1.lane_following_maneuver.lane_ids.front(), "1210"); + + // Switch route with a different lanelet to verify route prioritization + cmw->setMap(cmw->getMutableMap()); // re-trigger routing graph + carma_wm::test::setRouteByIds({start_lanelet_overlapping.id(), 1213}, cmw); + pd->wm_ = cmw; + maneuver_1.lane_following_maneuver.start_dist = 25.0; + maneuver_1.lane_following_maneuver.end_dist = 50.0; + maneuver_1.lane_following_maneuver.lane_ids.front() = "1211"; + pd->updateManeuverParameters(maneuver_1); + + EXPECT_NEAR(maneuver_1.lane_following_maneuver.start_dist, 21.0, 0.01); + EXPECT_NEAR(maneuver_1.lane_following_maneuver.end_dist, 46.0, 0.01); + EXPECT_EQ(maneuver_1.lane_following_maneuver.lane_ids.front(), overlapping_llt_id); + + // lanechange + carma_planning_msgs::msg::Maneuver maneuver_2; + maneuver_2.type = carma_planning_msgs::msg::Maneuver::LANE_CHANGE; + maneuver_2.lane_change_maneuver.start_dist = 25.0; + maneuver_2.lane_change_maneuver.end_dist = 50.0; + maneuver_2.lane_change_maneuver.starting_lane_id = "1211"; + maneuver_2.lane_change_maneuver.ending_lane_id = "1221"; + + cmw->setMap(cmw->getMutableMap()); // re-trigger routing graph + carma_wm::test::setRouteByIds({1210, 1213}, cmw); + pd->wm_ = cmw; + + pd->updateManeuverParameters(maneuver_2); + EXPECT_NEAR(maneuver_2.lane_change_maneuver.start_dist, 21.0, 0.01); + EXPECT_NEAR(maneuver_2.lane_change_maneuver.end_dist, 46.0, 0.01); + EXPECT_EQ(maneuver_2.lane_change_maneuver.starting_lane_id, "1210"); + + // Switch route with a different lanelet to verify route prioritization + cmw->setMap(cmw->getMutableMap()); // re-trigger routing graph + carma_wm::test::setRouteByIds({start_lanelet_overlapping.id(), 1211, 1212, 1213}, cmw); + pd->wm_ = cmw; + maneuver_2.lane_change_maneuver.start_dist = 25.0; // reset + maneuver_2.lane_change_maneuver.end_dist = 50.0; + maneuver_2.lane_change_maneuver.starting_lane_id = "1211"; + maneuver_2.lane_change_maneuver.ending_lane_id = "1221"; + + pd->updateManeuverParameters(maneuver_2); + EXPECT_NEAR(maneuver_2.lane_change_maneuver.start_dist, 21.0, 0.01); + EXPECT_NEAR(maneuver_2.lane_change_maneuver.end_dist, 46.0, 0.01); + EXPECT_EQ(maneuver_2.lane_change_maneuver.starting_lane_id, overlapping_llt_id); + + // Verify that the non-route lanelet is being picked if no suitable lanelet is on the route + cmw->setMap(cmw->getMutableMap()); // re-trigger routing graph + carma_wm::test::setRouteByIds({1201, 1203}, cmw); + pd->wm_ = cmw; + + // Lane Follow + maneuver_1.lane_following_maneuver.start_dist = 0.0; // Reset values + maneuver_1.lane_following_maneuver.end_dist = 25.0; + maneuver_1.lane_following_maneuver.lane_ids.front() = "1201"; + + pd->updateManeuverParameters(maneuver_1); + + EXPECT_NEAR(maneuver_1.lane_following_maneuver.start_dist, -4.0, 0.01); + EXPECT_NEAR(maneuver_1.lane_following_maneuver.end_dist, 21.0, 0.01); + EXPECT_EQ(maneuver_1.lane_following_maneuver.lane_ids.front(), "1200"); + + // Lanechange + cmw->setMap(cmw->getMutableMap()); // re-trigger routing graph + carma_wm::test::setRouteByIds({1221, 1213}, cmw); + pd->wm_ = cmw; + maneuver_2.lane_change_maneuver.start_dist = 0.0; // Reset values + maneuver_2.lane_change_maneuver.end_dist = 25.0; + maneuver_2.lane_change_maneuver.starting_lane_id = "1221"; + maneuver_2.lane_change_maneuver.ending_lane_id = "1211"; + + pd->updateManeuverParameters(maneuver_2); + EXPECT_NEAR(maneuver_2.lane_change_maneuver.start_dist, -4.0, 0.01); + EXPECT_NEAR(maneuver_2.lane_change_maneuver.end_dist, 21.0, 0.01); + EXPECT_EQ(maneuver_2.lane_change_maneuver.starting_lane_id, "1220"); + } } // namespace plan_delegator /*!