Completes Lane API bindings. (#64)

Signed-off-by: Franco Cipollone <franco.c@ekumenlabs.com>
maliput · Apr 23, 2024 · b2464eb · b2464eb
1 parent 3a20a96
commit b2464eb
Show file tree

Hide file tree

Showing 4 changed files with 168 additions and 2 deletions.
diff --git a/maliput-sys/src/api/api.h b/maliput-sys/src/api/api.h
@@ -134,6 +134,23 @@ std::unique_ptr<LanePositionResult> Lane_ToSegmentPosition(const Lane& lane, con
   return std::make_unique<LanePositionResult>(lane.ToSegmentPosition(inertial_pos));
 }
 
+const BranchPoint* Lane_GetBranchPoint(const Lane& lane, bool start) {
+  return lane.GetBranchPoint(start ? LaneEnd::kStart : LaneEnd::kFinish);
+}
+
+const LaneEndSet* Lane_GetConfluentBranches(const Lane& lane, bool start) {
+  return lane.GetConfluentBranches(start ? LaneEnd::kStart : LaneEnd::kFinish);
+}
+
+const LaneEndSet* Lane_GetOngoingBranches(const Lane& lane, bool start) {
+  return lane.GetOngoingBranches(start ? LaneEnd::kStart : LaneEnd::kFinish);
+}
+
+std::unique_ptr<LaneEnd> Lane_GetDefaultBranch(const Lane& lane, bool start) {
+  const auto default_branch = lane.GetDefaultBranch(start ? LaneEnd::kStart : LaneEnd::kFinish);
+  return default_branch ? std::make_unique<LaneEnd>(*default_branch) : nullptr;
+}
+
 std::unique_ptr<RoadPosition> RoadPosition_new(const Lane* lane, const LanePosition& pos) {
   return std::make_unique<RoadPosition>(lane, pos);
 }

diff --git a/maliput-sys/src/api/mod.rs b/maliput-sys/src/api/mod.rs
@@ -118,6 +118,10 @@ pub mod ffi {
         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>;
+        fn Lane_GetBranchPoint(lane: &Lane, start: bool) -> *const BranchPoint;
+        fn Lane_GetConfluentBranches(lane: &Lane, start: bool) -> *const LaneEndSet;
+        fn Lane_GetOngoingBranches(lane: &Lane, start: bool) -> *const LaneEndSet;
+        fn Lane_GetDefaultBranch(lane: &Lane, start: bool) -> UniquePtr<LaneEnd>;
 
         // Segment bindings definitions
         type Segment;

diff --git a/maliput/src/api/mod.rs b/maliput/src/api/mod.rs
@@ -628,12 +628,95 @@ impl<'a> Lane<'a> {
         let bounds = maliput_sys::api::ffi::Lane_elevation_bounds(self.lane, s, r);
         HBounds::new(bounds.min(), bounds.max())
     }
+    /// Returns the lane's BranchPoint for the specified end.
+    pub fn get_branch_point(&self, end: &LaneEnd) -> BranchPoint {
+        assert! {
+            end == &LaneEnd::Start(self.clone()) || end == &LaneEnd::Finish(self.clone()),
+            "LaneEnd must be an end of this lane {:?}",
+            end
+        }
+        BranchPoint {
+            branch_point: unsafe {
+                maliput_sys::api::ffi::Lane_GetBranchPoint(self.lane, end == &LaneEnd::Start(self.clone()))
+                    .as_ref()
+                    .expect("Underlying BranchPoint is null")
+            },
+        }
+    }
+    /// Returns the set of LaneEnd's which connect with this lane on the
+    /// same side of the BranchPoint at `end`. At a minimum,
+    /// this set will include this Lane.
+    pub fn get_confluent_branches(&self, end: &LaneEnd) -> LaneEndSet {
+        assert! {
+            end == &LaneEnd::Start(self.clone()) || end == &LaneEnd::Finish(self.clone()),
+            "LaneEnd must be an end of this lane {:?}",
+            end
+        }
+        LaneEndSet {
+            lane_end_set: unsafe {
+                maliput_sys::api::ffi::Lane_GetConfluentBranches(self.lane, end == &LaneEnd::Start(self.clone()))
+                    .as_ref()
+                    .expect("Underlying LaneEndSet is null")
+            },
+        }
+    }
+    /// Returns the set of LaneEnd's which continue onward from this lane at the
+    /// BranchPoint at `end`.
+    pub fn get_ongoing_branches(&self, end: &LaneEnd) -> LaneEndSet {
+        assert! {
+            end == &LaneEnd::Start(self.clone()) || end == &LaneEnd::Finish(self.clone()),
+            "LaneEnd must be an end of this lane {:?}",
+            end
+        }
+        LaneEndSet {
+            lane_end_set: unsafe {
+                maliput_sys::api::ffi::Lane_GetOngoingBranches(self.lane, end == &LaneEnd::Start(self.clone()))
+                    .as_ref()
+                    .expect("Underlying LaneEndSet is null")
+            },
+        }
+    }
+    /// Returns the default ongoing LaneEnd connected at `end`,
+    /// or None if no default branch has been established.
+    pub fn get_default_branch(&self, end: &LaneEnd) -> Option<LaneEnd> {
+        assert! {
+            end == &LaneEnd::Start(self.clone()) || end == &LaneEnd::Finish(self.clone()),
+            "LaneEnd must be an end of this lane {:?}",
+            end
+        }
+        let lane_end = maliput_sys::api::ffi::Lane_GetDefaultBranch(self.lane, end == &LaneEnd::Start(self.clone()));
+        match lane_end.is_null() {
+            true => None,
+            false => {
+                let lane_end_ref: &maliput_sys::api::ffi::LaneEnd =
+                    lane_end.as_ref().expect("Underlying LaneEnd is null");
+                let is_start = maliput_sys::api::ffi::LaneEnd_is_start(lane_end_ref);
+                let lane_ref = unsafe {
+                    maliput_sys::api::ffi::LaneEnd_lane(lane_end_ref)
+                        .as_ref()
+                        .expect("Underlying LaneEnd is null")
+                };
+                match is_start {
+                    true => Some(LaneEnd::Start(Lane { lane: lane_ref })),
+                    false => Some(LaneEnd::Finish(Lane { lane: lane_ref })),
+                }
+            }
+        }
+    }
     /// 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(""))
     }
 }
 
+/// Copy trait for Lane.
+/// A reference to the Lane is copied.
+impl Clone for Lane<'_> {
+    fn clone(&self) -> Self {
+        Lane { lane: self.lane }
+    }
+}
+
 /// A Segment represents a bundle of adjacent Lanes which share a
 /// continuously traversable road surface. Every [LanePosition] on a
 /// given [Lane] of a Segment has a corresponding [LanePosition] on each
@@ -994,6 +1077,51 @@ pub enum LaneEnd<'a> {
     Finish(Lane<'a>),
 }
 
+impl LaneEnd<'_> {
+    /// Get the Lane of the `LaneEnd`.
+    pub fn lane(&self) -> &Lane {
+        match self {
+            LaneEnd::Start(lane) => lane,
+            LaneEnd::Finish(lane) => lane,
+        }
+    }
+}
+
+impl PartialEq for LaneEnd<'_> {
+    fn eq(&self, other: &Self) -> bool {
+        match self {
+            LaneEnd::Start(lane) => match other {
+                LaneEnd::Start(other_lane) => lane.id() == other_lane.id(),
+                _ => false,
+            },
+            LaneEnd::Finish(lane) => match other {
+                LaneEnd::Finish(other_lane) => lane.id() == other_lane.id(),
+                _ => false,
+            },
+        }
+    }
+}
+
+impl Eq for LaneEnd<'_> {}
+
+impl std::fmt::Display for LaneEnd<'_> {
+    fn fmt(&self, f: &mut std::fmt::Formatter) -> std::fmt::Result {
+        match self {
+            LaneEnd::Start(lane) => write!(f, "LaneEnd::Start({})", lane.id()),
+            LaneEnd::Finish(lane) => write!(f, "LaneEnd::Finish({})", lane.id()),
+        }
+    }
+}
+
+impl std::fmt::Debug for LaneEnd<'_> {
+    fn fmt(&self, f: &mut std::fmt::Formatter) -> std::fmt::Result {
+        match self {
+            LaneEnd::Start(lane) => write!(f, "LaneEnd::Start({})", lane.id()),
+            LaneEnd::Finish(lane) => write!(f, "LaneEnd::Finish({})", lane.id()),
+        }
+    }
+}
+
 /// A set of LaneEnds.
 pub struct LaneEndSet<'a> {
     lane_end_set: &'a maliput_sys::api::ffi::LaneEndSet,

diff --git a/maliput/tests/lane_test.rs b/maliput/tests/lane_test.rs
@@ -75,20 +75,37 @@ fn lane_api() {
     let segment = lane.segment();
     let expected_segment_id = String::from("0_0");
     assert_eq!(segment.id(), expected_segment_id);
+    let cloned_lane = lane.clone();
+    assert_eq!(lane.id(), cloned_lane.id());
+
+    let lane_end = maliput::api::LaneEnd::Start(lane.clone());
+    let branch_point = lane.get_branch_point(&lane_end);
+    assert_eq!(branch_point.id(), "3");
+    let confluent_branches = lane.get_confluent_branches(&lane_end);
+    assert_eq!(confluent_branches.size(), 1);
+    let ongoing_branches = lane.get_ongoing_branches(&lane_end);
+    assert_eq!(ongoing_branches.size(), 0);
+    let default_branch = lane.get_default_branch(&lane_end);
+    assert_eq!(
+        default_branch.is_none(),
+        branch_point.get_default_branch(&lane_end).is_none()
+    );
 }
 
 #[test]
-fn lane_end_test() {
+fn lane_end_api_test() {
     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_end_start = maliput::api::LaneEnd::Start(road_geometry.get_lane(&lane_id));
+    let lane_end_end = maliput::api::LaneEnd::Finish(road_geometry.get_lane(&lane_id));
+    assert_eq!(&lane_end_start, &lane_end_start);
+    assert_ne!(&lane_end_start, &lane_end_end);
     match lane_end_start {
         maliput::api::LaneEnd::Start(lane) => assert_eq!(lane.id(), lane_id),
         maliput::api::LaneEnd::Finish(_) => panic!("Expected Start, got Finish"),
     }
-    let lane_end_end = maliput::api::LaneEnd::Finish(road_geometry.get_lane(&lane_id));
     match lane_end_end {
         maliput::api::LaneEnd::Start(_) => panic!("Expected Finish, got Start"),
         maliput::api::LaneEnd::Finish(lane) => assert_eq!(lane.id(), lane_id),