diff --git a/Cargo.toml b/Cargo.toml index 9843d48..a4130b8 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -1,6 +1,6 @@ [package] name = "urdf-viz" -version = "0.0.2" +version = "0.0.3" authors = ["Takashi Ogura "] description = "URDF visualization" license = "Apache-2.0" @@ -11,6 +11,7 @@ documentation = "http://docs.rs/urdf-viz" [dependencies] alga = "0.5.1" +assimp = "0.2.0" structopt = "0.1.0" structopt-derive = "0.1.0" env_logger = "0.4.2" diff --git a/README.md b/README.md index e2f83d9..4ec5b52 100644 --- a/README.md +++ b/README.md @@ -3,9 +3,6 @@ urdf_viz Visualize [URDF(Unified Robot Description Format)](http://wiki.ros.org/urdf) file. `urdf_viz` is written by rust-lang. -It supports `.obj` files as mesh format, but it can use other formats by converting -other files using `meshlabserver` command or `assimp` command. Please install `meshlab` and `assimp-utils` -if you need to visualize `.dae`, `.stl` or something. Install -------------- @@ -16,7 +13,7 @@ If you are using rust-lang already and `cargo` is installed, you can install by $ cargo install urdf_viz ``` -Install mesh converter commands +Install mesh converter commands (optional) ```bash $ sudo apt-get install meshlab assimp-utils @@ -39,17 +36,19 @@ $ urdf_viewer URDF_FILE.urdf ``` It is possible to use xacro file directly. -It will be converted by `rosrun xacro xacro` in `urdf_viewer`. +It will be converted by `rosrun xacro xacro` inside of `urdf_viewer`. ```bash $ urdf_viewer XACRO_FILE.urdf.xacro ``` -The default mesh converter is `meshlabserver`. If you failed to convert mesh files, -try `-a` option to use `assimp`. +The default mesh converter is `assimp`. Sometimes it fails to create currect +meshes. (for example, `nao`, `pepper` models fails) + +If you failed to convert mesh files, try `-m` option to use `meshlabserver`. ```bash -$ urdf_viewer -a URDF_FILE.urdf +$ urdf_viewer -m URDF_FILE.urdf ``` For other commands, please read `-h` option. @@ -68,7 +67,7 @@ In the GUI, you can * change the joint to be moved by `[` and `]` * Inverse kinematics (only positions) * `Shift` + Drag to use inverse kinematics(Y and Z axis) - * `Shift` + `Ctrl` + Drag to use inverse kinematics(Y and X axis) + * `Shift` + `Ctrl` + Drag to use inverse kinematics(X and Z axis) * change the move target for inverse kinematics by `,` and `.` * `r` key to set random joints * Move view point diff --git a/src/lib.rs b/src/lib.rs index 2130fd4..cfaf1c5 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -11,6 +11,7 @@ //! You need to install meshlab or assimp anyway. //! extern crate alga; +extern crate assimp; extern crate glfw; extern crate kiss3d; extern crate nalgebra as na; @@ -24,9 +25,12 @@ extern crate structopt; #[macro_use] extern crate structopt_derive; +use assimp::{Importer, LogStream}; +use kiss3d::resource::Mesh; use kiss3d::scene::SceneNode; use kiss3d::window::Window; use rayon::prelude::*; +use std::cell::RefCell; use regex::Regex; use std::collections::HashMap; use std::fs; @@ -37,10 +41,34 @@ use std::process::Command; use std::rc::Rc; pub enum MeshConvert { - Assimp, - Meshlab, + AssimpLibrary, + AssimpCommand, + MeshlabCommand, } +fn convert_assimp_scene_to_kiss3d_meshes(scene: assimp::Scene) -> Vec>> { + scene + .mesh_iter() + .map(|mesh| { + let vertices = mesh.vertex_iter() + .map(|v| na::Point3::new(v.x, v.y, v.z)) + .collect::>(); + let indices = mesh.face_iter() + .map(|f| { + if f.num_indices < 3 { + // TODO: fix this + na::Point3::new(0, 0, 0) + } else { + na::Point3::new(f[0], f[1], f[2]) + } + }) + .collect(); + Rc::new(RefCell::new(Mesh::new(vertices, indices, None, None, false))) + }) + .collect() +} + + fn get_cache_dir() -> &'static str { "/tmp/urdf_vis/" } @@ -112,7 +140,6 @@ pub fn convert_to_obj_file_by_assimp(filename: &Path, } } - fn rospack_find(package: &str) -> Option { let output = Command::new("rospack") .arg("find") @@ -144,6 +171,7 @@ fn expand_package_path(filename: &str, base_dir: &Path) -> String { } } + fn get_cache_or_obj_path(path: &Path) -> PathBuf { match path.extension() { Some(ext) => { @@ -158,15 +186,25 @@ fn get_cache_or_obj_path(path: &Path) -> PathBuf { Path::new(&cache_path).to_path_buf() } + fn convert_mesh_if_needed(filename: &str, mesh_convert: &MeshConvert, base_dir: &Path) { + match mesh_convert { + &MeshConvert::AssimpLibrary => return, + _ => {} + }; let replaced_filename = expand_package_path(filename, base_dir); let path = Path::new(&replaced_filename); assert!(path.exists(), "{} not found", replaced_filename); let new_path = get_cache_or_obj_path(path); if !new_path.exists() { match *mesh_convert { - MeshConvert::Assimp => convert_to_obj_file_by_assimp(path, new_path.as_path()), - MeshConvert::Meshlab => convert_to_obj_file_by_meshlab(path, new_path.as_path()), + MeshConvert::AssimpLibrary => return, + MeshConvert::AssimpCommand => { + convert_to_obj_file_by_assimp(path, new_path.as_path()) + } + MeshConvert::MeshlabCommand => { + convert_to_obj_file_by_meshlab(path, new_path.as_path()) + } } .unwrap_or_else(|err| { panic!("failed to convert mesh {:?}: {}", @@ -200,19 +238,27 @@ fn add_geometry(visual: &urdf_rs::Visual, return None; } let new_path = get_cache_or_obj_path(path); - // mtl_path works for only assimp - let mtl_path = new_path.with_extension("obj.mtl"); - debug!("mtl_path = {}", mtl_path.to_str().unwrap()); - // should be generated in advance - if !new_path.exists() { - error!("converted file {:?} not found", new_path.into_os_string()); - return None; + let na_scale = na::Vector3::new(scale[0] as f32, scale[1] as f32, scale[2] as f32); + if new_path.exists() && + new_path + .extension() + .unwrap_or_else(|| panic!("no extention: {:?}", new_path)) == + "obj" { + Some(window.add_obj(new_path.as_path(), Path::new(""), na_scale)) + } else { + let mut importer = Importer::new(); + importer.pre_transform_vertices(|x| x.enable = true); + match importer.read_file(path.to_str().unwrap()) { + Ok(as_scene) => { + let mut group = window.add_group(); + for mesh in convert_assimp_scene_to_kiss3d_meshes(as_scene) { + group.add_mesh(mesh.clone(), na_scale); + } + Some(group) + } + Err(_) => None, + } } - Some(window.add_obj(new_path.as_path(), - mtl_path.as_path(), - na::Vector3::new(scale[0] as f32, - scale[1] as f32, - scale[2] as f32))) } }; let rgba = &visual.material.color.rgba; @@ -248,6 +294,9 @@ impl Viewer { } } pub fn setup(&mut self, mesh_convert: MeshConvert, base_dir: &Path) { + LogStream::set_verbose_logging(true); + let mut log_stream = LogStream::stdout(); + log_stream.attach(); self.window .set_light(kiss3d::light::Light::StickToCamera); @@ -371,28 +420,36 @@ pub fn convert_xacro_if_needed_and_get_path(input_path: &Path) -> Result MeshConvert { - if self.assimp { - MeshConvert::Assimp + if self.meshlab { + MeshConvert::MeshlabCommand + } else if self.assimp { + MeshConvert::AssimpCommand } else { - MeshConvert::Meshlab + MeshConvert::AssimpLibrary } } } + #[test] fn test_func() { let input = Path::new("/home/user/robo.urdf"); diff --git a/src/urdf_viewer.rs b/src/urdf_viewer.rs index f682ff2..43b79c8 100644 --- a/src/urdf_viewer.rs +++ b/src/urdf_viewer.rs @@ -80,7 +80,7 @@ Up: joint angle +0.1 Down: joint angle -0.1 Ctrl+Drag: move joint Shift+Drag: IK (y, z) -Shift+Ctrl+Drag: IK (y, x) +Shift+Ctrl+Drag: IK (x, z) "; struct UrdfViewerApp { @@ -247,14 +247,14 @@ impl UrdfViewerApp { // [0]: y // [1]: z // [2]: x - target.translation.vector[0] -= - ((x - last_cur_pos_x) * ik_move_gain) as f32; + target.translation.vector[1] -= + ((y - last_cur_pos_y) * ik_move_gain) as f32; if is_ctrl { - target.translation.vector[2] += - ((y - last_cur_pos_y) * ik_move_gain) as f32; + target.translation.vector[2] -= + ((x - last_cur_pos_x) * ik_move_gain) as f32; } else { - target.translation.vector[1] -= - ((y - last_cur_pos_y) * ik_move_gain) as f32; + target.translation.vector[0] -= + ((x - last_cur_pos_x) * ik_move_gain) as f32; } self.update_ik_target_marker();