From e406e4cfb26b8abda31e19e877f7893e1306f354 Mon Sep 17 00:00:00 2001 From: Audrow Nash Date: Fri, 10 Nov 2023 21:35:16 +0000 Subject: [PATCH 1/6] Export package instead of URDF Signed-off-by: Audrow Nash --- rmf_site_editor/Cargo.toml | 1 + rmf_site_editor/src/lib.rs | 2 + .../src/package_exporter/generate_package.rs | 244 ++++++++++++++++++ rmf_site_editor/src/package_exporter/mod.rs | 5 + .../src/package_exporter/template.rs | 43 +++ .../templates/CMakeLists.txt.j2 | 20 ++ .../templates/display.launch.py.j2 | 31 +++ .../package_exporter/templates/package.xml.j2 | 25 ++ .../package_exporter/templates/urdf.rviz.j2 | 35 +++ rmf_site_editor/src/workcell/save.rs | 98 ++++--- 10 files changed, 475 insertions(+), 29 deletions(-) create mode 100644 rmf_site_editor/src/package_exporter/generate_package.rs create mode 100644 rmf_site_editor/src/package_exporter/mod.rs create mode 100644 rmf_site_editor/src/package_exporter/template.rs create mode 100644 rmf_site_editor/src/package_exporter/templates/CMakeLists.txt.j2 create mode 100644 rmf_site_editor/src/package_exporter/templates/display.launch.py.j2 create mode 100644 rmf_site_editor/src/package_exporter/templates/package.xml.j2 create mode 100644 rmf_site_editor/src/package_exporter/templates/urdf.rviz.j2 diff --git a/rmf_site_editor/Cargo.toml b/rmf_site_editor/Cargo.toml index 5bfeb813..388cc91e 100644 --- a/rmf_site_editor/Cargo.toml +++ b/rmf_site_editor/Cargo.toml @@ -48,6 +48,7 @@ utm = "0.1.6" sdformat_rs = { git = "https://github.com/open-rmf/sdf_rust_experimental", rev = "f86344f"} gz-fuel = { git = "https://github.com/open-rmf/gz-fuel-rs", branch = "first_implementation" } pathdiff = "*" +tera = "1.19.1" # only enable the 'dynamic_linking' feature if we're not building for web or windows # TODO(luca) it seems this can be enabled on windows, check diff --git a/rmf_site_editor/src/lib.rs b/rmf_site_editor/src/lib.rs index 03e4bcaa..eb907c57 100644 --- a/rmf_site_editor/src/lib.rs +++ b/rmf_site_editor/src/lib.rs @@ -19,6 +19,8 @@ pub mod save; use save::*; pub mod widgets; use widgets::{menu_bar::MenuPluginManager, *}; +pub mod package_exporter; +use package_exporter::*; pub mod occupancy; use occupancy::OccupancyPlugin; diff --git a/rmf_site_editor/src/package_exporter/generate_package.rs b/rmf_site_editor/src/package_exporter/generate_package.rs new file mode 100644 index 00000000..ed910b84 --- /dev/null +++ b/rmf_site_editor/src/package_exporter/generate_package.rs @@ -0,0 +1,244 @@ +use crate::template; +use dirs; +use rmf_site_format::{AssetSource, Geometry, Workcell}; +use std::error::Error; +use std::io::{Error as IoError, ErrorKind as IoErrorKind}; +use std::path::{Path, PathBuf}; + +pub fn generate_package( + workcell: &Workcell, + package_context: &template::PackageContext, + output_directory: &String, +) -> Result<(), Box> { + let new_package_name = &package_context.project_name; + + let mesh_directory_name = "meshes".to_string(); + let launch_directory_name = "launch".to_string(); + let urdf_directory_name = "urdf".to_string(); + let rviz_directory_name = "rviz".to_string(); + + // Create paths + let output_directory_path = std::path::Path::new(&output_directory); + let output_package_path = output_directory_path.join(&new_package_name); + let meshes_directory_path = output_package_path.join(&mesh_directory_name); + let launch_directory_path = output_package_path.join(&launch_directory_name); + let urdf_directory_path = output_package_path.join(&urdf_directory_name); + let rviz_directory_path = output_package_path.join(&rviz_directory_name); + + // Create directories + if output_package_path.exists() { + std::fs::remove_dir_all(&output_directory_path)?; + } + for directory_path in [ + &output_package_path, + &meshes_directory_path, + &launch_directory_path, + &urdf_directory_path, + &rviz_directory_path, + ] { + std::fs::create_dir_all(directory_path)?; + } + + // Create the package + write_urdf_and_copy_mesh_files( + &workcell, + &mesh_directory_name, + &meshes_directory_path, + &new_package_name, + &urdf_directory_path, + ); + generate_templates( + package_context, + &output_package_path, + &launch_directory_path, + &rviz_directory_path, + )?; + + Ok(()) +} + +fn write_urdf_and_copy_mesh_files( + workcell: &Workcell, + mesh_directory_name: &str, + meshes_directory_path: &std::path::Path, + new_package_name: &str, + urdf_directory_path: &std::path::Path, +) -> Result<(), Box> { + let asset_paths = get_mesh_paths(&workcell)?; + copy_files(&asset_paths, &meshes_directory_path)?; + + let new_workcell = convert_to_package_paths(&workcell, new_package_name, mesh_directory_name)?; + + let urdf_robot = new_workcell.to_urdf()?; + let urdf_file_path = urdf_directory_path.join("robot.urdf"); + let urdf_string = urdf_rs::write_to_string(&urdf_robot)?; + std::fs::write(urdf_file_path, urdf_string)?; + + Ok(()) +} + +fn convert_to_package_paths( + workcell: &Workcell, + package_name: &str, + mesh_directory_name: &str, +) -> Result> { + let mut workcell = workcell.clone(); + workcell + .visuals + .iter_mut() + .chain(workcell.collisions.iter_mut()) + .try_for_each(|(id, visual)| { + if let Geometry::Mesh { + source: asset_source, + scale, + } = &mut visual.bundle.geometry + { + let path = get_path_to_asset_file(asset_source)?; + + let file_name = PathBuf::from(&path) + .file_name() + .ok_or(IoError::new( + IoErrorKind::InvalidInput, + "Unable to get file name from path", + ))? + .to_str() + .ok_or(IoError::new( + IoErrorKind::InvalidInput, + "Unable to convert file name to str", + ))? + .to_owned(); + + let package_path = + format!("{}/{}/{}", package_name, mesh_directory_name, file_name); + *asset_source = AssetSource::Package(package_path); + } + Result::<(), Box>::Ok(()) + })?; + Ok(workcell) +} + +fn get_mesh_paths(workcell: &Workcell) -> Result, Box> { + let paths = workcell + .visuals + .iter() + .chain(workcell.collisions.iter()) + .filter_map(|(id, visual)| get_path_if_mesh_geometry(&visual.bundle.geometry).ok()?) + .collect(); + Ok(paths) +} + +fn get_path_if_mesh_geometry(geometry: &Geometry) -> Result, Box> { + if let Geometry::Mesh { + source: asset_source, + scale, + } = geometry + { + Ok(Some(get_path_to_asset_file(asset_source)?)) + } else { + Ok(None) + } +} + +fn copy_files(paths: &Vec, output_directory: &Path) -> Result<(), Box> { + for path in paths.iter() { + let file_name = PathBuf::from(path) + .file_name() + .ok_or(IoError::new( + IoErrorKind::InvalidInput, + "Unable to get file name from path", + ))? + .to_owned(); + let new_path = PathBuf::from(output_directory).join(file_name); + match std::fs::copy(path, &new_path) { + Ok(_) => {} + Err(e) => { + println!( + "Error copying file '{}' to '{}': {}", + path, + new_path.display(), + e + ); + } + } + } + Ok(()) +} + +// TODO(anyone) remove duplication with rmf_site_editor +fn cache_path() -> PathBuf { + let mut p = dirs::cache_dir().unwrap(); + p.push("open-robotics"); + p.push("rmf_site_editor"); + return p; +} + +fn get_path_to_asset_file(asset_source: &AssetSource) -> Result> { + if let AssetSource::Package(_) = asset_source { + let path = String::from(asset_source); + Ok((*urdf_rs::utils::expand_package_path(&path, None)).to_owned()) + } else if let AssetSource::Remote(asset_name) = asset_source { + let mut asset_path = cache_path(); + asset_path.push(PathBuf::from(&asset_name)); + Ok(asset_path + .to_str() + .ok_or(IoError::new( + IoErrorKind::InvalidInput, + "Unable to convert asset_path to str", + ))? + .to_string()) + } else if let AssetSource::Local(path) = asset_source { + Ok(path.clone()) + } else { + Err(IoError::new( + IoErrorKind::Unsupported, + "Not a supported asset type for exporting a workcell to a package", + ))? + } +} + +fn generate_templates( + package_context: &template::PackageContext, + package_directory: &std::path::Path, + launch_directory: &std::path::Path, + rviz_directory: &std::path::Path, +) -> Result<(), Box> { + let directory = Path::new(file!()) + .parent() + .ok_or_else(|| { + IoError::new( + IoErrorKind::Other, + format!("Could not get directory of {}", file!()), + ) + })? + .to_str() + .ok_or_else(|| { + IoError::new( + IoErrorKind::Other, + format!("Could not convert directory of {} to string", file!()), + ) + })?; + let templates = vec![ + template::Template { + name: "package.xml".to_string(), + path: format!("{}/templates/package.xml.j2", directory), + output_path: package_directory.join("package.xml"), + }, + template::Template { + name: "CMakeLists.txt".to_string(), + path: format!("{}/templates/CMakeLists.txt.j2", directory), + output_path: package_directory.join("CMakeLists.txt"), + }, + template::Template { + name: "urdf.rviz".to_string(), + path: format!("{}/templates/urdf.rviz.j2", directory), + output_path: rviz_directory.join("urdf.rviz"), + }, + template::Template { + name: "display.launch.py".to_string(), + path: format!("{}/templates/display.launch.py.j2", directory), + output_path: launch_directory.join("display.launch.py"), + }, + ]; + template::populate_and_save_templates(templates, package_context)?; + Ok(()) +} diff --git a/rmf_site_editor/src/package_exporter/mod.rs b/rmf_site_editor/src/package_exporter/mod.rs new file mode 100644 index 00000000..5d3e09b3 --- /dev/null +++ b/rmf_site_editor/src/package_exporter/mod.rs @@ -0,0 +1,5 @@ +pub mod generate_package; +pub use generate_package::generate_package; + +pub use template::{PackageContext, Person}; +pub mod template; diff --git a/rmf_site_editor/src/package_exporter/template.rs b/rmf_site_editor/src/package_exporter/template.rs new file mode 100644 index 00000000..286854ec --- /dev/null +++ b/rmf_site_editor/src/package_exporter/template.rs @@ -0,0 +1,43 @@ +use serde::Serialize; +use std::error::Error; +use std::path::PathBuf; +use tera::Tera; + +#[derive(Serialize)] +pub struct PackageContext { + pub project_name: String, + pub project_description: String, + pub project_version: String, + pub license: String, + pub maintainers: Vec, + pub dependencies: Vec, + pub fixed_frame: String, + pub urdf_file_name: String, +} + +#[derive(Serialize)] +pub struct Person { + pub name: String, + pub email: String, +} + +pub struct Template { + pub name: String, + pub path: String, + pub output_path: PathBuf, +} + +pub fn populate_and_save_templates( + templates: Vec