Fixed bug in waypoint.get_landmarks(). Removed landmarks with lane validity [0,0].

CHANGELOG.md

@@ -11,6 +11,7 @@
     - Added `actor.set_enable_gravity()` function to enable/disable the gravity affecting the actor
   * Upgraded to DirectX 12 on Windows
   * Fixed bug on Windows causing sun reflection artifacts
+  * Fixed bug in `waypoint.get_landmarks()` causing some landmarks to be missed when s = 0
   * Fixed the `actor.set_simulate_physics()` for pedestrians and vehicles
   * Fixed bug causing camera-based sensors to stop sending data
   * Fixed bug in the actor's id returned by the semantic lidar

LibCarla/source/carla/road/Map.cpp

@@ -343,7 +343,12 @@ namespace road {
         } else {
           distance_to_signal = waypoint.s - signal->GetDistance();
         }
-        Waypoint signal_waypoint = GetNext(waypoint, distance_to_signal).front();
+        Waypoint signal_waypoint;
+        if (distance_to_signal == 0) {
+          signal_waypoint = waypoint;
+        } else {
+          signal_waypoint = GetNext(waypoint, distance_to_signal).front();
+        }
         SignalSearchData signal_data{signal, signal_waypoint, distance_to_signal};
         result.emplace_back(signal_data);
       }
@@ -366,10 +371,17 @@ namespace road {
       result.emplace_back(signal_data);
     }
     // If we run out of remaining_lane_length we have to go to the successors.
-    for (const auto &successor : GetSuccessors(waypoint)) {
+    for (auto &successor : GetSuccessors(waypoint)) {
       if(_data.GetRoad(successor.road_id).IsJunction() && stop_at_junction){
         continue;
       }
+      auto& sucessor_lane = _data.GetRoad(successor.road_id).
+            GetLaneByDistance(successor.s, successor.lane_id);
+      if (successor.lane_id < 0) {
+        successor.s = sucessor_lane.GetDistance();
+      } else {
+        successor.s = sucessor_lane.GetDistance() + sucessor_lane.GetLength();
+      }
       auto sucessor_signals = GetSignalsInDistance(
           successor, distance - remaining_lane_length, stop_at_junction);
       for(auto& signal : sucessor_signals){

LibCarla/source/carla/road/MapBuilder.cpp

@@ -28,6 +28,7 @@
 
 #include <iterator>
 #include <memory>
+#include <algorithm>
 
 using namespace carla::road::element;
 
@@ -37,6 +38,7 @@ namespace road {
   boost::optional<Map> MapBuilder::Build() {
 
     CreatePointersBetweenRoadSegments();
+    RemoveZeroLaneValiditySignalReferences();
 
     for (auto &&info : _temp_road_info_container) {
       DEBUG_ASSERT(info.first != nullptr);
@@ -965,6 +967,37 @@ void MapBuilder::CreateController(
     }
   }
 
+  void MapBuilder::RemoveZeroLaneValiditySignalReferences() {
+    std::vector<element::RoadInfoSignal*> elements_to_remove;
+    for (auto * signal_reference : _temp_signal_reference_container) {
+      bool should_remove = true;
+      for (auto & lane_validity : signal_reference->_validities) {
+        if ( (lane_validity._from_lane != 0) ||
+             (lane_validity._to_lane != 0)) {
+          should_remove = false;
+          break;
+        }
+      }
+      if (should_remove) {
+        elements_to_remove.push_back(signal_reference);
+      }
+    }
+    for (auto* element : elements_to_remove) {
+      auto road_id = element->GetRoadId();
+      auto& road_info = _temp_road_info_container[GetRoad(road_id)];
+      road_info.erase(std::remove_if(road_info.begin(), road_info.end(),
+          [=] (auto& info_ptr) {
+            if (info_ptr.get() == element) {
+              return true;
+            }
+            return false;
+          }), road_info.end());
+      _temp_signal_reference_container.erase(std::remove(_temp_signal_reference_container.begin(),
+          _temp_signal_reference_container.end(), element),
+          _temp_signal_reference_container.end());
+    }
+  }
+
   void MapBuilder::CheckSignalsOnRoads(Map &map) {
     for (auto& signal_pair : map._data._signals) {
       auto& signal = signal_pair.second;

LibCarla/source/carla/road/MapBuilder.h

@@ -367,6 +367,9 @@ namespace road {
     /// Generates a default validity field for signal references with missing validity record in OpenDRIVE
     void GenerateDefaultValiditiesForSignalReferences();
 
+    /// Generates a default validity field for signal references with missing validity record in OpenDRIVE
+    void RemoveZeroLaneValiditySignalReferences();
+
     /// Checks signals overlapping driving lanes and emits a warning
     void CheckSignalsOnRoads(Map &map);