maliput_query/
maliput_query.rs

1// BSD 3-Clause License
2//
3// Copyright (c) 2025, 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
31//! A command line tool to query a road network built from a road network file.
32//!
33//! e.g: cargo run --bin maliput_query -- maliput/data/xodr/ArcLane.xodr
34//! e.g: cargo run --bin maliput_query -- --backend maliput_geopackage path/to/file.gpkg
35
36use clap::Parser;
37
38use maliput::api::{LanePosition, RoadNetwork, RoadNetworkBackend};
39use maliput::common::MaliputError;
40use std::collections::HashMap;
41
42// Define the MaliputQuery enum to represent different types of queries that can be made to the road network.
43enum MaliputQuery {
44    PrintAllLanes,
45    GetNumberOfLanes,
46    GetTotalLengthOfTheRoadGeometry,
47    GetLinearTolerance,
48    GetAngularTolerance,
49    GetLaneLength(String),
50    GetLaneBounds(String, f64),                                           // lane_id, s
51    GetSegmentBounds(String, f64),                                        // lane_id, s
52    ToRoadPosition(f64, f64, f64),                                        // x, y, z
53    FindSurfaceRoadPositionsAtXY(f64, f64, f64),                          // x, y, radius
54    ToLanePosition(String, f64, f64, f64),                                // lane_id, x, y, z
55    ToSegmentPosition(String, f64, f64, f64),                             // lane_id, x, y, z
56    ToInertialPosition(String, f64, f64, f64),                            // lane_id, s, r, h
57    GetOrientaiton(String, f64, f64, f64),                                // lane_id, s, r, h
58    OpenScenarioRoadPositionToMaliputRoadPosition(String, f64, f64),      // xodr_road_id, xodr_s, xodr_t
59    OpenScenarioLanePositionToMaliputRoadPosition(String, f64, i64, f64), // xodr_road_id, xodr_s, xodr_lane_id, offset
60    MaliputRoadPositionToOpenScenarioRoadPosition(String, f64, f64, f64), // lane_id, s, r, h
61    MaliputRoadPositionToOpenScenarioLanePosition(String, f64, f64, f64), // lane_id, s, r, h
62    OpenScenarioRelativeRoadPositionToMaliputRoadPosition(String, f64, f64, f64, f64), // xodr_road_id, xodr_s, xodr_t, xodr_ds, xodr_dt
63    OpenScenarioRelativeLanePositionWithDsToMaliputRoadPosition(String, i64, f64, f64, f64, f64), // xodr_road_id, xodr_lane_id, xodr_s, d_lane, xodr_ds, offset
64    OpenScenarioRelativeLanePositionWithDsLaneToMaliputRoadPosition(String, i64, f64, f64, f64, f64), // xodr_road_id, xodr_lane_id, xodr_s, d_lane, xodr_ds_lane, offset
65    GetRoadOrientationAtOpenScenarioRoadPosition(String, f64, f64), // xodr_road_id, xodr_s, xodr_t
66    Invalid,
67}
68
69impl MaliputQuery {
70    /// Converts the MaliputQuery to a string format that can be used in backend custom commands architecture.
71    /// ### Returns
72    /// - Some(String): A string representation of the command if it is a valid query.
73    /// - None: If the query is not a valid command for backend custom commands.
74    fn to_backend_custom_command_format(&self) -> Option<String> {
75        match self {
76            MaliputQuery::OpenScenarioRoadPositionToMaliputRoadPosition(xodr_road_id, xodr_s, xodr_t) => Some(format!(
77                "OpenScenarioRoadPositionToMaliputRoadPosition,{},{},{}",
78                xodr_road_id, xodr_s, xodr_t
79            )),
80            MaliputQuery::OpenScenarioLanePositionToMaliputRoadPosition(xodr_road_id, xodr_s, xodr_lane_id, offset) => {
81                Some(format!(
82                    "OpenScenarioLanePositionToMaliputRoadPosition,{},{},{},{}",
83                    xodr_road_id, xodr_s, xodr_lane_id, offset
84                ))
85            }
86            MaliputQuery::MaliputRoadPositionToOpenScenarioRoadPosition(lane_id, s, r, h) => Some(format!(
87                "MaliputRoadPositionToOpenScenarioRoadPosition,{},{},{},{}",
88                lane_id, s, r, h
89            )),
90            MaliputQuery::MaliputRoadPositionToOpenScenarioLanePosition(lane_id, s, r, h) => Some(format!(
91                "MaliputRoadPositionToOpenScenarioLanePosition,{},{},{},{}",
92                lane_id, s, r, h
93            )),
94            MaliputQuery::OpenScenarioRelativeRoadPositionToMaliputRoadPosition(
95                xodr_road_id,
96                xodr_s,
97                xodr_t,
98                xodr_ds,
99                xodr_dt,
100            ) => Some(format!(
101                "OpenScenarioRelativeRoadPositionToMaliputRoadPosition,{},{},{},{},{}",
102                xodr_road_id, xodr_s, xodr_t, xodr_ds, xodr_dt
103            )),
104            MaliputQuery::OpenScenarioRelativeLanePositionWithDsToMaliputRoadPosition(
105                xodr_road_id,
106                xodr_lane_id,
107                xodr_s,
108                d_lane,
109                xodr_ds,
110                offset,
111            ) => Some(format!(
112                "OpenScenarioRelativeLanePositionWithDsToMaliputRoadPosition,{},{},{},{},{},{}",
113                xodr_road_id, xodr_lane_id, xodr_s, d_lane, xodr_ds, offset
114            )),
115            MaliputQuery::OpenScenarioRelativeLanePositionWithDsLaneToMaliputRoadPosition(
116                xodr_road_id,
117                xodr_lane_id,
118                xodr_s,
119                d_lane,
120                xodr_ds_lane,
121                offset,
122            ) => Some(format!(
123                "OpenScenarioRelativeLanePositionWithDsLaneToMaliputRoadPosition,{},{},{},{},{},{}",
124                xodr_road_id, xodr_lane_id, xodr_s, d_lane, xodr_ds_lane, offset
125            )),
126            MaliputQuery::GetRoadOrientationAtOpenScenarioRoadPosition(xodr_road_id, xodr_s, xodr_t) => Some(format!(
127                "GetRoadOrientationAtOpenScenarioRoadPosition,{},{},{}",
128                xodr_road_id, xodr_s, xodr_t
129            )),
130            _ => None,
131        }
132    }
133}
134
135impl From<Vec<&str>> for MaliputQuery {
136    fn from(args: Vec<&str>) -> Self {
137        match args.as_slice() {
138            ["PrintAllLanes"] => MaliputQuery::PrintAllLanes,
139            ["GetNumberOfLanes"] => MaliputQuery::GetNumberOfLanes,
140            ["GetTotalLengthOfTheRoadGeometry"] => MaliputQuery::GetTotalLengthOfTheRoadGeometry,
141            ["GetLinearTolerance"] => MaliputQuery::GetLinearTolerance,
142            ["GetAngularTolerance"] => MaliputQuery::GetAngularTolerance,
143            ["GetLaneLength", lane_id] => MaliputQuery::GetLaneLength(lane_id.to_string()),
144            ["GetLaneBounds", lane_id, s] => MaliputQuery::GetLaneBounds(lane_id.to_string(), s.parse().unwrap()),
145            ["GetSegmentBounds", lane_id, s] => MaliputQuery::GetSegmentBounds(lane_id.to_string(), s.parse().unwrap()),
146            ["ToRoadPosition", x, y, z] => {
147                MaliputQuery::ToRoadPosition(x.parse().unwrap(), y.parse().unwrap(), z.parse().unwrap())
148            }
149            ["FindSurfaceRoadPositionsAtXY", x, y, radius] => MaliputQuery::FindSurfaceRoadPositionsAtXY(
150                x.parse().unwrap(),
151                y.parse().unwrap(),
152                radius.parse().unwrap(),
153            ),
154            ["ToLanePosition", lane_id, x, y, z] => MaliputQuery::ToLanePosition(
155                lane_id.to_string(),
156                x.parse().unwrap(),
157                y.parse().unwrap(),
158                z.parse().unwrap(),
159            ),
160            ["ToSegmentPosition", lane_id, x, y, z] => MaliputQuery::ToSegmentPosition(
161                lane_id.to_string(),
162                x.parse().unwrap(),
163                y.parse().unwrap(),
164                z.parse().unwrap(),
165            ),
166            ["ToInertialPosition", lane_id, s, r, h] => MaliputQuery::ToInertialPosition(
167                lane_id.to_string(),
168                s.parse().unwrap(),
169                r.parse().unwrap(),
170                h.parse().unwrap(),
171            ),
172            ["GetOrientation", lane_id, s, r, h] => MaliputQuery::GetOrientaiton(
173                lane_id.to_string(),
174                s.parse().unwrap(),
175                r.parse().unwrap(),
176                h.parse().unwrap(),
177            ),
178            ["OpenScenarioRoadPositionToMaliputRoadPosition", xodr_road_id, xodr_s, xodr_t] => {
179                MaliputQuery::OpenScenarioRoadPositionToMaliputRoadPosition(
180                    xodr_road_id.to_string(),
181                    xodr_s.parse().unwrap(),
182                    xodr_t.parse().unwrap(),
183                )
184            }
185            ["OpenScenarioLanePositionToMaliputRoadPosition", xodr_road_id, xodr_s, xodr_lane_id, offset] => {
186                MaliputQuery::OpenScenarioLanePositionToMaliputRoadPosition(
187                    xodr_road_id.to_string(),
188                    xodr_s.parse().unwrap(),
189                    xodr_lane_id.parse().unwrap(),
190                    offset.parse().unwrap(),
191                )
192            }
193            ["MaliputRoadPositionToOpenScenarioRoadPosition", lane_id, s, r, h] => {
194                MaliputQuery::MaliputRoadPositionToOpenScenarioRoadPosition(
195                    lane_id.to_string(),
196                    s.parse().unwrap(),
197                    r.parse().unwrap(),
198                    h.parse().unwrap(),
199                )
200            }
201            ["MaliputRoadPositionToOpenScenarioLanePosition", lane_id, s, r, h] => {
202                MaliputQuery::MaliputRoadPositionToOpenScenarioLanePosition(
203                    lane_id.to_string(),
204                    s.parse().unwrap(),
205                    r.parse().unwrap(),
206                    h.parse().unwrap(),
207                )
208            }
209            ["OpenScenarioRelativeRoadPositionToMaliputRoadPosition", xodr_road_id, xodr_s, xodr_t, xodr_ds, xodr_dt] => {
210                MaliputQuery::OpenScenarioRelativeRoadPositionToMaliputRoadPosition(
211                    xodr_road_id.to_string(),
212                    xodr_s.parse().unwrap(),
213                    xodr_t.parse().unwrap(),
214                    xodr_ds.parse().unwrap(),
215                    xodr_dt.parse().unwrap(),
216                )
217            }
218            ["OpenScenarioRelativeLanePositionWithDsToMaliputRoadPosition", xodr_road_id, xodr_lane_id, xodr_s, d_lane, xodr_ds, offset] => {
219                MaliputQuery::OpenScenarioRelativeLanePositionWithDsToMaliputRoadPosition(
220                    xodr_road_id.to_string(),
221                    xodr_lane_id.parse().unwrap(),
222                    xodr_s.parse().unwrap(),
223                    d_lane.parse().unwrap(),
224                    xodr_ds.parse().unwrap(),
225                    offset.parse().unwrap(),
226                )
227            }
228            ["OpenScenarioRelativeLanePositionWithDsLaneToMaliputRoadPosition", xodr_road_id, xodr_lane_id, xodr_s, d_lane, xodr_ds_lane, offset] => {
229                MaliputQuery::OpenScenarioRelativeLanePositionWithDsLaneToMaliputRoadPosition(
230                    xodr_road_id.to_string(),
231                    xodr_lane_id.parse().unwrap(),
232                    xodr_s.parse().unwrap(),
233                    d_lane.parse().unwrap(),
234                    xodr_ds_lane.parse().unwrap(),
235                    offset.parse().unwrap(),
236                )
237            }
238            ["GetRoadOrientationAtOpenScenarioRoadPosition", xodr_road_id, xodr_s, xodr_t] => {
239                MaliputQuery::GetRoadOrientationAtOpenScenarioRoadPosition(
240                    xodr_road_id.to_string(),
241                    xodr_s.parse().unwrap(),
242                    xodr_t.parse().unwrap(),
243                )
244            }
245            _ => MaliputQuery::Invalid,
246        }
247    }
248}
249
250impl MaliputQuery {
251    fn print_available_queries() {
252        println!("-> Available commands:");
253        println!("\t* General commands:");
254        println!("\t\t1. PrintAllLanes");
255        println!("\t\t2. GetNumberOfLanes");
256        println!("\t\t3. GetTotalLengthOfTheRoadGeometry");
257        println!("\t\t4. GetLinearTolerance");
258        println!("\t\t5. GetAngularTolerance");
259        println!("\t\t6. GetLaneLength <lane_id>");
260        println!("\t\t7. GetLaneBounds <lane_id> <s>");
261        println!("\t\t8. GetSegmentBounds <lane_id> <s>");
262        println!("\t\t9. ToRoadPosition <x> <y> <z>");
263        println!("\t\t10. ToLanePosition <lane_id> <x> <y> <z>");
264        println!("\t\t11. ToSegmentPosition <lane_id> <x> <y> <z>");
265        println!("\t\t12. ToInertialPosition <lane_id> <s> <r> <h>");
266        println!("\t\t13. GetOrientation <lane_id> <s> <r> <h>");
267        println!("\t\t14. FindSurfaceRoadPositionsAtXY <x> <y> <radius>");
268        println!("\t* Commands particular to maliput_malidrive / OpenDRIVE specification:");
269        println!("\t\t1. OpenScenarioRoadPositionToMaliputRoadPosition <xodr_road_id> <xodr_s> <xodr_t>");
270        println!(
271            "\t\t2. OpenScenarioLanePositionToMaliputRoadPosition <xodr_road_id> <xodr_s> <xodr_lane_id> <offset>"
272        );
273        println!("\t\t3. MaliputRoadPositionToOpenScenarioRoadPosition <lane_id> <s> <r> <h>");
274        println!("\t\t4. MaliputRoadPositionToOpenScenarioLanePosition <lane_id> <s> <r> <h>");
275        println!("\t\t5. OpenScenarioRelativeRoadPositionToMaliputRoadPosition <xodr_road_id> <xodr_s> <xodr_t> <xodr_ds> <xodr_dt>");
276        println!("\t\t6. OpenScenarioRelativeLanePositionWithDsToMaliputRoadPosition <xodr_road_id> <xodr_lane_id> <xodr_s> <d_lane> <xodr_ds> <offset>");
277        println!("\t\t7. OpenScenarioRelativeLanePositionWithDsLaneToMaliputRoadPosition <xodr_road_id> <xodr_lane_id> <xodr_s> <d_lane> <xodr_ds_lane> <offset>");
278        println!("\t\t8. GetRoadOrientationAtOpenScenarioRoadPosition <xodr_road_id> <xodr_s> <xodr_t>");
279    }
280}
281
282struct RoadNetworkQuery<'a> {
283    rn: &'a RoadNetwork,
284}
285
286impl<'a> RoadNetworkQuery<'a> {
287    fn new(rn: &'a RoadNetwork) -> Self {
288        RoadNetworkQuery { rn }
289    }
290
291    fn execute_query(&self, query: MaliputQuery) -> Result<(), MaliputError> {
292        let rg = self.rn.road_geometry();
293        let start_time = std::time::Instant::now();
294        match query {
295            MaliputQuery::PrintAllLanes => {
296                // Sort it by lane length in descending order and then print them
297                let mut lanes = rg.get_lanes();
298                lanes.sort_by(|a, b| b.length().partial_cmp(&a.length()).unwrap());
299                print_elapsed_time(start_time);
300                println!("-> All lanes in the road geometry: {} lanes", lanes.len());
301                for lane in lanes {
302                    println!(
303                        "\t* Lane ID: {}\t Length: {} meters \t InertiaPos at (s=0,r=0,h=0): {}",
304                        lane.id(),
305                        lane.length(),
306                        lane.to_inertial_position(&LanePosition::new(0., 0., 0.))?
307                    );
308                }
309            }
310            MaliputQuery::GetNumberOfLanes => {
311                let len = rg.get_lanes().len();
312                print_elapsed_time(start_time);
313                println!("-> Number of lanes: {}", len);
314            }
315            MaliputQuery::GetTotalLengthOfTheRoadGeometry => {
316                let lanes_num = rg.get_lanes().len();
317                let total_length = rg.get_lanes().iter().map(|lane| lane.length()).sum::<f64>();
318                print_elapsed_time(start_time);
319                println!("-> Total length of the road geometry: {} meters along {} lanes. The average lane length is {} meters.",
320                total_length,
321                lanes_num,
322                total_length / lanes_num as f64);
323            }
324            MaliputQuery::GetLinearTolerance => {
325                let linear_tolerance = rg.linear_tolerance();
326                print_elapsed_time(start_time);
327                println!("-> Linear tolerance of the road geometry: {} meters", linear_tolerance);
328            }
329            MaliputQuery::GetAngularTolerance => {
330                let angular_tolerance = rg.angular_tolerance();
331                print_elapsed_time(start_time);
332                println!(
333                    "-> Angular tolerance of the road geometry: {} radians",
334                    angular_tolerance
335                );
336            }
337            MaliputQuery::GetLaneLength(lane_id) => {
338                if let Some(lane) = rg.get_lane(&lane_id) {
339                    let lane_length = lane.length();
340                    print_elapsed_time(start_time);
341                    println!("-> Length of lane {}: {} meters", lane_id, lane_length);
342                } else {
343                    println!("-> Lane with ID {} not found.", lane_id);
344                }
345            }
346            MaliputQuery::GetLaneBounds(lane_id, s) => {
347                if let Some(lane) = rg.get_lane(&lane_id) {
348                    let lane_bounds = lane.lane_bounds(s)?;
349                    print_elapsed_time(start_time);
350                    println!(
351                        "-> Bounds of lane {} at s={}: max: {}, min: {}",
352                        lane_id,
353                        s,
354                        lane_bounds.max(),
355                        lane_bounds.min()
356                    );
357                } else {
358                    println!("-> Lane with ID {} not found.", lane_id);
359                }
360            }
361            MaliputQuery::GetSegmentBounds(lane_id, s) => {
362                if let Some(lane) = rg.get_lane(&lane_id) {
363                    let segment_bounds = lane.segment_bounds(s)?;
364                    print_elapsed_time(start_time);
365                    println!(
366                        "-> Segment bounds of lane {} at s={}: max: {}, min: {}",
367                        lane_id,
368                        s,
369                        segment_bounds.max(),
370                        segment_bounds.min()
371                    );
372                } else {
373                    println!("-> Lane with ID {} not found.", lane_id);
374                }
375            }
376            MaliputQuery::ToRoadPosition(x, y, z) => {
377                let road_position_result = rg.to_road_position(&maliput::api::InertialPosition::new(x, y, z))?;
378                print_elapsed_time(start_time);
379                println!("-> Road Position Result:");
380                println!("\t* Road Position:");
381                println!("\t\t* lane_id: {}", road_position_result.road_position.lane().id());
382                println!("\t\t* lane_position: {}", road_position_result.road_position.pos());
383                println!("\t* Nearest Position:");
384                println!("\t\t* inertial_position: {}", road_position_result.nearest_position);
385                println!("\t* Distance:");
386                println!("\t\t* distance: {}", road_position_result.distance);
387            }
388            MaliputQuery::FindSurfaceRoadPositionsAtXY(x, y, radius) => {
389                let road_positions = rg.find_surface_road_positions_at_xy(x, y, radius)?;
390                print_elapsed_time(start_time);
391                println!(
392                    "-> FindSurfaceRoadPositionsAtXY Result: {} positions found.",
393                    road_positions.len()
394                );
395                for (i, rpr) in road_positions.iter().enumerate() {
396                    println!("\t* Result #{}:", i);
397                    println!("\t\t* lane_id: {}", rpr.road_position.lane().id());
398                    println!("\t\t* lane_position: {}", rpr.road_position.pos());
399                    println!("\t\t* nearest_position: {}", rpr.nearest_position);
400                    println!("\t\t* distance: {}", rpr.distance);
401                }
402            }
403            MaliputQuery::ToLanePosition(lane_id, x, y, z) => {
404                if let Some(lane) = rg.get_lane(&lane_id) {
405                    let lane_position_result = lane.to_lane_position(&maliput::api::InertialPosition::new(x, y, z))?;
406                    print_elapsed_time(start_time);
407                    println!("-> Lane Position Result for lane {}:", lane_id);
408                    println!("\t* Lane Position:");
409                    println!("\t\t* lane_position: {}", lane_position_result.lane_position);
410                    println!("\t* Nearest Position:");
411                    println!("\t\t* inertial_position: {}", lane_position_result.nearest_position);
412                    println!("\t* Distance:");
413                    println!("\t\t* distance: {}", lane_position_result.distance);
414                } else {
415                    println!("Lane with ID {} not found.", lane_id);
416                }
417            }
418            MaliputQuery::ToSegmentPosition(lane_id, x, y, z) => {
419                if let Some(lane) = rg.get_lane(&lane_id) {
420                    let lane_position_result =
421                        lane.to_segment_position(&maliput::api::InertialPosition::new(x, y, z))?;
422                    print_elapsed_time(start_time);
423                    println!("-> Segment Position Result for lane {}:", lane_id);
424                    println!("\t* Lane Position:");
425                    println!("\t\t* lane_position: {}", lane_position_result.lane_position);
426                    println!("\t* Nearest Position:");
427                    println!("\t\t* inertial_position: {}", lane_position_result.nearest_position);
428                    println!("\t* Distance:");
429                    println!("\t\t* distance: {}", lane_position_result.distance);
430                } else {
431                    println!("Lane with ID {} not found.", lane_id);
432                }
433            }
434            MaliputQuery::ToInertialPosition(lane_id, s, r, h) => {
435                if let Some(lane) = rg.get_lane(&lane_id) {
436                    let inertial_position = lane.to_inertial_position(&maliput::api::LanePosition::new(s, r, h))?;
437                    print_elapsed_time(start_time);
438                    println!("-> Inertial Position Result for lane {}:", lane_id);
439                    println!("\t* inertial_position: {}", inertial_position);
440                } else {
441                    println!("-> Lane with ID {} not found.", lane_id);
442                }
443            }
444            MaliputQuery::GetOrientaiton(lane_id, s, r, h) => {
445                if let Some(lane) = rg.get_lane(&lane_id) {
446                    let orientation = lane.get_orientation(&maliput::api::LanePosition::new(s, r, h))?;
447                    print_elapsed_time(start_time);
448                    println!("-> Orientation Result for lane {}:", lane_id);
449                    println!("\t* orientation:");
450                    println!(
451                        "\t\t* roll: {}, pitch: {}, yaw: {}",
452                        orientation.roll(),
453                        orientation.pitch(),
454                        orientation.yaw()
455                    );
456                } else {
457                    println!("-> Lane with ID {} not found.", lane_id);
458                }
459            }
460            MaliputQuery::OpenScenarioRoadPositionToMaliputRoadPosition(..) => {
461                let command = query
462                    .to_backend_custom_command_format()
463                    .expect("Invalid query command format for OpenScenarioRoadPositionToMaliputRoadPosition");
464                let res = rg.backend_custom_command(&command)?;
465                print_elapsed_time(start_time);
466                let res: Vec<&str> = res.split(',').collect();
467                println!("-> OpenScenarioRoadPositionToMaliputRoadPosition Result:");
468                if res.len() == 4 {
469                    println!("\t* Maliput Road Position:");
470                    println!("\t\t* lane_id: {}", res[0]);
471                    println!("\t\t* s: {}", res[1]);
472                    println!("\t\t* r: {}", res[2]);
473                    println!("\t\t* h: {}", res[3]);
474                } else {
475                    println!("-> Invalid response from backend custom command: {}", res.join(", "));
476                }
477            }
478            MaliputQuery::OpenScenarioLanePositionToMaliputRoadPosition(..) => {
479                let command = query
480                    .to_backend_custom_command_format()
481                    .expect("Invalid query command format for OpenScenarioLanePositionToMaliputRoadPosition");
482                let res = rg.backend_custom_command(&command)?;
483                print_elapsed_time(start_time);
484                let res: Vec<&str> = res.split(',').collect();
485                println!("-> OpenScenarioLanePositionToMaliputRoadPosition Result:");
486                if res.len() == 4 {
487                    println!("\t* Maliput Road Position:");
488                    println!("\t\t* lane_id: {}", res[0]);
489                    println!("\t\t* s: {}", res[1]);
490                    println!("\t\t* r: {}", res[2]);
491                    println!("\t\t* h: {}", res[3]);
492                } else {
493                    println!("-> Invalid response from backend custom command: {}", res.join(", "));
494                }
495            }
496            MaliputQuery::MaliputRoadPositionToOpenScenarioRoadPosition(..) => {
497                let command = query
498                    .to_backend_custom_command_format()
499                    .expect("Invalid query command format for MaliputRoadPositionToOpenScenarioRoadPosition");
500                let res = rg.backend_custom_command(&command)?;
501                print_elapsed_time(start_time);
502                let res: Vec<&str> = res.split(',').collect();
503                println!("-> MaliputRoadPositionToOpenScenarioRoadPosition Result:");
504                if res.len() == 3 {
505                    println!("\t* OpenScenario Road Position:");
506                    println!("\t\t* xodr_road_id: {}", res[0]);
507                    println!("\t\t* xodr_s: {}", res[1]);
508                    println!("\t\t* xodr_t: {}", res[2]);
509                } else {
510                    println!("-> Invalid response from backend custom command: {}", res.join(", "));
511                }
512            }
513            MaliputQuery::MaliputRoadPositionToOpenScenarioLanePosition(..) => {
514                let command = query
515                    .to_backend_custom_command_format()
516                    .expect("Invalid query command format for MaliputRoadPositionToOpenScenarioLanePosition");
517                let res = rg.backend_custom_command(&command)?;
518                print_elapsed_time(start_time);
519                let res: Vec<&str> = res.split(',').collect();
520                println!("-> MaliputRoadPositionToOpenScenarioLanePosition Result for lane:");
521                if res.len() == 4 {
522                    println!("\t* OpenScenario Lane Position:");
523                    println!("\t\t* xodr_road_id: {}", res[0]);
524                    println!("\t\t* xodr_s: {}", res[1]);
525                    println!("\t\t* xodr_lane_id: {}", res[2]);
526                    println!("\t\t* offset: {}", res[3]);
527                } else {
528                    println!("-> Invalid response from backend custom command: {}", res.join(", "));
529                }
530            }
531            MaliputQuery::OpenScenarioRelativeRoadPositionToMaliputRoadPosition(..) => {
532                let command = query
533                    .to_backend_custom_command_format()
534                    .expect("Invalid query command format for OpenScenarioRelativeRoadPositionToMaliputRoadPosition");
535                let res = rg.backend_custom_command(&command)?;
536                print_elapsed_time(start_time);
537                let res: Vec<&str> = res.split(',').collect();
538                println!("-> OpenScenarioRelativeRoadPositionToMaliputRoadPosition Result:");
539                if res.len() == 4 {
540                    println!("\t* Maliput Road Position:");
541                    println!("\t\t* lane_id: {}", res[0]);
542                    println!("\t\t* s: {}", res[1]);
543                    println!("\t\t* r: {}", res[2]);
544                    println!("\t\t* h: {}", res[3]);
545                } else {
546                    println!("-> Invalid response from backend custom command: {}", res.join(", "));
547                }
548            }
549            MaliputQuery::OpenScenarioRelativeLanePositionWithDsToMaliputRoadPosition(..) => {
550                let command = query.to_backend_custom_command_format().expect(
551                    "Invalid query command format for OpenScenarioRelativeLanePositionWithDsToMaliputRoadPosition",
552                );
553                let res = rg.backend_custom_command(&command)?;
554                print_elapsed_time(start_time);
555                let res: Vec<&str> = res.split(',').collect();
556                println!("-> OpenScenarioRelativeLanePositionWithDsToMaliputRoadPosition Result:");
557                if res.len() == 4 {
558                    println!("\t* Maliput Road Position:");
559                    println!("\t\t* lane_id: {}", res[0]);
560                    println!("\t\t* s: {}", res[1]);
561                    println!("\t\t* r: {}", res[2]);
562                    println!("\t\t* h: {}", res[3]);
563                } else {
564                    println!("-> Invalid response from backend custom command: {}", res.join(", "));
565                }
566            }
567            MaliputQuery::OpenScenarioRelativeLanePositionWithDsLaneToMaliputRoadPosition(..) => {
568                let command = query.to_backend_custom_command_format().expect(
569                    "Invalid query command format for OpenScenarioRelativeLanePositionWithDsLaneToMaliputRoadPosition",
570                );
571                let res = rg.backend_custom_command(&command)?;
572                print_elapsed_time(start_time);
573                let res: Vec<&str> = res.split(',').collect();
574                println!("-> OpenScenarioRelativeLanePositionWithDsLaneToMaliputRoadPosition Result:");
575                if res.len() == 4 {
576                    println!("\t* Maliput Road Position:");
577                    println!("\t\t* lane_id: {}", res[0]);
578                    println!("\t\t* s: {}", res[1]);
579                    println!("\t\t* r: {}", res[2]);
580                    println!("\t\t* h: {}", res[3]);
581                } else {
582                    println!("-> Invalid response from backend custom command: {}", res.join(", "));
583                }
584            }
585            MaliputQuery::GetRoadOrientationAtOpenScenarioRoadPosition(..) => {
586                let command = query
587                    .to_backend_custom_command_format()
588                    .expect("Invalid query command format for GetRoadOrientationAtOpenScenarioRoadPosition");
589                let res = rg.backend_custom_command(&command)?;
590                print_elapsed_time(start_time);
591                let res: Vec<&str> = res.split(',').collect();
592                println!("-> GetRoadOrientationAtOpenScenarioRoadPosition Result:");
593                if res.len() == 3 {
594                    println!("\t* Orientation:");
595                    println!("\t\t* roll: {}", res[0]);
596                    println!("\t\t* pitch: {}", res[1]);
597                    println!("\t\t* yaw: {}", res[2]);
598                } else {
599                    println!("-> Invalid response from backend custom command: {}", res.join(", "));
600                }
601            }
602            MaliputQuery::Invalid => {
603                println!("-> Invalid query command. Please try again.");
604                println!();
605                MaliputQuery::print_available_queries();
606            }
607        }
608        /// Print the elapsed time for the query execution.
609        fn print_elapsed_time(start_time: std::time::Instant) {
610            let elapsed = start_time.elapsed();
611            println!("-> Query executed in {:.2?}.", elapsed);
612        }
613        Ok(())
614    }
615}
616
617#[derive(Parser, Debug)]
618#[command(author, version, about)]
619struct Args {
620    /// Path to the road network file (e.g., .xodr for maliput_malidrive, .gpkg for maliput_geopackage).
621    road_network_file_path: String,
622
623    /// The maliput backend to use for loading the road network.
624    #[cfg(feature = "maliput_malidrive")]
625    #[arg(short, long, default_value_t = RoadNetworkBackend::MaliputMalidrive)]
626    backend: RoadNetworkBackend,
627
628    /// The maliput backend to use for loading the road network.
629    #[cfg(all(not(feature = "maliput_malidrive"), feature = "maliput_geopackage"))]
630    #[arg(short, long, default_value_t = RoadNetworkBackend::MaliputGeopackage)]
631    backend: RoadNetworkBackend,
632
633    /// Tolerance indicates the precision of the geometry being built in order
634    /// to make G1 continuity guarantees. A value of 0.5 is high. Ideally,
635    /// tolerances of 0.05 or 0.005 is ideal.
636    #[arg(short, long, default_value_t = 0.05)]
637    linear_tolerance: f64,
638
639    #[arg(short, long)]
640    max_linear_tolerance: Option<f64>,
641
642    #[arg(short, long, default_value_t = 0.01)]
643    angular_tolerance: f64,
644
645    #[arg(long, default_value_t = false)]
646    allow_non_drivable_lanes: bool,
647
648    #[arg(long, default_value_t = false)]
649    disable_parallel_builder_policy: bool,
650
651    #[arg(long, default_value_t = maliput::common::LogLevel::Error)]
652    set_log_level: maliput::common::LogLevel,
653}
654
655// Parses the road network file path from the command line arguments.
656//  - if this is a relative path, it will be resolved against the current working directory.
657//  - if this is an absolute path, it will be used as is.
658use std::path::PathBuf;
659fn parse_file_path(path: &str) -> PathBuf {
660    let mut path_buf = PathBuf::from(path);
661    if !path_buf.is_absolute() {
662        // If the path is relative, resolve it against the current working directory.
663        if let Ok(current_dir) = std::env::current_dir() {
664            path_buf = current_dir.join(path_buf);
665        }
666    }
667    path_buf
668}
669
670fn main() -> Result<(), Box<dyn std::error::Error>> {
671    println!("Welcome to maliput_query app:");
672    println!("\t* This application allows you to query a road network built from a road network file.");
673    println!("\t* Usage: maliput_query <ROAD_NETWORK_FILE_PATH> [OPTIONS]");
674    println!();
675    let args = Args::parse();
676
677    // Configuration for the road network.
678    let file_path = parse_file_path(args.road_network_file_path.as_str());
679    let linear_tolerance: String = args.linear_tolerance.to_string();
680    let max_linear_tolerance = args.max_linear_tolerance;
681    let angular_tolerance = args.angular_tolerance.to_string();
682
683    // Build backend-specific road network properties.
684    let mut road_network_properties = HashMap::from([
685        ("road_geometry_id", "maliput_query_rg"),
686        ("linear_tolerance", linear_tolerance.as_str()),
687        ("angular_tolerance", angular_tolerance.as_str()),
688    ]);
689    match args.backend {
690        #[cfg(feature = "maliput_malidrive")]
691        RoadNetworkBackend::MaliputMalidrive => {
692            let omit_non_drivable_lanes = if args.allow_non_drivable_lanes { "false" } else { "true" };
693            let parallel_builder_policy = !args.disable_parallel_builder_policy;
694            road_network_properties.insert("opendrive_file", file_path.to_str().unwrap());
695            road_network_properties.insert("omit_nondrivable_lanes", omit_non_drivable_lanes);
696            road_network_properties.insert(
697                "build_policy",
698                if parallel_builder_policy {
699                    "parallel"
700                } else {
701                    "sequential"
702                },
703            );
704        }
705        #[cfg(feature = "maliput_geopackage")]
706        RoadNetworkBackend::MaliputGeopackage => {
707            road_network_properties.insert("gpkg_file", file_path.to_str().unwrap());
708        }
709    }
710    let max_linear_tolerance_str;
711    if let Some(max_tolerance) = max_linear_tolerance {
712        max_linear_tolerance_str = max_tolerance.to_string();
713        road_network_properties.insert("max_linear_tolerance", max_linear_tolerance_str.as_str());
714    }
715
716    println!();
717    println!("-> Setting maliput log level to: {}", args.set_log_level);
718    maliput::common::set_log_level(args.set_log_level);
719    println!("-> Road network builder options: {:?}", road_network_properties);
720    println!("->");
721    println!(
722        "-> Building Maliput Road Network using backend '{}' from file: {}",
723        args.backend,
724        file_path.display()
725    );
726    println!("-> This may take a while, depending on the size of the file and the complexity of the road network...");
727    println!("-> ...");
728    let now = std::time::Instant::now();
729    let road_network = RoadNetwork::new(args.backend, &road_network_properties)?;
730    let elapsed = now.elapsed();
731    println!("-> Road network loaded in {:.2?}.", elapsed);
732
733    // Print available commands
734    println!();
735    MaliputQuery::print_available_queries();
736    println!("To exit the application, type 'exit'.");
737
738    loop {
739        println!();
740        let mut input = String::new();
741        println!("-> Enter a query command (or 'exit' to quit):");
742        std::io::stdin().read_line(&mut input)?;
743        let input = input.trim();
744
745        if input == "exit" {
746            break;
747        }
748
749        let query: MaliputQuery = input.split_whitespace().collect::<Vec<&str>>().into();
750        let query_handler = RoadNetworkQuery::new(&road_network);
751        query_handler.execute_query(query)?;
752    }
753
754    Ok(())
755}
maliput_query/maliput_query.rs

maliput_query/
maliput_query.rs
