Added computation of conflicting road in junctions.

LibCarla/source/carla/geom/Rtree.h

@@ -97,23 +97,35 @@ namespace geom {
     /// return a bool to accept or reject the value
     /// [&](Rtree::TreeElement const &element){if (IsOk(element)) return true;
     /// else return false;}
-    template <typename Filter>
+    template <typename Geometry, typename Filter>
     std::vector<TreeElement> GetNearestNeighboursWithFilter(
-        const BPoint &point,
+        const Geometry &geometry,
         Filter filter,
         size_t number_neighbours = 1) const {
       std::vector<TreeElement> query_result;
       _rtree.query(
-          boost::geometry::index::nearest(point, static_cast<unsigned int>(number_neighbours)) &&
+          boost::geometry::index::nearest(geometry, static_cast<unsigned int>(number_neighbours)) &&
               boost::geometry::index::satisfies(filter),
           std::back_inserter(query_result));
       return query_result;
     }
 
-    std::vector<TreeElement> GetNearestNeighbours(const BPoint &point, size_t number_neighbours = 1) const {
+    template<typename Geometry>
+    std::vector<TreeElement> GetNearestNeighbours(const Geometry &geometry, size_t number_neighbours = 1) const {
       std::vector<TreeElement> query_result;
       _rtree.query(
-          boost::geometry::index::nearest(point, static_cast<unsigned int>(number_neighbours)),
+          boost::geometry::index::nearest(geometry, static_cast<unsigned int>(number_neighbours)),
+          std::back_inserter(query_result));
+      return query_result;
+    }
+
+    /// Returns segments that intersec the specified geometry
+    /// Warning: intersection between 3D segments is not implemented by boost
+    template<typename Geometry>
+    std::vector<TreeElement> GetIntersections(const Geometry &geometry) const {
+      std::vector<TreeElement> query_result;
+      _rtree.query(
+          boost::geometry::index::intersects(geometry),
           std::back_inserter(query_result));
       return query_result;
     }

LibCarla/source/carla/road/Junction.h

@@ -11,6 +11,7 @@
 #include "carla/road/RoadTypes.h"
 
 #include <unordered_map>
+#include <unordered_set>
 #include <vector>
 #include <string>
 
@@ -73,6 +74,16 @@ namespace road {
       return _bounding_box;
     }
 
+  std::vector<RoadId> GetConflictsOfRoad(RoadId road_id) const{
+    std::vector<RoadId> result;
+    if(_road_conflicts.count(road_id) > 0){
+      for(RoadId road : _road_conflicts.at(road_id)){
+        result.push_back(road);
+      }
+    }
+    return result;
+  }
+
   private:
 
     friend MapBuilder;
@@ -85,6 +96,9 @@ namespace road {
 
     std::set<ContId> _controllers;
 
+    std::unordered_map<RoadId, std::unordered_set<RoadId>>
+        _road_conflicts;
+
     carla::geom::BoundingBox _bounding_box;
   };
 

LibCarla/source/carla/road/Map.cpp

@@ -741,6 +741,77 @@ namespace road {
     return result;
   }
 
+  std::unordered_map<road::RoadId, std::unordered_set<road::RoadId>>
+      Map::ComputeJunctionConflicts(JuncId id) const {
+
+    const float epsilon = 0.0001f; // small delta in the road (set to 1
+                                     // milimeter to prevent numeric errors)
+    const Junction *junction = GetJunction(id);
+    std::unordered_map<road::RoadId, std::unordered_set<road::RoadId>>
+        conflicts;
+
+    // 2d typedefs
+    typedef boost::geometry::model::point
+        <float, 2, boost::geometry::cs::cartesian> Point2d;
+    typedef boost::geometry::model::segment<Point2d> Segment2d;
+    typedef boost::geometry::model::box<Rtree::BPoint> Box;
+
+    // box range
+    auto bbox_pos = junction->GetBoundingBox().location;
+    auto bbox_ext = junction->GetBoundingBox().extent;
+    auto min_corner = geom::Vector3D(
+        bbox_pos.x - bbox_ext.x,
+        bbox_pos.y - bbox_ext.y,
+        bbox_pos.z - bbox_ext.z - epsilon);
+    auto max_corner = geom::Vector3D(
+        bbox_pos.x + bbox_ext.x,
+        bbox_pos.y + bbox_ext.y,
+        bbox_pos.z + bbox_ext.z + epsilon);
+    Box box({min_corner.x, min_corner.y, min_corner.z},
+        {max_corner.x, max_corner.y, max_corner.z});
+    auto segments = _rtree.GetIntersections(box);
+
+    for (size_t i = 0; i < segments.size(); ++i){
+      auto &segment1 = segments[i];
+      auto waypoint1 = segment1.second.first;
+      JuncId junc_id1 = _data.GetRoad(waypoint1.road_id).GetJunctionId();
+      // only segments in the junction
+      if(junc_id1 != id){
+        continue;
+      }
+      Segment2d seg1{{segment1.first.first.get<0>(), segment1.first.first.get<1>()},
+          {segment1.first.second.get<0>(), segment1.first.second.get<1>()}};
+      for (size_t j = i + 1; j < segments.size(); ++j){
+        auto &segment2 = segments[j];
+        auto waypoint2 = segment2.second.first;
+        JuncId junc_id2 = _data.GetRoad(waypoint2.road_id).GetJunctionId();
+        // only segments in the junction
+        if(junc_id2 != id){
+          continue;
+        }
+        // discard same road
+        if(waypoint1.road_id == waypoint2.road_id){
+          continue;
+        }
+        Segment2d seg2{{segment2.first.first.get<0>(), segment2.first.first.get<1>()},
+            {segment2.first.second.get<0>(), segment2.first.second.get<1>()}};
+
+        double distance = boost::geometry::distance(seg1, seg2);
+        // better to set distance to lanewidth
+        if(distance > 2.0){
+          continue;
+        }
+        if(conflicts[waypoint1.road_id].count(waypoint2.road_id) == 0){
+          conflicts[waypoint1.road_id].insert(waypoint2.road_id);
+        }
+        if(conflicts[waypoint2.road_id].count(waypoint1.road_id) == 0){
+          conflicts[waypoint2.road_id].insert(waypoint1.road_id);
+        }
+      }
+    }
+    return conflicts;
+  }
+
   const Lane &Map::GetLane(Waypoint waypoint) const {
     return _data.GetRoad(waypoint.road_id).GetLaneById(waypoint.section_id, waypoint.lane_id);
   }
@@ -808,6 +879,7 @@ namespace road {
       std::vector<Rtree::TreeElement> &rtree_elements,
       geom::Transform &current_transform,
       Waypoint &current_waypoint,
+
       Waypoint &next_waypoint) {
     geom::Transform next_transform = ComputeTransform(next_waypoint);
     AddElementToRtree(rtree_elements, current_transform, next_transform,

LibCarla/source/carla/road/Map.h

@@ -139,6 +139,9 @@ namespace road {
 
     const Junction* GetJunction(JuncId id) const;
 
+    std::unordered_map<road::RoadId, std::unordered_set<road::RoadId>>
+        ComputeJunctionConflicts(JuncId id) const;
+
     /// Buids a mesh based on the OpenDRIVE
     geom::Mesh GenerateMesh(double distance) const;
 

LibCarla/source/carla/road/MapBuilder.cpp

@@ -61,7 +61,7 @@ namespace road {
     // or move it (will return move -> Map(Map &&))
     Map map(std::move(_map_data));
     CreateJunctionBoundingBoxes(map);
-
+    ComputeJunctionRoadConflicts(map);
 
     return map;
   }
@@ -870,6 +870,12 @@ void MapBuilder::CreateController(
     }
 }
 
+  void MapBuilder::ComputeJunctionRoadConflicts(Map &map) {
+    for (auto &junctionpair : map._data.GetJunctions()) {
+      auto& junction = junctionpair.second;
+      junction._road_conflicts = (map.ComputeJunctionConflicts(junction.GetId()));
+    }
+  }
 
 } // namespace road
 } // namespace carla

LibCarla/source/carla/road/MapBuilder.h

@@ -359,6 +359,9 @@ namespace road {
     /// Solve the references between Controllers and Juntions
     void SolveControllerAndJuntionReferences();
 
+    /// Compute the conflicts of the roads (intersecting roads)
+    void ComputeJunctionRoadConflicts(Map &map);
+
     /// Return the pointer to a lane object.
     Lane *GetEdgeLanePointer(RoadId road_id, bool from_start, LaneId lane_id);