diff --git a/rmf_site_editor/src/save.rs b/rmf_site_editor/src/save.rs index 2e1dde14..38146bb0 100644 --- a/rmf_site_editor/src/save.rs +++ b/rmf_site_editor/src/save.rs @@ -102,7 +102,7 @@ pub fn dispatch_save_events( if let Some(file) = default_files.get(ws_root).ok().map(|f| f.0.clone()) { file } else { - let Some(file) = FileDialog::new().save_file() else { + let Some(file) = FileDialog::new().save_file() else { continue; }; file diff --git a/rmf_site_format/src/workcell.rs b/rmf_site_format/src/workcell.rs index 6f881f30..be7aed58 100644 --- a/rmf_site_format/src/workcell.rs +++ b/rmf_site_format/src/workcell.rs @@ -28,6 +28,7 @@ use bevy::prelude::{Bundle, Component, Deref, DerefMut, Entity}; use bevy::reflect::TypeUuid; use glam::{EulerRot, Vec3}; use serde::{Deserialize, Serialize}; +use thiserror::Error as ThisError; use urdf_rs::{Collision, Robot, Visual}; /// Helper structure to serialize / deserialize entities with parents @@ -326,6 +327,14 @@ impl From for urdf_rs::Geometry { } } +#[derive(Debug, ThisError)] +pub enum WorkcellToUrdfError { + #[error("Invalid anchor type {0:?}")] + InvalidAnchorType(Anchor), + #[error("Urdf error: {0}")] + WriteToStringError(#[from] urdf_rs::UrdfError), +} + impl Workcell { pub fn to_writer(&self, writer: W) -> serde_json::Result<()> { serde_json::ser::to_writer_pretty(writer, self) @@ -335,7 +344,7 @@ impl Workcell { serde_json::ser::to_string_pretty(self) } - pub fn to_urdf(&self) -> urdf_rs::Result { + pub fn to_urdf(&self) -> Result { let workcell = self.clone(); let visuals_and_parents: Vec<(urdf_rs::Visual, _)> = workcell @@ -367,12 +376,16 @@ impl Workcell { }) .collect(); - let links: Vec<_> = workcell + let links: Result, WorkcellToUrdfError> = workcell .frames .into_iter() .map(|f| { let frame = f.1.bundle; + let name = match frame.name { + Some(name) => name.0, + None => format!("frame_{}", f.0), + }; let visual: Vec = visuals_and_parents .iter() .filter(|(_, parent)| parent == &f.1.parent) @@ -384,17 +397,14 @@ impl Workcell { .map(|(collision, _)| collision.clone()) .collect(); - let pose: urdf_rs::Pose = match frame.anchor { - Anchor::Pose3D(pose) => pose.into(), - Anchor::CategorizedTranslate2D(_) | Anchor::Translate2D(_) => { - unreachable!("Only 3D poses are supported for conversion to urdf") - } - }; - urdf_rs::Link { - name: match frame.name { - Some(name) => name.0, - None => format!("frame_{}", f.0), - }, + let pose: urdf_rs::Pose; + if let Anchor::Pose3D(p) = frame.anchor { + pose = p.into(); + } else { + return Err(WorkcellToUrdfError::InvalidAnchorType(frame.anchor)); + } + let link = urdf_rs::Link { + name, inertial: urdf_rs::Inertial { origin: pose, inertia: { @@ -411,9 +421,14 @@ impl Workcell { }, collision, visual, - } + }; + Ok(link) }) .collect(); + let links = match links { + Ok(links) => links, + Err(e) => return Err(e), + }; let robot = urdf_rs::Robot { name: workcell.properties.name, links, @@ -423,9 +438,12 @@ impl Workcell { return Ok(robot); } - pub fn to_urdf_string(&self) -> urdf_rs::Result { + pub fn to_urdf_string(&self) -> Result { match self.to_urdf() { - Ok(urdf) => urdf_rs::write_to_string(&urdf), + Ok(urdf) => match urdf_rs::write_to_string(&urdf) { + Ok(string) => Ok(string), + Err(e) => Err(WorkcellToUrdfError::WriteToStringError(e)), + }, Err(e) => Err(e), } }