diff --git a/maliput-sys/src/api/api.h b/maliput-sys/src/api/api.h index bcfc6c2..4fa803c 100644 --- a/maliput-sys/src/api/api.h +++ b/maliput-sys/src/api/api.h @@ -33,6 +33,7 @@ #include #include +#include #include #include #include @@ -84,6 +85,37 @@ bool InertialPosition_operator_eq(const InertialPosition& lhs, const InertialPos return lhs == rhs; } +rust::String Lane_id(const Lane& lane) { + return lane.id().string(); +} + +std::unique_ptr RoadPosition_new(const Lane* lane, const LanePosition& pos) { + return std::make_unique(lane, pos); +} + +std::unique_ptr RoadPosition_ToInertialPosition(const RoadPosition& road_pos) { + return std::make_unique(road_pos.ToInertialPosition()); +} + +const Lane* RoadPosition_lane(const RoadPosition& road_pos) { + return road_pos.lane; +} + +std::unique_ptr RoadPosition_pos(const RoadPosition& road_pos) { + return std::make_unique(road_pos.pos); +} + +std::unique_ptr RoadPositionResult_road_position(const RoadPositionResult& road_pos_res) { + return std::make_unique(road_pos_res.road_position); +} + +std::unique_ptr RoadPositionResult_nearest_position (const RoadPositionResult& road_pos_res) { + return std::make_unique(road_pos_res.nearest_position); +} + +double RoadPositionResult_distance(const RoadPositionResult& road_pos_res) { + return road_pos_res.distance; +} } // namespace api } // namespace maliput diff --git a/maliput-sys/src/api/mod.rs b/maliput-sys/src/api/mod.rs index a35be90..9ef21fe 100644 --- a/maliput-sys/src/api/mod.rs +++ b/maliput-sys/src/api/mod.rs @@ -81,6 +81,27 @@ pub mod ffi { -> UniquePtr; fn InertialPosition_operator_mul_scalar(lhs: &InertialPosition, scalar: f64) -> UniquePtr; + // Lane bindings definitions + type Lane; + fn to_left(self: &Lane) -> *const Lane; + fn to_right(self: &Lane) -> *const Lane; + fn length(self: &Lane) -> f64; + fn Lane_id(lane: &Lane) -> String; + + // RoadPosition bindings definitions + type RoadPosition; + /// # Safety + /// This function is unsafe because it dereferences `lane` pointers. + unsafe fn RoadPosition_new(lane: *const Lane, lane_pos: &LanePosition) -> UniquePtr; + fn RoadPosition_ToInertialPosition(road_position: &RoadPosition) -> UniquePtr; + fn RoadPosition_lane(road_position: &RoadPosition) -> *const Lane; + fn RoadPosition_pos(road_position: &RoadPosition) -> UniquePtr; + + // RoadPositionResult bindings definitions + type RoadPositionResult; + fn RoadPositionResult_road_position(result: &RoadPositionResult) -> UniquePtr; + fn RoadPositionResult_nearest_position(result: &RoadPositionResult) -> UniquePtr; + fn RoadPositionResult_distance(result: &RoadPositionResult) -> f64; } impl UniquePtr {} impl UniquePtr {} diff --git a/maliput/src/api/mod.rs b/maliput/src/api/mod.rs index f8a3bd4..a90e187 100644 --- a/maliput/src/api/mod.rs +++ b/maliput/src/api/mod.rs @@ -335,6 +335,104 @@ impl std::ops::Mul for InertialPosition { } } +/// A maliput::api::Lane +/// Wrapper around C++ implementation `maliput::api::Lane`. +pub struct Lane<'a> { + lane: &'a maliput_sys::api::ffi::Lane, +} + +impl<'a> Lane<'a> { + /// Get the left lane of the `Lane`. + pub fn to_left(&self) -> Option { + let lane = self.lane.to_left(); + if lane.is_null() { + None + } else { + unsafe { + Some(Lane { + lane: lane.as_ref().expect(""), + }) + } + } + } + /// Get the right lane of the `Lane`. + pub fn to_right(&self) -> Option { + let lane = self.lane.to_right(); + if lane.is_null() { + None + } else { + unsafe { + Some(Lane { + lane: lane.as_ref().expect(""), + }) + } + } + } + /// Get the length of the `Lane`. + pub fn length(&self) -> f64 { + self.lane.length() + } + /// Get the id of the `Lane` as a string. + pub fn id(&self) -> String { + maliput_sys::api::ffi::Lane_id(self.lane) + } +} + +/// A maliput::api::RoadPosition +/// Wrapper around C++ implementation `maliput::api::RoadPosition`. +pub struct RoadPosition { + rp: cxx::UniquePtr, +} + +impl RoadPosition { + /// Create a new `RoadPosition` with the given `lane` and `lane_pos`. + pub fn new(lane: &Lane, lane_pos: &LanePosition) -> RoadPosition { + unsafe { + RoadPosition { + rp: maliput_sys::api::ffi::RoadPosition_new(lane.lane, &lane_pos.lp), + } + } + } + /// Get the inertial position of the `RoadPosition` via doing a Lane::ToInertialPosition query call. + pub fn to_inertial_position(&self) -> InertialPosition { + InertialPosition { + ip: maliput_sys::api::ffi::RoadPosition_ToInertialPosition(&self.rp), + } + } + /// Get the lane of the `RoadPosition`. + pub fn lane(&self) -> Lane { + unsafe { + Lane { + lane: maliput_sys::api::ffi::RoadPosition_lane(&self.rp).as_ref().expect(""), + } + } + } + /// Get the lane position of the `RoadPosition`. + pub fn pos(&self) -> LanePosition { + LanePosition { + lp: maliput_sys::api::ffi::RoadPosition_pos(&self.rp), + } + } +} + +/// Represents the result of a RoadPosition query. +pub struct RoadPositionResult { + pub road_position: RoadPosition, + pub nearest_position: InertialPosition, + pub distance: f64, +} + +impl RoadPositionResult { + /// Create a new `RoadPositionResult` with the given `road_position`, `nearest_position`, and `distance`. + pub fn new(road_position: RoadPosition, nearest_position: InertialPosition, distance: f64) -> RoadPositionResult { + RoadPositionResult { + road_position, + nearest_position, + distance, + } + } +} + mod tests { mod lane_position { #[test] diff --git a/maliput/tests/common/mod.rs b/maliput/tests/common/mod.rs new file mode 100644 index 0000000..68591de --- /dev/null +++ b/maliput/tests/common/mod.rs @@ -0,0 +1,71 @@ +// 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. + +use maliput::api::RoadNetwork; +use std::collections::HashMap; + +pub fn create_t_shape_road_network() -> RoadNetwork { + // Set MALIPUT_PLUGIN_PATH + let maliput_malidrive_plugin_path = maliput_sdk::get_maliput_malidrive_plugin_path(); + std::env::set_var("MALIPUT_PLUGIN_PATH", maliput_malidrive_plugin_path); + + // Get location of odr resources + let package_location = std::env::var("CARGO_MANIFEST_DIR").unwrap(); + let xodr_path = format!("{}/data/xodr/TShapeRoad.xodr", package_location); + + let road_network_properties = HashMap::from([ + ("road_geometry_id", "my_rg_from_rust"), + ("opendrive_file", xodr_path.as_str()), + ("linear_tolerance", "0.01"), + ]); + RoadNetwork::new("maliput_malidrive", &road_network_properties) +} + +#[allow(dead_code)] +pub fn assert_inertial_position_equality( + left: &maliput::api::InertialPosition, + right: &maliput::api::InertialPosition, + tolerance: f64, +) { + assert!((left.x() - right.x()).abs() < tolerance); + assert!((left.y() - right.y()).abs() < tolerance); + assert!((left.z() - right.z()).abs() < tolerance); +} + +#[allow(dead_code)] +pub fn assert_lane_position_equality( + left: &maliput::api::LanePosition, + right: &maliput::api::LanePosition, + tolerance: f64, +) { + assert!((left.s() - right.s()).abs() < tolerance); + assert!((left.r() - right.r()).abs() < tolerance); + assert!((left.h() - right.h()).abs() < tolerance); +}