Provides support for RoadGeometry::IdIndex methods. (#46)

Signed-off-by: Franco Cipollone <[email protected]>

maliput-sys/build.rs

@@ -34,6 +34,7 @@ use std::path::PathBuf;
 
 fn main() -> Result<(), Box<dyn Error>> {
     println!("cargo:rerun-if-changed=build.rs");
+    println!("cargo:rerun-if-changed=src/api/api.h");
     println!("cargo:rerun-if-changed=src/api/mod.rs");
     println!("cargo:rerun-if-changed=src/lib.rs");
     println!("cargo:rerun-if-changed=src/math/math.h");

maliput-sys/src/api/api.h

@@ -32,6 +32,7 @@
 
 #include <memory>
 #include <sstream>
+#include <vector>
 
 #include <maliput/api/lane.h>
 #include <maliput/api/lane_data.h>
@@ -40,9 +41,13 @@
 
 #include <rust/cxx.h>
 
+#include "maliput-sys/src/api/mod.rs.h"
+
 namespace maliput {
 namespace api {
 
+struct ConstLanePtr;
+
 /// Creates a new maliput::api::LanePosition.
 /// Forwads to maliput::api::LanePosition(double s, double r, double h) constructor.
 std::unique_ptr<LanePosition> LanePosition_new(rust::f64 s, rust::f64 r, rust::f64 h) {
@@ -121,5 +126,22 @@ std::unique_ptr<RoadPositionResult> RoadGeometry_ToRoadPosition(const RoadGeomet
   return std::make_unique<RoadPositionResult>(road_geometry.ToRoadPosition(inertial_pos));
 }
 
+ConstLanePtr RoadGeometry_GetLane(const RoadGeometry& road_geometry, const rust::String& lane_id) {
+  return {road_geometry.ById().GetLane(LaneId{std::string(lane_id)})};
+}
+
+const std::vector<ConstLanePtr>& RoadGeometry_GetLanes(const RoadGeometry& road_geometry) {
+  static std::vector<ConstLanePtr> lanes;
+  const auto lanes_cpp = road_geometry.ById().GetLanes();
+  if (lanes.size() == lanes_cpp.size()) {
+    return lanes;
+  }
+  lanes.reserve(lanes_cpp.size());
+  for (const auto& lane : road_geometry.ById().GetLanes()) {
+    lanes.push_back(ConstLanePtr{lane.second});
+  }
+  return lanes;
+}
+
 } // namespace api
 } // namespace maliput

maliput-sys/src/api/mod.rs

@@ -30,6 +30,12 @@
 
 #[cxx::bridge(namespace = "maliput::api")]
 pub mod ffi {
+    /// Shared struct for `Lane` pointers.
+    /// This is needed because `*const Lane` can't be used directly in the CxxVector collection.
+    struct ConstLanePtr {
+        pub lane: *const Lane,
+    }
+
     unsafe extern "C++" {
         include!("api/api.h");
 
@@ -50,6 +56,8 @@ pub mod ffi {
             rg: &RoadGeometry,
             inertial_position: &InertialPosition,
         ) -> UniquePtr<RoadPositionResult>;
+        fn RoadGeometry_GetLane(rg: &RoadGeometry, lane_id: &String) -> ConstLanePtr;
+        fn RoadGeometry_GetLanes(rg: &RoadGeometry) -> &CxxVector<ConstLanePtr>;
         // LanePosition bindings definitions.
         type LanePosition;
         fn LanePosition_new(s: f64, r: f64, h: f64) -> UniquePtr<LanePosition>;

maliput/examples/road_geometry.rs

@@ -52,4 +52,11 @@ fn main() {
     println!("linear_tolerance: {}", road_geometry.linear_tolerance());
     println!("angular_tolerance: {}", road_geometry.angular_tolerance());
     println!("num_junctions: {}", road_geometry.num_junctions());
+
+    let lanes = road_geometry.get_lanes();
+    println!("num_lanes: {}", lanes.len());
+    println!("lanes: ");
+    for lane in lanes {
+        println!("\tlane id: {}", lane.id());
+    }
 }

maliput/src/api/mod.rs

@@ -36,18 +36,57 @@ pub struct RoadGeometry<'a> {
 }
 
 impl<'a> RoadGeometry<'a> {
+    /// Returns the number of Junctions in the RoadGeometry.
+    ///
+    /// Return value is non-negative.
     pub fn num_junctions(&self) -> i32 {
         self.rg.num_junctions()
     }
+    /// Returns the tolerance guaranteed for linear measurements (positions).
     pub fn linear_tolerance(&self) -> f64 {
         self.rg.linear_tolerance()
     }
+    /// Returns the tolerance guaranteed for angular measurements (orientations).
     pub fn angular_tolerance(&self) -> f64 {
         self.rg.angular_tolerance()
     }
+    /// Returns the number of BranchPoints in the RoadGeometry.
+    ///
+    /// Return value is non-negative.
     pub fn num_branch_points(&self) -> i32 {
         self.rg.num_branch_points()
     }
+    /// Determines the RoadPosition corresponding to InertialPosition `inertial_position`.
+    ///
+    /// Returns a RoadPositionResult. Its RoadPosition is the point in the
+    /// RoadGeometry's manifold which is, in the `Inertial`-frame, closest to
+    /// `inertial_position`. Its InertialPosition is the `Inertial`-frame equivalent of the
+    /// RoadPosition and its distance is the Cartesian distance from
+    /// `inertial_position` to the nearest point.
+    ///
+    /// This method guarantees that its result satisfies the condition that
+    /// `result.lane.to_lane_position(result.pos)` is within `linear_tolerance()`
+    /// of the returned InertialPosition.
+    ///
+    /// The map from RoadGeometry to the `Inertial`-frame is not onto (as a bounded
+    /// RoadGeometry cannot completely cover the unbounded Cartesian universe).
+    /// If `inertial_position` does represent a point contained within the volume
+    /// of the RoadGeometry, then result distance is guaranteed to be less
+    /// than or equal to `linear_tolerance()`.
+    ///
+    /// The map from RoadGeometry to `Inertial`-frame is not necessarily one-to-one.
+    /// Different `(s,r,h)` coordinates from different Lanes, potentially from
+    /// different Segments, may map to the same `(x,y,z)` `Inertial`-frame location.
+    ///
+    /// If `inertial_position` is contained within the volumes of multiple Segments,
+    /// then ToRoadPosition() will choose a Segment which yields the minimum
+    /// height `h` value in the result.  If the chosen Segment has multiple
+    /// Lanes, then ToRoadPosition() will choose a Lane which contains
+    /// `inertial_position` within its `lane_bounds()` if possible, and if that is
+    /// still ambiguous, it will further select a Lane which minimizes the
+    /// absolute value of the lateral `r` coordinate in the result.
+    ///
+    /// Wrapper around C++ implementation `maliput::api::RoadGeometry::ToRoadPosition`.
     pub fn to_road_position(&self, inertial_position: &InertialPosition) -> RoadPositionResult {
         let rpr = maliput_sys::api::ffi::RoadGeometry_ToRoadPosition(self.rg, &inertial_position.ip);
         RoadPositionResult {
@@ -60,6 +99,45 @@ impl<'a> RoadGeometry<'a> {
             distance: maliput_sys::api::ffi::RoadPositionResult_distance(&rpr),
         }
     }
+    /// Get the lane matching given `lane_id`.
+    pub fn get_lane(&self, lane_id: &String) -> Lane {
+        unsafe {
+            Lane {
+                lane: maliput_sys::api::ffi::RoadGeometry_GetLane(self.rg, lane_id)
+                    .lane
+                    .as_ref()
+                    .expect(""),
+            }
+        }
+    }
+    /// Get all lanes of the `RoadGeometry`.
+    /// Returns a vector of `Lane`.
+    /// # Example
+    /// ```rust, no_run
+    /// use maliput::api::RoadNetwork;
+    /// use std::collections::HashMap;
+    ///
+    /// let maliput_malidrive_plugin_path = maliput_sdk::get_maliput_malidrive_plugin_path();
+    /// std::env::set_var("MALIPUT_PLUGIN_PATH", maliput_malidrive_plugin_path);
+    /// let package_location = std::env::var("CARGO_MANIFEST_DIR").unwrap();
+    /// let xodr_path = format!("{}/data/xodr/TShapeRoad.xodr", package_location);
+    /// let road_network_properties = HashMap::from([("road_geometry_id", "my_rg_from_rust"), ("opendrive_file", xodr_path.as_str())]);
+    /// let road_network = RoadNetwork::new("maliput_malidrive", &road_network_properties);
+    /// let road_geometry = road_network.road_geometry();
+    /// let lanes = road_geometry.get_lanes();
+    /// for lane in lanes {
+    ///    println!("lane_id: {}", lane.id());
+    /// }
+    /// ```
+    pub fn get_lanes(&self) -> Vec<Lane> {
+        let lanes = maliput_sys::api::ffi::RoadGeometry_GetLanes(self.rg);
+        lanes
+            .into_iter()
+            .map(|l| Lane {
+                lane: unsafe { l.lane.as_ref().expect("") },
+            })
+            .collect::<Vec<Lane>>()
+    }
 }
 
 /// A RoadNetwork.

maliput/tests/road_geometry_test.rs

@@ -57,3 +57,19 @@ fn to_road_position() {
         road_geometry.linear_tolerance(),
     );
 }
+
+#[test]
+fn by_index() {
+    let road_network = common::create_t_shape_road_network();
+    let road_geometry = road_network.road_geometry();
+    let lane_id = String::from("0_0_1");
+    let lane = road_geometry.get_lane(&lane_id);
+    assert_eq!(lane.id(), "0_0_1");
+
+    let lanes = road_geometry.get_lanes();
+    assert_eq!(lanes.len(), 12);
+    let lanes = road_geometry.get_lanes();
+    assert_eq!(lanes.len(), 12);
+    let lanes = road_geometry.get_lanes();
+    assert_eq!(lanes.len(), 12);
+}