Add helper function for dora-rerun

examples/reachy1-human-shadowing/realsense-dev.yml

@@ -32,4 +32,8 @@ nodes:
       realsense/image: camera/image
       realsense/depth: camera/depth
       realsense/points3d: dora-mediapipe/points3d
-      series_pose: human-shadowing/pose
+      jointstate_reachy: human-shadowing/pose
+    env:
+      # git clone git clone https://github.com/aubrune/reachy_description.git
+      # Replace package path with local path relative to the urdf. Ex: meshes/...
+      reachy_urdf: /home/peter/Documents/work/reachy_description/reachy.urdf

node-hub/dora-rerun/src/lib.rs

@@ -4,13 +4,13 @@ use std::{collections::HashMap, env::VarError, path::Path};
 
 use dora_node_api::{
     arrow::{
-        array::{Array, AsArray, Float32Array, Float64Array, StringArray, UInt16Array, UInt8Array},
+        array::{Array, AsArray, Float64Array, StringArray, UInt16Array, UInt8Array},
         datatypes::Float32Type,
     },
     dora_core::config::DataId,
     into_vec, DoraNode, Event, Parameter,
 };
-use eyre::{eyre, Context, ContextCompat, Result};
+use eyre::{eyre, Context, Result};
 
 use rerun::{
     components::ImageBuffer,
@@ -319,11 +319,7 @@ pub fn lib_main() -> Result<()> {
                 };
                 mask_cache.insert(id.clone(), masks.clone());
             } else if id.as_str().contains("jointstate") {
-                let buffer: &Float32Array = data
-                    .as_any()
-                    .downcast_ref()
-                    .context("jointstate is not float32")?;
-                let mut positions: Vec<f32> = buffer.values().to_vec();
+                let mut positions: Vec<f32> = into_vec(&data)?;
 
                 // Match file name
                 let mut id = id.as_str().replace("jointstate_", "");
@@ -343,7 +339,11 @@ pub fn lib_main() -> Result<()> {
 
                     update_visualization(&rec, chain, &id, &positions)?;
                 } else {
-                    println!("Could not find chain for {}", id);
+                    println!(
+                        "Could not find chain for {}. Only contains: {:#?}",
+                        id,
+                        chains.keys()
+                    );
                 }
             } else if id.as_str().contains("series") {
                 update_series(&rec, id, data).context("could not plot series")?;

node-hub/dora-rerun/src/urdf.rs

@@ -88,8 +88,8 @@ pub fn init_urdf(rec: &RecordingStream) -> Result<HashMap<String, Chain<f32>>> {
                 ),
             )
             .unwrap();
-            chains.insert(path, chain);
         }
+        chains.insert(path, chain);
     }
 
     Ok(chains)
@@ -113,11 +113,14 @@ pub fn update_visualization(
             .world_transform()
             .context("Could not get world transform")?;
         let link_to_parent = if link_name != "base_link" {
-            let parent = link.parent().context("could not get parent")?;
-            parent
-                .world_transform()
-                .context("Could not get world transform")?
-                .inv_mul(&link_to_world)
+            if let Some(parent) = link.parent() {
+                parent
+                    .world_transform()
+                    .context("Could not get world transform")?
+                    .inv_mul(&link_to_world)
+            } else {
+                link_to_world
+            }
         } else {
             link_to_world
         };