Adds some Intersection/IntersectionBook API & bindings. (#78)

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

.gitignore

@@ -4,3 +4,6 @@
 
 # Bazel's output
 */bazel-*
+
+# VSCode
+.vscode

maliput-sys/src/api/api.h

@@ -35,6 +35,8 @@
 #include <vector>
 
 #include <maliput/api/branch_point.h>
+#include <maliput/api/intersection.h>
+#include <maliput/api/intersection_book.h>
 #include <maliput/api/junction.h>
 #include <maliput/api/lane.h>
 #include <maliput/api/lane_data.h>
@@ -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<LaneEnd> BranchPoint_GetDefaultBranch(const BranchPoint& branch_
   return default_branch ? std::make_unique<LaneEnd>(*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<std::vector<MutIntersectionPtr>> IntersectionBook_GetIntersections(IntersectionBook& intersection_book) {
+  const auto intersections_cpp = intersection_book.GetIntersections();
+  std::vector<MutIntersectionPtr> intersections;
+  intersections.reserve(intersections_cpp.size());
+  for (const auto& intersection : intersections_cpp) {
+    intersections.push_back(MutIntersectionPtr{intersection});
+  }
+  return std::make_unique<std::vector<MutIntersectionPtr>>(std::move(intersections));
+}
+
 } // namespace api
 } // namespace maliput

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<LaneEnd>;
+
+        // 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<CxxVector<MutIntersectionPtr>>;
+
     }
     impl UniquePtr<RoadNetwork> {}
     impl UniquePtr<LanePosition> {}

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<LaneEnd> {
-        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<Intersection> {
+        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::<Vec<Intersection>>()
+        }
+    }
+
+    /// Gets the specified Intersection.
+    ///
+    /// ## Arguments
+    ///   * `id` - The id of the Intersection to get.
+    ///
+    /// ## Returns
+    ///   * An Option<Intersection>
+    ///     * 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<Intersection> {
+        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]

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,

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());
+}