Adds ToRoadPosition bindings and rust api. (#40)

Signed-off-by: Franco Cipollone <franco.c@ekumenlabs.com>

maliput-sys/src/api/api.h

@@ -117,5 +117,9 @@ double RoadPositionResult_distance(const RoadPositionResult& road_pos_res) {
   return road_pos_res.distance;
 }
 
+std::unique_ptr<RoadPositionResult> RoadGeometry_ToRoadPosition(const RoadGeometry& road_geometry, const InertialPosition& inertial_pos) {
+  return std::make_unique<RoadPositionResult>(road_geometry.ToRoadPosition(inertial_pos));
+}
+
 } // namespace api
 } // namespace maliput

maliput-sys/src/api/mod.rs

@@ -46,7 +46,10 @@ pub mod ffi {
         fn linear_tolerance(self: &RoadGeometry) -> f64;
         fn angular_tolerance(self: &RoadGeometry) -> f64;
         fn num_branch_points(self: &RoadGeometry) -> i32;
-
+        fn RoadGeometry_ToRoadPosition(
+            rg: &RoadGeometry,
+            inertial_position: &InertialPosition,
+        ) -> UniquePtr<RoadPositionResult>;
         // LanePosition bindings definitions.
         type LanePosition;
         fn LanePosition_new(s: f64, r: f64, h: f64) -> UniquePtr<LanePosition>;

maliput/src/api/mod.rs

@@ -48,6 +48,18 @@ impl<'a> RoadGeometry<'a> {
     pub fn num_branch_points(&self) -> i32 {
         self.rg.num_branch_points()
     }
+    pub fn to_road_position(&self, inertial_position: &InertialPosition) -> RoadPositionResult {
+        let rpr = maliput_sys::api::ffi::RoadGeometry_ToRoadPosition(self.rg, &inertial_position.ip);
+        RoadPositionResult {
+            road_position: RoadPosition {
+                rp: maliput_sys::api::ffi::RoadPositionResult_road_position(&rpr),
+            },
+            nearest_position: InertialPosition {
+                ip: maliput_sys::api::ffi::RoadPositionResult_nearest_position(&rpr),
+            },
+            distance: maliput_sys::api::ffi::RoadPositionResult_distance(&rpr),
+        }
+    }
 }
 
 /// A RoadNetwork.

maliput/tests/lane_test.rs

@@ -0,0 +1,49 @@
+// 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 to_left_right() {
+    let inertial_pos = maliput::api::InertialPosition::new(5.0, 1.75, 0.0);
+    let expected_lane_id = String::from("0_0_1");
+    let road_network = common::create_t_shape_road_network();
+    let road_geometry = road_network.road_geometry();
+
+    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 left_lane = lane.to_left();
+    let right_lane = lane.to_right();
+    // In TShapeRoad map there is no left lane from current lane.
+    assert!(left_lane.is_none());
+    // In TShapeRoad map there is a right lane from current lane.
+    assert!(right_lane.is_some());
+    assert_eq!(right_lane.unwrap().id(), "0_0_-1");
+}

maliput/tests/road_geometry_test.rs

@@ -0,0 +1,59 @@
+// 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 linear_tolerance() {
+    let road_network = common::create_t_shape_road_network();
+    let road_geometry = road_network.road_geometry();
+    assert_eq!(road_geometry.linear_tolerance(), 0.01);
+}
+
+#[test]
+fn to_road_position() {
+    let expected_nearest_position = maliput::api::InertialPosition::new(5.0, 1.75, 0.0);
+    let expected_lane_position = maliput::api::LanePosition::new(5.0, 0.0, 0.0);
+    let expected_lane_id = String::from("0_0_1");
+    let road_network = common::create_t_shape_road_network();
+    let road_geometry = road_network.road_geometry();
+
+    let road_position_result = road_geometry.to_road_position(&expected_nearest_position);
+    assert_eq!(road_position_result.road_position.lane().id(), expected_lane_id);
+    common::assert_lane_position_equality(
+        &road_position_result.road_position.pos(),
+        &expected_lane_position,
+        road_geometry.linear_tolerance(),
+    );
+    common::assert_inertial_position_equality(
+        &road_position_result.nearest_position,
+        &expected_nearest_position,
+        road_geometry.linear_tolerance(),
+    );
+}