Export package instead of URDF

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

rmf_site_editor/src/lib.rs

@@ -19,7 +19,6 @@ pub mod save;
 use save::*;
 pub mod widgets;
 use widgets::{menu_bar::MenuPluginManager, *};
-
 pub mod occupancy;
 use occupancy::OccupancyPlugin;
 pub mod issue;

rmf_site_editor/src/workcell/mod.rs

@@ -33,6 +33,8 @@ pub use model::*;
 pub mod save;
 pub use save::*;
 
+pub mod urdf_package_exporter;
+
 pub mod workcell;
 pub use workcell::*;
 

rmf_site_editor/src/workcell/save.rs

@@ -17,9 +17,10 @@
 
 use bevy::ecs::system::SystemState;
 use bevy::prelude::*;
-use std::path::PathBuf;
+use std::path::{Path as SysPath, PathBuf};
 
 use crate::site::{CollisionMeshMarker, Pending, VisualMeshMarker};
+use crate::workcell::urdf_package_exporter::{generate_package, PackageContext, Person};
 use crate::ExportFormat;
 
 use thiserror::Error as ThisError;
@@ -293,19 +294,6 @@ pub fn save_workcell(world: &mut World) {
         .drain()
         .collect();
     for save_event in save_events {
-        let path = save_event.to_file;
-        info!(
-            "Saving to {}",
-            path.to_str().unwrap_or("<failed to render??>")
-        );
-        let f = match std::fs::File::create(path) {
-            Ok(f) => f,
-            Err(err) => {
-                error!("Unable to save file: {err}");
-                continue;
-            }
-        };
-
         let workcell = match generate_workcell(world, save_event.root) {
             Ok(root) => root,
             Err(err) => {
@@ -314,23 +302,71 @@ pub fn save_workcell(world: &mut World) {
             }
         };
 
+        let path = save_event.to_file;
         match save_event.format {
-            ExportFormat::Default => match workcell.to_writer(f) {
-                Ok(()) => {
-                    info!("Save successful");
+            ExportFormat::Default => {
+                info!(
+                    "Saving to {}",
+                    path.to_str().unwrap_or("<failed to render??>")
+                );
+                let f = match std::fs::File::create(path) {
+                    Ok(f) => f,
+                    Err(err) => {
+                        error!("Unable to save file: {err}");
+                        continue;
+                    }
+                };
+                match workcell.to_writer(f) {
+                    Ok(()) => {
+                        info!("Save successful");
+                    }
+                    Err(err) => {
+                        error!("Save failed: {err}");
+                    }
                 }
-                Err(err) => {
-                    error!("Save failed: {err}");
-                }
-            },
-            ExportFormat::Urdf => match workcell.to_urdf_writer(f) {
-                Ok(()) => {
-                    info!("Save successful");
-                }
-                Err(err) => {
-                    error!("Save failed: {err}");
-                }
-            },
+            }
+            ExportFormat::Urdf => {
+                match export_package(&path, &workcell) {
+                    Ok(()) => {
+                        info!("Successfully exported package");
+                    }
+                    Err(err) => {
+                        error!("Failed to export package: {err}");
+                    }
+                };
+            }
         }
     }
 }
+
+fn export_package(path: &SysPath, workcell: &Workcell) -> Result<(), Box<dyn std::error::Error>> {
+    let output_directory = path
+        .parent()
+        .ok_or("Not able to get parent")?
+        .to_str()
+        .ok_or("Invalid output directory")?
+        .to_string();
+    let package_name = path
+        .file_stem()
+        .ok_or("Not able to get file_stem")?
+        .to_str()
+        .ok_or("Invalid file name")?
+        .to_string();
+
+    let package_context = PackageContext {
+        license: "TODO".to_string(),
+        maintainers: vec![Person {
+            name: "TODO".to_string(),
+            email: "todo@todo.com".to_string(),
+        }],
+        project_name: package_name,
+        fixed_frame: "world".to_string(),
+        dependencies: vec![],
+        project_description: "TODO".to_string(),
+        project_version: "0.0.1".to_string(),
+        urdf_file_name: "robot.urdf".to_string(),
+    };
+
+    generate_package(workcell, &package_context, &output_directory)?;
+    Ok(())
+}

rmf_site_editor/src/workcell/urdf_package_exporter/generate_package.rs

@@ -0,0 +1,242 @@
+use crate::site_asset_io::cache_path;
+use crate::workcell::urdf_package_exporter::template;
+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<dyn Error>> {
+    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<dyn Error>> {
+    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<Workcell, Box<dyn Error>> {
+    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<dyn Error>>::Ok(())
+        })?;
+    Ok(workcell)
+}
+
+fn get_mesh_paths(workcell: &Workcell) -> Result<Vec<String>, Box<dyn Error>> {
+    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<Option<String>, Box<dyn Error>> {
+    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<String>, output_directory: &Path) -> Result<(), Box<dyn Error>> {
+    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(())
+}
+
+fn get_path_to_asset_file(asset_source: &AssetSource) -> Result<String, Box<dyn Error>> {
+    match asset_source {
+        AssetSource::Package(path) => {
+            Ok((*urdf_rs::utils::expand_package_path(&path, None)).to_owned())
+        }
+        AssetSource::Remote(asset_name) => {
+            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())
+        }
+        AssetSource::Local(path) => Ok(path.clone()),
+        AssetSource::Search(_)
+        | AssetSource::OSMTile {
+            zoom: _,
+            latitude: _,
+            longitude: _,
+        }
+        | AssetSource::Bundled(_) => 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<dyn Error>> {
+    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(())
+}

rmf_site_editor/src/workcell/urdf_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;