diff --git a/examples/reachy1-human-shadowing/realsense-dev.yml b/examples/reachy1-human-shadowing/realsense-dev.yml index b4ee4309..a3e4ebcb 100755 --- a/examples/reachy1-human-shadowing/realsense-dev.yml +++ b/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 diff --git a/node-hub/dora-rerun/src/lib.rs b/node-hub/dora-rerun/src/lib.rs index a75b3cd4..5c154b56 100644 --- a/node-hub/dora-rerun/src/lib.rs +++ b/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 = buffer.values().to_vec(); + let mut positions: Vec = 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")?; diff --git a/node-hub/dora-rerun/src/urdf.rs b/node-hub/dora-rerun/src/urdf.rs index 796a4a90..e84d17c2 100644 --- a/node-hub/dora-rerun/src/urdf.rs +++ b/node-hub/dora-rerun/src/urdf.rs @@ -88,8 +88,8 @@ pub fn init_urdf(rec: &RecordingStream) -> Result>> { ), ) .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 };