rerun_urdf 0.1.0

use nalgebra::{self as na, UnitQuaternion};
use rerun::RecordingStreamBuilder;
use rerun_urdf::RerunUrdfLoader;
use std::collections::HashMap;

fn main() -> anyhow::Result<()> {
    // 1) Rerun：spawn/连接/或保存 rrd（任选其一）
    let rec = RecordingStreamBuilder::new("urdf_rust").spawn()?;
    // let rec = RecordingStreamBuilder::new("urdf_pure_rust").connect_grpc()?;
    // let rec = RecordingStreamBuilder::new("urdf_pure_rust").save("run.rrd")?;

    // 2) 构建场景（不做 FK）
    let urdf_path = "E:\\yixing\\code\\Robot-Exp\\drives\\asserts\\jaka\\jaka_minicobo.urdf";
    let mesh_path = "E:\\yixing\\code\\Robot-Exp\\drives\\asserts\\";
    let loader = RerunUrdfLoader::from_urdf(urdf_path, Some(mesh_path), "world/robots/r1")?;

    // 3) 一次性静态落地几何
    loader.register_statics(&rec, na::Isometry3::identity())?;

    // 4) 生产者按帧提供各 link 世界位姿：这里做个假数据示范
    let mut frame: i64 = 0;
    loop {
        let mut poses: HashMap<String, na::Isometry3<f64>> = HashMap::new();

        // 生产者应当提供 **所有 link** 的位姿；至少提供你想显示的那些
        for (i, link) in loader.link_names().enumerate() {
            // 示例：让某个 link 摆动，其它为恒等
            let pose = if link == "base_link" {
                na::Isometry3::identity()
            } else {
                na::Isometry3::from_parts(
                    [0.0, 0.0, 0.0].into(),
                    UnitQuaternion::from_axis_angle(
                        &na::Vector3::z_axis(),
                        frame as f64 * i as f64 * 0.03,
                    ),
                )
            };
            poses.insert(link.to_string(), pose);
        }

        loader.log_frame(&rec, frame, &poses, "frame");
        frame += 1;
        std::thread::sleep(std::time::Duration::from_millis(30));
    }
}