fix(‎LibCarla): port lane marking, waypoint loop, lane corners, and pugixml null fixes

LibCarla/source/carla/road/Lane.cpp

@@ -204,7 +204,7 @@ namespace road {
   }
 
   std::pair<geom::Vector3D, geom::Vector3D> Lane::GetCornerPositions(
-      const double s, const float extra_width) const {
+      const double parameter_s, const float extra_width) const {
     const Road *road = GetRoad();
     DEBUG_ASSERT(road != nullptr);
 
@@ -217,6 +217,7 @@ namespace road {
     RELEASE_ASSERT(GetId() >= lanes.begin()->first);
     RELEASE_ASSERT(GetId() <= lanes.rbegin()->first);
 
+    const double s = geom::Math::Clamp(parameter_s, 0.0, road->GetLength());
     float lane_t_offset = 0.0f;
 
     if (GetId() < 0) {

LibCarla/source/carla/road/Map.cpp

@@ -599,7 +599,20 @@ namespace road {
           successor.road_id != waypoint.road_id ||
           successor.section_id != waypoint.section_id ||
           successor.lane_id != waypoint.lane_id);
-      result = ConcatVectors(result, GetNext(successor, distance - remaining_lane_length));
+      // Fix situations where the next waypoint is in the opposite direction and
+      // this waypoint is its successor, so this function would end up in a loop
+      bool is_broken = false;
+      for (const auto &future_successor : GetSuccessors(successor)) {
+          if (future_successor.road_id == waypoint.road_id
+               && future_successor.lane_id == waypoint.lane_id
+               && future_successor.section_id == waypoint.section_id){
+            is_broken = true;
+            break;
+          }
+      }
+      if (!is_broken){
+        result = ConcatVectors(result, GetNext(successor, distance - remaining_lane_length));
+      }
     }
     return result;
   }
@@ -635,7 +648,20 @@ namespace road {
           successor.road_id != waypoint.road_id ||
           successor.section_id != waypoint.section_id ||
           successor.lane_id != waypoint.lane_id);
-      result = ConcatVectors(result, GetPrevious(successor, distance - remaining_lane_length));
+      // Fix situations, when next waypoint is in the opposite direction and
+      // this waypoint is his predecessor, so this function would end up in a loop
+      bool is_broken = false;
+      for (const auto &future_predecessor : GetPredecessors(successor)) {
+          if (future_predecessor.road_id == waypoint.road_id
+               && future_predecessor.lane_id == waypoint.lane_id
+               && future_predecessor.section_id == waypoint.section_id){
+            is_broken = true;
+            break;
+          }
+      }
+      if (!is_broken){
+        result = ConcatVectors(result, GetPrevious(successor, distance - remaining_lane_length));
+      }
     }
     return result;
   }
@@ -1268,12 +1294,14 @@ namespace road {
             while(s_current < s_end){
               if(lane->GetWidth(s_current) != 0.0f){
                 const auto edges = lane->GetCornerPositions(s_current, 0);
-                geom::Vector3D director = edges.second - edges.first;
-                geom::Vector3D treeposition = edges.first - director.MakeUnitVector() * distancefromdrivinglineborder;
-                geom::Transform lanetransform = lane->ComputeTransform(s_current);
-                geom::Transform treeTransform(treeposition, lanetransform.rotation);
-                const carla::road::element::RoadInfoSpeed* roadinfo = lane->GetInfo<carla::road::element::RoadInfoSpeed>(s_current);
-                transforms.push_back(std::make_pair(treeTransform,roadinfo->GetType()));
+                if (edges.first != edges.second) {
+                  geom::Vector3D director = edges.second - edges.first;
+                  geom::Vector3D treeposition = edges.first - director.MakeUnitVector() * distancefromdrivinglineborder;
+                  geom::Transform lanetransform = lane->ComputeTransform(s_current);
+                  geom::Transform treeTransform(treeposition, lanetransform.rotation);
+                  const carla::road::element::RoadInfoSpeed* roadinfo = lane->GetInfo<carla::road::element::RoadInfoSpeed>(s_current);
+                  transforms.push_back(std::make_pair(treeTransform,roadinfo->GetType()));
+                }
               }
               s_current += distancebetweentrees;
             }

LibCarla/source/carla/road/MeshFactory.cpp

@@ -760,14 +760,11 @@ std::map<road::Lane::LaneType , std::vector<std::unique_ptr<Mesh>>> MeshFactory:
           case carla::road::element::LaneMarking::Type::Solid: {
             size_t currentIndex = out_mesh.GetVertices().size() + 1;
 
-            std::pair<geom::Vector3D, geom::Vector3D> edges = lane.GetCornerPositions(s_current, 0);
-
-            geom::Vector3D director = edges.second - edges.first;
-            director /= director.Length();
-            geom::Vector3D endmarking = edges.first + director * static_cast<float>(lane_mark_info.width);
+            std::pair<geom::Vector3D, geom::Vector3D> edges =
+              ComputeEdgesForLanemark(lane_section, lane, s_current, lane_mark_info.width, 0.0f);
 
             out_mesh.AddVertex(edges.first);
-            out_mesh.AddVertex(endmarking);
+            out_mesh.AddVertex(edges.second);
 
             out_mesh.AddIndex(currentIndex);
             out_mesh.AddIndex(currentIndex + 1);
@@ -784,28 +781,21 @@ std::map<road::Lane::LaneType , std::vector<std::unique_ptr<Mesh>>> MeshFactory:
             size_t currentIndex = out_mesh.GetVertices().size() + 1;
 
             std::pair<geom::Vector3D, geom::Vector3D> edges =
-              lane.GetCornerPositions(s_current, road_param.extra_lane_width);
-
-            geom::Vector3D director = edges.second - edges.first;
-            director /= director.Length();
-            geom::Vector3D endmarking = edges.first + director * static_cast<float>(lane_mark_info.width);
+              ComputeEdgesForLanemark(lane_section, lane, s_current, lane_mark_info.width, road_param.extra_lane_width);
 
             out_mesh.AddVertex(edges.first);
-            out_mesh.AddVertex(endmarking);
+            out_mesh.AddVertex(edges.second);
 
             s_current += road_param.resolution * 3;
             if (s_current > s_end)
             {
               s_current = s_end;
             }
-            edges = lane.GetCornerPositions(s_current, road_param.extra_lane_width);
 
-            director = edges.second - edges.first;
-            director /= director.Length();
-            endmarking = edges.first + director * static_cast<float>(lane_mark_info.width);
+            edges = ComputeEdgesForLanemark(lane_section, lane, s_current, lane_mark_info.width, road_param.extra_lane_width);
 
             out_mesh.AddVertex(edges.first);
-            out_mesh.AddVertex(endmarking);
+            out_mesh.AddVertex(edges.second);
 
             out_mesh.AddIndex(currentIndex);
             out_mesh.AddIndex(currentIndex + 1);
@@ -863,13 +853,12 @@ std::map<road::Lane::LaneType , std::vector<std::unique_ptr<Mesh>>> MeshFactory:
       const carla::road::element::RoadInfoMarkRecord* road_info_mark = lane.GetInfo<carla::road::element::RoadInfoMarkRecord>(s_current);
       if (road_info_mark != nullptr) {
         carla::road::element::LaneMarking lane_mark_info(*road_info_mark);
-        std::pair<geom::Vector3D, geom::Vector3D> edges = lane.GetCornerPositions(s_end, 0);
-        geom::Vector3D director = edges.second - edges.first;
-        director /= director.Length();
-        geom::Vector3D endmarking = edges.first + director * static_cast<float>(lane_mark_info.width);
+
+        std::pair<geom::Vector3D, geom::Vector3D> edges =
+              ComputeEdgesForLanemark(lane_section, lane, s_end, lane_mark_info.width, 0.0f);
 
         out_mesh.AddVertex(edges.first);
-        out_mesh.AddVertex(endmarking);
+        out_mesh.AddVertex(edges.second);
       }
       inout.push_back(std::make_unique<Mesh>(out_mesh));
     }
@@ -927,28 +916,20 @@ std::map<road::Lane::LaneType , std::vector<std::unique_ptr<Mesh>>> MeshFactory:
             size_t currentIndex = out_mesh.GetVertices().size() + 1;
 
             std::pair<geom::Vector3D, geom::Vector3D> edges =
-              lane.GetCornerPositions(s_current, road_param.extra_lane_width);
-
-            geom::Vector3D director = edges.second - edges.first;
-            director /= director.Length();
-            geom::Vector3D endmarking = edges.first + director * static_cast<float>(lane_mark_info.width);
+              ComputeEdgesForLanemark(lane_section, lane, s_current, lane_mark_info.width, road_param.extra_lane_width);
 
             out_mesh.AddVertex(edges.first);
-            out_mesh.AddVertex(endmarking);
+            out_mesh.AddVertex(edges.second);
 
             s_current += road_param.resolution * 3;
             if (s_current > s_end) {
               s_current = s_end;
             }
 
-            edges = lane.GetCornerPositions(s_current, road_param.extra_lane_width);
-
-            director = edges.second - edges.first;
-            director /= director.Length();
-            endmarking = edges.first + director * static_cast<float>(lane_mark_info.width);
+            edges = ComputeEdgesForLanemark(lane_section, lane, s_current, lane_mark_info.width, road_param.extra_lane_width);
 
             out_mesh.AddVertex(edges.first);
-            out_mesh.AddVertex(endmarking);
+            out_mesh.AddVertex(edges.second);
 
             out_mesh.AddIndex(currentIndex);
             out_mesh.AddIndex(currentIndex + 1);
@@ -1150,5 +1131,34 @@ std::map<road::Lane::LaneType , std::vector<std::unique_ptr<Mesh>>> MeshFactory:
   }
 
 
+  std::pair<geom::Vector3D, geom::Vector3D> MeshFactory::ComputeEdgesForLanemark(
+      const road::LaneSection& lane_section,
+      const road::Lane& lane,
+      const double s_current,
+      const double lanemark_width,
+      const float extra_width) const {
+    std::pair<geom::Vector3D, geom::Vector3D> edges =
+      lane.GetCornerPositions(s_current, extra_width);
+
+    geom::Vector3D director;
+    if (edges.first != edges.second) {
+      director = edges.second - edges.first;
+      director /= director.Length();
+    } else {
+      const std::map<road::LaneId, road::Lane> & lanes = lane_section.GetLanes();
+      for (const auto& lane_pair : lanes) {
+        std::pair<geom::Vector3D, geom::Vector3D> another_edge =
+          lane_pair.second.GetCornerPositions(s_current, extra_width);
+        if (another_edge.first != another_edge.second) {
+          director = another_edge.second - another_edge.first;
+          director /= director.Length();
+          break;
+        }
+      }
+    }
+    geom::Vector3D endmarking = edges.first + director * static_cast<float>(lanemark_width);
+    return std::make_pair(edges.first, endmarking);
+  }
+
 } // namespace geom
 } // namespace carla

LibCarla/source/carla/road/MeshFactory.h

@@ -148,6 +148,16 @@ namespace geom {
 
     RoadParameters road_param;
 
+  private:
+
+    // Calculate the points on both sides of the lane mark for the specified s_current
+    std::pair<geom::Vector3D, geom::Vector3D> ComputeEdgesForLanemark(
+      const road::LaneSection& lane_section,
+      const road::Lane& lane,
+      const double s_current,
+      const double lanemark_width,
+      const float extra_width) const;
+
   };
 
 } // namespace geom

LibCarla/source/test/common/test_pugixml.cpp

@@ -0,0 +1,112 @@
+// Copyright (c) 2026 Computer Vision Center (CVC) at the Universitat Autonoma
+// de Barcelona (UAB).
+//
+// This work is licensed under the terms of the MIT license.
+// For a copy, see <https://opensource.org/licenses/MIT>.
+
+#include "test.h"
+#include <pugixml/pugixml.hpp>
+
+using namespace pugi;
+
+TEST(pugixml, null_pointer_dereference_insert_move_before) {
+  // Create an empty node (without root, _root = nullptr)
+  xml_node empty_node;
+  ASSERT_FALSE(empty_node);
+
+  // Create a valid document
+  xml_document doc;
+  xml_node root = doc.append_child("root");
+  xml_node child1 = root.append_child("child1");
+  xml_node child2 = root.append_child("child2");
+
+  // Attempt insert_move_before from an empty node
+  // Before the patch this would crash, now it should return an empty xml_node
+  xml_node result = empty_node.insert_move_before(child2, child1);
+
+  // Verify it returns an empty node without crashing
+  EXPECT_FALSE(result);
+}
+
+TEST(pugixml, null_pointer_dereference_insert_move_after) {
+  // Create an empty node
+  xml_node empty_node;
+
+  // Create a valid document
+  xml_document doc;
+  xml_node root = doc.append_child("root");
+  xml_node child1 = root.append_child("child1");
+  xml_node child2 = root.append_child("child2");
+
+  // Attempt insert_move_after from an empty node
+  xml_node result = empty_node.insert_move_after(child2, child1);
+
+  // Verify it returns an empty node without crashing
+  EXPECT_FALSE(result);
+}
+
+TEST(pugixml, null_pointer_dereference_insert_child_before) {
+  // Create an empty node
+  xml_node empty_node;
+
+  // Create a valid document
+  xml_document doc;
+  xml_node root = doc.append_child("root");
+  xml_node child = root.append_child("child");
+
+  // Attempt insert_child_before from an empty node
+  xml_node result = empty_node.insert_child_before(node_element, child);
+
+  // Verify it returns an empty node without crashing
+  EXPECT_FALSE(result);
+}
+
+TEST(pugixml, null_pointer_dereference_insert_child_after) {
+  // Create an empty node
+  xml_node empty_node;
+
+  // Create a valid document
+  xml_document doc;
+  xml_node root = doc.append_child("root");
+  xml_node child = root.append_child("child");
+
+  // Attempt insert_child_after from an empty node
+  xml_node result = empty_node.insert_child_after(node_element, child);
+
+  // Verify it returns an empty node without crashing
+  EXPECT_FALSE(result);
+}
+
+TEST(pugixml, null_pointer_dereference_insert_copy_before) {
+  // Create an empty node
+  xml_node empty_node;
+
+  // Create a valid document
+  xml_document doc;
+  xml_node root = doc.append_child("root");
+  xml_node child = root.append_child("child");
+  xml_node proto = root.append_child("proto");
+
+  // Attempt insert_copy_before from an empty node
+  xml_node result = empty_node.insert_copy_before(proto, child);
+
+  // Verify it returns an empty node without crashing
+  EXPECT_FALSE(result);
+}
+
+TEST(pugixml, null_pointer_dereference_insert_copy_after) {
+  // Create an empty node
+  xml_node empty_node;
+
+  // Create a valid document
+  xml_document doc;
+  xml_node root = doc.append_child("root");
+  xml_node child = root.append_child("child");
+  xml_node proto = root.append_child("proto");
+
+  // Attempt insert_copy_after from an empty node
+  xml_node result = empty_node.insert_copy_after(proto, child);
+
+  // Verify it returns an empty node without crashing
+  EXPECT_FALSE(result);
+}