From 8fc9637cdbb6530a3be9e92c3e1159f0fbe8ba2c Mon Sep 17 00:00:00 2001 From: Franco Cipollone <53065142+francocipollone@users.noreply.github.com> Date: Thu, 18 Apr 2024 08:31:01 -0300 Subject: [PATCH] Adds BranchPoint bindings. (#63) Signed-off-by: Franco Cipollone --- maliput-sys/src/api/api.h | 13 ++++ maliput-sys/src/api/mod.rs | 11 +++ maliput/src/api/mod.rs | 116 ++++++++++++++++++++++++++++ maliput/tests/branch_point_tests.rs | 66 ++++++++++++++++ maliput/tests/lane_test.rs | 6 -- 5 files changed, 206 insertions(+), 6 deletions(-) create mode 100644 maliput/tests/branch_point_tests.rs diff --git a/maliput-sys/src/api/api.h b/maliput-sys/src/api/api.h index 875b057..7c9a769 100644 --- a/maliput-sys/src/api/api.h +++ b/maliput-sys/src/api/api.h @@ -199,6 +199,10 @@ const std::vector& RoadGeometry_GetLanes(const RoadGeometry& road_ return lanes; } +const BranchPoint* RoadGeometry_GetBranchPoint(const RoadGeometry& road_geometry, const rust::String& branch_point_id) { + return road_geometry.ById().GetBranchPoint(BranchPointId{std::string(branch_point_id)}); +} + const Segment* RoadGeometry_GetSegment(const RoadGeometry& road_geometry, const rust::String& segment_id) { return road_geometry.ById().GetSegment(SegmentId{std::string(segment_id)}); } @@ -283,5 +287,14 @@ bool LaneEnd_is_start(const LaneEnd& lane_end) { return lane_end.end == LaneEnd::kStart; } +rust::String BranchPoint_id(const BranchPoint& branch_point) { + return branch_point.id().string(); +} + +std::unique_ptr BranchPoint_GetDefaultBranch(const BranchPoint& branch_point, const LaneEnd& end) { + const auto default_branch = branch_point.GetDefaultBranch(end); + return default_branch ? std::make_unique(*default_branch) : nullptr; +} + } // namespace api } // namespace maliput diff --git a/maliput-sys/src/api/mod.rs b/maliput-sys/src/api/mod.rs index 22b15be..a253592 100644 --- a/maliput-sys/src/api/mod.rs +++ b/maliput-sys/src/api/mod.rs @@ -67,6 +67,7 @@ pub mod ffi { fn RoadGeometry_GetLanes(rg: &RoadGeometry) -> &CxxVector; fn RoadGeometry_GetSegment(rg: &RoadGeometry, segment_id: &String) -> *const Segment; fn RoadGeometry_GetJunction(rg: &RoadGeometry, junction_id: &String) -> *const Junction; + fn RoadGeometry_GetBranchPoint(rg: &RoadGeometry, branch_point_id: &String) -> *const BranchPoint; // LanePosition bindings definitions. type LanePosition; fn LanePosition_new(s: f64, r: f64, h: f64) -> UniquePtr; @@ -221,6 +222,16 @@ pub mod ffi { type LaneEndSet; fn size(self: &LaneEndSet) -> i32; fn get(self: &LaneEndSet, index: i32) -> &LaneEnd; + + // BranchPoint bindings definitions + type BranchPoint; + fn BranchPoint_id(branch_point: &BranchPoint) -> String; + fn road_geometry(self: &BranchPoint) -> *const RoadGeometry; + fn GetConfluentBranches(self: &BranchPoint, end: &LaneEnd) -> *const LaneEndSet; + fn GetOngoingBranches(self: &BranchPoint, end: &LaneEnd) -> *const LaneEndSet; + fn GetASide(self: &BranchPoint) -> *const LaneEndSet; + fn GetBSide(self: &BranchPoint) -> *const LaneEndSet; + fn BranchPoint_GetDefaultBranch(branch_point: &BranchPoint, end: &LaneEnd) -> UniquePtr; } impl UniquePtr {} impl UniquePtr {} diff --git a/maliput/src/api/mod.rs b/maliput/src/api/mod.rs index 938ae77..7b398ca 100644 --- a/maliput/src/api/mod.rs +++ b/maliput/src/api/mod.rs @@ -28,6 +28,8 @@ // OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +use maliput_sys::api::ffi::BranchPoint_GetDefaultBranch; + use crate::math::Matrix3; use crate::math::Quaternion; use crate::math::RollPitchYaw; @@ -165,6 +167,16 @@ impl<'a> RoadGeometry<'a> { } } } + /// Get the branch point matching given `branch_point_id`. + pub fn get_branch_point(&self, branch_point_id: &String) -> BranchPoint { + unsafe { + BranchPoint { + branch_point: maliput_sys::api::ffi::RoadGeometry_GetBranchPoint(self.rg, branch_point_id) + .as_ref() + .expect("Underlying BranchPoint is null"), + } + } + } } /// A RoadNetwork. @@ -1010,6 +1022,110 @@ impl<'a> LaneEndSet<'a> { } } +/// A BranchPoint is a node in the network of a RoadGeometry at which +/// Lanes connect to one another. A BranchPoint is a collection of LaneEnds +/// specifying the Lanes (and, in particular, which ends of the Lanes) are +/// connected at the BranchPoint. +/// +/// LaneEnds participating in a BranchPoint are grouped into two sets, +/// arbitrarily named "A-side" and "B-side". LaneEnds on the same "side" +/// have coincident into-the-lane tangent vectors, which are anti-parallel +/// to those of LaneEnds on the other side. +pub struct BranchPoint<'a> { + branch_point: &'a maliput_sys::api::ffi::BranchPoint, +} + +impl<'a> BranchPoint<'a> { + /// Get the id of the `BranchPoint` as a string. + pub fn id(&self) -> String { + maliput_sys::api::ffi::BranchPoint_id(self.branch_point) + } + pub fn road_geometry(&self) -> RoadGeometry { + unsafe { + RoadGeometry { + rg: self.branch_point.road_geometry().as_ref().expect(""), + } + } + } + /// Returns the set of LaneEnds on the same side as the given LaneEnd. + /// E.g: For a T-junction, this would return the set of LaneEnds on the merging side. + pub fn get_confluent_branches(&self, end: &LaneEnd) -> LaneEndSet { + let lane_end_set_ptr = self.branch_point.GetConfluentBranches( + BranchPoint::from_lane_end_to_ffi(end) + .as_ref() + .expect("Underlying LaneEnd is null"), + ); + LaneEndSet { + lane_end_set: unsafe { lane_end_set_ptr.as_ref().expect("Underlying LaneEndSet is null") }, + } + } + /// Returns the set of LaneEnds on the opposite side as the given LaneEnd. + /// E.g: For a T-junction, this would return the LaneEnds which end flows into the junction. + pub fn get_ongoing_branches(&self, end: &LaneEnd) -> LaneEndSet { + let lane_end_set_ptr = self.branch_point.GetOngoingBranches( + BranchPoint::from_lane_end_to_ffi(end) + .as_ref() + .expect("Underlying LaneEnd is null"), + ); + LaneEndSet { + lane_end_set: unsafe { lane_end_set_ptr.as_ref().expect("Underlying LaneEndSet is null") }, + } + } + /// Returns the default ongoing branch (if any) for the given `end`. + /// This typically represents what would be considered "continuing + /// through-traffic" from `end` (e.g., as opposed to a branch executing + /// a turn). + /// + /// If `end` has no default-branch at this BranchPoint, the return + /// value will be None. + pub fn get_default_branch(&self, end: &LaneEnd) -> Option { + let lane_end = BranchPoint_GetDefaultBranch( + self.branch_point, + BranchPoint::from_lane_end_to_ffi(end) + .as_ref() + .expect("Underlying LaneEnd is null"), + ); + 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 })), + } + } + } + } + /// Returns the set of LaneEnds grouped together on the "A-side". + pub fn get_a_side(&self) -> LaneEndSet { + let lane_end_set_ptr = self.branch_point.GetASide(); + LaneEndSet { + lane_end_set: unsafe { lane_end_set_ptr.as_ref().expect("Underlying LaneEndSet is null") }, + } + } + /// Returns the set of LaneEnds grouped together on the "B-side". + pub fn get_b_side(&self) -> LaneEndSet { + let lane_end_set_ptr = self.branch_point.GetBSide(); + LaneEndSet { + lane_end_set: unsafe { lane_end_set_ptr.as_ref().expect("Underlying LaneEndSet is null") }, + } + } + /// Convert LaneEnd enum to LaneEnd ffi. + fn from_lane_end_to_ffi(end: &LaneEnd) -> cxx::UniquePtr { + match end { + LaneEnd::Start(lane) => unsafe { maliput_sys::api::ffi::LaneEnd_new(lane.lane, true) }, + LaneEnd::Finish(lane) => unsafe { maliput_sys::api::ffi::LaneEnd_new(lane.lane, false) }, + } + } +} + mod tests { mod lane_position { #[test] diff --git a/maliput/tests/branch_point_tests.rs b/maliput/tests/branch_point_tests.rs new file mode 100644 index 0000000..5f68b44 --- /dev/null +++ b/maliput/tests/branch_point_tests.rs @@ -0,0 +1,66 @@ +// BSD 3-Clause License +// +// Copyright (c) 2024, Woven by Toyota. +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, this +// list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// +// * Neither the name of the copyright holder nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +// DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +// FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +// DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +// SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +// OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +mod common; + +#[test] +fn branch_point_api() { + let road_network = common::create_t_shape_road_network(); + let road_geometry = road_network.road_geometry(); + let branch_point_id = String::from("2"); + let branch_point = road_geometry.get_branch_point(&branch_point_id); + assert_eq!(branch_point.id(), branch_point_id); + assert_eq!(branch_point.road_geometry().id(), road_geometry.id()); + // Testing that the api works. The actual values are not important, they are tested in the + // cpp tests. + let lane_end_set = branch_point.get_a_side(); + assert_eq!(lane_end_set.size(), 1); + let lane_end = lane_end_set.get(0); + let confluent_branches = branch_point.get_confluent_branches(&lane_end); + assert_eq!(confluent_branches.size(), 1); + let ongoing_branches = branch_point.get_ongoing_branches(&lane_end); + assert_eq!(ongoing_branches.size(), 2); + let lane_end_set = branch_point.get_b_side(); + assert_eq!(lane_end_set.size(), 2); + + // Test default branch. + let default_lane_end = branch_point.get_default_branch(&lane_end); + assert!(default_lane_end.is_some()); + match default_lane_end.unwrap() { + maliput::api::LaneEnd::Start(l) | maliput::api::LaneEnd::Finish(l) => assert_eq!(l.id(), "9_0_-1"), + } + + // Test that the default branch is None when the lane_end point to the other side. + let the_other_lane_end = match lane_end { + maliput::api::LaneEnd::Start(l) => maliput::api::LaneEnd::Finish(l), + maliput::api::LaneEnd::Finish(l) => maliput::api::LaneEnd::Start(l), + }; + let default_branch = branch_point.get_default_branch(&the_other_lane_end); + assert!(default_branch.is_none()); +} diff --git a/maliput/tests/lane_test.rs b/maliput/tests/lane_test.rs index f87d9fe..2b70e27 100644 --- a/maliput/tests/lane_test.rs +++ b/maliput/tests/lane_test.rs @@ -94,9 +94,3 @@ fn lane_end_test() { maliput::api::LaneEnd::Finish(lane) => assert_eq!(lane.id(), lane_id), } } - -#[test] -fn lane_end_set_test() { - // TODO: Implement this test once BranchPoint is implemented as - // the latter provides methods to get LaneEndSet. -}