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

maliput_query/
maliput_query.rs
