maliput/api/mod.rs
1// BSD 3-Clause License
2//
3// Copyright (c) 2024, Woven by Toyota.
4// All rights reserved.
5//
6// Redistribution and use in source and binary forms, with or without
7// modification, are permitted provided that the following conditions are met:
8//
9// * Redistributions of source code must retain the above copyright notice, this
10// list of conditions and the following disclaimer.
11//
12// * Redistributions in binary form must reproduce the above copyright notice,
13// this list of conditions and the following disclaimer in the documentation
14// and/or other materials provided with the distribution.
15//
16// * Neither the name of the copyright holder nor the names of its
17// contributors may be used to endorse or promote products derived from
18// this software without specific prior written permission.
19//
20// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
21// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
22// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
23// DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
24// FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
25// DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
26// SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
27// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
28// OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
29// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
30
31use crate::common::MaliputError;
32use crate::math::Matrix3;
33use crate::math::Quaternion;
34use crate::math::RollPitchYaw;
35use crate::math::Vector3;
36
37pub mod rules;
38
39/// Represents a complete Maliput road network.
40///
41/// A `RoadNetwork` is the main entry point for interacting with a road map in Maliput.
42/// It serves as a container for all the elements that describe a road network,
43/// including its physical layout and the rules of the road.
44///
45/// It provides access to the following key components:
46///
47/// * [`RoadGeometry`]: The geometric description of the road surfaces.
48/// * [`rules::RoadRulebook`]: The set of traffic rules, like speed limits and right-of-way.
49/// * [`rules::TrafficLightBook`]: A catalog of all traffic lights in the network.
50/// * [`IntersectionBook`]: A collection of logical intersections and their states.
51/// TODO: Complete with other books when available (e.g., `RuleRegistry / PhaseRingBook / etc)
52///
53/// More info can be found at https://maliput.readthedocs.io/en/latest/html/deps/maliput/html/maliput_design.html.
54///
55/// # Example
56///
57/// ```rust, no_run
58/// use maliput::api::RoadNetwork;
59/// use std::collections::HashMap;
60///
61/// // Properties to load an OpenDRIVE file using the "maliput_malidrive" backend.
62/// let package_location = std::env::var("CARGO_MANIFEST_DIR").unwrap();
63/// let xodr_path = format!("{}/data/xodr/TShapeRoad.xodr", package_location);
64/// let road_network_properties = HashMap::from([("road_geometry_id", "my_rg_from_rust"), ("opendrive_file", xodr_path.as_str())]);
65///
66/// // Create the RoadNetwork by specifying the loader ID and properties.
67/// let road_network = RoadNetwork::new("maliput_malidrive", &road_network_properties).unwrap();
68/// ```
69pub struct RoadNetwork {
70 pub(crate) rn: cxx::UniquePtr<maliput_sys::api::ffi::RoadNetwork>,
71}
72
73impl RoadNetwork {
74 /// Create a new `RoadNetwork` with the given `road_network_loader_id` and `properties`.
75 ///
76 /// # Arguments
77 ///
78 /// * `road_network_loader_id` - The id of the road network loader. It identifies the backend to be used (e.g., "maliput_malidrive").
79 /// * `properties` - The properties of the road network.
80 ///
81 /// # Details
82 /// It relies on `maliput_sys::plugin::ffi::CreateRoadNetwork` to create a new `RoadNetwork`.
83 ///
84 /// # Returns
85 /// A result containing the `RoadNetwork` or a `MaliputError` if the creation fails.
86 pub fn new(
87 road_network_loader_id: &str,
88 properties: &std::collections::HashMap<&str, &str>,
89 ) -> Result<RoadNetwork, MaliputError> {
90 // Translate the properties to ffi types
91 let mut properties_vec = Vec::new();
92 for (key, value) in properties.iter() {
93 properties_vec.push(format!("{}:{}", key, value));
94 }
95 // Append the maliput_malidrive plugin path to MALIPUT_PLUGIN_PATH.
96 // If MALIPUT_PLUGIN_PATH is not set, it will be created.
97 let malidrive_plugin_path = maliput_sdk::get_maliput_malidrive_plugin_path();
98 let new_path = match std::env::var_os("MALIPUT_PLUGIN_PATH") {
99 Some(current_path) => {
100 let mut paths = std::env::split_paths(¤t_path).collect::<Vec<_>>();
101 paths.push(malidrive_plugin_path);
102 std::env::join_paths(paths).unwrap()
103 }
104 None => maliput_sdk::get_maliput_malidrive_plugin_path().into(),
105 };
106 std::env::set_var("MALIPUT_PLUGIN_PATH", new_path);
107 let rn = maliput_sys::plugin::ffi::CreateRoadNetwork(&road_network_loader_id.to_string(), &properties_vec)?;
108 Ok(RoadNetwork { rn })
109 }
110
111 /// Get the `RoadGeometry` of the `RoadNetwork`.
112 pub fn road_geometry(&self) -> RoadGeometry {
113 unsafe {
114 RoadGeometry {
115 rg: self.rn.road_geometry().as_ref().expect(""),
116 }
117 }
118 }
119 /// Get the `IntersectionBook` of the `RoadNetwork`.
120 pub fn intersection_book(&mut self) -> IntersectionBook {
121 let intersection_book_ffi = self
122 .rn
123 .as_mut()
124 .expect("Underlying RoadNetwork is null")
125 .intersection_book();
126 IntersectionBook {
127 intersection_book: unsafe {
128 intersection_book_ffi
129 .as_mut()
130 .expect("Underlying IntersectionBook is null")
131 },
132 }
133 }
134 /// Get the `TrafficLightBook` of the `RoadNetwork`.
135 pub fn traffic_light_book(&self) -> rules::TrafficLightBook {
136 let traffic_light_book_ffi = self.rn.traffic_light_book();
137 rules::TrafficLightBook {
138 traffic_light_book: unsafe {
139 traffic_light_book_ffi
140 .as_ref()
141 .expect("Underlying TrafficLightBook is null")
142 },
143 }
144 }
145 /// Get the `RoadRulebook` of the `RoadNetwork`.
146 pub fn rulebook(&self) -> rules::RoadRulebook {
147 let rulebook_ffi = self.rn.rulebook();
148 rules::RoadRulebook {
149 road_rulebook: unsafe { rulebook_ffi.as_ref().expect("Underlying RoadRulebook is null") },
150 }
151 }
152}
153
154/// Represents the geometry of a road network.
155///
156/// `RoadGeometry` is the top-level container for the road network's geometric
157/// description. It is composed of a set of `Junction`s, which in turn contain
158/// `Segment`s and `Lane`s.
159///
160/// It provides access to the entire road network's geometric structure,
161/// allowing for queries about its components (e.g., finding a `Lane` by its ID)
162/// and for coordinate conversions between the inertial frame and the road network's
163/// intrinsic coordinate systems (lane coordinates).
164///
165/// An instance of `RoadGeometry` is typically obtained from a `RoadNetwork`.
166///
167/// More info can be found at https://maliput.readthedocs.io/en/latest/html/deps/maliput/html/maliput_design.html.
168///
169/// # Example of obtaining a `RoadGeometry`
170///
171/// ```rust, no_run
172/// use maliput::api::RoadNetwork;
173/// use std::collections::HashMap;
174///
175/// let package_location = std::env::var("CARGO_MANIFEST_DIR").unwrap();
176/// let xodr_path = format!("{}/data/xodr/TShapeRoad.xodr", package_location);
177/// let road_network_properties = HashMap::from([("road_geometry_id", "my_rg_from_rust"), ("opendrive_file", xodr_path.as_str())]);
178/// let road_network = RoadNetwork::new("maliput_malidrive", &road_network_properties).unwrap();
179/// let road_geometry = road_network.road_geometry();
180/// println!("RoadGeometry ID: {}", road_geometry.id());
181/// ```
182pub struct RoadGeometry<'a> {
183 rg: &'a maliput_sys::api::ffi::RoadGeometry,
184}
185
186impl<'a> RoadGeometry<'a> {
187 /// Returns the id of the RoadGeometry.
188 pub fn id(&self) -> String {
189 maliput_sys::api::ffi::RoadGeometry_id(self.rg)
190 }
191 /// Returns the number of Junctions in the RoadGeometry.
192 ///
193 /// Return value is non-negative.
194 pub fn num_junctions(&self) -> i32 {
195 self.rg.num_junctions()
196 }
197 /// Returns the tolerance guaranteed for linear measurements (positions).
198 pub fn linear_tolerance(&self) -> f64 {
199 self.rg.linear_tolerance()
200 }
201 /// Returns the tolerance guaranteed for angular measurements (orientations).
202 pub fn angular_tolerance(&self) -> f64 {
203 self.rg.angular_tolerance()
204 }
205 /// Returns the number of BranchPoints in the RoadGeometry.
206 ///
207 /// Return value is non-negative.
208 pub fn num_branch_points(&self) -> i32 {
209 self.rg.num_branch_points()
210 }
211 /// Determines the [RoadPosition] corresponding to [InertialPosition] `inertial_position`.
212 ///
213 /// Returns a RoadPositionResult. Its [RoadPosition] is the point in the
214 /// [RoadGeometry]'s manifold which is, in the `Inertial`-frame, closest to
215 /// `inertial_position`. Its InertialPosition is the `Inertial`-frame equivalent of the
216 /// [RoadPosition] and its distance is the Cartesian distance from
217 /// `inertial_position` to the nearest point.
218 ///
219 /// This method guarantees that its result satisfies the condition that
220 /// `result.lane.to_lane_position(result.pos)` is within `linear_tolerance()`
221 /// of the returned [InertialPosition].
222 ///
223 /// The map from [RoadGeometry] to the `Inertial`-frame is not onto (as a bounded
224 /// [RoadGeometry] cannot completely cover the unbounded Cartesian universe).
225 /// If `inertial_position` does represent a point contained within the volume
226 /// of the RoadGeometry, then result distance is guaranteed to be less
227 /// than or equal to `linear_tolerance()`.
228 ///
229 /// The map from [RoadGeometry] to `Inertial`-frame is not necessarily one-to-one.
230 /// Different `(s,r,h)` coordinates from different Lanes, potentially from
231 /// different Segments, may map to the same `(x,y,z)` `Inertial`-frame location.
232 ///
233 /// If `inertial_position` is contained within the volumes of multiple Segments,
234 /// then to_road_position() will choose a [Segment] which yields the minimum
235 /// height `h` value in the result. If the chosen [Segment] has multiple
236 /// Lanes, then to_road_position() will choose a [Lane] which contains
237 /// `inertial_position` within its `lane_bounds()` if possible, and if that is
238 /// still ambiguous, it will further select a [Lane] which minimizes the
239 /// absolute value of the lateral `r` coordinate in the result.
240 ///
241 /// # Arguments
242 /// * `inertial_position` - The [InertialPosition] to convert into a [RoadPosition].
243 ///
244 /// # Return
245 /// A [RoadPositionResult] with the nearest [RoadPosition], the corresponding [InertialPosition]
246 /// to that [RoadPosition] and the distance between the input and output [InertialPosition]s.
247 pub fn to_road_position(&self, inertial_position: &InertialPosition) -> Result<RoadPositionResult, MaliputError> {
248 let rpr = maliput_sys::api::ffi::RoadGeometry_ToRoadPosition(self.rg, &inertial_position.ip)?;
249 Ok(RoadPositionResult {
250 road_position: RoadPosition {
251 rp: maliput_sys::api::ffi::RoadPositionResult_road_position(&rpr),
252 },
253 nearest_position: InertialPosition {
254 ip: maliput_sys::api::ffi::RoadPositionResult_nearest_position(&rpr),
255 },
256 distance: maliput_sys::api::ffi::RoadPositionResult_distance(&rpr),
257 })
258 }
259
260 /// TODO(#197): Implement find_road_positions
261 /// Obtains all [RoadPosition]s within a radius of the inertial_position.
262 ///
263 /// Only Lanes whose segment regions include points that are within radius of
264 /// inertial position are included in the search. For each of these Lanes,
265 /// include the [RoadPosition] or [RoadPosition]s with the minimum distance to
266 /// inertial position in the returned result.
267 ///
268 /// # Arguments
269 ///
270 /// * `inertial_position` - The [InertialPosition] to search around.
271 /// * `radius` - The radius around the [InertialPosition] to search for [RoadPosition]s.
272 ///
273 /// # Return
274 ///
275 /// A vector of [RoadPositionResult]s.
276 pub fn find_road_positions(
277 &self,
278 _inertial_position: &[InertialPosition],
279 _radius: f64,
280 ) -> Result<Vec<RoadPositionResult>, MaliputError> {
281 Err(MaliputError::Other(
282 "find_road_positions is not implemented yet".to_string(),
283 ))
284 }
285
286 /// Get the lane matching given `lane_id`.
287 ///
288 /// # Arguments
289 /// * `lane_id` - The id of the lane.
290 ///
291 /// # Return
292 /// The lane with the given id.
293 /// If no lane is found with the given id, return None.
294 pub fn get_lane(&self, lane_id: &String) -> Option<Lane> {
295 let lane = maliput_sys::api::ffi::RoadGeometry_GetLane(self.rg, lane_id);
296 if lane.lane.is_null() {
297 return None;
298 }
299 Some(Lane {
300 lane: unsafe { lane.lane.as_ref().expect("") },
301 })
302 }
303 /// Get all lanes of the `RoadGeometry`.
304 /// Returns a vector of `Lane`.
305 /// # Example
306 /// ```rust, no_run
307 /// use maliput::api::RoadNetwork;
308 /// use std::collections::HashMap;
309 ///
310 /// let package_location = std::env::var("CARGO_MANIFEST_DIR").unwrap();
311 /// let xodr_path = format!("{}/data/xodr/TShapeRoad.xodr", package_location);
312 /// let road_network_properties = HashMap::from([("road_geometry_id", "my_rg_from_rust"), ("opendrive_file", xodr_path.as_str())]);
313 /// let road_network = RoadNetwork::new("maliput_malidrive", &road_network_properties).unwrap();
314 /// let road_geometry = road_network.road_geometry();
315 /// let lanes = road_geometry.get_lanes();
316 /// for lane in lanes {
317 /// println!("lane_id: {}", lane.id());
318 /// }
319 /// ```
320 pub fn get_lanes(&self) -> Vec<Lane> {
321 let lanes = maliput_sys::api::ffi::RoadGeometry_GetLanes(self.rg);
322 lanes
323 .into_iter()
324 .map(|l| Lane {
325 lane: unsafe { l.lane.as_ref().expect("") },
326 })
327 .collect::<Vec<Lane>>()
328 }
329 /// Get the segment matching given `segment_id`.
330 ///
331 /// # Arguments
332 /// * `segment_id` - The id of the segment.
333 ///
334 /// # Return
335 /// The segment with the given id.
336 /// If no segment is found with the given id, return None.
337 pub fn get_segment(&self, segment_id: &String) -> Option<Segment> {
338 let segment = maliput_sys::api::ffi::RoadGeometry_GetSegment(self.rg, segment_id);
339 if segment.is_null() {
340 return None;
341 }
342 unsafe {
343 Some(Segment {
344 segment: segment.as_ref().expect(""),
345 })
346 }
347 }
348 /// Get the junction matching given `junction_id`.
349 ///
350 /// # Arguments
351 /// * `junction_id` - The id of the junction.
352 ///
353 /// # Return
354 /// The junction with the given id.
355 /// If no junction is found with the given id, return None.
356 pub fn get_junction(&self, junction_id: &String) -> Option<Junction> {
357 let junction = maliput_sys::api::ffi::RoadGeometry_GetJunction(self.rg, junction_id);
358 if junction.is_null() {
359 return None;
360 }
361 unsafe {
362 Some(Junction {
363 junction: junction.as_ref().expect(""),
364 })
365 }
366 }
367 /// Get the branch point matching given `branch_point_id`.
368 ///
369 /// # Arguments
370 /// * `branch_point_id` - The id of the branch point.
371 ///
372 /// # Return
373 /// The branch point with the given id.
374 /// If no branch point is found with the given id, return None.
375 pub fn get_branch_point(&self, branch_point_id: &String) -> Option<BranchPoint> {
376 let branch_point = maliput_sys::api::ffi::RoadGeometry_GetBranchPoint(self.rg, branch_point_id);
377 if branch_point.is_null() {
378 return None;
379 }
380 unsafe {
381 Some(BranchPoint {
382 branch_point: branch_point.as_ref().expect(""),
383 })
384 }
385 }
386 /// Execute a custom command on the backend.
387 ///
388 /// # Details
389 /// The documentation of the custom command should be provided by the backend: https://github.com/maliput/maliput_malidrive/blob/main/src/maliput_malidrive/base/road_geometry.h
390 ///
391 /// # Arguments
392 /// * `command` - The command to execute.
393 ///
394 /// # Return
395 /// The result of the command.
396 // pub fn backend_custom_command(&self, command: &String) -> String {
397 pub fn backend_custom_command(&self, command: &String) -> Result<String, MaliputError> {
398 Ok(maliput_sys::api::ffi::RoadGeometry_BackendCustomCommand(
399 self.rg, command,
400 )?)
401 }
402 /// Obtains the Geo Reference info of this RoadGeometry.
403 ///
404 /// # Return
405 /// A string containing the Geo Reference projection, if any.
406 pub fn geo_reference_info(&self) -> String {
407 maliput_sys::api::ffi::RoadGeometry_GeoReferenceInfo(self.rg)
408 }
409}
410
411/// A 3-dimensional position in a `Lane`-frame, consisting of three components:
412///
413/// * s is longitudinal position, as arc-length along a Lane's reference line.
414/// * r is lateral position, perpendicular to the reference line at s. +r is to
415/// to the left when traveling in the direction of +s.
416/// * h is height above the road surface.
417///
418/// # Example
419///
420/// ```rust, no_run
421/// use maliput::api::LanePosition;
422///
423/// let lane_pos = LanePosition::new(1.0, 2.0, 3.0);
424/// println!("lane_pos = {}", lane_pos);
425/// assert_eq!(lane_pos.s(), 1.0);
426/// assert_eq!(lane_pos.r(), 2.0);
427/// assert_eq!(lane_pos.h(), 3.0);
428/// ```
429pub struct LanePosition {
430 lp: cxx::UniquePtr<maliput_sys::api::ffi::LanePosition>,
431}
432
433impl LanePosition {
434 /// Create a new `LanePosition` with the given `s`, `r`, and `h` components.
435 pub fn new(s: f64, r: f64, h: f64) -> LanePosition {
436 LanePosition {
437 lp: maliput_sys::api::ffi::LanePosition_new(s, r, h),
438 }
439 }
440 /// Get the `s` component of the `LanePosition`.
441 pub fn s(&self) -> f64 {
442 self.lp.s()
443 }
444 /// Get the `r` component of the `LanePosition`.
445 pub fn r(&self) -> f64 {
446 self.lp.r()
447 }
448 /// Get the `h` component of the `LanePosition`.
449 pub fn h(&self) -> f64 {
450 self.lp.h()
451 }
452
453 /// Returns all components as 3-vector `[s, r, h]`.
454 pub fn srh(&self) -> Vector3 {
455 let srh = self.lp.srh();
456 Vector3::new(srh.x(), srh.y(), srh.z())
457 }
458
459 /// Set the `s` component of the `LanePosition`.
460 pub fn set_s(&mut self, s: f64) {
461 self.lp.as_mut().expect("Underlying LanePosition is null").set_s(s);
462 }
463
464 /// Set the `r` component of the `LanePosition`.
465 pub fn set_r(&mut self, r: f64) {
466 self.lp.as_mut().expect("Underlying LanePosition is null").set_r(r);
467 }
468
469 /// Set the `h` component of the `LanePosition`.
470 pub fn set_h(&mut self, h: f64) {
471 self.lp.as_mut().expect("Underlying LanePosition is null").set_h(h);
472 }
473
474 /// Set all components from 3-vector `[s, r, h]`.
475 pub fn set_srh(&mut self, srh: &Vector3) {
476 let ffi_vec = maliput_sys::math::ffi::Vector3_new(srh.x(), srh.y(), srh.z());
477 self.lp
478 .as_mut()
479 .expect("Underlying LanePosition is null")
480 .set_srh(&ffi_vec);
481 }
482}
483
484impl PartialEq for LanePosition {
485 fn eq(&self, other: &Self) -> bool {
486 self.srh() == other.srh()
487 }
488}
489
490impl Eq for LanePosition {}
491
492impl std::fmt::Display for LanePosition {
493 fn fmt(&self, f: &mut std::fmt::Formatter) -> std::fmt::Result {
494 write!(f, "{}", maliput_sys::api::ffi::LanePosition_to_str(&self.lp))
495 }
496}
497
498impl std::fmt::Debug for LanePosition {
499 fn fmt(&self, f: &mut std::fmt::Formatter) -> std::fmt::Result {
500 f.debug_struct("LanePosition")
501 .field("s", &self.s())
502 .field("r", &self.r())
503 .field("h", &self.h())
504 .finish()
505 }
506}
507
508/// A position in 3-dimensional geographical Cartesian space, i.e., in the
509/// `Inertial`-frame, consisting of three components x, y, and z.
510///
511/// # Example
512///
513/// ```rust, no_run
514/// use maliput::api::InertialPosition;
515///
516/// let inertial_pos = InertialPosition::new(1.0, 2.0, 3.0);
517/// println!("inertial_pos = {}", inertial_pos);
518/// assert_eq!(inertial_pos.x(), 1.0);
519/// assert_eq!(inertial_pos.y(), 2.0);
520/// assert_eq!(inertial_pos.z(), 3.0);
521/// ```
522pub struct InertialPosition {
523 ip: cxx::UniquePtr<maliput_sys::api::ffi::InertialPosition>,
524}
525
526impl InertialPosition {
527 /// Create a new `InertialPosition` with the given `x`, `y`, and `z` components.
528 pub fn new(x: f64, y: f64, z: f64) -> InertialPosition {
529 InertialPosition {
530 ip: maliput_sys::api::ffi::InertialPosition_new(x, y, z),
531 }
532 }
533 /// Get the `x` component of the `InertialPosition`.
534 pub fn x(&self) -> f64 {
535 self.ip.x()
536 }
537 /// Get the `y` component of the `InertialPosition`.
538 pub fn y(&self) -> f64 {
539 self.ip.y()
540 }
541 /// Get the `z` component of the `InertialPosition`.
542 pub fn z(&self) -> f64 {
543 self.ip.z()
544 }
545
546 /// Returns all components as 3-vector `[x, y, z]`.
547 pub fn xyz(&self) -> Vector3 {
548 let xyz = self.ip.xyz();
549 Vector3::new(xyz.x(), xyz.y(), xyz.z())
550 }
551
552 /// Set the `x` component of the `InertialPosition`.
553 pub fn set_x(&mut self, x: f64) {
554 self.ip.as_mut().expect("Underlying InertialPosition is null").set_x(x);
555 }
556
557 /// Set the `y` component of the `InertialPosition`.
558 pub fn set_y(&mut self, y: f64) {
559 self.ip.as_mut().expect("Underlying InertialPosition is null").set_y(y);
560 }
561
562 /// Set the `z` component of the `InertialPosition`.
563 pub fn set_z(&mut self, z: f64) {
564 self.ip.as_mut().expect("Underlying InertialPosition is null").set_z(z);
565 }
566
567 /// Set all components from 3-vector `[x, y, z]`.
568 pub fn set_xyz(&mut self, xyz: &Vector3) {
569 let ffi_vec = maliput_sys::math::ffi::Vector3_new(xyz.x(), xyz.y(), xyz.z());
570 self.ip
571 .as_mut()
572 .expect("Underlying InertialPosition is null")
573 .set_xyz(&ffi_vec);
574 }
575
576 /// Get the length of `InertialPosition`.
577 pub fn length(&self) -> f64 {
578 self.ip.length()
579 }
580
581 /// Get the distance between two `InertialPosition`.
582 pub fn distance(&self, other: &InertialPosition) -> f64 {
583 self.ip.Distance(&other.ip)
584 }
585}
586
587impl PartialEq for InertialPosition {
588 fn eq(&self, other: &Self) -> bool {
589 maliput_sys::api::ffi::InertialPosition_operator_eq(&self.ip, &other.ip)
590 }
591}
592
593impl Eq for InertialPosition {}
594
595impl std::fmt::Display for InertialPosition {
596 fn fmt(&self, f: &mut std::fmt::Formatter) -> std::fmt::Result {
597 write!(f, "{}", maliput_sys::api::ffi::InertialPosition_to_str(&self.ip))
598 }
599}
600
601impl std::fmt::Debug for InertialPosition {
602 fn fmt(&self, f: &mut std::fmt::Formatter) -> std::fmt::Result {
603 f.debug_struct("InertialPosition")
604 .field("x", &self.x())
605 .field("y", &self.y())
606 .field("z", &self.z())
607 .finish()
608 }
609}
610
611impl std::ops::Add for InertialPosition {
612 type Output = InertialPosition;
613
614 fn add(self, other: InertialPosition) -> InertialPosition {
615 InertialPosition {
616 ip: maliput_sys::api::ffi::InertialPosition_operator_sum(&self.ip, &other.ip),
617 }
618 }
619}
620
621impl std::ops::Sub for InertialPosition {
622 type Output = InertialPosition;
623
624 fn sub(self, other: InertialPosition) -> InertialPosition {
625 InertialPosition {
626 ip: maliput_sys::api::ffi::InertialPosition_operator_sub(&self.ip, &other.ip),
627 }
628 }
629}
630
631impl std::ops::Mul<f64> for InertialPosition {
632 type Output = InertialPosition;
633
634 fn mul(self, scalar: f64) -> InertialPosition {
635 InertialPosition {
636 ip: maliput_sys::api::ffi::InertialPosition_operator_mul_scalar(&self.ip, scalar),
637 }
638 }
639}
640
641/// Bounds in the lateral dimension (r component) of a `Lane`-frame, consisting
642/// of a pair of minimum and maximum r value. The bounds must straddle r = 0,
643/// i.e., the minimum must be <= 0 and the maximum must be >= 0.
644pub struct RBounds {
645 min: f64,
646 max: f64,
647}
648
649impl RBounds {
650 /// Create a new `RBounds` with the given `min` and `max` values.
651 pub fn new(min: f64, max: f64) -> RBounds {
652 RBounds { min, max }
653 }
654 /// Get the `min` value of the `RBounds`.
655 pub fn min(&self) -> f64 {
656 self.min
657 }
658 /// Get the `max` value of the `RBounds`.
659 pub fn max(&self) -> f64 {
660 self.max
661 }
662 /// Set the `min` value of the `RBounds`.
663 pub fn set_min(&mut self, min: f64) {
664 self.min = min;
665 }
666 /// Set the `max` value of the `RBounds`.
667 pub fn set_max(&mut self, max: f64) {
668 self.max = max;
669 }
670}
671
672/// Bounds in the elevation dimension (`h` component) of a `Lane`-frame,
673/// consisting of a pair of minimum and maximum `h` value. The bounds
674/// must straddle `h = 0`, i.e., the minimum must be `<= 0` and the
675/// maximum must be `>= 0`.
676pub struct HBounds {
677 min: f64,
678 max: f64,
679}
680
681impl HBounds {
682 /// Create a new `HBounds` with the given `min` and `max` values.
683 pub fn new(min: f64, max: f64) -> HBounds {
684 HBounds { min, max }
685 }
686 /// Get the `min` value of the `HBounds`.
687 pub fn min(&self) -> f64 {
688 self.min
689 }
690 /// Get the `max` value of the `HBounds`.
691 pub fn max(&self) -> f64 {
692 self.max
693 }
694 /// Set the `min` value of the `HBounds`.
695 pub fn set_min(&mut self, min: f64) {
696 self.min = min;
697 }
698 /// Set the `max` value of the `HBounds`.
699 pub fn set_max(&mut self, max: f64) {
700 self.max = max;
701 }
702}
703
704/// Isometric velocity vector in a `Lane`-frame.
705///
706/// sigma_v, rho_v, and eta_v are the components of velocity in a
707/// (sigma, rho, eta) coordinate system. (sigma, rho, eta) have the same
708/// orientation as the (s, r, h) at any given point in space, however they
709/// form an isometric system with a Cartesian distance metric. Hence,
710/// IsoLaneVelocity represents a "real" physical velocity vector (albeit
711/// with an orientation relative to the road surface).
712#[derive(Default, Copy, Clone, Debug, PartialEq)]
713pub struct IsoLaneVelocity {
714 pub sigma_v: f64,
715 pub rho_v: f64,
716 pub eta_v: f64,
717}
718
719impl IsoLaneVelocity {
720 /// Create a new `IsoLaneVelocity` with the given `sigma_v`, `rho_v`, and `eta_v` components.
721 pub fn new(sigma_v: f64, rho_v: f64, eta_v: f64) -> IsoLaneVelocity {
722 IsoLaneVelocity { sigma_v, rho_v, eta_v }
723 }
724}
725
726/// A Lane represents a lane of travel in a road network. A Lane defines
727/// a curvilinear coordinate system covering the road surface, with a
728/// longitudinal 's' coordinate that expresses the arc-length along a
729/// central reference curve. The reference curve nominally represents
730/// an ideal travel trajectory along the Lane.
731///
732/// Lanes are grouped by Segment. All Lanes belonging to a Segment
733/// represent the same road surface, but with different coordinate
734/// parameterizations (e.g., each Lane has its own reference curve).
735pub struct Lane<'a> {
736 lane: &'a maliput_sys::api::ffi::Lane,
737}
738
739impl<'a> Lane<'a> {
740 /// Returns the id of the Lane.
741 ///
742 /// The id is a unique identifier for the Lane within the RoadGeometry.
743 ///
744 /// # Returns
745 /// A `String` containing the id of the Lane.
746 pub fn id(&self) -> String {
747 maliput_sys::api::ffi::Lane_id(self.lane)
748 }
749 /// Returns the index of this Lane within the Segment which owns it.
750 pub fn index(&self) -> i32 {
751 self.lane.index()
752 }
753 /// Returns the Segment to which this Lane belongs.
754 ///
755 /// # Returns
756 /// A [`Segment`] containing the Segment to which this Lane belongs.
757 pub fn segment(&self) -> Segment<'a> {
758 unsafe {
759 Segment {
760 segment: self.lane.segment().as_ref().expect(""),
761 }
762 }
763 }
764 /// Returns the adjacent lane to the left, if one exists.
765 ///
766 /// "Left" is defined as the direction of `+r` in the `(s, r, h)` lane-coordinate frame.
767 ///
768 /// # Returns
769 ///
770 /// An [`Option<Lane>`] containing the left lane, or `None` if this is the
771 /// leftmost lane.
772 pub fn to_left(&self) -> Option<Lane> {
773 let lane = self.lane.to_left();
774 if lane.is_null() {
775 None
776 } else {
777 unsafe {
778 Some(Lane {
779 lane: lane.as_ref().expect(""),
780 })
781 }
782 }
783 }
784 /// Returns the adjacent lane to the right, if one exists.
785 ///
786 /// "Right" is defined as the direction of `-r` in the `(s, r, h)` lane-coordinate
787 /// frame.
788 ///
789 /// # Returns
790 ///
791 /// An [`Option<Lane>`] containing the right lane, or `None` if this is the
792 /// rightmost lane.
793 pub fn to_right(&self) -> Option<Lane> {
794 let lane = self.lane.to_right();
795 if lane.is_null() {
796 None
797 } else {
798 unsafe {
799 Some(Lane {
800 lane: lane.as_ref().expect(""),
801 })
802 }
803 }
804 }
805 /// Returns the arc-length of the Lane along its reference curve.
806 ///
807 /// The value of length() is also the maximum s-coordinate for this Lane;
808 /// i.e., the domain of s is [0, length()].
809 ///
810 /// # Returns
811 ///
812 /// The length of the Lane in meters.
813 pub fn length(&self) -> f64 {
814 self.lane.length()
815 }
816
817 /// Get the orientation of the `Lane` at the given `LanePosition`.
818 pub fn get_orientation(&self, lane_position: &LanePosition) -> Result<Rotation, MaliputError> {
819 Ok(Rotation {
820 r: maliput_sys::api::ffi::Lane_GetOrientation(self.lane, lane_position.lp.as_ref().expect(""))?,
821 })
822 }
823 /// # Brief
824 /// Get the [InertialPosition] of the [Lane] at the given [LanePosition].
825 ///
826 /// # Notes
827 /// Note there is no constraint for the `r` coordinate, as it can be outside the lane boundaries.
828 /// In that scenario, the resultant inertial position represents a point in the `s-r` plane at the given `s` and `h`
829 /// coordinates. It's on the user side to verify, if needed, that the lane position is within lane boundaries.
830 /// Bare in mind that the inertial position will be a point in the `s-r` plane, but *not* necessarily on the road surface.
831 ///
832 /// # Arguments
833 /// * `lane_position` - A maliput [LanePosition].
834 ///
835 /// # Precondition
836 /// The s component of `lane_position` must be in domain [0, Lane::length()].
837 ///
838 /// # Return
839 /// The [InertialPosition] corresponding to the input [LanePosition].
840 pub fn to_inertial_position(&self, lane_position: &LanePosition) -> Result<InertialPosition, MaliputError> {
841 Ok(InertialPosition {
842 ip: maliput_sys::api::ffi::Lane_ToInertialPosition(self.lane, lane_position.lp.as_ref().expect(""))?,
843 })
844 }
845 /// Determines the LanePosition corresponding to InertialPosition `inertial_position`.
846 /// The LanePosition is expected to be contained within the lane's boundaries.
847 /// See [Lane::to_segment_position] method.
848 ///
849 /// This method guarantees that its result satisfies the condition that
850 /// `to_inertial_position(result.lane_position)` is within `linear_tolerance()`
851 /// of `result.nearest_position`.
852 ///
853 /// # Arguments
854 /// * `inertial_position` - A [InertialPosition] to get a [LanePosition] from.
855 ///
856 /// # Return
857 /// A [LanePositionResult] with the closest [LanePosition], the corresponding [InertialPosition] to that [LanePosition]
858 /// and the distance between the input and output [InertialPosition]s.
859 pub fn to_lane_position(&self, inertial_position: &InertialPosition) -> Result<LanePositionResult, MaliputError> {
860 let lpr = maliput_sys::api::ffi::Lane_ToLanePosition(self.lane, inertial_position.ip.as_ref().expect(""))?;
861 Ok(LanePositionResult {
862 lane_position: LanePosition {
863 lp: maliput_sys::api::ffi::LanePositionResult_road_position(&lpr),
864 },
865 nearest_position: InertialPosition {
866 ip: maliput_sys::api::ffi::LanePositionResult_nearest_position(&lpr),
867 },
868 distance: maliput_sys::api::ffi::LanePositionResult_distance(&lpr),
869 })
870 }
871 /// Determines the [LanePosition] corresponding to [InertialPosition] `inertial_position`.
872 /// The [LanePosition] is expected to be contained within the segment's boundaries.
873 /// See [Lane::to_lane_position] method.
874 ///
875 /// This method guarantees that its result satisfies the condition that
876 /// `to_inertial_position(result.lane_position)` is within `linear_tolerance()`
877 /// of `result.nearest_position`.
878 ///
879 /// # Arguments
880 /// * `inertial_position` - A [InertialPosition] to get a SegmentPosition from.
881 ///
882 /// # Return
883 /// A [LanePositionResult] with the closest [LanePosition] within the segment, the corresponding
884 /// [InertialPosition] to that [LanePosition] and the distance between the input and output
885 /// [InertialPosition]s.
886 pub fn to_segment_position(
887 &self,
888 inertial_position: &InertialPosition,
889 ) -> Result<LanePositionResult, MaliputError> {
890 let spr = maliput_sys::api::ffi::Lane_ToSegmentPosition(self.lane, inertial_position.ip.as_ref().expect(""))?;
891 Ok(LanePositionResult {
892 lane_position: LanePosition {
893 lp: maliput_sys::api::ffi::LanePositionResult_road_position(&spr),
894 },
895 nearest_position: InertialPosition {
896 ip: maliput_sys::api::ffi::LanePositionResult_nearest_position(&spr),
897 },
898 distance: maliput_sys::api::ffi::LanePositionResult_distance(&spr),
899 })
900 }
901 /// Returns the nominal lateral (r) bounds for the lane as a function of s.
902 ///
903 /// These are the lateral bounds for a position that is considered to be
904 /// "staying in the lane".
905 ///
906 /// See also [Lane::segment_bounds] that defines the whole surface.
907 ///
908 /// # Arguments
909 /// * `s` - The longitudinal position along the lane's reference line.
910 ///
911 /// # Returns
912 /// A [RBounds] containing the lateral bounds of the lane at the given `s.
913 ///
914 /// # Errors
915 /// If lane bounds cannot be computed, an error is returned. This can happen if the
916 /// `s` value is out of bounds (i.e., not in the range [0, Lane::length()]).
917 pub fn lane_bounds(&self, s: f64) -> Result<RBounds, MaliputError> {
918 let bounds = maliput_sys::api::ffi::Lane_lane_bounds(self.lane, s)?;
919 Ok(RBounds::new(bounds.min(), bounds.max()))
920 }
921 /// Returns the lateral segment (r) bounds of the lane as a function of s.
922 ///
923 /// These are the lateral bounds for a position that is considered to be
924 /// "on segment", reflecting the physical extent of the surface of the
925 /// lane's segment.
926 ///
927 /// See also [Lane::lane_bounds] that defines what's considered to be "staying
928 /// in the lane".
929 ///
930 /// # Arguments
931 /// * `s` - The longitudinal position along the lane's reference line.
932 ///
933 /// # Returns
934 /// A [RBounds] containing the lateral segment bounds of the lane at the given `
935 /// s`.
936 ///
937 /// # Errors
938 /// If segment bounds cannot be computed, an error is returned. This can happen if the
939 /// `s` value is out of bounds (i.e., not in the range [0, Lane::length()]).
940 pub fn segment_bounds(&self, s: f64) -> Result<RBounds, MaliputError> {
941 let bounds = maliput_sys::api::ffi::Lane_segment_bounds(self.lane, s)?;
942 Ok(RBounds::new(bounds.min(), bounds.max()))
943 }
944 /// Returns the elevation (`h`) bounds of the lane as a function of `(s, r)`.
945 ///
946 /// These are the elevation bounds for a position that is considered to be
947 /// within the Lane's volume modeled by the RoadGeometry.
948 ///
949 /// `s` is within [0, `length()`] of this Lane and `r` is within
950 /// `lane_bounds(s)`.
951 ///
952 /// # Arguments
953 /// * `s` - The longitudinal position along the lane's reference line.
954 /// * `r` - The lateral position perpendicular to the reference line at `s`.
955 ///
956 /// # Returns
957 /// A [HBounds] containing the elevation bounds of the lane at the given `(s, r)`.
958 ///
959 /// # Errors
960 /// If elevation bounds cannot be computed, an error is returned. This can happen if the
961 /// `s` value is out of bounds (i.e., not in the range [0, Lane::length()]) or if `r` is not within the
962 /// lane bounds at `s`.
963 pub fn elevation_bounds(&self, s: f64, r: f64) -> Result<HBounds, MaliputError> {
964 let bounds = maliput_sys::api::ffi::Lane_elevation_bounds(self.lane, s, r)?;
965 Ok(HBounds::new(bounds.min(), bounds.max()))
966 }
967 /// Computes derivatives of [LanePosition] given a velocity vector `velocity`.
968 /// `velocity` is a isometric velocity vector oriented in the `Lane`-frame
969 /// at `position`.
970 ///
971 /// # Arguments
972 /// * `lane_position` - A [LanePosition] at which to evaluate the derivatives.
973 /// * `velocity` - An [IsoLaneVelocity] representing the velocity vector in the `Lane`-frame
974 /// at `lane_position`.
975 ///
976 /// # Returns
977 /// Returns `Lane`-frame derivatives packed into a [LanePosition] struct.
978 pub fn eval_motion_derivatives(&self, lane_position: &LanePosition, velocity: &IsoLaneVelocity) -> LanePosition {
979 LanePosition {
980 lp: maliput_sys::api::ffi::Lane_EvalMotionDerivatives(
981 self.lane,
982 lane_position.lp.as_ref().expect(""),
983 velocity.sigma_v,
984 velocity.rho_v,
985 velocity.eta_v,
986 ),
987 }
988 }
989 /// Returns the lane's [BranchPoint] for the specified end.
990 ///
991 /// # Argument
992 /// * `end` - This lane's start or end [LaneEnd].
993 ///
994 /// # Return
995 /// The lane's [BranchPoint] for the specified end.
996 pub fn get_branch_point(&self, end: &LaneEnd) -> Result<BranchPoint, MaliputError> {
997 if end != &LaneEnd::Start(self.clone()) && end != &LaneEnd::Finish(self.clone()) {
998 return Err(MaliputError::AssertionError(format!(
999 "LaneEnd must be an end of this lane {:?}",
1000 end
1001 )));
1002 }
1003 Ok(BranchPoint {
1004 branch_point: unsafe {
1005 maliput_sys::api::ffi::Lane_GetBranchPoint(self.lane, end == &LaneEnd::Start(self.clone()))
1006 .as_ref()
1007 .expect("Underlying BranchPoint is null")
1008 },
1009 })
1010 }
1011 /// Returns the set of [LaneEnd]'s which connect with this lane on the
1012 /// same side of the [BranchPoint] at `end`. At a minimum,
1013 /// this set will include this [Lane].
1014 ///
1015 /// # Arguments
1016 /// * `end` - This lane's start or end [LaneEnd].
1017 ///
1018 /// # Return
1019 /// A [LaneEndSet] with all the [LaneEnd]s at the same side of the [BranchPoint] at `end`.
1020 pub fn get_confluent_branches(&self, end: &LaneEnd) -> Result<LaneEndSet, MaliputError> {
1021 if end != &LaneEnd::Start(self.clone()) && end != &LaneEnd::Finish(self.clone()) {
1022 return Err(MaliputError::AssertionError(format!(
1023 "LaneEnd must be an end of this lane {:?}",
1024 end
1025 )));
1026 }
1027 Ok(LaneEndSet {
1028 lane_end_set: unsafe {
1029 maliput_sys::api::ffi::Lane_GetConfluentBranches(self.lane, end == &LaneEnd::Start(self.clone()))?
1030 .as_ref()
1031 .expect("Underlying LaneEndSet is null")
1032 },
1033 })
1034 }
1035 /// Returns the set of [LaneEnd]s which continue onward from this lane at the
1036 /// [BranchPoint] at `end`.
1037 ///
1038 /// # Arguments
1039 /// * `end` - This lane's start or end [LaneEnd].
1040 ///
1041 /// # Return
1042 /// A [LaneEndSet] with all the [LaneEnd]s at the opposite side of the [BranchPoint] at `end`.
1043 pub fn get_ongoing_branches(&self, end: &LaneEnd) -> Result<LaneEndSet, MaliputError> {
1044 if end != &LaneEnd::Start(self.clone()) && end != &LaneEnd::Finish(self.clone()) {
1045 return Err(MaliputError::AssertionError(format!(
1046 "LaneEnd must be an end of this lane {:?}",
1047 end
1048 )));
1049 }
1050 Ok(LaneEndSet {
1051 lane_end_set: unsafe {
1052 maliput_sys::api::ffi::Lane_GetOngoingBranches(self.lane, end == &LaneEnd::Start(self.clone()))?
1053 .as_ref()
1054 .expect("Underlying LaneEndSet is null")
1055 },
1056 })
1057 }
1058 /// Returns the default ongoing LaneEnd connected at `end`,
1059 /// or None if no default branch has been established.
1060 ///
1061 /// # Arguments
1062 /// * `end` - This lane's start or end [LaneEnd].
1063 ///
1064 /// # Return
1065 /// An `Option<LaneEnd>` containing the default branch if it exists, or None
1066 /// if no default branch has been established.
1067 pub fn get_default_branch(&self, end: &LaneEnd) -> Option<LaneEnd> {
1068 assert! {
1069 end == &LaneEnd::Start(self.clone()) || end == &LaneEnd::Finish(self.clone()),
1070 "LaneEnd must be an end of this lane {:?}",
1071 end
1072 }
1073 let lane_end = maliput_sys::api::ffi::Lane_GetDefaultBranch(self.lane, end == &LaneEnd::Start(self.clone()));
1074 match lane_end.is_null() {
1075 true => None,
1076 false => {
1077 let lane_end_ref: &maliput_sys::api::ffi::LaneEnd =
1078 lane_end.as_ref().expect("Underlying LaneEnd is null");
1079 let is_start = maliput_sys::api::ffi::LaneEnd_is_start(lane_end_ref);
1080 let lane_ref = unsafe {
1081 maliput_sys::api::ffi::LaneEnd_lane(lane_end_ref)
1082 .as_ref()
1083 .expect("Underlying LaneEnd is null")
1084 };
1085 match is_start {
1086 true => Some(LaneEnd::Start(Lane { lane: lane_ref })),
1087 false => Some(LaneEnd::Finish(Lane { lane: lane_ref })),
1088 }
1089 }
1090 }
1091 }
1092 /// Evaluates if the `Lane` contains the given `LanePosition`.
1093 ///
1094 /// # Arguments
1095 /// * `lane_position` - A [LanePosition] to check if it is contained within the `Lane`.
1096 ///
1097 /// # Returns
1098 /// A boolean indicating whether the `Lane` contains the `LanePosition`.
1099 pub fn contains(&self, lane_position: &LanePosition) -> bool {
1100 self.lane.Contains(lane_position.lp.as_ref().expect(""))
1101 }
1102}
1103
1104/// Copy trait for Lane.
1105/// A reference to the Lane is copied.
1106impl Clone for Lane<'_> {
1107 fn clone(&self) -> Self {
1108 Lane { lane: self.lane }
1109 }
1110}
1111
1112/// A Segment represents a bundle of adjacent Lanes which share a
1113/// continuously traversable road surface. Every [LanePosition] on a
1114/// given [Lane] of a Segment has a corresponding [LanePosition] on each
1115/// other [Lane], all with the same height-above-surface h, that all
1116/// map to the same GeoPoint in 3-space.
1117///
1118/// Segments are grouped by [Junction].
1119pub struct Segment<'a> {
1120 segment: &'a maliput_sys::api::ffi::Segment,
1121}
1122
1123impl<'a> Segment<'a> {
1124 /// Returns the id of the Segment.
1125 /// The id is a unique identifier for the Segment within the RoadGeometry.
1126 ///
1127 /// # Returns
1128 /// A `String` containing the id of the Segment.
1129 pub fn id(&self) -> String {
1130 maliput_sys::api::ffi::Segment_id(self.segment)
1131 }
1132 /// Returns the [Junction] to which this Segment belongs.
1133 ///
1134 /// # Returns
1135 /// An [`Result<Junction, MaliputError>`] containing the Junction to which this Segment belongs.
1136 /// If the Segment does not belong to a Junction, an error is returned.
1137 pub fn junction(&self) -> Result<Junction, MaliputError> {
1138 let junction = self.segment.junction()?;
1139 if junction.is_null() {
1140 return Err(MaliputError::AssertionError(
1141 "Segment does not belong to a Junction".to_string(),
1142 ));
1143 }
1144 unsafe {
1145 Ok(Junction {
1146 junction: junction.as_ref().expect(""),
1147 })
1148 }
1149 }
1150 /// Returns the number of lanes in the Segment.
1151 ///
1152 /// # Returns
1153 /// The number of lanes in the Segment.
1154 pub fn num_lanes(&self) -> i32 {
1155 self.segment.num_lanes()
1156 }
1157 /// Returns the lane at the given `index`.
1158 ///
1159 /// # Arguments
1160 /// * `index` - The index of the lane to retrieve.
1161 ///
1162 /// # Returns
1163 /// A [`Lane`] containing the lane at the given index.
1164 pub fn lane(&self, index: i32) -> Result<Lane, MaliputError> {
1165 if index < 0 || index >= self.num_lanes() {
1166 return Err(MaliputError::AssertionError(format!(
1167 "Index {} is out of bounds for Segment with {} lanes",
1168 index,
1169 self.num_lanes()
1170 )));
1171 }
1172 let lane = self.segment.lane(index)?;
1173 unsafe {
1174 Ok(Lane {
1175 lane: lane.as_ref().expect(""),
1176 })
1177 }
1178 }
1179}
1180
1181/// A Junction is a closed set of [Segment]s which have physically
1182/// coplanar road surfaces, in the sense that [RoadPosition]s with the
1183/// same h value (height above surface) in the domains of two [Segment]s
1184/// map to the same [InertialPosition]. The [Segment]s need not be directly
1185/// connected to one another in the network topology.
1186///
1187/// Junctions are grouped by [RoadGeometry].
1188pub struct Junction<'a> {
1189 junction: &'a maliput_sys::api::ffi::Junction,
1190}
1191
1192impl<'a> Junction<'a> {
1193 /// Returns the id of the Junction.
1194 /// The id is a unique identifier for the Junction within the RoadGeometry.
1195 ///
1196 /// # Returns
1197 /// A `String` containing the id of the Junction.
1198 pub fn id(&self) -> String {
1199 maliput_sys::api::ffi::Junction_id(self.junction)
1200 }
1201 /// Returns the [RoadGeometry] to which this Junction belongs.
1202 ///
1203 /// # Returns
1204 /// A [`RoadGeometry`] containing the RoadGeometry to which this Junction belongs.
1205 pub fn road_geometry(&self) -> RoadGeometry {
1206 unsafe {
1207 RoadGeometry {
1208 rg: self.junction.road_geometry().as_ref().expect(""),
1209 }
1210 }
1211 }
1212 /// Returns the number of segments in the Junction.
1213 ///
1214 /// # Returns
1215 /// The number of segments in the Junction.
1216 pub fn num_segments(&self) -> i32 {
1217 self.junction.num_segments()
1218 }
1219 /// Returns the segment at the given `index`.
1220 ///
1221 /// # Arguments
1222 /// * `index` - The index of the segment to retrieve.
1223 ///
1224 /// # Returns
1225 /// A [Result<Segment, MaliputError>] containing the segment at the given index.
1226 /// If the index is out of bounds, an error is returned.
1227 pub fn segment(&self, index: i32) -> Result<Segment, MaliputError> {
1228 unsafe {
1229 Ok(Segment {
1230 segment: self.junction.segment(index)?.as_ref().expect(""),
1231 })
1232 }
1233 }
1234}
1235
1236/// A position in the road network compound by a specific lane and a lane-frame position in that lane.
1237/// This position is defined by a [Lane] and a [LanePosition].
1238pub struct RoadPosition {
1239 rp: cxx::UniquePtr<maliput_sys::api::ffi::RoadPosition>,
1240}
1241
1242impl RoadPosition {
1243 /// Create a new `RoadPosition` with the given `lane` and `lane_pos`.
1244 ///
1245 /// # Arguments
1246 /// * `lane` - A reference to a [Lane] that this `RoadPosition` is associated with.
1247 /// * `lane_pos` - A reference to a [LanePosition] that defines the position within the lane.
1248 ///
1249 /// # Returns
1250 /// A new `RoadPosition` instance.
1251 pub fn new(lane: &Lane, lane_pos: &LanePosition) -> RoadPosition {
1252 unsafe {
1253 RoadPosition {
1254 rp: maliput_sys::api::ffi::RoadPosition_new(lane.lane, &lane_pos.lp),
1255 }
1256 }
1257 }
1258 /// Computes the [InertialPosition] corresponding to this `RoadPosition`.
1259 ///
1260 /// # Notes
1261 /// This is an indirection to [Lane::to_inertial_position] method.
1262 ///
1263 /// # Returns
1264 /// An [InertialPosition] corresponding to this `RoadPosition`.
1265 pub fn to_inertial_position(&self) -> InertialPosition {
1266 InertialPosition {
1267 ip: maliput_sys::api::ffi::RoadPosition_ToInertialPosition(&self.rp),
1268 }
1269 }
1270 /// Gets the [Lane] associated with this `RoadPosition`.
1271 ///
1272 /// # Returns
1273 /// A [Lane] that this `RoadPosition` is associated with.
1274 pub fn lane(&self) -> Lane {
1275 unsafe {
1276 Lane {
1277 lane: maliput_sys::api::ffi::RoadPosition_lane(&self.rp).as_ref().expect(""),
1278 }
1279 }
1280 }
1281 /// Gets the [LanePosition] associated with this `RoadPosition`.
1282 ///
1283 /// # Returns
1284 /// A [LanePosition] that defines the position within the lane for this `RoadPosition`.
1285 pub fn pos(&self) -> LanePosition {
1286 LanePosition {
1287 lp: maliput_sys::api::ffi::RoadPosition_pos(&self.rp),
1288 }
1289 }
1290}
1291
1292/// Represents the result of a RoadPosition query.
1293/// This struct contains the `RoadPosition`, the nearest `InertialPosition` to that `RoadPosition`,
1294/// and the distance between the input `InertialPosition` and the nearest `InertialPosition`.
1295///
1296/// This struct is typically used as return type for the methods: [RoadGeometry::to_road_position] and [RoadGeometry::find_road_positions].
1297pub struct RoadPositionResult {
1298 /// The candidate RoadPosition returned by the query.
1299 pub road_position: RoadPosition,
1300 /// The nearest InertialPosition to the candidate RoadPosition.
1301 /// This is the position in the inertial frame that is closest to the candidate RoadPosition
1302 pub nearest_position: InertialPosition,
1303 /// The distance between the input InertialPosition and the nearest InertialPosition.
1304 pub distance: f64,
1305}
1306
1307impl RoadPositionResult {
1308 /// Create a new `RoadPositionResult` with the given `road_position`, `nearest_position`, and `distance`.
1309 pub fn new(road_position: RoadPosition, nearest_position: InertialPosition, distance: f64) -> RoadPositionResult {
1310 RoadPositionResult {
1311 road_position,
1312 nearest_position,
1313 distance,
1314 }
1315 }
1316}
1317
1318/// Represents the result of a LanePosition query.
1319/// This struct contains the `LanePosition`, the nearest `InertialPosition` to that `LanePosition`,
1320/// and the distance between the input `InertialPosition` and the nearest `InertialPosition`.
1321///
1322/// This struct is typically used as return type for the methods: [Lane::to_lane_position] and [Lane::to_segment_position].
1323pub struct LanePositionResult {
1324 /// The candidate LanePosition within the Lane' lane-bounds or segment-bounds
1325 /// depending if [Lane::to_lane_position] or [Lane::to_segment_position] respectively, was called.
1326 /// The LanePosition is closest to a `inertial_position` supplied to [Lane::to_lane_position]
1327 /// (measured by the Cartesian metric in the `Inertial`-frame).
1328 pub lane_position: LanePosition,
1329 /// The position that exactly corresponds to `lane_position`.
1330 pub nearest_position: InertialPosition,
1331 /// The Cartesian distance between `nearest_position` and the
1332 /// `inertial_position` supplied to [Lane::to_lane_position] / [Lane::to_segment_position].
1333 pub distance: f64,
1334}
1335
1336impl LanePositionResult {
1337 /// Create a new `LanePositionResult` with the given `lane_position`, `nearest_position`, and `distance`.
1338 pub fn new(lane_position: LanePosition, nearest_position: InertialPosition, distance: f64) -> LanePositionResult {
1339 LanePositionResult {
1340 lane_position,
1341 nearest_position,
1342 distance,
1343 }
1344 }
1345}
1346
1347/// A 3-dimensional rotation in the road network.
1348/// This struct represents a rotation in the road network, which can be defined
1349/// using a quaternion or roll-pitch-yaw angles.
1350/// It provides methods to create a rotation, convert between representations,
1351/// and apply the rotation to an inertial position.
1352pub struct Rotation {
1353 r: cxx::UniquePtr<maliput_sys::api::ffi::Rotation>,
1354}
1355
1356impl Default for Rotation {
1357 fn default() -> Self {
1358 Self::new()
1359 }
1360}
1361
1362impl Rotation {
1363 /// Create a new `Rotation`.
1364 pub fn new() -> Rotation {
1365 Rotation {
1366 r: maliput_sys::api::ffi::Rotation_new(),
1367 }
1368 }
1369 /// Create a new `Rotation` from a `Quaternion`.
1370 pub fn from_quat(q: &Quaternion) -> Rotation {
1371 let q_ffi = maliput_sys::math::ffi::Quaternion_new(q.w(), q.x(), q.y(), q.z());
1372 Rotation {
1373 r: maliput_sys::api::ffi::Rotation_from_quat(&q_ffi),
1374 }
1375 }
1376 /// Create a new `Rotation` from a `RollPitchYaw`.
1377 pub fn from_rpy(rpy: &RollPitchYaw) -> Rotation {
1378 let rpy_ffi = maliput_sys::math::ffi::RollPitchYaw_new(rpy.roll_angle(), rpy.pitch_angle(), rpy.yaw_angle());
1379 Rotation {
1380 r: maliput_sys::api::ffi::Rotation_from_rpy(&rpy_ffi),
1381 }
1382 }
1383 /// Get the roll of the `Rotation`.
1384 pub fn roll(&self) -> f64 {
1385 self.r.roll()
1386 }
1387 /// Get the pitch of the `Rotation`.
1388 pub fn pitch(&self) -> f64 {
1389 self.r.pitch()
1390 }
1391 /// Get the yaw of the `Rotation`.
1392 pub fn yaw(&self) -> f64 {
1393 self.r.yaw()
1394 }
1395 /// Get a quaternion representation of the `Rotation`.
1396 pub fn quat(&self) -> Quaternion {
1397 let q_ffi = self.r.quat();
1398 Quaternion::new(q_ffi.w(), q_ffi.x(), q_ffi.y(), q_ffi.z())
1399 }
1400 /// Get a roll-pitch-yaw representation of the `Rotation`.
1401 pub fn rpy(&self) -> RollPitchYaw {
1402 let rpy_ffi = maliput_sys::api::ffi::Rotation_rpy(&self.r);
1403 RollPitchYaw::new(rpy_ffi.roll_angle(), rpy_ffi.pitch_angle(), rpy_ffi.yaw_angle())
1404 }
1405 /// Set the `Rotation` from a `Quaternion`.
1406 pub fn set_quat(&mut self, q: &Quaternion) {
1407 let q_ffi = maliput_sys::math::ffi::Quaternion_new(q.w(), q.x(), q.y(), q.z());
1408 maliput_sys::api::ffi::Rotation_set_quat(self.r.pin_mut(), &q_ffi);
1409 }
1410 /// Get the matrix representation of the `Rotation`.
1411 pub fn matrix(&self) -> Matrix3 {
1412 let matrix_ffi: cxx::UniquePtr<maliput_sys::math::ffi::Matrix3> =
1413 maliput_sys::api::ffi::Rotation_matrix(&self.r);
1414 let row_0 = maliput_sys::math::ffi::Matrix3_row(matrix_ffi.as_ref().expect(""), 0);
1415 let row_1 = maliput_sys::math::ffi::Matrix3_row(matrix_ffi.as_ref().expect(""), 1);
1416 let row_2 = maliput_sys::math::ffi::Matrix3_row(matrix_ffi.as_ref().expect(""), 2);
1417 Matrix3::new(
1418 Vector3::new(row_0.x(), row_0.y(), row_0.z()),
1419 Vector3::new(row_1.x(), row_1.y(), row_1.z()),
1420 Vector3::new(row_2.x(), row_2.y(), row_2.z()),
1421 )
1422 }
1423 /// Get the distance between two `Rotation`.
1424 pub fn distance(&self, other: &Rotation) -> f64 {
1425 self.r.Distance(&other.r)
1426 }
1427 /// Apply the `Rotation` to an `InertialPosition`.
1428 pub fn apply(&self, v: &InertialPosition) -> InertialPosition {
1429 InertialPosition {
1430 ip: maliput_sys::api::ffi::Rotation_Apply(&self.r, &v.ip),
1431 }
1432 }
1433 /// Get the reverse of the `Rotation`.
1434 pub fn reverse(&self) -> Rotation {
1435 Rotation {
1436 r: maliput_sys::api::ffi::Rotation_Reverse(&self.r),
1437 }
1438 }
1439}
1440
1441/// Directed, inclusive longitudinal (s value) range from s0 to s1.
1442pub struct SRange {
1443 s_range: cxx::UniquePtr<maliput_sys::api::ffi::SRange>,
1444}
1445
1446impl SRange {
1447 /// Creates a new `SRange` with the given `s0` and `s1`.
1448 ///
1449 /// # Arguments
1450 /// * `s0` - The starting value of the range.
1451 /// * `s1` - The ending value of the range.
1452 ///
1453 /// # Returns
1454 /// A new `SRange` instance.
1455 pub fn new(s0: f64, s1: f64) -> SRange {
1456 SRange {
1457 s_range: maliput_sys::api::ffi::SRange_new(s0, s1),
1458 }
1459 }
1460 /// Returns the s0 of the `SRange`.
1461 ///
1462 /// # Returns
1463 /// The starting value of the range.
1464 pub fn s0(&self) -> f64 {
1465 self.s_range.s0()
1466 }
1467 /// Returns the s1 of the `SRange`.
1468 ///
1469 /// # Returns
1470 /// The ending value of the range.
1471 pub fn s1(&self) -> f64 {
1472 self.s_range.s1()
1473 }
1474 /// Sets the s0 of the `SRange`.
1475 ///
1476 /// # Arguments
1477 /// * `s0` - The new starting value of the range.
1478 pub fn set_s0(&mut self, s0: f64) {
1479 self.s_range.as_mut().expect("Underlying SRange is null").set_s0(s0);
1480 }
1481 /// Sets the s1 of the `SRange`.
1482 ///
1483 /// # Arguments
1484 /// * `s1` - The new ending value of the range.
1485 pub fn set_s1(&mut self, s1: f64) {
1486 self.s_range.as_mut().expect("Underlying SRange is null").set_s1(s1);
1487 }
1488 /// Get the size of the `SRange`.
1489 ///
1490 /// # Returns
1491 /// The size of the range, which is the difference between s1 and s0.
1492 pub fn size(&self) -> f64 {
1493 self.s_range.size()
1494 }
1495 /// Defines whether this SRange is in the direction of +s (i.e., s1() > s0()).
1496 ///
1497 /// # Returns
1498 /// A boolean indicating whether the SRange is in the direction of +s.
1499 pub fn with_s(&self) -> bool {
1500 self.s_range.WithS()
1501 }
1502 /// Determines whether this SRange intersects with `s_range`.
1503 ///
1504 /// # Arguments
1505 /// * `s_range` - Another `SRange` to check for intersection.
1506 /// * `tolerance` - A tolerance value to consider when checking for intersection.
1507 ///
1508 /// # Returns
1509 /// A boolean indicating whether this SRange intersects with `s_range`.
1510 pub fn intersects(&self, s_range: &SRange, tolerance: f64) -> bool {
1511 self.s_range.Intersects(&s_range.s_range, tolerance)
1512 }
1513 /// Determines whether this SRange contains `s_range`.
1514 ///
1515 /// # Arguments
1516 /// * `s_range` - Another `SRange` to check if it is contained within this SRange.
1517 /// * `tolerance` - A tolerance value to consider when checking for containment.
1518 pub fn contains(&self, s_range: &SRange, tolerance: f64) -> bool {
1519 self.s_range.Contains(&s_range.s_range, tolerance)
1520 }
1521 /// Get the intersection of this SRange with `s_range`.
1522 ///
1523 /// # Arguments
1524 /// * `s_range` - Another `SRange` to get the intersection with.
1525 /// * `tolerance` - A tolerance value to consider when checking for intersection.
1526 ///
1527 /// # Returns
1528 /// An `Option<SRange>` containing the intersection of this SRange with `s_range`.
1529 /// If the intersection is empty, it returns None.
1530 pub fn get_intersection(&self, s_range: &SRange, tolerance: f64) -> Option<SRange> {
1531 let intersection = maliput_sys::api::ffi::SRange_GetIntersection(&self.s_range, &s_range.s_range, tolerance);
1532 match intersection.is_null() {
1533 true => None,
1534 false => Some(SRange { s_range: intersection }),
1535 }
1536 }
1537}
1538
1539impl std::fmt::Debug for SRange {
1540 fn fmt(&self, f: &mut std::fmt::Formatter) -> std::fmt::Result {
1541 write!(f, "SRange {{ s0: {}, s1: {} }}", self.s0(), self.s1())
1542 }
1543}
1544
1545/// Directed longitudinal range of a specific Lane, identified by a LaneId.
1546/// Similar to [SRange], but associated with a specific Lane.
1547pub struct LaneSRange {
1548 pub(crate) lane_s_range: cxx::UniquePtr<maliput_sys::api::ffi::LaneSRange>,
1549}
1550
1551impl LaneSRange {
1552 /// Creates a new `LaneSRange` with the given `lane_id` and `s_range`.
1553 /// # Arguments
1554 /// * `lane_id` - A `String` representing the id of the lane.
1555 /// * `s_range` - A reference to an [SRange] that defines the longitudinal range of the lane.
1556 ///
1557 /// # Returns
1558 /// A new `LaneSRange` instance.
1559 pub fn new(lane_id: &String, s_range: &SRange) -> LaneSRange {
1560 LaneSRange {
1561 lane_s_range: maliput_sys::api::ffi::LaneSRange_new(lane_id, &s_range.s_range),
1562 }
1563 }
1564 /// Returns the lane id of the `LaneSRange`.
1565 ///
1566 /// # Returns
1567 /// A `String` containing the id of the lane associated with this `LaneSRange`.
1568 pub fn lane_id(&self) -> String {
1569 maliput_sys::api::ffi::LaneSRange_lane_id(&self.lane_s_range)
1570 }
1571 /// Returns the [SRange] of the `LaneSRange`.
1572 ///
1573 /// # Returns
1574 /// An [SRange] containing the longitudinal range of the lane associated with this `LaneSRange`.
1575 pub fn s_range(&self) -> SRange {
1576 SRange {
1577 s_range: maliput_sys::api::ffi::LaneSRange_s_range(&self.lane_s_range),
1578 }
1579 }
1580 /// Returns the length of the `LaneSRange`.
1581 ///
1582 /// This is equivalent to `s_range.size()`.
1583 ///
1584 /// # Returns
1585 /// A `f64` representing the length of the `LaneSRange`.
1586 pub fn length(&self) -> f64 {
1587 self.lane_s_range.length()
1588 }
1589 /// Determines whether this LaneSRange intersects with `lane_s_range`.
1590 ///
1591 /// # Arguments
1592 /// * `lane_s_range` - Another `LaneSRange` to check for intersection.
1593 /// * `tolerance` - A tolerance value to consider when checking for intersection.
1594 ///
1595 /// # Returns
1596 /// A boolean indicating whether this LaneSRange intersects with `lane_s_range`.
1597 pub fn intersects(&self, lane_s_range: &LaneSRange, tolerance: f64) -> bool {
1598 self.lane_s_range.Intersects(&lane_s_range.lane_s_range, tolerance)
1599 }
1600 /// Determines whether this LaneSRange contains `lane_s_range`.
1601 ///
1602 /// # Arguments
1603 /// * `lane_s_range` - Another `LaneSRange` to check if it is contained within this LaneSRange.
1604 /// * `tolerance` - A tolerance value to consider when checking for containment.
1605 ///
1606 /// # Returns
1607 /// A boolean indicating whether this LaneSRange contains `lane_s_range`.
1608 /// This checks if the `s_range` of `lane_s_range` is fully contained
1609 /// within the `s_range` of this `LaneSRange`, considering the lane id.
1610 /// If the lane id does not match, it returns false.
1611 pub fn contains(&self, lane_s_range: &LaneSRange, tolerance: f64) -> bool {
1612 self.lane_s_range.Contains(&lane_s_range.lane_s_range, tolerance)
1613 }
1614 /// Computes the intersection of this `LaneSRange` with `lane_s_range`.
1615 ///
1616 /// # Arguments
1617 /// * `lane_s_range` - Another `LaneSRange` to get the intersection with.
1618 /// * `tolerance` - A tolerance value to consider when checking for intersection.
1619 ///
1620 /// # Returns
1621 /// An `Option<LaneSRange>` containing the intersection of this `LaneSRange` with `lane_s_range`.
1622 /// If the lane ids do not match, it returns None.
1623 /// If the intersection is empty, it returns None.
1624 pub fn get_intersection(&self, lane_s_range: &LaneSRange, tolerance: f64) -> Option<LaneSRange> {
1625 let intersection = maliput_sys::api::ffi::LaneSRange_GetIntersection(
1626 &self.lane_s_range,
1627 &lane_s_range.lane_s_range,
1628 tolerance,
1629 );
1630 match intersection.is_null() {
1631 true => None,
1632 false => Some(LaneSRange {
1633 lane_s_range: intersection,
1634 }),
1635 }
1636 }
1637}
1638
1639impl std::fmt::Debug for LaneSRange {
1640 fn fmt(&self, f: &mut std::fmt::Formatter) -> std::fmt::Result {
1641 write!(
1642 f,
1643 "LaneSRange {{ lane_id: {}, s_range: {:?} }}",
1644 self.lane_id(),
1645 self.s_range()
1646 )
1647 }
1648}
1649
1650/// A route, possibly spanning multiple (end-to-end) lanes.
1651///
1652/// The sequence of [LaneSRange]s should be contiguous by either presenting
1653/// laterally adjacent [LaneSRange]s, or consecutive [LaneSRange]s. (In other words,
1654/// taken as a Lane-space path with r=0 and h=0, it should present a
1655/// G1-continuous curve.)
1656pub struct LaneSRoute {
1657 lane_s_route: cxx::UniquePtr<maliput_sys::api::ffi::LaneSRoute>,
1658}
1659
1660impl LaneSRoute {
1661 /// Creates a new `LaneSRoute` with the given `ranges`.
1662 ///
1663 /// # Arguments
1664 /// * `ranges` - A vector of [LaneSRange] to create the [LaneSRoute].
1665 ///
1666 /// # Returns
1667 /// A new `LaneSRoute` instance containing the provided ranges.
1668 pub fn new(ranges: Vec<LaneSRange>) -> LaneSRoute {
1669 let mut lane_s_ranges_cpp = cxx::CxxVector::new();
1670 for range in &ranges {
1671 lane_s_ranges_cpp
1672 .as_mut()
1673 .unwrap()
1674 .push(maliput_sys::api::ffi::ConstLaneSRangeRef {
1675 lane_s_range: &range.lane_s_range,
1676 });
1677 }
1678 LaneSRoute {
1679 lane_s_route: maliput_sys::api::ffi::LaneSRoute_new(&lane_s_ranges_cpp),
1680 }
1681 }
1682
1683 /// Returns the sequence of [LaneSRange]s.
1684 ///
1685 /// # Returns
1686 /// A vector of [LaneSRange]s that make up this [LaneSRoute].
1687 pub fn ranges(&self) -> Vec<LaneSRange> {
1688 let mut ranges = Vec::new();
1689 let lane_s_ranges = self.lane_s_route.ranges();
1690 for range in lane_s_ranges {
1691 ranges.push(LaneSRange {
1692 lane_s_range: maliput_sys::api::ffi::LaneSRange_new(
1693 &maliput_sys::api::ffi::LaneSRange_lane_id(range),
1694 maliput_sys::api::ffi::LaneSRange_s_range(range).as_ref().expect(""),
1695 ),
1696 })
1697 }
1698 ranges
1699 }
1700
1701 /// Computes the accumulated length of all [LaneSRange]s.
1702 ///
1703 /// # Returns
1704 /// A `f64` representing the total length of the [LaneSRoute].
1705 pub fn length(&self) -> f64 {
1706 self.lane_s_route.length()
1707 }
1708
1709 /// Determines whether this LaneSRoute intersects with `other`.
1710 ///
1711 /// # Arguments
1712 /// * `other` - The other LaneSRoute to check for intersection.
1713 /// * `tolerance` - The tolerance to use for intersection checks.
1714 ///
1715 /// # Returns
1716 /// * `true` if the two LaneSRoute intersect, `false` otherwise.
1717 pub fn intersects(&self, other: &LaneSRoute, tolerance: f64) -> bool {
1718 self.lane_s_route.Intersects(&other.lane_s_route, tolerance)
1719 }
1720}
1721
1722impl std::fmt::Debug for LaneSRoute {
1723 fn fmt(&self, f: &mut std::fmt::Formatter) -> std::fmt::Result {
1724 write!(f, "LaneSRoute {{ ranges: {:?} }}", self.ranges())
1725 }
1726}
1727
1728/// A specific endpoint of a specific Lane.
1729pub enum LaneEnd<'a> {
1730 /// The start of the Lane. ("s == 0")
1731 Start(Lane<'a>),
1732 /// The end of the Lane. ("s == length")
1733 Finish(Lane<'a>),
1734}
1735
1736impl LaneEnd<'_> {
1737 /// Gets the Lane of the `LaneEnd`.
1738 ///
1739 /// # Returns
1740 /// A reference to the [Lane] associated with this `LaneEnd`.
1741 /// This will return the Lane for both Start and Finish variants.
1742 pub fn lane(&self) -> &Lane {
1743 match self {
1744 LaneEnd::Start(lane) => lane,
1745 LaneEnd::Finish(lane) => lane,
1746 }
1747 }
1748}
1749
1750impl PartialEq for LaneEnd<'_> {
1751 fn eq(&self, other: &Self) -> bool {
1752 match self {
1753 LaneEnd::Start(lane) => match other {
1754 LaneEnd::Start(other_lane) => lane.id() == other_lane.id(),
1755 _ => false,
1756 },
1757 LaneEnd::Finish(lane) => match other {
1758 LaneEnd::Finish(other_lane) => lane.id() == other_lane.id(),
1759 _ => false,
1760 },
1761 }
1762 }
1763}
1764
1765impl Eq for LaneEnd<'_> {}
1766
1767impl std::fmt::Display for LaneEnd<'_> {
1768 fn fmt(&self, f: &mut std::fmt::Formatter) -> std::fmt::Result {
1769 match self {
1770 LaneEnd::Start(lane) => write!(f, "LaneEnd::Start({})", lane.id()),
1771 LaneEnd::Finish(lane) => write!(f, "LaneEnd::Finish({})", lane.id()),
1772 }
1773 }
1774}
1775
1776impl std::fmt::Debug for LaneEnd<'_> {
1777 fn fmt(&self, f: &mut std::fmt::Formatter) -> std::fmt::Result {
1778 match self {
1779 LaneEnd::Start(lane) => write!(f, "LaneEnd::Start({})", lane.id()),
1780 LaneEnd::Finish(lane) => write!(f, "LaneEnd::Finish({})", lane.id()),
1781 }
1782 }
1783}
1784
1785/// A set of LaneEnds.
1786pub struct LaneEndSet<'a> {
1787 lane_end_set: &'a maliput_sys::api::ffi::LaneEndSet,
1788}
1789
1790impl<'a> LaneEndSet<'a> {
1791 /// Obtains the size of the LaneEndSet.
1792 ///
1793 /// # Returns
1794 /// The number of LaneEnds in the set.
1795 pub fn size(&self) -> i32 {
1796 self.lane_end_set.size()
1797 }
1798 /// Gets the [LaneEnd] at the given index.
1799 ///
1800 /// # Arguments
1801 /// * `index` - The index of the LaneEnd to retrieve.
1802 ///
1803 /// # Returns
1804 /// A [Result<LaneEnd, MaliputError>] containing the LaneEnd at the given index.
1805 /// If the index is out of bounds, an error is returned.
1806 pub fn get(&self, index: i32) -> Result<LaneEnd, MaliputError> {
1807 let lane_end = self.lane_end_set.get(index)?;
1808 // Obtain end type and lane reference.
1809 let is_start = maliput_sys::api::ffi::LaneEnd_is_start(lane_end);
1810 let lane_ref = unsafe {
1811 maliput_sys::api::ffi::LaneEnd_lane(lane_end)
1812 .as_ref()
1813 .expect("Underlying LaneEnd is null")
1814 };
1815 // Create a LaneEnd enum variant.
1816 Ok(match is_start {
1817 true => LaneEnd::Start(Lane { lane: lane_ref }),
1818 false => LaneEnd::Finish(Lane { lane: lane_ref }),
1819 })
1820 }
1821
1822 /// Converts the LaneEndSet to a map of lane-id to LaneEnd.
1823 ///
1824 /// # Returns
1825 /// A `HashMap<String, LaneEnd>` where the key is the lane id and
1826 /// the value is the corresponding LaneEnd.
1827 pub fn to_lane_map(&self) -> std::collections::HashMap<String, LaneEnd> {
1828 (0..self.size())
1829 .map(|i| {
1830 let end = self.get(i).unwrap();
1831 (end.lane().id(), end)
1832 })
1833 .collect()
1834 }
1835}
1836
1837/// A BranchPoint is a node in the network of a RoadGeometry at which
1838/// Lanes connect to one another. A BranchPoint is a collection of LaneEnds
1839/// specifying the Lanes (and, in particular, which ends of the Lanes) are
1840/// connected at the BranchPoint.
1841///
1842/// LaneEnds participating in a BranchPoint are grouped into two sets,
1843/// arbitrarily named "A-side" and "B-side". LaneEnds on the same "side"
1844/// have coincident into-the-lane tangent vectors, which are anti-parallel
1845/// to those of LaneEnds on the other side.
1846pub struct BranchPoint<'a> {
1847 branch_point: &'a maliput_sys::api::ffi::BranchPoint,
1848}
1849
1850impl<'a> BranchPoint<'a> {
1851 /// Returns the id of the BranchPoint.
1852 ///
1853 /// # Returns
1854 /// A `String` containing the id of the BranchPoint.
1855 pub fn id(&self) -> String {
1856 maliput_sys::api::ffi::BranchPoint_id(self.branch_point)
1857 }
1858 /// Returns the [RoadGeometry] to which this BranchPoint belongs.
1859 ///
1860 /// # Returns
1861 /// A [RoadGeometry] containing the RoadGeometry to which this BranchPoint belongs.
1862 pub fn road_geometry(&self) -> RoadGeometry {
1863 unsafe {
1864 RoadGeometry {
1865 rg: self.branch_point.road_geometry().as_ref().expect(""),
1866 }
1867 }
1868 }
1869 /// Returns the set of [LaneEnd]s on the same side as the given [LaneEnd].
1870 /// E.g: For a T-junction, this would return the set of LaneEnds on the merging side.
1871 ///
1872 /// # Arguments
1873 /// * `end` - This branch's start or end [LaneEnd].
1874 ///
1875 /// # Return
1876 /// A [LaneEndSet] of [LaneEnd]s on the same side as the given [LaneEnd].
1877 pub fn get_confluent_branches(&self, end: &LaneEnd) -> Result<LaneEndSet, MaliputError> {
1878 let lane_end_set_ptr = self.branch_point.GetConfluentBranches(
1879 BranchPoint::from_lane_end_to_ffi(end)
1880 .as_ref()
1881 .expect("Underlying LaneEnd is null"),
1882 )?;
1883 Ok(LaneEndSet {
1884 lane_end_set: unsafe { lane_end_set_ptr.as_ref().expect("Underlying LaneEndSet is null") },
1885 })
1886 }
1887 /// Returns the set of [LaneEnd]s on the opposite side as the given [LaneEnd].
1888 /// E.g: For a T-junction, this would return the [LaneEnd]s which end flows into the junction.
1889 ///
1890 /// # Arguments
1891 /// * `end` - This branch's start or end [LaneEnd].
1892 ///
1893 /// # Return
1894 /// A [LaneEndSet] of [LaneEnd]s on the opposite side as the given [LaneEnd].
1895 pub fn get_ongoing_branches(&self, end: &LaneEnd) -> Result<LaneEndSet, MaliputError> {
1896 let lane_end_set_ptr = self.branch_point.GetOngoingBranches(
1897 BranchPoint::from_lane_end_to_ffi(end)
1898 .as_ref()
1899 .expect("Underlying LaneEnd is null"),
1900 )?;
1901 Ok(LaneEndSet {
1902 lane_end_set: unsafe { lane_end_set_ptr.as_ref().expect("Underlying LaneEndSet is null") },
1903 })
1904 }
1905 /// Returns the default ongoing branch (if any) for the given `end`.
1906 /// This typically represents what would be considered "continuing
1907 /// through-traffic" from `end` (e.g., as opposed to a branch executing
1908 /// a turn).
1909 ///
1910 /// If `end` has no default-branch at this BranchPoint, the return
1911 /// value will be None.
1912 ///
1913 /// # Arguments
1914 /// * `end` - The [LaneEnd] for which to get the default branch.
1915 ///
1916 /// # Returns
1917 /// An `Option<LaneEnd>` containing the default branch if it exists.
1918 /// If no default branch exists, it returns None.
1919 pub fn get_default_branch(&self, end: &LaneEnd) -> Option<LaneEnd> {
1920 let lane_end = maliput_sys::api::ffi::BranchPoint_GetDefaultBranch(
1921 self.branch_point,
1922 BranchPoint::from_lane_end_to_ffi(end)
1923 .as_ref()
1924 .expect("Underlying LaneEnd is null"),
1925 );
1926 match lane_end.is_null() {
1927 true => None,
1928 false => {
1929 let lane_end_ref: &maliput_sys::api::ffi::LaneEnd =
1930 lane_end.as_ref().expect("Underlying LaneEnd is null");
1931 let is_start = maliput_sys::api::ffi::LaneEnd_is_start(lane_end_ref);
1932 let lane_ref = unsafe {
1933 maliput_sys::api::ffi::LaneEnd_lane(lane_end_ref)
1934 .as_ref()
1935 .expect("Underlying LaneEnd is null")
1936 };
1937 match is_start {
1938 true => Some(LaneEnd::Start(Lane { lane: lane_ref })),
1939 false => Some(LaneEnd::Finish(Lane { lane: lane_ref })),
1940 }
1941 }
1942 }
1943 }
1944 /// Returns the set of LaneEnds grouped together on the "A-side".
1945 ///
1946 /// # Returns
1947 /// A [LaneEndSet] containing the LaneEnds on the "A-side" of the BranchPoint.
1948 pub fn get_a_side(&self) -> LaneEndSet {
1949 let lane_end_set_ptr = self.branch_point.GetASide();
1950 LaneEndSet {
1951 lane_end_set: unsafe { lane_end_set_ptr.as_ref().expect("Underlying LaneEndSet is null") },
1952 }
1953 }
1954 /// Returns the set of LaneEnds grouped together on the "B-side".
1955 ///
1956 /// # Returns
1957 /// A [LaneEndSet] containing the LaneEnds on the "B-side" of the BranchPoint.
1958 /// This is the opposite side of the "A-side".
1959 pub fn get_b_side(&self) -> LaneEndSet {
1960 let lane_end_set_ptr = self.branch_point.GetBSide();
1961 LaneEndSet {
1962 lane_end_set: unsafe { lane_end_set_ptr.as_ref().expect("Underlying LaneEndSet is null") },
1963 }
1964 }
1965 /// Converts LaneEnd enum to LaneEnd ffi.
1966 fn from_lane_end_to_ffi(end: &LaneEnd) -> cxx::UniquePtr<maliput_sys::api::ffi::LaneEnd> {
1967 match end {
1968 LaneEnd::Start(lane) => unsafe { maliput_sys::api::ffi::LaneEnd_new(lane.lane, true) },
1969 LaneEnd::Finish(lane) => unsafe { maliput_sys::api::ffi::LaneEnd_new(lane.lane, false) },
1970 }
1971 }
1972}
1973
1974/// An abstract convenience class that aggregates information pertaining to an
1975/// intersection. Its primary purpose is to serve as a single source of this
1976/// information and to remove the need for users to query numerous disparate
1977/// data structures and state providers.
1978pub struct Intersection<'a> {
1979 intersection: &'a mut maliput_sys::api::ffi::Intersection,
1980}
1981
1982impl<'a> Intersection<'a> {
1983 /// Returns the id of the `Intersection` as a string.
1984 pub fn id(&self) -> String {
1985 maliput_sys::api::ffi::Intersection_id(self.intersection)
1986 }
1987}
1988
1989/// A book of Intersections.
1990pub struct IntersectionBook<'a> {
1991 intersection_book: &'a mut maliput_sys::api::ffi::IntersectionBook,
1992}
1993
1994impl<'a> IntersectionBook<'a> {
1995 /// Returns all Intersections in the book.
1996 ///
1997 /// # Returns
1998 /// A vector of [Intersection]s containing all Intersections in the book.
1999 pub fn get_intersections(&mut self) -> Vec<Intersection> {
2000 let book_pin = unsafe { std::pin::Pin::new_unchecked(&mut *self.intersection_book) };
2001 let intersections_cpp = maliput_sys::api::ffi::IntersectionBook_GetIntersections(book_pin);
2002 unsafe {
2003 intersections_cpp
2004 .into_iter()
2005 .map(|intersection| Intersection {
2006 intersection: intersection
2007 .intersection
2008 .as_mut()
2009 .expect("Underlying Intersection is null"),
2010 })
2011 .collect::<Vec<Intersection>>()
2012 }
2013 }
2014
2015 /// Gets the specified Intersection.
2016 ///
2017 /// # Arguments
2018 /// * `id` - The id of the Intersection to get.
2019 ///
2020 /// # Returns
2021 /// * An `Option<Intersection>`
2022 /// * Some(Intersection) - The Intersection with the specified id.
2023 /// * None - If the Intersection with the specified id does not exist.
2024 pub fn get_intersection(&mut self, id: &str) -> Option<Intersection> {
2025 let book_pin = unsafe { std::pin::Pin::new_unchecked(&mut *self.intersection_book) };
2026 let intersection_option = unsafe {
2027 maliput_sys::api::ffi::IntersectionBook_GetIntersection(book_pin, &String::from(id))
2028 .intersection
2029 .as_mut()
2030 };
2031 match &intersection_option {
2032 None => None,
2033 Some(_) => Some(Intersection {
2034 intersection: intersection_option.expect("Underlying Intersection is null"),
2035 }),
2036 }
2037 }
2038}
2039
2040mod tests {
2041 mod lane_position {
2042 #[test]
2043 fn lane_position_new() {
2044 let lane_pos = crate::api::LanePosition::new(1.0, 2.0, 3.0);
2045 assert_eq!(lane_pos.s(), 1.0);
2046 assert_eq!(lane_pos.r(), 2.0);
2047 assert_eq!(lane_pos.h(), 3.0);
2048 }
2049
2050 #[test]
2051 fn equality() {
2052 let v = crate::api::LanePosition::new(1.0, 2.0, 3.0);
2053 let w = crate::api::LanePosition::new(1.0, 2.0, 3.0);
2054 assert_eq!(v, w);
2055 let z = crate::api::LanePosition::new(4.0, 5.0, 6.0);
2056 assert_ne!(v, z);
2057 }
2058
2059 #[test]
2060 fn set_s() {
2061 let mut lane_pos = crate::api::LanePosition::new(1.0, 2.0, 3.0);
2062 lane_pos.set_s(4.0);
2063 assert_eq!(lane_pos.s(), 4.0);
2064 }
2065
2066 #[test]
2067 fn set_r() {
2068 let mut lane_pos = crate::api::LanePosition::new(1.0, 2.0, 3.0);
2069 lane_pos.set_r(4.0);
2070 assert_eq!(lane_pos.r(), 4.0);
2071 }
2072
2073 #[test]
2074 fn set_h() {
2075 let mut lane_pos = crate::api::LanePosition::new(1.0, 2.0, 3.0);
2076 lane_pos.set_h(4.0);
2077 assert_eq!(lane_pos.h(), 4.0);
2078 }
2079
2080 #[test]
2081 fn set_srh() {
2082 use crate::math::Vector3;
2083 let mut lane_pos = crate::api::LanePosition::new(1.0, 2.0, 3.0);
2084 let vector = Vector3::new(4.0, 5.0, 6.0);
2085 lane_pos.set_srh(&vector);
2086 assert_eq!(lane_pos.s(), 4.0);
2087 assert_eq!(lane_pos.r(), 5.0);
2088 assert_eq!(lane_pos.h(), 6.0);
2089 }
2090 }
2091
2092 mod inertial_position {
2093
2094 #[test]
2095 fn inertial_position_new() {
2096 let inertial_pos = crate::api::InertialPosition::new(1.0, 2.0, 3.0);
2097 assert_eq!(inertial_pos.x(), 1.0);
2098 assert_eq!(inertial_pos.y(), 2.0);
2099 assert_eq!(inertial_pos.z(), 3.0);
2100 }
2101
2102 #[test]
2103 fn equality() {
2104 let v = crate::api::InertialPosition::new(1.0, 2.0, 3.0);
2105 let w = crate::api::InertialPosition::new(1.0, 2.0, 3.0);
2106 assert_eq!(v, w);
2107 let z = crate::api::InertialPosition::new(4.0, 5.0, 6.0);
2108 assert_ne!(v, z);
2109 }
2110
2111 #[test]
2112 fn set_x() {
2113 let mut inertial_pos = crate::api::InertialPosition::new(1.0, 2.0, 3.0);
2114 inertial_pos.set_x(4.0);
2115 assert_eq!(inertial_pos.x(), 4.0);
2116 }
2117
2118 #[test]
2119 fn set_y() {
2120 let mut inertial_pos = crate::api::InertialPosition::new(1.0, 2.0, 3.0);
2121 inertial_pos.set_y(4.0);
2122 assert_eq!(inertial_pos.y(), 4.0);
2123 }
2124
2125 #[test]
2126 fn set_z() {
2127 let mut inertial_pos = crate::api::InertialPosition::new(1.0, 2.0, 3.0);
2128 inertial_pos.set_z(4.0);
2129 assert_eq!(inertial_pos.z(), 4.0);
2130 }
2131
2132 #[test]
2133 fn set_xyz() {
2134 use crate::math::Vector3;
2135 let mut inertial_pos = crate::api::InertialPosition::new(1.0, 2.0, 3.0);
2136 let vector = Vector3::new(4.0, 5.0, 6.0);
2137 inertial_pos.set_xyz(&vector);
2138 assert_eq!(inertial_pos.x(), 4.0);
2139 assert_eq!(inertial_pos.y(), 5.0);
2140 assert_eq!(inertial_pos.z(), 6.0);
2141 }
2142
2143 #[test]
2144 fn xyz() {
2145 let inertial_pos = crate::api::InertialPosition::new(1.0, 2.0, 3.0);
2146 assert_eq!(inertial_pos.xyz(), crate::math::Vector3::new(1.0, 2.0, 3.0));
2147 }
2148
2149 #[test]
2150 fn length() {
2151 let inertial_pos = crate::api::InertialPosition::new(3.0, 0.0, 4.0);
2152 assert_eq!(inertial_pos.length(), 5.0);
2153 }
2154
2155 #[test]
2156 fn distance() {
2157 let inertial_pos1 = crate::api::InertialPosition::new(1.0, 1.0, 1.0);
2158 let inertial_pos2 = crate::api::InertialPosition::new(5.0, 1.0, 1.0);
2159 assert_eq!(inertial_pos1.distance(&inertial_pos2), 4.0);
2160 }
2161
2162 #[test]
2163 fn str() {
2164 let inertial_pos = crate::api::InertialPosition::new(1.0, 2.0, 3.0);
2165 assert_eq!(inertial_pos.to_string(), "(x = 1, y = 2, z = 3)");
2166 }
2167
2168 #[test]
2169 fn add_operation() {
2170 let inertial_pos1 = crate::api::InertialPosition::new(1.0, 2.0, 3.0);
2171 let inertial_pos2 = crate::api::InertialPosition::new(4.0, 5.0, 6.0);
2172 let inertial_pos3 = inertial_pos1 + inertial_pos2;
2173 assert_eq!(inertial_pos3.x(), 5.0);
2174 assert_eq!(inertial_pos3.y(), 7.0);
2175 assert_eq!(inertial_pos3.z(), 9.0);
2176 }
2177
2178 #[test]
2179 fn sub_operation() {
2180 let inertial_pos1 = crate::api::InertialPosition::new(1.0, 2.0, 3.0);
2181 let inertial_pos2 = crate::api::InertialPosition::new(4.0, 5.0, 6.0);
2182 let inertial_pos3 = inertial_pos1 - inertial_pos2;
2183 assert_eq!(inertial_pos3.x(), -3.0);
2184 assert_eq!(inertial_pos3.y(), -3.0);
2185 assert_eq!(inertial_pos3.z(), -3.0);
2186 }
2187
2188 #[test]
2189 fn mul_scalar_operation() {
2190 let inertial_pos1 = crate::api::InertialPosition::new(1.0, 2.0, 3.0);
2191 let inertial_pos2 = inertial_pos1 * 2.0;
2192 assert_eq!(inertial_pos2.x(), 2.0);
2193 assert_eq!(inertial_pos2.y(), 4.0);
2194 assert_eq!(inertial_pos2.z(), 6.0);
2195 }
2196 }
2197 mod rotation {
2198 #[test]
2199 fn rotation_new() {
2200 let rotation = crate::api::Rotation::new();
2201 assert_eq!(rotation.roll(), 0.0);
2202 assert_eq!(rotation.pitch(), 0.0);
2203 assert_eq!(rotation.yaw(), 0.0);
2204 }
2205
2206 #[test]
2207 fn from_quat() {
2208 let quat = crate::math::Quaternion::new(1.0, 0.0, 0.0, 0.0);
2209 let rotation = crate::api::Rotation::from_quat(&quat);
2210 assert_eq!(rotation.roll(), 0.0);
2211 assert_eq!(rotation.pitch(), 0.0);
2212 assert_eq!(rotation.yaw(), 0.0);
2213 }
2214
2215 #[test]
2216 fn from_rpy() {
2217 let rpy = crate::math::RollPitchYaw::new(0.0, 0.0, 0.0);
2218 let rotation = crate::api::Rotation::from_rpy(&rpy);
2219 assert_eq!(rotation.roll(), 0.0);
2220 assert_eq!(rotation.pitch(), 0.0);
2221 assert_eq!(rotation.yaw(), 0.0);
2222 }
2223
2224 #[test]
2225 fn set_quat() {
2226 let mut rotation = crate::api::Rotation::new();
2227 let quat = crate::math::Quaternion::new(1.0, 0.0, 0.0, 0.0);
2228 rotation.set_quat(&quat);
2229 assert_eq!(rotation.roll(), 0.0);
2230 assert_eq!(rotation.pitch(), 0.0);
2231 assert_eq!(rotation.yaw(), 0.0);
2232 }
2233
2234 #[test]
2235 fn matrix() {
2236 let rotation = crate::api::Rotation::new();
2237 let matrix = rotation.matrix();
2238 assert_eq!(matrix.row(0), crate::math::Vector3::new(1.0, 0.0, 0.0));
2239 assert_eq!(matrix.row(1), crate::math::Vector3::new(0.0, 1.0, 0.0));
2240 assert_eq!(matrix.row(2), crate::math::Vector3::new(0.0, 0.0, 1.0));
2241 }
2242 }
2243
2244 mod s_range {
2245 #[test]
2246 fn s_range_new() {
2247 let s_range = crate::api::SRange::new(1.0, 2.0);
2248 assert_eq!(s_range.s0(), 1.0);
2249 assert_eq!(s_range.s1(), 2.0);
2250 }
2251 #[test]
2252 fn s_range_api() {
2253 let s_range_1 = crate::api::SRange::new(1.0, 3.0);
2254 let s_range_2 = crate::api::SRange::new(2.0, 4.0);
2255 assert_eq!(s_range_1.size(), 2.0);
2256 assert!(s_range_1.with_s());
2257 assert!(s_range_1.intersects(&s_range_2, 0.0));
2258 assert!(!s_range_1.contains(&s_range_2, 0.0));
2259 }
2260 #[test]
2261 fn s_range_setters() {
2262 let mut s_range = crate::api::SRange::new(0.0, 4.0);
2263 s_range.set_s0(1.0);
2264 s_range.set_s1(3.0);
2265 assert_eq!(s_range.s0(), 1.0);
2266 assert_eq!(s_range.s1(), 3.0);
2267 }
2268 #[test]
2269 fn s_range_get_intersection_with_intersection() {
2270 let s_range_1 = crate::api::SRange::new(1.0, 3.0);
2271 let s_range_2 = crate::api::SRange::new(2.0, 4.0);
2272 let intersection = s_range_1.get_intersection(&s_range_2, 0.0);
2273 assert!(intersection.is_some());
2274 let intersection = intersection.unwrap();
2275 assert_eq!(intersection.s0(), 2.0);
2276 assert_eq!(intersection.s1(), 3.0);
2277 }
2278 #[test]
2279 fn s_range_get_intersection_with_no_intersection() {
2280 let s_range_1 = crate::api::SRange::new(1.0, 2.0);
2281 let s_range_2 = crate::api::SRange::new(3.0, 4.0);
2282 let intersection = s_range_1.get_intersection(&s_range_2, 0.0);
2283 assert!(intersection.is_none());
2284 }
2285 }
2286
2287 mod lane_s_range {
2288 #[test]
2289 fn lane_s_range_new() {
2290 let lane_s_range =
2291 crate::api::LaneSRange::new(&String::from("lane_test"), &crate::api::SRange::new(1.0, 2.0));
2292 assert_eq!(lane_s_range.lane_id(), "lane_test");
2293 assert_eq!(lane_s_range.s_range().s0(), 1.0);
2294 assert_eq!(lane_s_range.s_range().s1(), 2.0);
2295 assert_eq!(lane_s_range.length(), 1.0);
2296 }
2297 #[test]
2298 fn lane_s_range_api() {
2299 let lane_s_range_1 =
2300 crate::api::LaneSRange::new(&String::from("lane_test"), &crate::api::SRange::new(1.0, 2.0));
2301 let lane_s_range_2 =
2302 crate::api::LaneSRange::new(&String::from("lane_test"), &crate::api::SRange::new(2.0, 3.0));
2303 assert!(lane_s_range_1.intersects(&lane_s_range_2, 0.0));
2304 assert!(!lane_s_range_1.contains(&lane_s_range_2, 0.0));
2305 }
2306 #[test]
2307 fn lane_s_range_get_intersection_with_intersection() {
2308 let lane_s_range_1 =
2309 crate::api::LaneSRange::new(&String::from("lane_test"), &crate::api::SRange::new(1.0, 3.0));
2310 let lane_s_range_2 =
2311 crate::api::LaneSRange::new(&String::from("lane_test"), &crate::api::SRange::new(2.0, 4.0));
2312 let intersection = lane_s_range_1.get_intersection(&lane_s_range_2, 0.0);
2313 assert!(intersection.is_some());
2314 let intersection = intersection.unwrap();
2315 assert_eq!(intersection.lane_id(), "lane_test");
2316 assert_eq!(intersection.s_range().s0(), 2.0);
2317 assert_eq!(intersection.s_range().s1(), 3.0);
2318 }
2319 #[test]
2320 fn lane_s_range_get_intersection_with_no_intersection() {
2321 let lane_s_range_1 =
2322 crate::api::LaneSRange::new(&String::from("lane test_1"), &crate::api::SRange::new(1.0, 3.0));
2323 let lane_s_range_2 =
2324 crate::api::LaneSRange::new(&String::from("lane_test_2"), &crate::api::SRange::new(2.0, 4.0));
2325 let intersection = lane_s_range_1.get_intersection(&lane_s_range_2, 0.0);
2326 assert!(intersection.is_none());
2327 }
2328 }
2329
2330 mod lane_s_route {
2331 // Helper function to create a LaneSRoute
2332 // with two LaneSRange.
2333 // # Arguments
2334 // * `s0_0` - The s0 of the first LaneSRange.
2335 // * `s1_0` - The s1 of the first LaneSRange.
2336 // * `s0_1` - The s0 of the second LaneSRange.
2337 // * `s1_1` - The s1 of the second LaneSRange.
2338 fn _get_lane_s_route(s0_0: f64, s1_0: f64, s0_1: f64, s1_1: f64) -> crate::api::LaneSRoute {
2339 let ranges = vec![
2340 crate::api::LaneSRange::new(&String::from("lane_test_1"), &crate::api::SRange::new(s0_0, s1_0)),
2341 crate::api::LaneSRange::new(&String::from("lane_test_2"), &crate::api::SRange::new(s0_1, s1_1)),
2342 ];
2343 crate::api::LaneSRoute::new(ranges)
2344 }
2345 #[test]
2346 fn lane_s_route_new() {
2347 let lane_s_route = _get_lane_s_route(0., 10., 0., 15.);
2348 assert!(!lane_s_route.lane_s_route.is_null());
2349 let ranges = lane_s_route.ranges();
2350 assert_eq!(ranges.len(), 2);
2351 assert_eq!(ranges[0].lane_id(), "lane_test_1");
2352 assert_eq!(ranges[1].lane_id(), "lane_test_2");
2353 }
2354 #[test]
2355 fn lane_s_route_length() {
2356 let lane_s_route = _get_lane_s_route(0., 10., 0., 15.);
2357 assert_eq!(lane_s_route.length(), 25.0);
2358 }
2359 #[test]
2360 fn lane_s_route_intersects() {
2361 let lane_s_route = _get_lane_s_route(0., 10., 0., 10.);
2362 let lane_s_route_that_intersects = _get_lane_s_route(5., 9., 5., 9.);
2363 let lane_s_route_that_not_intersects = _get_lane_s_route(11., 20., 11., 20.);
2364 assert!(lane_s_route.intersects(&lane_s_route_that_intersects, 0.0));
2365 assert!(!lane_s_route.intersects(&lane_s_route_that_not_intersects, 0.0));
2366 }
2367 }
2368}