urdf_viz/
utils.rs

1use std::collections::HashMap;
2
3#[cfg(not(target_family = "wasm"))]
4pub use native::*;
5#[cfg(target_family = "wasm")]
6pub use wasm::*;
7
8pub(crate) fn replace_package_with_path(
9    filename: &str,
10    package_path: &HashMap<String, String>,
11) -> Option<String> {
12    let path = filename.strip_prefix("package://")?;
13    let (package_name, path) = path.split_once('/')?;
14    let package_path = package_path.get(package_name)?;
15    Some(format!(
16        "{}/{path}",
17        package_path.strip_suffix('/').unwrap_or(package_path),
18    ))
19}
20
21pub(crate) fn is_url(path: &str) -> bool {
22    path.starts_with("https://") || path.starts_with("http://")
23}
24
25#[cfg(not(target_family = "wasm"))]
26mod native {
27    use std::{
28        collections::HashMap,
29        ffi::OsStr,
30        fs, mem,
31        path::Path,
32        sync::{Arc, Mutex},
33    };
34
35    use tracing::error;
36
37    use crate::{utils::is_url, Result};
38
39    fn read_urdf(path: &str, xacro_args: &[(String, String)]) -> Result<(urdf_rs::Robot, String)> {
40        let urdf_text = if Path::new(path).extension().and_then(OsStr::to_str) == Some("xacro") {
41            urdf_rs::utils::convert_xacro_to_urdf_with_args(path, xacro_args)?
42        } else if is_url(path) {
43            ureq::get(path)
44                .call()
45                .map_err(|e| crate::Error::Other(e.to_string()))?
46                .into_string()?
47        } else {
48            fs::read_to_string(path)?
49        };
50        let robot = urdf_rs::read_from_string(&urdf_text)?;
51        Ok((robot, urdf_text))
52    }
53
54    #[derive(Debug)]
55    pub struct RobotModel {
56        pub(crate) path: String,
57        pub(crate) urdf_text: Arc<Mutex<String>>,
58        robot: urdf_rs::Robot,
59        package_path: HashMap<String, String>,
60        xacro_args: Vec<(String, String)>,
61    }
62
63    impl RobotModel {
64        pub fn new(
65            path: impl Into<String>,
66            package_path: HashMap<String, String>,
67            xacro_args: &[(String, String)],
68        ) -> Result<Self> {
69            let path = path.into();
70            let (robot, urdf_text) = read_urdf(&path, xacro_args)?;
71            Ok(Self {
72                path,
73                urdf_text: Arc::new(Mutex::new(urdf_text)),
74                robot,
75                package_path,
76                xacro_args: xacro_args.to_owned(),
77            })
78        }
79
80        pub async fn from_text(
81            path: impl Into<String>,
82            urdf_text: impl Into<String>,
83            package_path: HashMap<String, String>,
84        ) -> Result<Self> {
85            let path = path.into();
86            let urdf_text = urdf_text.into();
87            let robot = urdf_rs::read_from_string(&urdf_text)?;
88            Ok(Self {
89                path,
90                urdf_text: Arc::new(Mutex::new(urdf_text)),
91                robot,
92                package_path,
93                xacro_args: Vec::new(),
94            })
95        }
96
97        pub(crate) fn get(&mut self) -> &urdf_rs::Robot {
98            &self.robot
99        }
100
101        pub(crate) fn reload(&mut self) {
102            match read_urdf(&self.path, &self.xacro_args) {
103                Ok((robot, text)) => {
104                    self.robot = robot;
105                    *self.urdf_text.lock().unwrap() = text;
106                }
107                Err(e) => {
108                    error!("{e}");
109                }
110            }
111        }
112
113        pub(crate) fn take_package_path_map(&mut self) -> HashMap<String, String> {
114            mem::take(&mut self.package_path)
115        }
116    }
117
118    #[cfg(feature = "assimp")]
119    /// http request -> write to tempfile -> return that file
120    pub(crate) fn fetch_tempfile(url: &str) -> Result<tempfile::NamedTempFile> {
121        use std::io::{Read, Write};
122
123        const RESPONSE_SIZE_LIMIT: usize = 10 * 1_024 * 1_024;
124
125        let mut buf: Vec<u8> = vec![];
126        ureq::get(url)
127            .call()
128            .map_err(|e| crate::Error::Other(e.to_string()))?
129            .into_reader()
130            .take((RESPONSE_SIZE_LIMIT + 1) as u64)
131            .read_to_end(&mut buf)?;
132        if buf.len() > RESPONSE_SIZE_LIMIT {
133            return Err(crate::Error::Other(format!("{url} is too big")));
134        }
135        let mut file = tempfile::NamedTempFile::new()?;
136        file.write_all(&buf)?;
137        Ok(file)
138    }
139}
140
141#[cfg(target_family = "wasm")]
142mod wasm {
143    use std::{
144        collections::HashMap,
145        mem,
146        path::Path,
147        str,
148        sync::{Arc, Mutex},
149    };
150
151    use base64::{engine::general_purpose::STANDARD as BASE64, Engine};
152    use js_sys::Uint8Array;
153    use serde::{Deserialize, Serialize};
154    use tracing::debug;
155    use wasm_bindgen::JsCast;
156    use wasm_bindgen_futures::JsFuture;
157    use web_sys::Response;
158
159    use crate::{utils::is_url, Error, Result};
160
161    #[derive(Serialize, Deserialize)]
162    pub(crate) struct Mesh {
163        pub(crate) path: String,
164        data: MeshData,
165    }
166
167    impl Mesh {
168        pub(crate) fn decode(data: &str) -> Result<Self> {
169            let mut mesh: Self = serde_json::from_str(data).map_err(|e| e.to_string())?;
170            match &mesh.data {
171                MeshData::None => {}
172                MeshData::Base64(s) => {
173                    mesh.data = MeshData::Bytes(BASE64.decode(s).map_err(|e| e.to_string())?);
174                }
175                MeshData::Bytes(_) => unreachable!(),
176            }
177            Ok(mesh)
178        }
179
180        pub(crate) fn bytes(&self) -> Option<&[u8]> {
181            match &self.data {
182                MeshData::None => None,
183                MeshData::Bytes(bytes) => Some(bytes),
184                MeshData::Base64(_) => unreachable!(),
185            }
186        }
187    }
188
189    #[derive(Serialize, Deserialize)]
190    enum MeshData {
191        Base64(String),
192        Bytes(Vec<u8>),
193        None,
194    }
195
196    pub fn window() -> Result<web_sys::Window> {
197        Ok(web_sys::window().ok_or("failed to get window")?)
198    }
199
200    async fn fetch(input_file: &str) -> Result<Response> {
201        let promise = window()?.fetch_with_str(input_file);
202
203        let response = JsFuture::from(promise)
204            .await?
205            .dyn_into::<Response>()
206            .unwrap();
207
208        Ok(response)
209    }
210
211    pub async fn read_to_string(input_file: impl AsRef<str>) -> Result<String> {
212        let promise = fetch(input_file.as_ref()).await?.text()?;
213
214        let s = JsFuture::from(promise).await?;
215
216        Ok(s.as_string()
217            .ok_or_else(|| format!("{} is not string", input_file.as_ref()))?)
218    }
219
220    pub async fn read(input_file: impl AsRef<str>) -> Result<Vec<u8>> {
221        let promise = fetch(input_file.as_ref()).await?.array_buffer()?;
222
223        let bytes = JsFuture::from(promise).await?;
224
225        Ok(Uint8Array::new(&bytes).to_vec())
226    }
227
228    pub async fn load_mesh(
229        robot: &mut urdf_rs::Robot,
230        urdf_path: impl AsRef<Path>,
231        package_path: &HashMap<String, String>,
232    ) -> Result<()> {
233        let urdf_path = urdf_path.as_ref();
234        for geometry in robot.links.iter_mut().flat_map(|link| {
235            link.visual
236                .iter_mut()
237                .map(|v| &mut v.geometry)
238                .chain(link.collision.iter_mut().map(|c| &mut c.geometry))
239        }) {
240            if let urdf_rs::Geometry::Mesh { filename, .. } = geometry {
241                let input_file = if is_url(filename) {
242                    filename.clone()
243                } else if filename.starts_with("package://") {
244                    crate::utils::replace_package_with_path(filename, package_path).ok_or_else(||
245                        format!(
246                            "ros package ({filename}) is not supported in wasm; consider using `package-path[]` URL parameter",
247                        ))?
248                } else if filename.starts_with("file://") {
249                    return Err(Error::from(format!(
250                        "local file ({filename}) is not supported in wasm",
251                    )));
252                } else {
253                    // We don't use url::Url::path/set_path here, because
254                    // urdf_path may be a relative path to a file bundled
255                    // with the server. Path::with_file_name works for wasm
256                    // where the separator is /, so we use it.
257                    urdf_path
258                        .with_file_name(&filename)
259                        .to_str()
260                        .unwrap()
261                        .to_string()
262                };
263
264                debug!("loading {input_file}");
265                let data = MeshData::Base64(BASE64.encode(read(&input_file).await?));
266
267                let new = serde_json::to_string(&Mesh {
268                    path: filename.clone(),
269                    data,
270                })
271                .unwrap();
272                *filename = new;
273            }
274        }
275        Ok(())
276    }
277
278    #[derive(Debug)]
279    pub struct RobotModel {
280        pub(crate) path: String,
281        pub(crate) urdf_text: Arc<Mutex<String>>,
282        robot: urdf_rs::Robot,
283        package_path: HashMap<String, String>,
284    }
285
286    impl RobotModel {
287        pub async fn new(
288            path: impl Into<String>,
289            package_path: HashMap<String, String>,
290        ) -> Result<Self> {
291            let path = path.into();
292            let urdf_text = read_to_string(&path).await?;
293            Self::from_text(path, urdf_text, package_path).await
294        }
295
296        pub async fn from_text(
297            path: impl Into<String>,
298            urdf_text: impl Into<String>,
299            package_path: HashMap<String, String>,
300        ) -> Result<Self> {
301            let path = path.into();
302            let urdf_text = urdf_text.into();
303            let mut robot = urdf_rs::read_from_string(&urdf_text)?;
304            load_mesh(&mut robot, &path, &package_path).await?;
305            Ok(Self {
306                path,
307                urdf_text: Arc::new(Mutex::new(urdf_text)),
308                robot,
309                package_path,
310            })
311        }
312
313        pub(crate) fn get(&mut self) -> &urdf_rs::Robot {
314            &self.robot
315        }
316
317        pub(crate) fn take_package_path_map(&mut self) -> HashMap<String, String> {
318            mem::take(&mut self.package_path)
319        }
320    }
321}
322
323#[cfg(test)]
324mod tests {
325    use super::*;
326
327    #[test]
328    fn test_replace_package_with_path() {
329        let mut package_path = HashMap::new();
330        package_path.insert("a".to_owned(), "path".to_owned());
331        assert_eq!(
332            replace_package_with_path("package://a/b/c", &package_path),
333            Some("path/b/c".to_owned())
334        );
335        assert_eq!(
336            replace_package_with_path("package://a", &package_path),
337            None
338        );
339        assert_eq!(
340            replace_package_with_path("package://b/b/c", &package_path),
341            None
342        );
343        assert_eq!(replace_package_with_path("a", &package_path), None);
344    }
345}
urdf_viz/utils.rs

urdf_viz/
utils.rs
