From cccafcb864ff05a098b95d365cf266d9d72113e9 Mon Sep 17 00:00:00 2001 From: Taiki Endo Date: Wed, 19 Oct 2022 17:34:23 +0900 Subject: [PATCH] Support replacing ROS package path by command line --- README.md | 4 ++ examples/wasm/src/lib.rs | 4 +- src/app.rs | 22 ++++++++ src/bin/urdf-viz.rs | 3 +- src/urdf.rs | 39 ++++++------- src/utils.rs | 119 +++++++++++++++++++++++++-------------- src/viewer.rs | 20 ++++++- 7 files changed, 142 insertions(+), 69 deletions(-) diff --git a/README.md b/README.md index a501987..2ff1754 100644 --- a/README.md +++ b/README.md @@ -93,6 +93,10 @@ urdf-viz -h If there are no "package://" in mesh tag, and don't use xacro you can skip install of ROS. +If there are "package://" in mesh tag, but path or URL to package is known and +don't use xacro you can also skip install of ROS [by replacing package with path +or URL](https://github.com/openrr/urdf-viz/pull/176). + ## GUI Usage In the GUI, you can do some operations with keyboard and mouse. diff --git a/examples/wasm/src/lib.rs b/examples/wasm/src/lib.rs index ce8c70c..74943f7 100644 --- a/examples/wasm/src/lib.rs +++ b/examples/wasm/src/lib.rs @@ -18,7 +18,9 @@ async fn run() -> Result<(), JsValue> { if opt.input_urdf_or_xacro.is_empty() { opt.input_urdf_or_xacro = SAMPLE_URDF_PATH.to_string(); } - let urdf_robot = urdf_viz::utils::RobotModel::new(&opt.input_urdf_or_xacro).await?; + let package_path = opt.create_package_path_map()?; + let urdf_robot = + urdf_viz::utils::RobotModel::new(&opt.input_urdf_or_xacro, package_path).await?; let ik_constraints = opt.create_ik_constraints(); let mut app = UrdfViewerApp::new( urdf_robot, diff --git a/src/app.rs b/src/app.rs index 9bc9225..ba09f63 100644 --- a/src/app.rs +++ b/src/app.rs @@ -19,6 +19,7 @@ use k::prelude::*; use kiss3d::event::{Action, Key, Modifiers, WindowEvent}; use kiss3d::window::{self, Window}; use serde::Deserialize; +use std::collections::HashMap; use std::fmt; use std::path::PathBuf; use std::sync::atomic::{AtomicUsize, Ordering::Relaxed}; @@ -137,6 +138,7 @@ pub struct UrdfViewerApp { show_frames: bool, ik_constraints: k::Constraints, point_size: f32, + package_path: HashMap, } impl UrdfViewerApp { @@ -152,6 +154,7 @@ impl UrdfViewerApp { ground_height: Option, ) -> Result { let input_path = PathBuf::from(&urdf_robot.path); + let package_path = urdf_robot.take_package_path_map(); let robot: k::Chain = urdf_robot.get().into(); println!("{robot}"); let (mut viewer, mut window) = Viewer::with_background_color("urdf-viz", background_color); @@ -163,6 +166,7 @@ impl UrdfViewerApp { urdf_robot.get(), input_path.parent(), is_collision, + &package_path, ); viewer.add_axis_cylinders(&mut window, "origin", 1.0); if let Some(h) = ground_height { @@ -208,6 +212,7 @@ impl UrdfViewerApp { show_frames: false, ik_constraints: k::Constraints::default(), point_size: 10.0, + package_path, }) } pub fn handle(&self) -> Arc { @@ -322,6 +327,7 @@ impl UrdfViewerApp { self.urdf_robot.get(), self.input_path.parent(), self.is_collision, + &self.package_path, ); const FRAME_ARROW_SIZE: f32 = 0.2; self.robot.iter().for_each(|n| { @@ -428,6 +434,7 @@ impl UrdfViewerApp { self.urdf_robot.get(), self.input_path.parent(), self.is_collision, + &self.package_path, ); self.update_robot(); } @@ -944,6 +951,10 @@ pub struct Opt { #[structopt(long = "ground-height")] pub ground_height: Option, + + /// Replace `package://PACKAGE` in mesh tag with PATH. + #[structopt(long = "package-path", value_name = "PACKAGE=PATH")] + pub package_path: Vec, } fn default_back_ground_color_b() -> f32 { @@ -969,6 +980,17 @@ impl Opt { } } + pub fn create_package_path_map(&self) -> Result, Error> { + let mut map = HashMap::with_capacity(self.package_path.len()); + for replace in &self.package_path { + let (package_name, path) = replace.split_once('=').ok_or_else(|| { + format!("--package-path may only accept PACKAGE=KEY format, but found '{replace}'") + })?; + map.insert(package_name.to_owned(), path.to_owned()); + } + Ok(map) + } + #[cfg(target_family = "wasm")] pub fn from_params() -> Result { let href = crate::utils::window()?.location().href()?; diff --git a/src/bin/urdf-viz.rs b/src/bin/urdf-viz.rs index 7a0a091..66c2075 100644 --- a/src/bin/urdf-viz.rs +++ b/src/bin/urdf-viz.rs @@ -27,7 +27,8 @@ async fn main() -> urdf_viz::Result<()> { tracing_subscriber::fmt::init(); let opt = Opt::from_args(); debug!(?opt); - let urdf_robot = urdf_viz::utils::RobotModel::new(&opt.input_urdf_or_xacro)?; + let package_path = opt.create_package_path_map()?; + let urdf_robot = urdf_viz::utils::RobotModel::new(&opt.input_urdf_or_xacro, package_path)?; let ik_constraints = opt.create_ik_constraints(); let mut app = UrdfViewerApp::new( urdf_robot, diff --git a/src/urdf.rs b/src/urdf.rs index 3d93a4b..4b7b50b 100644 --- a/src/urdf.rs +++ b/src/urdf.rs @@ -3,6 +3,7 @@ use crate::mesh::load_mesh; use k::nalgebra as na; use kiss3d::scene::SceneNode; use std::borrow::Cow; +use std::collections::HashMap; use std::path::Path; use tracing::*; @@ -12,6 +13,7 @@ pub fn add_geometry( base_dir: Option<&Path>, group: &mut SceneNode, use_texture: bool, + package_path: &HashMap, ) -> Result { match *geometry { urdf_rs::Geometry::Box { ref size } => { @@ -64,10 +66,17 @@ pub fn add_geometry( scale, } => { let scale = scale.unwrap_or(DEFAULT_MESH_SCALE); - let replaced_filename = if cfg!(target_family = "wasm") { - Cow::Borrowed(filename) - } else { - let replaced_filename = urdf_rs::utils::expand_package_path(filename, base_dir); + let mut filename = Cow::Borrowed(&**filename); + if !cfg!(target_family = "wasm") { + // On WASM, this is handled in utils::load_mesh + if filename.starts_with("package://") { + if let Some(replaced_filename) = + crate::utils::replace_package_with_path(&filename, package_path) + { + filename = Cow::Owned(replaced_filename); + } + }; + let replaced_filename = urdf_rs::utils::expand_package_path(&filename, base_dir); if !replaced_filename.starts_with("https://") && !replaced_filename.starts_with("http://") && !Path::new(&replaced_filename).exists() @@ -76,26 +85,14 @@ pub fn add_geometry( } // TODO: remove Cow::Owned once https://github.com/openrr/urdf-rs/pull/41 // is released in the next breaking release of urdf-rs. - Cow::Owned(replaced_filename) - }; + filename = Cow::Owned(replaced_filename); + } let na_scale = na::Vector3::new(scale[0] as f32, scale[1] as f32, scale[2] as f32); - debug!("filename = {replaced_filename}"); + debug!("filename = {filename}"); if cfg!(feature = "assimp") { - load_mesh( - replaced_filename.as_str(), - na_scale, - opt_color, - group, - use_texture, - ) + load_mesh(&filename, na_scale, opt_color, group, use_texture) } else { - match load_mesh( - replaced_filename.as_str(), - na_scale, - opt_color, - group, - use_texture, - ) { + match load_mesh(&filename, na_scale, opt_color, group, use_texture) { Ok(scene) => Ok(scene), Err(e) => { error!("{e}"); diff --git a/src/utils.rs b/src/utils.rs index 549497c..760e655 100644 --- a/src/utils.rs +++ b/src/utils.rs @@ -1,20 +1,40 @@ +use std::collections::HashMap; + #[cfg(not(target_family = "wasm"))] pub use native::*; #[cfg(target_family = "wasm")] pub use wasm::*; +pub(crate) fn replace_package_with_path( + filename: &str, + package_path: &HashMap, +) -> Option { + let path = filename.strip_prefix("package://")?; + let (package_name, path) = path.split_once('/').unwrap_or((path, "")); + let package_path = package_path.get(package_name)?; + Some(format!( + "{}/{path}", + package_path.strip_suffix('/').unwrap_or(package_path), + )) +} + #[cfg(not(target_family = "wasm"))] mod native { - use std::{ffi::OsStr, fs, path::Path, sync::Arc}; + use std::{collections::HashMap, ffi::OsStr, fs, mem, path::Path, sync::Arc}; use parking_lot::Mutex; use tracing::error; use crate::Result; - fn read_urdf(path: impl AsRef) -> Result<(urdf_rs::Robot, String)> { - let urdf_text = if path.as_ref().extension().and_then(OsStr::to_str) == Some("xacro") { + fn read_urdf(path: &str) -> Result<(urdf_rs::Robot, String)> { + let urdf_text = if Path::new(path).extension().and_then(OsStr::to_str) == Some("xacro") { urdf_rs::utils::convert_xacro_to_urdf(path)? + } else if path.starts_with("https://") || path.starts_with("http://") { + ureq::get(path) + .call() + .map_err(|e| crate::Error::Other(e.to_string()))? + .into_string()? } else { fs::read_to_string(path)? }; @@ -27,22 +47,25 @@ mod native { pub(crate) path: String, pub(crate) urdf_text: Arc>, robot: urdf_rs::Robot, + package_path: HashMap, } impl RobotModel { - pub fn new(path: impl Into) -> Result { + pub fn new(path: impl Into, package_path: HashMap) -> Result { let path = path.into(); let (robot, urdf_text) = read_urdf(&path)?; Ok(Self { path, urdf_text: Arc::new(Mutex::new(urdf_text)), robot, + package_path, }) } pub async fn from_text( path: impl Into, urdf_text: impl Into, + package_path: HashMap, ) -> Result { let path = path.into(); let urdf_text = urdf_text.into(); @@ -51,6 +74,7 @@ mod native { path, urdf_text: Arc::new(Mutex::new(urdf_text)), robot, + package_path, }) } @@ -69,12 +93,16 @@ mod native { } } } + + pub(crate) fn take_package_path_map(&mut self) -> HashMap { + mem::take(&mut self.package_path) + } } } #[cfg(target_family = "wasm")] mod wasm { - use std::{path::Path, str, sync::Arc}; + use std::{collections::HashMap, mem, path::Path, str, sync::Arc}; use js_sys::Uint8Array; use parking_lot::Mutex; @@ -170,13 +198,11 @@ mod wasm { Ok(Uint8Array::new(&bytes).to_vec()) } - async fn read_urdf(input_file: impl AsRef) -> Result<(urdf_rs::Robot, String)> { - let s = read_to_string(input_file).await?; - let robot = urdf_rs::read_from_string(&s)?; - Ok((robot, s)) - } - - pub async fn load_mesh(robot: &mut urdf_rs::Robot, urdf_path: impl AsRef) -> Result<()> { + pub async fn load_mesh( + robot: &mut urdf_rs::Robot, + urdf_path: impl AsRef, + package_path: &HashMap, + ) -> Result<()> { let urdf_path = urdf_path.as_ref(); for geometry in robot.links.iter_mut().flat_map(|link| { link.visual @@ -185,28 +211,30 @@ mod wasm { .chain(link.collision.iter_mut().map(|c| &mut c.geometry)) }) { if let urdf_rs::Geometry::Mesh { filename, .. } = geometry { - let input_file = - if filename.starts_with("https://") || filename.starts_with("http://") { - filename.clone() - } else if filename.starts_with("package://") { - return Err(Error::from(format!( - "ros package ({filename}) is not supported in wasm", - ))); - } else if filename.starts_with("file://") { - return Err(Error::from(format!( - "local file ({filename}) is not supported in wasm", - ))); - } else { - // We don't use url::Url::path/set_path here, because - // urdf_path may be a relative path to a file bundled - // with the server. Path::with_file_name works for wasm - // where the separator is /, so we use it. - urdf_path - .with_file_name(&filename) - .to_str() - .unwrap() - .to_string() - }; + let input_file = if filename.starts_with("https://") + || filename.starts_with("http://") + { + filename.clone() + } else if filename.starts_with("package://") { + crate::utils::replace_package_with_path(filename, package_path).ok_or_else(|| + format!( + "ros package ({filename}) is not supported in wasm; consider using `package-path[]` URL parameter", + ))? + } else if filename.starts_with("file://") { + return Err(Error::from(format!( + "local file ({filename}) is not supported in wasm", + ))); + } else { + // We don't use url::Url::path/set_path here, because + // urdf_path may be a relative path to a file bundled + // with the server. Path::with_file_name works for wasm + // where the separator is /, so we use it. + urdf_path + .with_file_name(&filename) + .to_str() + .unwrap() + .to_string() + }; let kind = if input_file.ends_with(".obj") || input_file.ends_with(".OBJ") { MeshKind::Obj @@ -242,37 +270,42 @@ mod wasm { pub(crate) path: String, pub(crate) urdf_text: Arc>, robot: urdf_rs::Robot, + package_path: HashMap, } impl RobotModel { - pub async fn new(path: impl Into) -> Result { + pub async fn new( + path: impl Into, + package_path: HashMap, + ) -> Result { let path = path.into(); - let (mut robot, urdf_text) = read_urdf(&path).await?; - load_mesh(&mut robot, &path).await?; - Ok(Self { - path, - urdf_text: Arc::new(Mutex::new(urdf_text)), - robot, - }) + let urdf_text = read_to_string(&path).await?; + Self::from_text(path, urdf_text, package_path).await } pub async fn from_text( path: impl Into, urdf_text: impl Into, + package_path: HashMap, ) -> Result { let path = path.into(); let urdf_text = urdf_text.into(); let mut robot = urdf_rs::read_from_string(&urdf_text)?; - load_mesh(&mut robot, &path).await?; + load_mesh(&mut robot, &path, &package_path).await?; Ok(Self { path, urdf_text: Arc::new(Mutex::new(urdf_text)), robot, + package_path, }) } pub(crate) fn get(&mut self) -> &urdf_rs::Robot { &self.robot } + + pub(crate) fn take_package_path_map(&mut self) -> HashMap { + mem::take(&mut self.package_path) + } } } diff --git a/src/viewer.rs b/src/viewer.rs index f05d50d..121eab7 100644 --- a/src/viewer.rs +++ b/src/viewer.rs @@ -51,16 +51,28 @@ impl Viewer { self.is_texture_enabled = true; } - pub fn add_robot(&mut self, window: &mut Window, urdf_robot: &urdf_rs::Robot) { - self.add_robot_with_base_dir(window, urdf_robot, None); + pub fn add_robot( + &mut self, + window: &mut Window, + urdf_robot: &urdf_rs::Robot, + package_path: &HashMap, + ) { + self.add_robot_with_base_dir(window, urdf_robot, None, package_path); } pub fn add_robot_with_base_dir( &mut self, window: &mut Window, urdf_robot: &urdf_rs::Robot, base_dir: Option<&Path>, + package_path: &HashMap, ) { - self.add_robot_with_base_dir_and_collision_flag(window, urdf_robot, base_dir, false); + self.add_robot_with_base_dir_and_collision_flag( + window, + urdf_robot, + base_dir, + false, + package_path, + ); } pub fn add_robot_with_base_dir_and_collision_flag( &mut self, @@ -68,6 +80,7 @@ impl Viewer { urdf_robot: &urdf_rs::Robot, base_dir: Option<&Path>, is_collision: bool, + package_path: &HashMap, ) { self.link_joint_map = k::urdf::link_to_joint_map(urdf_robot); @@ -103,6 +116,7 @@ impl Viewer { base_dir, &mut scene_group, self.is_texture_enabled, + package_path, ) { Ok(mut base_group) => { // set initial origin offset