From 8501885a54ca0b93c2444461c92ba438cf408ee7 Mon Sep 17 00:00:00 2001 From: Franco Cipollone <53065142+francocipollone@users.noreply.github.com> Date: Tue, 26 Mar 2024 12:24:13 -0300 Subject: [PATCH] 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 --- maliput-sys/src/api/api.h | 32 +++++++++ maliput-sys/src/api/mod.rs | 22 +++++++ maliput/src/api/mod.rs | 132 +++++++++++++++++++++++++++++++++++++ maliput/tests/lane_test.rs | 17 +++++ 4 files changed, 203 insertions(+) diff --git a/maliput-sys/src/api/api.h b/maliput-sys/src/api/api.h index f9f25a0..c1455cb 100644 --- a/maliput-sys/src/api/api.h +++ b/maliput-sys/src/api/api.h @@ -102,6 +102,26 @@ std::unique_ptr Lane_ToInertialPosition(const Lane& lane, cons return std::make_unique(lane.ToInertialPosition(lane_position)); } +std::unique_ptrLane_lane_bounds(const Lane& lane, rust::f64 s) { + return std::make_unique(lane.lane_bounds(s)); +} + +std::unique_ptrLane_segment_bounds(const Lane& lane, rust::f64 s) { + return std::make_unique(lane.segment_bounds(s)); +} + +std::unique_ptrLane_elevation_bounds(const Lane& lane, rust::f64 s, rust::f64 r) { + return std::make_unique(lane.elevation_bounds(s, r)); +} + +std::unique_ptr Lane_ToLanePosition(const Lane& lane, const InertialPosition& inertial_pos) { + return std::make_unique(lane.ToLanePosition(inertial_pos)); +} + +std::unique_ptr Lane_ToSegmentPosition(const Lane& lane, const InertialPosition& inertial_pos) { + return std::make_unique(lane.ToSegmentPosition(inertial_pos)); +} + std::unique_ptr RoadPosition_new(const Lane* lane, const LanePosition& pos) { return std::make_unique(lane, pos); } @@ -130,6 +150,18 @@ double RoadPositionResult_distance(const RoadPositionResult& road_pos_res) { return road_pos_res.distance; } +std::unique_ptr LanePositionResult_road_position(const LanePositionResult& lane_pos_res) { + return std::make_unique(lane_pos_res.lane_position); +} + +std::unique_ptr LanePositionResult_nearest_position (const LanePositionResult& lane_pos_res) { + return std::make_unique(lane_pos_res.nearest_position); +} + +double LanePositionResult_distance(const LanePositionResult& lane_pos_res) { + return lane_pos_res.distance; +} + std::unique_ptr RoadGeometry_ToRoadPosition(const RoadGeometry& road_geometry, const InertialPosition& inertial_pos) { return std::make_unique(road_geometry.ToRoadPosition(inertial_pos)); } diff --git a/maliput-sys/src/api/mod.rs b/maliput-sys/src/api/mod.rs index 25a91e5..01f23b7 100644 --- a/maliput-sys/src/api/mod.rs +++ b/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; + fn Lane_segment_bounds(lane: &Lane, s: f64) -> UniquePtr; + fn Lane_elevation_bounds(lane: &Lane, s: f64, r: f64) -> UniquePtr; fn Lane_GetOrientation(lane: &Lane, lane_position: &LanePosition) -> UniquePtr; fn Lane_ToInertialPosition(lane: &Lane, lane_position: &LanePosition) -> UniquePtr; + fn Lane_ToLanePosition(lane: &Lane, inertial_position: &InertialPosition) -> UniquePtr; + fn Lane_ToSegmentPosition(lane: &Lane, inertial_position: &InertialPosition) -> UniquePtr; // RoadPosition bindings definitions type RoadPosition; @@ -122,6 +129,12 @@ pub mod ffi { fn RoadPositionResult_nearest_position(result: &RoadPositionResult) -> UniquePtr; fn RoadPositionResult_distance(result: &RoadPositionResult) -> f64; + // LanePositionResult bindings definitions + type LanePositionResult; + fn LanePositionResult_road_position(result: &LanePositionResult) -> UniquePtr; + fn LanePositionResult_nearest_position(result: &LanePositionResult) -> UniquePtr; + fn LanePositionResult_distance(result: &LanePositionResult) -> f64; + // Rotation bindings definitions type Rotation; fn Rotation_new() -> UniquePtr; @@ -138,6 +151,15 @@ pub mod ffi { fn Rotation_Apply(r: &Rotation, ip: &InertialPosition) -> UniquePtr; fn Rotation_Reverse(r: &Rotation) -> UniquePtr; + // 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 {} impl UniquePtr {} diff --git a/maliput/src/api/mod.rs b/maliput/src/api/mod.rs index 49f5991..f6788ca 100644 --- a/maliput/src/api/mod.rs +++ b/maliput/src/api/mod.rs @@ -428,6 +428,59 @@ impl std::ops::Mul 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> { @@ -435,6 +488,10 @@ pub struct Lane<'a> { } 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 { 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 { diff --git a/maliput/tests/lane_test.rs b/maliput/tests/lane_test.rs index eca8e62..182547e 100644 --- a/maliput/tests/lane_test.rs +++ b/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.