Add IsValid function for waypoints

LibCarla/source/carla/road/Map.cpp

@@ -267,6 +267,11 @@ namespace road {
   // -- Map: Waypoint generation -----------------------------------------------
   // ===========================================================================
 
+  bool Map::IsValid(Waypoint waypoint) const {
+    /// @todo Check if it's inside the road length.
+    return GetLane(waypoint) != nullptr;
+  }
+
   std::vector<Waypoint> Map::GetSuccessors(const Waypoint waypoint) const {
     auto *lane = GetLane(waypoint);
     THROW_INVALID_INPUT_ASSERT(lane != nullptr);
@@ -292,7 +297,7 @@ namespace road {
 
   std::vector<Waypoint> Map::GetNext(
       const Waypoint waypoint,
-      float distance) const {
+      const float distance) const {
     THROW_INVALID_INPUT_ASSERT(waypoint.lane_id != 0);
 
     float distance_on_next_segment;

LibCarla/source/carla/road/Map.h

@@ -71,6 +71,9 @@ namespace road {
     /// -- Waypoint generation -------------------------------------------------
     /// ========================================================================
 
+    /// Return whether @a waypoint represents a valid point on the map.
+    bool IsValid(Waypoint waypoint) const;
+
     /// Return the list of waypoints placed at the entrance of each drivable
     /// successor lane; i.e., the list of each waypoint in the next road segment
     /// that a vehicle could drive from @a waypoint.

LibCarla/source/test/client/test_opendrive.cpp

@@ -258,14 +258,16 @@ TEST(road, iterate_waypoints) {
     ASSERT_TRUE(m.has_value());
     auto &map = *m;
     for (auto &&wp : map.GenerateWaypoints(5.0f)) {
+      ASSERT_TRUE(map.IsValid(wp));
       for (auto &&next : map.GetNext(wp, 4.0f)) {
+        ASSERT_TRUE(map.IsValid(next));
         auto right = map.GetRight(next);
         if (right.has_value()) {
-          ASSERT_LT(right->lane_id, next.lane_id);
+          ASSERT_TRUE(map.IsValid(right));
         }
         auto left = map.GetLeft(next);
         if (left.has_value()) {
-          ASSERT_GT(left->lane_id, next.lane_id);
+          ASSERT_TRUE(map.IsValid(left));
         }
       }
     }