diff --git a/.gitignore b/.gitignore index 934d184..8241006 100644 --- a/.gitignore +++ b/.gitignore @@ -4,3 +4,6 @@ # Bazel's output */bazel-* + +# VSCode +.vscode diff --git a/maliput-sys/src/api/api.h b/maliput-sys/src/api/api.h index 8d4a61e..8627455 100644 --- a/maliput-sys/src/api/api.h +++ b/maliput-sys/src/api/api.h @@ -35,6 +35,8 @@ #include #include +#include +#include #include #include #include @@ -51,6 +53,7 @@ namespace maliput { namespace api { struct ConstLanePtr; +struct MutIntersectionPtr; /// Creates a new maliput::api::LanePosition. /// Forwads to maliput::api::LanePosition(double s, double r, double h) constructor. @@ -317,5 +320,24 @@ std::unique_ptr BranchPoint_GetDefaultBranch(const BranchPoint& branch_ return default_branch ? std::make_unique(*default_branch) : nullptr; } +rust::String Intersection_id(const Intersection& intersection) { + return intersection.id().string(); +} + +MutIntersectionPtr IntersectionBook_GetIntersection( IntersectionBook& intersection_book, const rust::String& intersection_id) { + return {intersection_book.GetIntersection(Intersection::Id{std::string(intersection_id)})}; +} + +// IntersectionBook_GetIntersections +std::unique_ptr> IntersectionBook_GetIntersections(IntersectionBook& intersection_book) { + const auto intersections_cpp = intersection_book.GetIntersections(); + std::vector intersections; + intersections.reserve(intersections_cpp.size()); + for (const auto& intersection : intersections_cpp) { + intersections.push_back(MutIntersectionPtr{intersection}); + } + return std::make_unique>(std::move(intersections)); +} + } // namespace api } // namespace maliput diff --git a/maliput-sys/src/api/mod.rs b/maliput-sys/src/api/mod.rs index 2b20a51..cb1e435 100644 --- a/maliput-sys/src/api/mod.rs +++ b/maliput-sys/src/api/mod.rs @@ -35,6 +35,11 @@ pub mod ffi { struct ConstLanePtr { pub lane: *const Lane, } + /// Shared struct for `Intersection` pointers. + /// This is needed because `*mut Intersection` can't be used directly in the CxxVector collection. + struct MutIntersectionPtr { + pub intersection: *mut Intersection, + } unsafe extern "C++" { include!("api/api.h"); @@ -49,12 +54,14 @@ pub mod ffi { type RollPitchYaw = crate::math::ffi::RollPitchYaw; #[namespace = "maliput::api"] - type RoadNetwork; - type RoadGeometry; // RoadNetwork bindings definitions. + type RoadNetwork; fn road_geometry(self: &RoadNetwork) -> *const RoadGeometry; - fn RoadGeometry_id(road_geometry: &RoadGeometry) -> String; + fn intersection_book(self: Pin<&mut RoadNetwork>) -> *mut IntersectionBook; + // RoadGeometry bindings definitions. + type RoadGeometry; + fn RoadGeometry_id(road_geometry: &RoadGeometry) -> String; fn num_junctions(self: &RoadGeometry) -> i32; fn linear_tolerance(self: &RoadGeometry) -> f64; fn angular_tolerance(self: &RoadGeometry) -> f64; @@ -243,6 +250,18 @@ pub mod ffi { fn GetASide(self: &BranchPoint) -> *const LaneEndSet; fn GetBSide(self: &BranchPoint) -> *const LaneEndSet; fn BranchPoint_GetDefaultBranch(branch_point: &BranchPoint, end: &LaneEnd) -> UniquePtr; + + // Intersection bindings definitions + type Intersection; + fn Intersection_id(intersection: &Intersection) -> String; + + // IntersectionBook bindings definitions + type IntersectionBook; + fn IntersectionBook_GetIntersection(book: Pin<&mut IntersectionBook>, id: &String) -> MutIntersectionPtr; + fn IntersectionBook_GetIntersections( + book: Pin<&mut IntersectionBook>, + ) -> UniquePtr>; + } impl UniquePtr {} impl UniquePtr {} diff --git a/maliput/src/api/mod.rs b/maliput/src/api/mod.rs index 36d511a..dd9c052 100644 --- a/maliput/src/api/mod.rs +++ b/maliput/src/api/mod.rs @@ -28,8 +28,6 @@ // 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; @@ -229,6 +227,21 @@ impl RoadNetwork { } } } + /// Get the `IntersectionBook` of the `RoadNetwork`. + pub fn intersection_book(&mut self) -> IntersectionBook { + let intersection_book_ffi = self + .rn + .as_mut() + .expect("Underlying RoadNetwork is null") + .intersection_book(); + IntersectionBook { + intersection_book: unsafe { + intersection_book_ffi + .as_mut() + .expect("Underlying IntersectionBook is null") + }, + } + } } /// A Lane Position. @@ -1245,7 +1258,7 @@ impl<'a> BranchPoint<'a> { /// 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( + let lane_end = maliput_sys::api::ffi::BranchPoint_GetDefaultBranch( self.branch_point, BranchPoint::from_lane_end_to_ffi(end) .as_ref() @@ -1292,6 +1305,69 @@ impl<'a> BranchPoint<'a> { } } +/// An abstract convenience class that aggregates information pertaining to an +/// intersection. Its primary purpose is to serve as a single source of this +/// information and to remove the need for users to query numerous disparate +/// data structures and state providers. +pub struct Intersection<'a> { + intersection: &'a mut maliput_sys::api::ffi::Intersection, +} + +impl<'a> Intersection<'a> { + /// Get the id of the `Intersection` as a string. + pub fn id(&self) -> String { + maliput_sys::api::ffi::Intersection_id(self.intersection) + } +} + +/// A book of Intersections. +pub struct IntersectionBook<'a> { + intersection_book: &'a mut maliput_sys::api::ffi::IntersectionBook, +} + +impl<'a> IntersectionBook<'a> { + /// Gets a list of all Intersections within this book. + pub fn get_intersections(&mut self) -> Vec { + let book_pin = unsafe { std::pin::Pin::new_unchecked(&mut *self.intersection_book) }; + let intersections_cpp = maliput_sys::api::ffi::IntersectionBook_GetIntersections(book_pin); + unsafe { + intersections_cpp + .into_iter() + .map(|intersection| Intersection { + intersection: intersection + .intersection + .as_mut() + .expect("Underlying Intersection is null"), + }) + .collect::>() + } + } + + /// Gets the specified Intersection. + /// + /// ## Arguments + /// * `id` - The id of the Intersection to get. + /// + /// ## Returns + /// * An Option + /// * Some(Intersection) - The Intersection with the specified id. + /// * None - If the Intersection with the specified id does not exist. + pub fn get_intersection(&mut self, id: &str) -> Option { + let book_pin = unsafe { std::pin::Pin::new_unchecked(&mut *self.intersection_book) }; + let intersection_option = unsafe { + maliput_sys::api::ffi::IntersectionBook_GetIntersection(book_pin, &String::from(id)) + .intersection + .as_mut() + }; + match &intersection_option { + None => None, + Some(_) => Some(Intersection { + intersection: intersection_option.expect("Underlying Intersection is null"), + }), + } + } +} + mod tests { mod lane_position { #[test] diff --git a/maliput/tests/common/mod.rs b/maliput/tests/common/mod.rs index 73c59b9..679069b 100644 --- a/maliput/tests/common/mod.rs +++ b/maliput/tests/common/mod.rs @@ -28,9 +28,10 @@ // 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::api::RoadNetwork; +use maliput::{api::RoadNetwork, ResourceManager}; use std::collections::HashMap; +#[allow(dead_code)] pub fn create_t_shape_road_network() -> RoadNetwork { // Get location of odr resources let package_location = std::env::var("CARGO_MANIFEST_DIR").unwrap(); @@ -44,6 +45,28 @@ pub fn create_t_shape_road_network() -> RoadNetwork { RoadNetwork::new("maliput_malidrive", &road_network_properties) } +#[allow(dead_code)] +pub fn create_t_shape_road_network_with_books() -> RoadNetwork { + let rm = ResourceManager::new(); + let t_shape_xodr_path = rm + .get_resource_path_by_name("maliput_malidrive", "TShapeRoad.xodr") + .unwrap(); + let t_shape_books_path = rm + .get_resource_path_by_name("maliput_malidrive", "TShapeRoad.yaml") + .unwrap(); + + let road_network_properties = HashMap::from([ + ("road_geometry_id", "my_rg_from_rust"), + ("opendrive_file", t_shape_xodr_path.to_str().unwrap()), + ("road_rule_book", t_shape_books_path.to_str().unwrap()), + ("traffic_light_book", t_shape_books_path.to_str().unwrap()), + ("phase_ring_book", t_shape_books_path.to_str().unwrap()), + ("intersection_book", t_shape_books_path.to_str().unwrap()), + ("linear_tolerance", "0.01"), + ]); + RoadNetwork::new("maliput_malidrive", &road_network_properties) +} + #[allow(dead_code)] pub fn assert_inertial_position_equality( left: &maliput::api::InertialPosition, diff --git a/maliput/tests/intesection_tests.rs b/maliput/tests/intesection_tests.rs new file mode 100644 index 0000000..09656e3 --- /dev/null +++ b/maliput/tests/intesection_tests.rs @@ -0,0 +1,53 @@ +// 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 intersection_api() { + let mut road_network = common::create_t_shape_road_network_with_books(); + let expected_intersection_id = String::from("TIntersection"); + + let road_geometry = road_network.road_geometry(); + assert_eq!(road_geometry.id(), "my_rg_from_rust"); + + let mut book = road_network.intersection_book(); + let intersections = book.get_intersections(); + assert_eq!(intersections.len(), 1); + intersections.iter().for_each(|intersection| { + assert_eq!(intersection.id(), expected_intersection_id); + }); + + let intersection = book.get_intersection(&expected_intersection_id); + assert!(intersection.is_some()); + assert_eq!(intersection.expect("").id(), expected_intersection_id); + + let wrong_intersection = book.get_intersection("wrong_id"); + assert!(wrong_intersection.is_none()); +}