urdf_rs/
utils.rs

Help
1use crate::deserialize::Robot;
2use crate::errors::*;
3use crate::funcs::*;
4
5use regex::Regex;
6use std::borrow::Cow;
7use std::path::Path;
8use std::process::Command;
9use std::sync::LazyLock;
10
11pub fn convert_xacro_to_urdf_with_args<P>(filename: P, args: &[(String, String)]) -> Result<String>
12where
13    P: AsRef<Path>,
14{
15    let filename = filename.as_ref();
16    let output = Command::new("rosrun")
17        .args(["xacro", "xacro", "--inorder"])
18        .arg(filename)
19        .args(args.iter().map(|(k, v)| format!("{}:={}", k, v)))
20        .output()
21        .or_else(|_| {
22            Command::new("xacro")
23                .arg(filename)
24                .args(args.iter().map(|(k, v)| format!("{}:={}", k, v)))
25                .output()
26        })
27        .map_err(|e| {
28            format!("failed to execute xacro; consider installing xacro by `apt-get install ros-*-xacro`: {e}")
29        })?;
30    if output.status.success() {
31        Ok(String::from_utf8(output.stdout)?)
32    } else {
33        Err(ErrorKind::Command {
34            msg: format!("failed to xacro for {}", filename.display()),
35            stdout: String::from_utf8_lossy(&output.stdout).into_owned(),
36            stderr: String::from_utf8_lossy(&output.stderr).into_owned(),
37        }
38        .into())
39    }
40}
41
42pub fn convert_xacro_to_urdf<P>(filename: P) -> Result<String>
43where
44    P: AsRef<Path>,
45{
46    convert_xacro_to_urdf_with_args(filename, &[])
47}
48
49pub fn rospack_find(package: &str) -> Result<String> {
50    let output = Command::new("rospack")
51        .arg("find")
52        .arg(package)
53        .output()
54        // support ROS2
55        .or_else(|_| {
56            Command::new("ros2")
57                .args(["pkg", "prefix", "--share"])
58                .arg(package)
59                .output()
60        })
61        .map_err(|e| {
62            format!("failed to execute neither `rospack` nor `ros2 pkg`; consider installing ROS or replacing 'package://' with path: {e}")
63        })?;
64    if output.status.success() {
65        Ok(String::from_utf8(output.stdout)?.trim().to_string())
66    } else {
67        Err(ErrorKind::Command {
68            msg: format!("failed to find ros package {package}"),
69            stdout: String::from_utf8_lossy(&output.stdout).into_owned(),
70            stderr: String::from_utf8_lossy(&output.stderr).into_owned(),
71        }
72        .into())
73    }
74}
75
76// Note: We return Result here, although there is currently no branch that returns an error.
77// This is to avoid a breaking change when changing the error about package:// from panic to error.
78pub fn expand_package_path<'a>(filename: &'a str, base_dir: Option<&Path>) -> Result<Cow<'a, str>> {
79    static RE: LazyLock<Regex> = LazyLock::new(|| Regex::new("^package://(\\w+)/").unwrap());
80
81    Ok(if filename.starts_with("package://") {
82        RE.replace(filename, |ma: &regex::Captures<'_>| {
83            // TODO: It's better to propagate the error to the caller,
84            // but regex doesn't provide API like try_replace.
85            let found_path = rospack_find(&ma[1]).unwrap();
86            found_path + "/"
87        })
88    } else if filename.starts_with("https://") || filename.starts_with("http://") {
89        filename.into()
90    } else if let Some(abs_path) = filename.strip_prefix("file://") {
91        abs_path.into()
92    } else if let Some(base_dir) = base_dir {
93        let mut relative_path_from_urdf = base_dir.to_owned();
94        relative_path_from_urdf.push(filename);
95        relative_path_from_urdf
96            .into_os_string()
97            .into_string()
98            .unwrap()
99            .into()
100    } else {
101        filename.into()
102    })
103}
104
105pub fn read_urdf_or_xacro_with_args<P>(input_path: P, args: &[(String, String)]) -> Result<Robot>
106where
107    P: AsRef<Path>,
108{
109    let input_path = input_path.as_ref();
110    if let Some(ext) = input_path.extension() {
111        if ext == "xacro" {
112            let urdf_utf = convert_xacro_to_urdf_with_args(input_path, args)?;
113            read_from_string(&urdf_utf)
114        } else {
115            read_file(input_path)
116        }
117    } else {
118        Err(ErrorKind::Other(format!(
119            "failed to get extension from {}",
120            input_path.display()
121        ))
122        .into())
123    }
124}
125
126pub fn read_urdf_or_xacro<P>(input_path: P) -> Result<Robot>
127where
128    P: AsRef<Path>,
129{
130    read_urdf_or_xacro_with_args(input_path, &[])
131}
132
133#[test]
134fn it_works() {
135    // test only for not packages
136    assert_eq!(expand_package_path("home/aaa", None).unwrap(), "home/aaa");
137    assert_eq!(
138        expand_package_path("home/aaa.obj", Some(Path::new(""))).unwrap(),
139        "home/aaa.obj"
140    );
141    assert_eq!(
142        expand_package_path("home/aaa.obj", Some(Path::new("/var"))).unwrap(),
143        "/var/home/aaa.obj"
144    );
145    assert_eq!(
146        expand_package_path("/home/aaa.obj", Some(Path::new(""))).unwrap(),
147        "/home/aaa.obj"
148    );
149    assert_eq!(
150        expand_package_path("file:///home/aaa.obj", Some(Path::new("/var"))).unwrap(),
151        "/home/aaa.obj"
152    );
153    assert_eq!(
154        expand_package_path("http://aaa.obj", Some(Path::new("/var"))).unwrap(),
155        "http://aaa.obj"
156    );
157    assert_eq!(
158        expand_package_path("https://aaa.obj", Some(Path::new("/var"))).unwrap(),
159        "https://aaa.obj"
160    );
161    assert!(read_urdf_or_xacro("sample.urdf").is_ok());
162    assert!(read_urdf_or_xacro("sample_urdf").is_err());
163}
urdf_rs/utils.rs

urdf_rs/
utils.rs
