Add unique ID for waypoints based on their position on the road

LibCarla/source/carla/client/Waypoint.h

@@ -8,8 +8,9 @@
 
 #include "carla/Memory.h"
 #include "carla/NonCopyable.h"
-#include "carla/road/element/Waypoint.h"
 #include "carla/road/element/RoadInfoMarkRecord.h"
+#include "carla/road/element/Waypoint.h"
+#include "carla/road/element/WaypointHash.h"
 
 namespace carla {
 namespace client {
@@ -31,6 +32,14 @@ namespace client {
 
     ~Waypoint();
 
+    /// Returns an unique Id identifying this waypoint.
+    ///
+    /// The Id takes into account OpenDrive's road Id, lane Id, and s distance
+    /// on its road segment up to half-centimetre precision.
+    uint64_t GetId() const {
+      return road::element::WaypointHash()(_waypoint);
+    }
+
     const geom::Transform &GetTransform() const {
       return _transform;
     }

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

@@ -8,8 +8,9 @@
 
 #include "carla/road/element/RoadInfoVisitor.h"
 
-#include <string>
 #include <map>
+#include <string>
+#include <vector>
 
 namespace carla {
 namespace road {

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

@@ -37,6 +37,10 @@ namespace element {
       return _lane_id;
     }
 
+    double GetDistance() const {
+      return _dist;
+    }
+
     const std::string &GetType() const;
 
     const RoadSegment &GetRoadSegment() const;

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

@@ -0,0 +1,27 @@
+// Copyright (c) 2017 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 "carla/road/element/WaypointHash.h"
+
+#include "carla/road/element/Waypoint.h"
+
+#include <boost/container_hash/hash.hpp>
+
+namespace carla {
+namespace road {
+namespace element {
+
+  uint64_t WaypointHash::operator()(const Waypoint &waypoint) const {
+    uint64_t seed = 0u;
+    boost::hash_combine(seed, waypoint.GetRoadId());
+    boost::hash_combine(seed, waypoint.GetLaneId());
+    boost::hash_combine(seed, static_cast<float>(std::floor(waypoint.GetDistance() * 200.0)));
+    return seed;
+  }
+
+} // namespace element
+} // namespace road
+} // namespace carla

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

@@ -0,0 +1,31 @@
+// Copyright (c) 2017 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>.
+
+#pragma once
+
+#include <cstdint>
+
+namespace carla {
+namespace road {
+namespace element {
+
+  class Waypoint;
+
+  struct WaypointHash {
+
+    using argument_type = Waypoint;
+
+    using result_type = uint64_t;
+
+    /// Generates an unique id for @a waypoint based on its road_id, lane_id,
+    /// and "s" offset. The "s" offset is truncated to half centimetre
+    /// precision.
+    uint64_t operator()(const Waypoint &waypoint) const;
+  };
+
+} // namespace element
+} // namespace road
+} // namespace carla

PythonAPI/source/libcarla/Map.cpp

@@ -78,6 +78,7 @@ void export_map() {
   ;
 
   class_<cc::Waypoint, boost::noncopyable, boost::shared_ptr<cc::Waypoint>>("Waypoint", no_init)
+    .add_property("id", &cc::Waypoint::GetId)
     .add_property("transform", CALL_RETURNING_COPY(cc::Waypoint, GetTransform))
     .add_property("is_intersection", &cc::Waypoint::IsIntersection)
     .add_property("lane_width", &cc::Waypoint::GetLaneWidth)