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

maliput_query/
maliput_query.rs
