Road structure working with tests

LibCarla/source/carla/road/Map.cpp

@@ -13,12 +13,16 @@ namespace road {
     return _elements.count(id);
   }
 
-  const RoadElement *Map::GetRoad(id_type id) {
+  const RoadSegment *Map::GetRoad(id_type id) const {
     if (ExistId(id)) {
       return _elements.find(id)->second.get();
     }
     return nullptr;
   }
 
+  uint32_t Map::GetRoadCount() const {
+    return _elements.size();
+  }
+
 } // namespace road
 } // namespace carla

LibCarla/source/carla/road/Map.h

@@ -29,15 +29,19 @@ namespace road {
 
     bool ExistId(id_type id) const;
 
-    const RoadElement *GetRoad(id_type id);
+    const RoadSegment *GetRoad(id_type id) const;
 
-    const RoadElement &NearestRoad(const geom::Location &loc);
+    uint32_t GetRoadCount() const;
+
+    const RoadSegment &NearestRoad(const geom::Location &loc);
 
   private:
 
     friend class MapBuilder;
+
     Map() {}
-    std::map<id_type, std::unique_ptr<RoadElement>> _elements;
+
+    std::map<id_type, std::unique_ptr<RoadSegment>> _elements;
   };
 
 } // namespace road

LibCarla/source/carla/road/MapBuilder.cpp

@@ -15,9 +15,19 @@ namespace road {
   }
 
   Map MapBuilder::Build() {
+    // Move the RoadSegmentDefinitions needed information to a RoadSegments
+    for (auto &&id_seg : _temp_sections) {
+      MakeElement<RoadSegment>(id_seg.first, std::move(id_seg.second));
+    }
 
-    for (auto &&seg : _temp_sections) {
-      MakeElement<RoadSegment>(seg.first, std::move(seg.second));
+    // Create the pointers between RoadSegments based on the ids
+    for (auto &&id_seg : _temp_sections) {
+      for (auto &t : id_seg.second.GetPredecessorID()) {
+        _map._elements[id_seg.first]->PredEmplaceBack(_map._elements[t].get());
+      }
+      for (auto &t : id_seg.second.GetSuccessorID()) {
+        _map._elements[id_seg.first]->SuccEmplaceBack(_map._elements[t].get());
+      }
     }
 
     // _map is a memeber of MapBuilder so you must especify if
@@ -26,5 +36,10 @@ namespace road {
     return std::move(_map);
   }
 
+  bool MapBuilder::InterpretRoadFlow() {
+    // todo
+    return false;
+  }
+
 } // namespace road
 } // namespace carla

LibCarla/source/carla/road/MapBuilder.h

@@ -14,8 +14,6 @@
 namespace carla {
 namespace road {
 
-  using temp_section_type = std::map<id_type, RoadSegmentDefinition &>;
-
   class MapBuilder {
   public:
 
@@ -33,9 +31,12 @@ namespace road {
       return r;
     }
 
+    bool InterpretRoadFlow();
+
   private:
+
     Map _map;
-    temp_section_type _temp_sections;
+    std::map<id_type, RoadSegmentDefinition &> _temp_sections;
   };
 
 } // namespace road

LibCarla/source/carla/road/element/Geometry.h

@@ -33,14 +33,15 @@ namespace element {
     double GetHeading() {
       return _heading;
     }
+    virtual ~Geometry() = default;
 
   private:
 
-    GeometryType _type;            // geometry type
-    double _length;                // length of the road section [meters]
+    GeometryType _type;             // geometry type
+    double _length;                 // length of the road section [meters]
 
-    double _start_position_offset; // s-offset [meters]
-    double _heading;               // start orientation [radians]
+    double _start_position_offset;  // s-offset [meters]
+    double _heading;                // start orientation [radians]
 
     geom::Location _start_position; // [meters]
 

LibCarla/source/carla/road/element/RoadElement.h

@@ -34,4 +34,4 @@ namespace element {
 
 } // namespace element
 } // namespace road
-} // namespace carla
+} // namespace carla

LibCarla/source/carla/road/element/RoadSegment.h

@@ -7,7 +7,6 @@
 #pragma once
 
 #include "Types.h"
-#include "RoadElement.h"
 #include "carla/geom/Location.h"
 
 #include <set>
@@ -18,9 +17,11 @@ namespace carla {
 namespace road {
 namespace element {
 
-  class RoadSegment : public RoadElement {
+  class RoadSegment {
   public:
 
+    RoadSegment(id_type id) : _id(id) {}
+
     RoadSegment(RoadSegmentDefinition &&def)
       : _id(def.GetId()),
         _geom(std::move(def._geom)) {
@@ -38,11 +39,51 @@ namespace element {
 
     // returns single info given a type and a distance
     template <typename T>
-    T GetInfo(double dist);
+    T GetInfo(double dist) const;
 
     // returns info vector given a type and a distance
     template <typename T>
-    std::vector<T> GetInfo(double dist);
+    std::vector<T> GetInfo(double dist) const;
+
+    void PredEmplaceBack(RoadSegment *s) {
+      _predecessors.emplace_back(s);
+    }
+
+    void SuccEmplaceBack(RoadSegment *s) {
+      _successors.emplace_back(s);
+    }
+
+    bool HaveSuccessors() const {
+      return _successors.size() > 0;
+    }
+
+    bool HavePredecessors() const {
+      return _predecessors.size() > 0;
+    }
+
+    std::vector<id_type> GetSuccessorsIds() const {
+      std::vector<id_type> s_vec;
+      for (auto &&succ : _successors) {
+        s_vec.emplace_back(succ->GetId());
+      }
+      return s_vec;
+    }
+
+    std::vector<id_type> GetPredecessorsIds() const {
+      std::vector<id_type> p_vec;
+      for (auto &&pred : _predecessors) {
+        p_vec.emplace_back(pred->GetId());
+      }
+      return p_vec;
+    }
+
+    const std::vector<RoadSegment *> GetSuccessors() const {
+      return _successors;
+    }
+
+    const std::vector<RoadSegment *> GetPredecessors() const {
+      return _predecessors;
+    }
 
   private:
 
@@ -55,6 +96,8 @@ namespace element {
       }
     };
 
+    // friend class Map;
+
     id_type _id;
     std::vector<RoadSegment *> _predecessors;
     std::vector<RoadSegment *> _successors;

LibCarla/source/carla/road/element/Types.h

@@ -20,6 +20,7 @@ namespace element {
   struct RoadInfo {
     // distance from Road's start location
     double d = 0; // [meters]
+    virtual ~RoadInfo() = default;
   };
 
   struct SpeedLimit : public RoadInfo {
@@ -64,20 +65,21 @@ namespace element {
       _info.emplace_back(std::make_unique<T>(std::forward<Args>(args) ...));
     }
 
-    const std::vector<id_type> &GetPredecessorID_Vector() const {
+    const std::vector<id_type> &GetPredecessorID() const {
       return _predecessor_id;
     }
-    const std::vector<id_type> &GetSuccessorID_Vector() const {
+    const std::vector<id_type> &GetSuccessorID() const {
       return _successor_id;
     }
-    const std::vector<std::unique_ptr<Geometry>> &GetGeometry_Vector() const {
+    const std::vector<std::unique_ptr<Geometry>> &GetGeometry() const {
       return _geom;
     }
-    const std::vector<std::unique_ptr<RoadInfo>> &GetInfo_Vector() const {
+    const std::vector<std::unique_ptr<RoadInfo>> &GetInfo() const {
       return _info;
     }
 
   private:
+
     friend class RoadSegment;
     id_type _id;
     std::vector<id_type> _predecessor_id;

LibCarla/source/test/test_road.cpp

@@ -56,4 +56,148 @@ TEST(road, add_geom_info) {
 
   builder.AddRoadSegmentDefinition(def);
   builder.Build();
+}
+
+TEST(road, connections) {
+  MapBuilder builder;
+  RoadSegmentDefinition def1(1);
+  RoadSegmentDefinition def2(2);
+  RoadSegmentDefinition def3(3);
+  RoadSegmentDefinition def4(4);
+
+  def1.AddPredecessorID(4);
+  def2.AddPredecessorID(1);
+  def3.AddPredecessorID(1);
+  def3.AddPredecessorID(2);
+  def4.AddPredecessorID(3);
+
+  def1.AddSuccessorID(2);
+  def1.AddSuccessorID(3);
+  def2.AddSuccessorID(3);
+  def3.AddSuccessorID(4);
+
+  builder.AddRoadSegmentDefinition(def1);
+  builder.AddRoadSegmentDefinition(def2);
+  builder.AddRoadSegmentDefinition(def3);
+  builder.AddRoadSegmentDefinition(def4);
+
+  Map m = builder.Build();
+
+  ASSERT_TRUE(m.ExistId(1));
+  ASSERT_TRUE(m.ExistId(2));
+  ASSERT_TRUE(m.ExistId(3));
+  ASSERT_TRUE(m.ExistId(4));
+
+  const RoadSegment *s1 = m.GetRoad(1);
+  const RoadSegment *s2 = m.GetRoad(2);
+  const RoadSegment *s3 = m.GetRoad(3);
+  const RoadSegment *s4 = m.GetRoad(4);
+
+  ASSERT_NE(s1, nullptr);
+  ASSERT_NE(s2, nullptr);
+  ASSERT_NE(s3, nullptr);
+  ASSERT_NE(s4, nullptr);
+
+  // Road 1
+  ASSERT_TRUE(s1->HavePredecessors());
+  ASSERT_TRUE(s1->HaveSuccessors());
+
+  std::vector<id_type> pred = s1->GetPredecessorsIds();
+  std::vector<RoadSegment *> pred_ptr = s1->GetPredecessors();
+
+  ASSERT_EQ(pred.size(), 1U);
+  ASSERT_EQ(pred[0], 4U); // same id
+  ASSERT_EQ(m.GetRoad(pred[0]), s4); // same memory adress
+  ASSERT_EQ(s4, pred_ptr[0]); // same memory adress
+
+  std::vector<id_type> succ = s1->GetSuccessorsIds();
+  std::vector<RoadSegment *> succ_ptr = s1->GetSuccessors();
+
+  ASSERT_EQ(succ.size(), 2U);
+  ASSERT_EQ(succ[0], 2U);
+  ASSERT_EQ(succ[1], 3U);
+  ASSERT_EQ(m.GetRoad(succ[0]), s2);
+  ASSERT_EQ(m.GetRoad(succ[1]), s3);
+  ASSERT_EQ(s2, succ_ptr[0]);
+  ASSERT_EQ(s3, succ_ptr[1]);
+
+  pred.clear();
+  succ.clear();
+  pred_ptr.clear();
+  succ_ptr.clear();
+
+  // Road 2
+  ASSERT_TRUE(s2->HavePredecessors());
+  ASSERT_TRUE(s2->HaveSuccessors());
+
+  pred = s2->GetPredecessorsIds();
+  pred_ptr = s2->GetPredecessors();
+
+  ASSERT_EQ(pred.size(), 1U);
+  ASSERT_EQ(pred[0], 1U);
+  ASSERT_EQ(m.GetRoad(pred[0]), s1);
+  ASSERT_EQ(s1, pred_ptr[0]);
+
+  succ = s2->GetSuccessorsIds();
+  succ_ptr = s2->GetSuccessors();
+
+  ASSERT_EQ(succ.size(), 1U);
+  ASSERT_EQ(succ[0], 3U);
+  ASSERT_EQ(m.GetRoad(succ[0]), s3);
+  ASSERT_EQ(s3, succ_ptr[0]);
+
+  pred.clear();
+  succ.clear();
+  pred_ptr.clear();
+  succ_ptr.clear();
+
+  // Road 3
+  ASSERT_TRUE(s3->HavePredecessors());
+  ASSERT_TRUE(s3->HaveSuccessors());
+
+  pred = s3->GetPredecessorsIds();
+  pred_ptr = s3->GetPredecessors();
+
+  ASSERT_EQ(pred.size(), 2U);
+  ASSERT_EQ(pred[0], 1U);
+  ASSERT_EQ(pred[1], 2U);
+  ASSERT_EQ(m.GetRoad(pred[0]), s1);
+  ASSERT_EQ(m.GetRoad(pred[1]), s2);
+  ASSERT_EQ(s1, pred_ptr[0]);
+  ASSERT_EQ(s2, pred_ptr[1]);
+
+  succ = s3->GetSuccessorsIds();
+  succ_ptr = s3->GetSuccessors();
+
+  ASSERT_EQ(succ.size(), 1U);
+  ASSERT_EQ(succ[0], 4U);
+  ASSERT_EQ(m.GetRoad(succ[0]), s4);
+  ASSERT_EQ(s4, succ_ptr[0]);
+
+  pred.clear();
+  succ.clear();
+  pred_ptr.clear();
+  succ_ptr.clear();
+
+  // Road 4
+  ASSERT_TRUE(s4->HavePredecessors());
+  ASSERT_FALSE(s4->HaveSuccessors());
+
+  pred = s4->GetPredecessorsIds();
+  pred_ptr = s4->GetPredecessors();
+
+  ASSERT_EQ(pred.size(), 1U);
+  ASSERT_EQ(pred[0], 3U);
+  ASSERT_EQ(m.GetRoad(pred[0]), s3);
+  ASSERT_EQ(s3, pred_ptr[0]);
+
+  succ = s4->GetSuccessorsIds();
+  succ_ptr = s4->GetSuccessors();
+
+  ASSERT_EQ(succ.size(), 0U);
+
+  pred.clear();
+  succ.clear();
+  pred_ptr.clear();
+  succ_ptr.clear();
 }