Added GetNearestLane

LibCarla/source/carla/client/Waypoint.h

@@ -26,6 +26,14 @@ namespace client {
       return _waypoint.GetTransform();
     }
 
+    road::element::id_type GetRoadId() const {
+      return _waypoint.GetRoadId();
+    }
+
+    int GetLaneId() const {
+      return _waypoint.GetLaneId();
+    }
+
     std::vector<SharedPtr<Waypoint>> Next(double distance) const;
 
   private:

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

@@ -184,19 +184,56 @@ namespace element {
       return last;
     }
 
-    int GetNearestLane(double, const geom::Location &) const {
-      /*
-      DirectedPoint dp = GetDirectedPointIn(dist);
+    std::pair<int, geom::Location> GetNearestLane(double dist, const geom::Location &loc) const {
+      // Because Unreal's coordinates
+      const geom::Location corrected_loc = geom::Location(loc.x, loc.y, loc.z);
 
-      double dist_dp_loc = geom::Math::Distance2D(dp.location, loc);
+      const DirectedPoint dp_center_road = GetDirectedPointIn(dist);
+      auto info = GetInfo<RoadInfoLane>(0.0);
 
-      const RoadInfoLane *road_info_lane = GetInfo<RoadInfoLane>(dist);
-      for (auto &&lane_id : road_info_lane->getLanesIDs()) {
-        const LaneInfo *info = road_info_lane->getLane(lane_id);
-        // search for info width
+      int nearest_lane_id = 0;
+      geom::Location nearest_loc;
+      double nearest_dist = std::numeric_limits<double>::max();
+      double current_width = 0;
+
+      // Left lanes
+      for (auto &&current_lane_id : info->getLanesIDs(carla::road::element::RoadInfoLane::which_lane_e::Left)) {
+        DirectedPoint dp_center_lane = dp_center_road;
+        const double half_width = info->getLane(current_lane_id)->_width * 0.5;
+
+        current_width += half_width;
+        if (info->getLane(current_lane_id)->_type == "driving") {
+          dp_center_lane.ApplyLateralOffset(-current_width);
+          const double current_dist = geom::Math::Distance2D(dp_center_lane.location, corrected_loc);
+          if(current_dist < nearest_dist) {
+            nearest_dist = current_dist;
+            nearest_lane_id = current_lane_id;
+            nearest_loc = dp_center_lane.location;
+          }
+        }
+        current_width += half_width;
       }
-      */
-      return 0;
+
+      current_width = 0.0;
+      // Right lanes
+      for (auto &&current_lane_id : info->getLanesIDs(carla::road::element::RoadInfoLane::which_lane_e::Right)) {
+        DirectedPoint dp_center_lane = dp_center_road;
+        const double half_width = info->getLane(current_lane_id)->_width * 0.5;
+
+        current_width += half_width;
+        if (info->getLane(current_lane_id)->_type == "driving") {
+          dp_center_lane.ApplyLateralOffset(current_width);
+          const double current_dist = geom::Math::Distance2D(dp_center_lane.location, corrected_loc);
+          if(current_dist < nearest_dist) {
+            nearest_dist = current_dist;
+            nearest_lane_id = current_lane_id;
+            nearest_loc = dp_center_lane.location;
+          }
+        }
+        current_width += half_width;
+      }
+
+      return std::make_pair(nearest_lane_id, nearest_loc);
     }
 
     const double &GetLength() const {

LibCarla/source/carla/road/element/Waypoint.cpp

@@ -12,8 +12,6 @@ namespace carla {
 namespace road {
 namespace element {
 
-  Waypoint::Waypoint() {}
-
   Waypoint::Waypoint(SharedPtr<const Map> m, const geom::Location &loc)
     : _map(m) {
 
@@ -32,14 +30,15 @@ namespace element {
 
     assert(_dist <= _map->GetData().GetRoad(_road_id)->GetLength());
 
+    const auto nearest_lane = _map->GetData().GetRoad(_road_id)->GetNearestLane(_dist, loc);
+    _lane_id = nearest_lane.first;
+
     const road::element::DirectedPoint dp =
         _map->GetData().GetRoad(_road_id)->GetDirectedPointIn(_dist);
 
     const geom::Rotation rot(0.0, geom::Math::to_degrees(dp.tangent), 0.0);
 
-    _transform = geom::Transform(dp.location, rot);
-
-    _lane_id = _map->GetData().GetRoad(_road_id)->GetNearestLane(_dist, loc);
+    _transform = geom::Transform(nearest_lane.second, rot);
   }
 
   RoadInfoList Waypoint::GetRoadInfo() const {

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

@@ -27,10 +27,14 @@ namespace element {
       return _transform;
     }
 
-    const id_type &GetRoadId() const {
+    id_type GetRoadId() const {
       return _road_id;
     }
 
+    int GetLaneId() const {
+      return _lane_id;
+    }
+
     std::vector<Waypoint> Next(double distance) const {
       (void) distance;
       return std::vector<Waypoint>();

PythonAPI/source/libcarla/Map.cpp

@@ -65,6 +65,8 @@ void export_map() {
 
   class_<cc::Waypoint, boost::noncopyable, boost::shared_ptr<cc::Waypoint>>("Waypoint", no_init)
     .add_property("transform", CALL_RETURNING_COPY(cc::Waypoint, GetTransform))
+    .add_property("road_id", &cc::Waypoint::GetRoadId)
+    .add_property("lane_id", &cc::Waypoint::GetLaneId)
     .def("next", &cc::Waypoint::Next, (args("distance")))
     .def(self_ns::str(self_ns::self))
   ;