Support replacing ROS package path by command line

openrr · Oct 19, 2022 · cccafcb · cccafcb
1 parent 33bed2e
commit cccafcb
Show file tree

Hide file tree

Showing 7 changed files with 142 additions and 69 deletions.
diff --git 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
@@ -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
@@ -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<String, String>,
 }
 
 impl UrdfViewerApp {
@@ -152,6 +154,7 @@ impl UrdfViewerApp {
         ground_height: Option<f32>,
     ) -> Result<Self, Error> {
         let input_path = PathBuf::from(&urdf_robot.path);
+        let package_path = urdf_robot.take_package_path_map();
         let robot: k::Chain<f32> = 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<RobotStateHandle> {
@@ -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<f32>,
+
+    /// Replace `package://PACKAGE` in mesh tag with PATH.
+    #[structopt(long = "package-path", value_name = "PACKAGE=PATH")]
+    pub package_path: Vec<String>,
 }
 
 fn default_back_ground_color_b() -> f32 {
@@ -969,6 +980,17 @@ impl Opt {
         }
     }
 
+    pub fn create_package_path_map(&self) -> Result<HashMap<String, String>, 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<Self, Error> {
         let href = crate::utils::window()?.location().href()?;

diff --git 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
@@ -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<String, String>,
 ) -> Result<SceneNode> {
     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}");