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

maliput_query/
maliput_query.rs
