Bindings and API for RBounds, HBounds, LanePositionResult and Lane's …

…methods (#47)

* Bindings for lane bounds.
* Bindings for Lane::Contains method.
* Bindings for Lane::index method.
* Adds ToLanePosition/ToSegmentPosition bindings.

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

maliput-sys/src/api/api.h

@@ -102,6 +102,26 @@ std::unique_ptr<InertialPosition> Lane_ToInertialPosition(const Lane& lane, cons
   return std::make_unique<InertialPosition>(lane.ToInertialPosition(lane_position));
 }
 
+std::unique_ptr<RBounds>Lane_lane_bounds(const Lane& lane, rust::f64 s) {
+  return std::make_unique<RBounds>(lane.lane_bounds(s));
+}
+
+std::unique_ptr<RBounds>Lane_segment_bounds(const Lane& lane, rust::f64 s) {
+  return std::make_unique<RBounds>(lane.segment_bounds(s));
+}
+
+std::unique_ptr<HBounds>Lane_elevation_bounds(const Lane& lane, rust::f64 s, rust::f64 r) {
+  return std::make_unique<HBounds>(lane.elevation_bounds(s, r));
+}
+
+std::unique_ptr<LanePositionResult> Lane_ToLanePosition(const Lane& lane, const InertialPosition& inertial_pos) {
+  return std::make_unique<LanePositionResult>(lane.ToLanePosition(inertial_pos));
+}
+
+std::unique_ptr<LanePositionResult> Lane_ToSegmentPosition(const Lane& lane, const InertialPosition& inertial_pos) {
+  return std::make_unique<LanePositionResult>(lane.ToSegmentPosition(inertial_pos));
+}
+
 std::unique_ptr<RoadPosition> RoadPosition_new(const Lane* lane, const LanePosition& pos) {
   return std::make_unique<RoadPosition>(lane, pos);
 }
@@ -130,6 +150,18 @@ double RoadPositionResult_distance(const RoadPositionResult& road_pos_res) {
   return road_pos_res.distance;
 }
 
+std::unique_ptr<LanePosition> LanePositionResult_road_position(const LanePositionResult& lane_pos_res) {
+  return std::make_unique<LanePosition>(lane_pos_res.lane_position);
+}
+
+std::unique_ptr<InertialPosition> LanePositionResult_nearest_position (const LanePositionResult& lane_pos_res) {
+  return std::make_unique<InertialPosition>(lane_pos_res.nearest_position);
+}
+
+double LanePositionResult_distance(const LanePositionResult& lane_pos_res) {
+  return lane_pos_res.distance;
+}
+
 std::unique_ptr<RoadPositionResult> RoadGeometry_ToRoadPosition(const RoadGeometry& road_geometry, const InertialPosition& inertial_pos) {
   return std::make_unique<RoadPositionResult>(road_geometry.ToRoadPosition(inertial_pos));
 }

maliput-sys/src/api/mod.rs

@@ -102,10 +102,17 @@ pub mod ffi {
         type Lane;
         fn to_left(self: &Lane) -> *const Lane;
         fn to_right(self: &Lane) -> *const Lane;
+        fn index(self: &Lane) -> i32;
         fn length(self: &Lane) -> f64;
+        fn Contains(self: &Lane, lane_position: &LanePosition) -> bool;
         fn Lane_id(lane: &Lane) -> String;
+        fn Lane_lane_bounds(lane: &Lane, s: f64) -> UniquePtr<RBounds>;
+        fn Lane_segment_bounds(lane: &Lane, s: f64) -> UniquePtr<RBounds>;
+        fn Lane_elevation_bounds(lane: &Lane, s: f64, r: f64) -> UniquePtr<HBounds>;
         fn Lane_GetOrientation(lane: &Lane, lane_position: &LanePosition) -> UniquePtr<Rotation>;
         fn Lane_ToInertialPosition(lane: &Lane, lane_position: &LanePosition) -> UniquePtr<InertialPosition>;
+        fn Lane_ToLanePosition(lane: &Lane, inertial_position: &InertialPosition) -> UniquePtr<LanePositionResult>;
+        fn Lane_ToSegmentPosition(lane: &Lane, inertial_position: &InertialPosition) -> UniquePtr<LanePositionResult>;
 
         // RoadPosition bindings definitions
         type RoadPosition;
@@ -122,6 +129,12 @@ pub mod ffi {
         fn RoadPositionResult_nearest_position(result: &RoadPositionResult) -> UniquePtr<InertialPosition>;
         fn RoadPositionResult_distance(result: &RoadPositionResult) -> f64;
 
+        // LanePositionResult bindings definitions
+        type LanePositionResult;
+        fn LanePositionResult_road_position(result: &LanePositionResult) -> UniquePtr<LanePosition>;
+        fn LanePositionResult_nearest_position(result: &LanePositionResult) -> UniquePtr<InertialPosition>;
+        fn LanePositionResult_distance(result: &LanePositionResult) -> f64;
+
         // Rotation bindings definitions
         type Rotation;
         fn Rotation_new() -> UniquePtr<Rotation>;
@@ -138,6 +151,15 @@ pub mod ffi {
         fn Rotation_Apply(r: &Rotation, ip: &InertialPosition) -> UniquePtr<InertialPosition>;
         fn Rotation_Reverse(r: &Rotation) -> UniquePtr<Rotation>;
 
+        // RBounds bindings definitions
+        type RBounds;
+        fn min(self: &RBounds) -> f64;
+        fn max(self: &RBounds) -> f64;
+
+        // HBounds bindings definitions
+        type HBounds;
+        fn min(self: &HBounds) -> f64;
+        fn max(self: &HBounds) -> f64;
     }
     impl UniquePtr<RoadNetwork> {}
     impl UniquePtr<LanePosition> {}

maliput/src/api/mod.rs

@@ -428,13 +428,70 @@ impl std::ops::Mul<f64> for InertialPosition {
     }
 }
 
+/// Bounds in the lateral dimension (r component) of a `Lane`-frame, consisting
+/// of a pair of minimum and maximum r value.  The bounds must straddle r = 0,
+/// i.e., the minimum must be <= 0 and the maximum must be >= 0.
+pub struct RBounds {
+    min: f64,
+    max: f64,
+}
+
+impl RBounds {
+    pub fn new(min: f64, max: f64) -> RBounds {
+        RBounds { min, max }
+    }
+    pub fn min(&self) -> f64 {
+        self.min
+    }
+    pub fn max(&self) -> f64 {
+        self.max
+    }
+    pub fn set_min(&mut self, min: f64) {
+        self.min = min;
+    }
+    pub fn set_max(&mut self, max: f64) {
+        self.max = max;
+    }
+}
+
+/// Bounds in the elevation dimension (`h` component) of a `Lane`-frame,
+/// consisting of a pair of minimum and maximum `h` value.  The bounds
+/// must straddle `h = 0`, i.e., the minimum must be `<= 0` and the
+/// maximum must be `>= 0`.
+pub struct HBounds {
+    min: f64,
+    max: f64,
+}
+
+impl HBounds {
+    pub fn new(min: f64, max: f64) -> HBounds {
+        HBounds { min, max }
+    }
+    pub fn min(&self) -> f64 {
+        self.min
+    }
+    pub fn max(&self) -> f64 {
+        self.max
+    }
+    pub fn set_min(&mut self, min: f64) {
+        self.min = min;
+    }
+    pub fn set_max(&mut self, max: f64) {
+        self.max = max;
+    }
+}
+
 /// A maliput::api::Lane
 /// Wrapper around C++ implementation `maliput::api::Lane`.
 pub struct Lane<'a> {
     lane: &'a maliput_sys::api::ffi::Lane,
 }
 
 impl<'a> Lane<'a> {
+    /// Returns the index of this Lane within the Segment which owns it.
+    pub fn index(&self) -> i32 {
+        self.lane.index()
+    }
     /// Get the left lane of the `Lane`.
     pub fn to_left(&self) -> Option<Lane> {
         let lane = self.lane.to_left();
@@ -481,6 +538,63 @@ impl<'a> Lane<'a> {
             ip: maliput_sys::api::ffi::Lane_ToInertialPosition(self.lane, lane_position.lp.as_ref().expect("")),
         }
     }
+    /// Determines the LanePosition corresponding to InertialPosition `inertial_position`.
+    /// The LanePosition is expected to be contained within the lane's boundaries.
+    /// See [Lane::to_segment_position] method.
+    ///
+    /// This method guarantees that its result satisfies the condition that
+    /// `to_inertial_position(result.lane_position)` is within `linear_tolerance()`
+    ///  of `result.nearest_position`.
+    pub fn to_lane_position(&self, inertial_position: &InertialPosition) -> LanePositionResult {
+        let lpr = maliput_sys::api::ffi::Lane_ToLanePosition(self.lane, inertial_position.ip.as_ref().expect(""));
+        LanePositionResult {
+            lane_position: LanePosition {
+                lp: maliput_sys::api::ffi::LanePositionResult_road_position(&lpr),
+            },
+            nearest_position: InertialPosition {
+                ip: maliput_sys::api::ffi::LanePositionResult_nearest_position(&lpr),
+            },
+            distance: maliput_sys::api::ffi::LanePositionResult_distance(&lpr),
+        }
+    }
+    /// Determines the LanePosition corresponding to InertialPosition `inertial_position`.
+    /// The LanePosition is expected to be contained within the segment's boundaries.
+    /// See [Lane::to_lane_position] method.
+    ///
+    /// This method guarantees that its result satisfies the condition that
+    /// `to_inertial_position(result.lane_position)` is within `linear_tolerance()`
+    ///  of `result.nearest_position`.
+    pub fn to_segment_position(&self, inertial_position: &InertialPosition) -> LanePositionResult {
+        let spr = maliput_sys::api::ffi::Lane_ToSegmentPosition(self.lane, inertial_position.ip.as_ref().expect(""));
+        LanePositionResult {
+            lane_position: LanePosition {
+                lp: maliput_sys::api::ffi::LanePositionResult_road_position(&spr),
+            },
+            nearest_position: InertialPosition {
+                ip: maliput_sys::api::ffi::LanePositionResult_nearest_position(&spr),
+            },
+            distance: maliput_sys::api::ffi::LanePositionResult_distance(&spr),
+        }
+    }
+    /// Get the lane bounds of the `Lane` at the given `s`.
+    pub fn lane_bounds(&self, s: f64) -> RBounds {
+        let bounds = maliput_sys::api::ffi::Lane_lane_bounds(self.lane, s);
+        RBounds::new(bounds.min(), bounds.max())
+    }
+    /// Get the segment bounds of the `Lane` at the given `s`.
+    pub fn segment_bounds(&self, s: f64) -> RBounds {
+        let bounds = maliput_sys::api::ffi::Lane_segment_bounds(self.lane, s);
+        RBounds::new(bounds.min(), bounds.max())
+    }
+    /// Get the elevation bounds of the `Lane` at the given `s` and `r`.
+    pub fn elevation_bounds(&self, s: f64, r: f64) -> HBounds {
+        let bounds = maliput_sys::api::ffi::Lane_elevation_bounds(self.lane, s, r);
+        HBounds::new(bounds.min(), bounds.max())
+    }
+    /// Check if the `Lane` contains the given `LanePosition`.
+    pub fn contains(&self, lane_position: &LanePosition) -> bool {
+        self.lane.Contains(lane_position.lp.as_ref().expect(""))
+    }
 }
 
 /// A maliput::api::RoadPosition
@@ -538,6 +652,24 @@ impl RoadPositionResult {
     }
 }
 
+/// Represents the result of a LanePosition query.
+pub struct LanePositionResult {
+    pub lane_position: LanePosition,
+    pub nearest_position: InertialPosition,
+    pub distance: f64,
+}
+
+impl LanePositionResult {
+    /// Create a new `LanePositionResult` with the given `lane_position`, `nearest_position`, and `distance`.
+    pub fn new(lane_position: LanePosition, nearest_position: InertialPosition, distance: f64) -> LanePositionResult {
+        LanePositionResult {
+            lane_position,
+            nearest_position,
+            distance,
+        }
+    }
+}
+
 /// A maliput::api::Rotation
 /// A wrapper around C++ implementation `maliput::api::Rotation`.
 pub struct Rotation {

maliput/tests/lane_test.rs

@@ -40,6 +40,19 @@ fn lane_api() {
     let road_position_result = road_geometry.to_road_position(&inertial_pos);
     assert_eq!(road_position_result.road_position.lane().id(), expected_lane_id);
     let lane = road_position_result.road_position.lane();
+    let index = lane.index();
+    assert_eq!(index, 1);
+    let contains = lane.contains(&road_position_result.road_position.pos());
+    assert!(contains);
+    let lane_bounds = lane.lane_bounds(0.0);
+    assert_eq!(lane_bounds.min(), -1.75);
+    assert_eq!(lane_bounds.max(), 1.75);
+    let segment_bounds = lane.segment_bounds(0.0);
+    assert_eq!(segment_bounds.min(), -5.25);
+    assert_eq!(segment_bounds.max(), 1.75);
+    let elevation_bounds = lane.elevation_bounds(0.0, 0.0);
+    assert_eq!(elevation_bounds.min(), 0.0);
+    assert_eq!(elevation_bounds.max(), 5.0);
     let orientation = lane.get_orientation(&road_position_result.road_position.pos());
     assert_eq!(orientation.roll(), 0.0);
     assert_eq!(orientation.pitch(), 0.0);
@@ -48,6 +61,10 @@ fn lane_api() {
     assert!((ret_inertial_position.x() - inertial_pos.x()).abs() < tolerance);
     assert!((ret_inertial_position.y() - inertial_pos.y()).abs() < tolerance);
     assert!((ret_inertial_position.z() - inertial_pos.z()).abs() < tolerance);
+    let ret_lane_position = lane.to_lane_position(&ret_inertial_position);
+    assert_eq!(ret_lane_position.distance, road_position_result.distance);
+    let ret_segment_position = lane.to_segment_position(&inertial_pos);
+    assert_eq!(ret_segment_position.distance, road_position_result.distance);
     let left_lane = lane.to_left();
     let right_lane = lane.to_right();
     // In TShapeRoad map there is no left lane from current lane.