1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163

use crate::deserialize::Robot;
use crate::errors::*;
use crate::funcs::*;

use once_cell::sync::Lazy;
use regex::Regex;
use std::borrow::Cow;
use std::path::Path;
use std::process::Command;

pub fn convert_xacro_to_urdf_with_args<P>(filename: P, args: &[(String, String)]) -> Result<String>
where
    P: AsRef<Path>,
{
    let filename = filename.as_ref();
    let output = Command::new("rosrun")
        .args(["xacro", "xacro", "--inorder"])
        .arg(filename)
        .args(args.iter().map(|(k, v)| format!("{}:={}", k, v)))
        .output()
        .or_else(|_| {
            Command::new("xacro")
                .arg(filename)
                .args(args.iter().map(|(k, v)| format!("{}:={}", k, v)))
                .output()
        })
        .map_err(|e| {
            format!("failed to execute xacro; consider installing xacro by `apt-get install ros-*-xacro`: {e}")
        })?;
    if output.status.success() {
        Ok(String::from_utf8(output.stdout)?)
    } else {
        Err(ErrorKind::Command {
            msg: format!("failed to xacro for {}", filename.display()),
            stdout: String::from_utf8_lossy(&output.stdout).into_owned(),
            stderr: String::from_utf8_lossy(&output.stderr).into_owned(),
        }
        .into())
    }
}

pub fn convert_xacro_to_urdf<P>(filename: P) -> Result<String>
where
    P: AsRef<Path>,
{
    convert_xacro_to_urdf_with_args(filename, &[])
}

pub fn rospack_find(package: &str) -> Result<String> {
    let output = Command::new("rospack")
        .arg("find")
        .arg(package)
        .output()
        // support ROS2
        .or_else(|_| {
            Command::new("ros2")
                .args(["pkg", "prefix", "--share"])
                .arg(package)
                .output()
        })
        .map_err(|e| {
            format!("failed to execute neither `rospack` nor `ros2 pkg`; consider installing ROS or replacing 'package://' with path: {e}")
        })?;
    if output.status.success() {
        Ok(String::from_utf8(output.stdout)?.trim().to_string())
    } else {
        Err(ErrorKind::Command {
            msg: format!("failed to find ros package {package}"),
            stdout: String::from_utf8_lossy(&output.stdout).into_owned(),
            stderr: String::from_utf8_lossy(&output.stderr).into_owned(),
        }
        .into())
    }
}

// Note: We return Result here, although there is currently no branch that returns an error.
// This is to avoid a breaking change when changing the error about package:// from panic to error.
pub fn expand_package_path<'a>(filename: &'a str, base_dir: Option<&Path>) -> Result<Cow<'a, str>> {
    static RE: Lazy<Regex> = Lazy::new(|| Regex::new("^package://(\\w+)/").unwrap());

    Ok(if filename.starts_with("package://") {
        RE.replace(filename, |ma: &regex::Captures<'_>| {
            // TODO: It's better to propagate the error to the caller,
            // but regex doesn't provide API like try_replace.
            let found_path = rospack_find(&ma[1]).unwrap();
            found_path + "/"
        })
    } else if filename.starts_with("https://") || filename.starts_with("http://") {
        filename.into()
    } else if let Some(abs_path) = filename.strip_prefix("file://") {
        abs_path.into()
    } else if let Some(base_dir) = base_dir {
        let mut relative_path_from_urdf = base_dir.to_owned();
        relative_path_from_urdf.push(filename);
        relative_path_from_urdf
            .into_os_string()
            .into_string()
            .unwrap()
            .into()
    } else {
        filename.into()
    })
}

pub fn read_urdf_or_xacro_with_args<P>(input_path: P, args: &[(String, String)]) -> Result<Robot>
where
    P: AsRef<Path>,
{
    let input_path = input_path.as_ref();
    if let Some(ext) = input_path.extension() {
        if ext == "xacro" {
            let urdf_utf = convert_xacro_to_urdf_with_args(input_path, args)?;
            read_from_string(&urdf_utf)
        } else {
            read_file(input_path)
        }
    } else {
        Err(ErrorKind::Other(format!(
            "failed to get extension from {}",
            input_path.display()
        ))
        .into())
    }
}

pub fn read_urdf_or_xacro<P>(input_path: P) -> Result<Robot>
where
    P: AsRef<Path>,
{
    read_urdf_or_xacro_with_args(input_path, &[])
}

#[test]
fn it_works() {
    // test only for not packages
    assert_eq!(expand_package_path("home/aaa", None).unwrap(), "home/aaa");
    assert_eq!(
        expand_package_path("home/aaa.obj", Some(Path::new(""))).unwrap(),
        "home/aaa.obj"
    );
    assert_eq!(
        expand_package_path("home/aaa.obj", Some(Path::new("/var"))).unwrap(),
        "/var/home/aaa.obj"
    );
    assert_eq!(
        expand_package_path("/home/aaa.obj", Some(Path::new(""))).unwrap(),
        "/home/aaa.obj"
    );
    assert_eq!(
        expand_package_path("file:///home/aaa.obj", Some(Path::new("/var"))).unwrap(),
        "/home/aaa.obj"
    );
    assert_eq!(
        expand_package_path("http://aaa.obj", Some(Path::new("/var"))).unwrap(),
        "http://aaa.obj"
    );
    assert_eq!(
        expand_package_path("https://aaa.obj", Some(Path::new("/var"))).unwrap(),
        "https://aaa.obj"
    );
    assert!(read_urdf_or_xacro("sample.urdf").is_ok());
    assert!(read_urdf_or_xacro("sample_urdf").is_err());
}