diff --git a/src/wrappers/material_and_mesh.rs b/src/wrappers/material_and_mesh.rs index 460a094..f573f2f 100644 --- a/src/wrappers/material_and_mesh.rs +++ b/src/wrappers/material_and_mesh.rs @@ -9,6 +9,7 @@ use derive_more::From; use glam::Vec3; use nalgebra::Vector3; use urdf_rs::Visual; +use urdf_rs::Collision; #[derive(From, Clone)] pub struct VisualWrapper(Visual); @@ -35,82 +36,113 @@ impl From<&VisualWrapper> for MaterialFlag { } } +impl HasGeometry for VisualWrapper { + fn get_geometry(&self) -> &urdf_rs::Geometry { + &self.0.geometry + } +} + impl From<&VisualWrapper> for FileCheckPicker { fn from(value: &VisualWrapper) -> Self { - let visual = &value.0; - // let urdf_rotation_flipOLD = Matrix3::new( - // 0.0, 0.0, -1.0, - // 0.0, 1.0, 0.0, - // 1.0, 0.0, 0.0, - // ); - let box_allign = [ - Vec3::new(0.0, 0.0, -1.0), - Vec3::new(0.0, 1.0, 0.0), - Vec3::new(1.0, 0.0, 0.0), - ]; - let cylinder_align = [ - Vec3::new(-1.0, 0.0, 0.0), - Vec3::new(0.0, 0.0, 1.0), - Vec3::new(0.0, 1.0, 0.0), - ]; + to_flag_geometry(value) + } +} - let urdf_geometry = &visual.geometry; +#[derive(From, Clone)] +pub struct CollisionWrapper(Collision); - let flag_geometry = match urdf_geometry { - urdf_rs::Geometry::Box { size } => { - let bevy_size = /*urdf_rotation_flip * */ Vector3::new(size[0], size[1], size[2]); - FileCheckPicker::PureComponent(GeometryFlag { - primitive: MeshPrimitive::Cuboid { - //size: (*size).map(|f| f as f32), - size: [ - bevy_size[0] as f32, - bevy_size[1] as f32, - bevy_size[2] as f32, - ], - }, - orientation_matrix: box_allign, - }) - } - urdf_rs::Geometry::Cylinder { radius, length } => { - //let bevy_size = Vector3::new(radius, length, radius); - FileCheckPicker::PureComponent(GeometryFlag { - primitive: MeshPrimitive::Cylinder { - radius: *radius as f32, - length: *length as f32, - }, - orientation_matrix: cylinder_align, - }) - } - urdf_rs::Geometry::Capsule { radius, length } => { - FileCheckPicker::PureComponent(GeometryFlag { - primitive: MeshPrimitive::Capsule { - radius: *radius as f32, - length: *length as f32, - }, - //FIXME: - ..default() - }) - } - urdf_rs::Geometry::Sphere { radius } => FileCheckPicker::PureComponent(GeometryFlag { - primitive: MeshPrimitive::Sphere { +impl HasGeometry for CollisionWrapper { + fn get_geometry(&self) -> &urdf_rs::Geometry { + &self.0.geometry + } +} + +impl From<&CollisionWrapper> for FileCheckPicker { + fn from(value: &CollisionWrapper) -> Self { + to_flag_geometry(value) + } +} + +trait HasGeometry { + fn get_geometry(&self) -> &urdf_rs::Geometry; +} + +fn to_flag_geometry(wrapper: &T) -> FileCheckPicker +where + T: HasGeometry, +{ + let urdf_geometry = wrapper.get_geometry(); + + // let urdf_rotation_flipOLD = Matrix3::new( + // 0.0, 0.0, -1.0, + // 0.0, 1.0, 0.0, + // 1.0, 0.0, 0.0, + // ); + let box_allign = [ + Vec3::new(0.0, 0.0, -1.0), + Vec3::new(0.0, 1.0, 0.0), + Vec3::new(1.0, 0.0, 0.0), + ]; + let cylinder_align = [ + Vec3::new(-1.0, 0.0, 0.0), + Vec3::new(0.0, 0.0, 1.0), + Vec3::new(0.0, 1.0, 0.0), + ]; + + let flag_geometry = match urdf_geometry { + urdf_rs::Geometry::Box { size } => { + let bevy_size = /*urdf_rotation_flip * */ Vector3::new(size[0], size[1], size[2]); + FileCheckPicker::PureComponent(GeometryFlag { + primitive: MeshPrimitive::Cuboid { + //size: (*size).map(|f| f as f32), + size: [ + bevy_size[0] as f32, + bevy_size[1] as f32, + bevy_size[2] as f32, + ], + }, + orientation_matrix: box_allign, + }) + } + urdf_rs::Geometry::Cylinder { radius, length } => { + //let bevy_size = Vector3::new(radius, length, radius); + FileCheckPicker::PureComponent(GeometryFlag { + primitive: MeshPrimitive::Cylinder { radius: *radius as f32, + length: *length as f32, }, + orientation_matrix: cylinder_align, + }) + } + urdf_rs::Geometry::Capsule { radius, length } => { + FileCheckPicker::PureComponent(GeometryFlag { + primitive: MeshPrimitive::Capsule { + radius: *radius as f32, + length: *length as f32, + }, + //FIXME: ..default() - }), - urdf_rs::Geometry::Mesh { filename, .. } => { - //let asset_source = AssetSource::Package(filename.clone()); + }) + } + urdf_rs::Geometry::Sphere { radius } => FileCheckPicker::PureComponent(GeometryFlag { + primitive: MeshPrimitive::Sphere { + radius: *radius as f32, + }, + ..default() + }), + urdf_rs::Geometry::Mesh { filename, .. } => { + //let asset_source = AssetSource::Package(filename.clone()); - //let asset_path = parse_urdf_source(asset_source); - //println!("file name is {:#?}", filename); - FileCheckPicker::PathComponent( - //AssetSource::Package(filename.clone()); - GeometryFile { - //source: AssetSource::Package(filename.clone()), - source: filename.clone(), - }, - ) - } - }; - return flag_geometry; - } -} \ No newline at end of file + //let asset_path = parse_urdf_source(asset_source); + //println!("file name is {:#?}", filename); + FileCheckPicker::PathComponent( + //AssetSource::Package(filename.clone()); + GeometryFile { + //source: AssetSource::Package(filename.clone()), + source: filename.clone(), + }, + ) + } + }; + return flag_geometry; +} diff --git a/src/wrappers/urdf.rs b/src/wrappers/urdf.rs index 07b087e..1a13dcf 100644 --- a/src/wrappers/urdf.rs +++ b/src/wrappers/urdf.rs @@ -28,6 +28,7 @@ use bevy_ecs::{prelude::*, query::QueryData}; use crate::loaders::urdf_loader::Urdf; +use super::material_and_mesh::CollisionWrapper; use super::material_and_mesh::VisualWrapper; /// the collection of things that qualify as a "link", in the ROS 2 context. @@ -48,7 +49,6 @@ impl LazyDeserialize for Urdf { } } - impl<'a> FromStructure for Urdf { fn into_entities(commands: &mut Commands, value: Self, _: AssetSpawnRequest) { //let name = request.item.clone(); @@ -110,28 +110,40 @@ impl<'a> FromStructure for Urdf { }; commands .entity(e) - .insert(MaterialFlag::from(&visual_wrapper)); + .insert(MaterialFlag::from(&visual_wrapper)) + .insert(VisibilityBundle::default()) + .insert(TransformBundle { + //local: temp_rotate_for_demo, + ..default() + }) + .insert(GeometryShiftMarked::default()); + } + if let Some(collision) = link.collision.first() { + let collision_wrapper = CollisionWrapper::from(collision.clone()); + match FileCheckPicker::from(&collision_wrapper) { + FileCheckPicker::PureComponent(t) => commands.entity(e).insert(t), + FileCheckPicker::PathComponent(u) => commands.entity(e).insert(u), + }; + commands + .entity(e) + .insert(VisibilityBundle::default()) + .insert(TransformBundle { + //local: temp_rotate_for_demo, + ..default() + }) + .insert(ColliderFlag::default()) + .insert(SolverGroupsFlag { + memberships: GroupWrapper::GROUP_1, + filters: GroupWrapper::GROUP_2, + }) + .insert(GeometryShiftMarked::default()) + .insert(RigidBodyFlag::Dynamic) + .insert(CcdFlag::default()); } //let mut temp_rotate_for_demo = spawn_request.position; //FIXME: urdf meshes have their verticies re-oriented to match bevy's cordinate system, but their rotation isn't rotated back // to account for this, this will need a proper fix later. //temp_rotate_for_demo.rotate_x(-PI * 0.5); - - commands - .entity(e) - .insert(VisibilityBundle::default()) - .insert(TransformBundle { - //local: temp_rotate_for_demo, - ..default() - }) - .insert(ColliderFlag::default()) - .insert(SolverGroupsFlag { - memberships: GroupWrapper::GROUP_1, - filters: GroupWrapper::GROUP_2, - }) - .insert(GeometryShiftMarked::default()) - .insert(RigidBodyFlag::Dynamic) - .insert(CcdFlag::default()); } for (_, joint) in structured_joint_map.iter() {