Clean up a bit

Author: MichaelGrupp

crates/store/re_data_loader/src/loader_urdf.rs

@@ -6,7 +6,7 @@ use std::{
 use ahash::{HashMap, HashMapExt as _, HashSet, HashSetExt as _};
 use anyhow::{Context as _, bail};
 use itertools::Itertools as _;
-use urdf_rs::{Geometry, Joint, Link, LinkName, Material, Robot, Vec3, Vec4};
+use urdf_rs::{Geometry, Joint, Link, Material, Robot, Vec3, Vec4};
 
 use re_chunk::{ChunkBuilder, ChunkId, EntityPath, RowId, TimePoint};
 use re_log_types::{EntityPathPart, StoreId};
@@ -282,7 +282,7 @@ fn log_robot(
         .map(|prefix| prefix / EntityPath::from_single_string(urdf_tree.name.clone()))
         .unwrap_or_else(|| EntityPath::from_single_string(urdf_tree.name.clone()));
 
-    // Log the robot's root coordinate frame_id.
+    // The robot's root coordinate frame_id.
     send_archetype(
         tx,
         store_id,
@@ -379,8 +379,8 @@ fn log_joint(
         joint_path.clone(),
         origin,
         timepoint,
-        Some(parent),
-        Some(child),
+        parent.link.clone(),
+        child.link.clone(),
     )?;
 
     log_debug_format(
@@ -432,21 +432,17 @@ fn log_joint(
 
 fn transform_from_pose(
     origin: &urdf_rs::Pose,
-    parent: Option<&LinkName>,
-    child: Option<&LinkName>,
+    parent_frame: String,
+    child_frame: String,
 ) -> Transform3D {
     let urdf_rs::Pose { xyz, rpy } = origin;
     let translation = [xyz[0] as f32, xyz[1] as f32, xyz[2] as f32];
     let quaternion = quat_xyzw_from_roll_pitch_yaw(rpy[0] as f32, rpy[1] as f32, rpy[2] as f32);
-    let transform = Transform3D::update_fields()
+    Transform3D::update_fields()
         .with_translation(translation)
-        .with_quaternion(quaternion);
-    if let (Some(parent), Some(child)) = (parent, child) {
-        return transform
-            .with_parent_frame(parent.link.clone())
-            .with_child_frame(child.link.clone());
-    }
-    transform
+        .with_quaternion(quaternion)
+        .with_parent_frame(parent_frame)
+        .with_child_frame(child_frame)
 }
 
 fn send_transform(
@@ -455,31 +451,24 @@ fn send_transform(
     entity_path: EntityPath,
     origin: &urdf_rs::Pose,
     timepoint: &TimePoint,
-    parent: Option<&LinkName>,
-    child: Option<&LinkName>,
+    parent_frame: String,
+    child_frame: String,
 ) -> anyhow::Result<()> {
-    let urdf_rs::Pose { xyz, rpy } = origin;
-    let is_identity = xyz.0 == [0.0, 0.0, 0.0] && rpy.0 == [0.0, 0.0, 0.0];
-
-    if is_identity {
-        Ok(()) // avoid noise
-    } else {
-        // TODO: remove axis log this after debugging
-        send_archetype(
-            tx,
-            store_id,
-            entity_path.clone(),
-            timepoint,
-            &TransformAxes3D::update_fields().with_axis_length(0.1),
-        )?;
-        send_archetype(
-            tx,
-            store_id,
-            entity_path,
-            timepoint,
-            &transform_from_pose(origin, parent, child),
-        )
-    }
+    // TODO: remove axis log this after debugging
+    send_archetype(
+        tx,
+        store_id,
+        entity_path.clone(),
+        timepoint,
+        &TransformAxes3D::update_fields().with_axis_length(0.1),
+    )?;
+    send_archetype(
+        tx,
+        store_id,
+        entity_path,
+        timepoint,
+        &transform_from_pose(origin, parent_frame, child_frame),
+    )
 }
 
 /// Log the given value using its `Debug` formatting.
@@ -540,19 +529,15 @@ fn log_link(
         &CoordinateFrame::update_fields().with_frame_id(link.name.clone()),
     )?;
 
-    let link_parent = urdf_tree
-        .get_parent_of_link(&link.name)
-        .map(|joint| &joint.child);
-
     for (i, visual) in visual.iter().enumerate() {
         let urdf_rs::Visual {
             name,
             origin,
             geometry,
             material,
         } = visual;
-        let name = name.clone().unwrap_or_else(|| format!("visual_{i}"));
-        let vis_entity = link_entity / EntityPathPart::new(name.clone());
+        let visual_name = name.clone().unwrap_or_else(|| format!("visual_{i}"));
+        let visual_entity = link_entity / EntityPathPart::new(visual_name.clone());
 
         // Prefer inline defined material properties if present, otherwise fall back to global material.
         let material = material.as_ref().and_then(|mat| {
@@ -563,32 +548,33 @@ fn log_link(
             }
         });
 
-        let link_child = urdf_rs::LinkName { link: name };
-        // TODO
         send_transform(
             tx,
             store_id,
-            vis_entity.clone(),
+            visual_entity.clone(),
             origin,
             timepoint,
-            link_parent,
-            Some(&link_child),
+            link.name.clone(),
+            visual_name,
         )?;
 
-        if let Some(parent) = link_parent {
-            let coordinate_frame =
-                CoordinateFrame::update_fields().with_frame_id(parent.link.clone());
-            send_archetype(
-                tx,
-                store_id,
-                vis_entity.clone(),
-                timepoint,
-                &coordinate_frame,
-            )?;
-        }
+        let coordinate_frame = CoordinateFrame::update_fields().with_frame_id(link.name.clone());
+        send_archetype(
+            tx,
+            store_id,
+            visual_entity.clone(),
+            timepoint,
+            &coordinate_frame,
+        )?;
 
         log_geometry(
-            urdf_tree, tx, store_id, vis_entity, geometry, material, timepoint,
+            urdf_tree,
+            tx,
+            store_id,
+            visual_entity,
+            geometry,
+            material,
+            timepoint,
         )?;
     }
 
@@ -598,31 +584,27 @@ fn log_link(
             origin,
             geometry,
         } = collision;
-        let name = name.clone().unwrap_or_else(|| format!("collision_{i}"));
-        let collision_entity = link_entity / EntityPathPart::new(name.clone());
+        let collision_name = name.clone().unwrap_or_else(|| format!("collision_{i}"));
+        let collision_entity = link_entity / EntityPathPart::new(collision_name.clone());
 
-        let link_child = urdf_rs::LinkName { link: name };
         send_transform(
             tx,
             store_id,
             collision_entity.clone(),
             origin,
             timepoint,
-            link_parent,
-            Some(&link_child),
+            link.name.clone(),
+            collision_name,
         )?;
 
-        if let Some(parent) = link_parent {
-            let coordinate_frame =
-                CoordinateFrame::update_fields().with_frame_id(parent.link.clone());
-            send_archetype(
-                tx,
-                store_id,
-                collision_entity.clone(),
-                timepoint,
-                &coordinate_frame,
-            )?;
-        }
+        let coordinate_frame = CoordinateFrame::update_fields().with_frame_id(link.name.clone());
+        send_archetype(
+            tx,
+            store_id,
+            collision_entity.clone(),
+            timepoint,
+            &coordinate_frame,
+        )?;
 
         log_geometry(
             urdf_tree,