remove k from production

lunabotics/lunabot/src/apps/production.rs

@@ -11,6 +11,7 @@ use lunabot_ai::{run_ai, Action, Input, PollWhen};
 use nalgebra::{Scale3, Transform3};
 use pathfinding::grid::Grid;
 use serde::Deserialize;
+use simple_motion::{ChainBuilder, NodeSerde};
 use streaming::camera_streaming;
 use tasker::{get_tokio_handle, shared::OwnedData, tokio, BlockOn};
 use tracing::error;
@@ -20,7 +21,7 @@ use crate::{
     pipelines::thalassic::ThalassicData,
 };
 
-use super::{create_packet_builder, create_robot_chain};
+use super::create_packet_builder;
 
 mod apriltag;
 mod camera;
@@ -55,6 +56,7 @@ pub struct LunabotApp {
     pub cameras: FxHashMap<String, CameraInfo>,
     pub depth_cameras: FxHashMap<String, DepthCameraInfo>,
     pub apriltags: FxHashMap<String, Apriltag>,
+    pub robot_layout: String,
 }
 
 impl LunabotApp {
@@ -78,7 +80,12 @@ impl LunabotApp {
 
         let _guard = get_tokio_handle().enter();
 
-        let robot_chain = create_robot_chain();
+        let robot_chain = NodeSerde::from_reader(
+            std::fs::File::open(self.robot_layout).expect("Failed to read robot chain"),
+        )
+        .expect("Failed to parse robot chain");
+        let robot_chain = ChainBuilder::from(robot_chain).finish_static();
+
         let localizer = Localizer::new(robot_chain.clone(), None);
         let localizer_ref = localizer.get_ref();
         std::thread::spawn(|| localizer.run());
@@ -127,10 +134,10 @@ impl LunabotApp {
                         port,
                         camera::CameraInfo {
                             k_node: robot_chain
-                                .find_link(&link_name)
+                                .get_node_with_name(&link_name)
                                 .context("Failed to find camera link")
                                 .unwrap()
-                                .clone(),
+                                .into(),
                             focal_length_x_px,
                             focal_length_y_px,
                             stream_index,
@@ -174,10 +181,10 @@ impl LunabotApp {
                         serial,
                         depth::DepthCameraInfo {
                             k_node: robot_chain
-                                .find_link(&link_name)
+                                .get_node_with_name(&link_name)
                                 .context("Failed to find camera link")
                                 .unwrap()
-                                .clone(),
+                                .into(),
                             ignore_apriltags: observe_apriltags,
                             stream_index,
                         },
@@ -210,7 +217,7 @@ impl LunabotApp {
         );
 
         run_ai(
-            robot_chain,
+            robot_chain.into(),
             |action, inputs| match action {
                 Action::SetStage(stage) => {
                     lunabot_stage.store(stage);

lunabotics/lunabot/src/apps/production/apriltag.rs

@@ -4,8 +4,7 @@ use apriltag::{families::TagStandard41h12, DetectorBuilder, Image, TagParams};
 use apriltag_image::{image::ImageBuffer, ImageExt};
 use apriltag_nalgebra::PoseExt;
 use fxhash::FxHashMap;
-use k::{Isometry3, UnitQuaternion, Vector3};
-use nalgebra::Point3;
+use nalgebra::{Point3, Isometry3, UnitQuaternion, Vector3};
 use serde::Deserialize;
 
 #[derive(Deserialize, Clone, Copy)]

lunabotics/lunabot/src/apps/production/camera.rs

@@ -5,6 +5,7 @@ use super::apriltag::{
     AprilTagDetector,
 };
 use fxhash::{FxHashMap, FxHashSet};
+use simple_motion::StaticImmutableNode;
 use tasker::shared::OwnedData;
 use tracing::{error, warn};
 use udev::Udev;
@@ -18,7 +19,7 @@ use super::{
 };
 
 pub struct CameraInfo {
-    pub k_node: k::Node<f64>,
+    pub k_node: StaticImmutableNode,
     pub focal_length_x_px: f64,
     pub focal_length_y_px: f64,
     pub stream_index: usize,
@@ -128,7 +129,7 @@ pub fn enumerate_cameras(
                 det.add_tag(tag.tag_position, tag.get_quat(), tag.tag_width, tag_id);
             }
             let localizer_ref = localizer_ref.clone();
-            let mut inverse_local = k_node.origin();
+            let mut inverse_local = k_node.get_local_isometry();
             inverse_local.inverse_mut();
             det.detection_callbacks_ref().add_fn(move |observation| {
                 // println!(

lunabotics/lunabot/src/apps/production/depth.rs

@@ -15,6 +15,7 @@ use realsense_rust::{
     kind::{Rs2CameraInfo, Rs2Format, Rs2ProductLine, Rs2StreamKind},
     pipeline::InactivePipeline,
 };
+use simple_motion::StaticImmutableNode;
 use tasker::shared::OwnedData;
 use thalassic::DepthProjectorBuilder;
 use tracing::{error, warn};
@@ -30,7 +31,7 @@ use crate::{
 use super::{apriltag::Apriltag, streaming::CameraStream};
 
 pub struct DepthCameraInfo {
-    pub k_node: k::Node<f64>,
+    pub k_node: StaticImmutableNode,
     pub ignore_apriltags: bool,
     pub stream_index: usize,
 }
@@ -235,7 +236,7 @@ pub fn enumerate_depth_cameras(
                     det.add_tag(tag.tag_position, tag.get_quat(), tag.tag_width, tag_id);
                 }
                 let localizer_ref = localizer_ref.clone();
-                let mut inverse_local = k_node.origin();
+                let mut inverse_local = k_node.get_local_isometry();
                 inverse_local.inverse_mut();
                 det.detection_callbacks_ref().add_fn(move |observation| {
                     localizer_ref.set_april_tag_isometry(
@@ -329,9 +330,7 @@ pub fn enumerate_depth_cameras(
                         frame.width() * frame.height(),
                     );
 
-                    let Some(camera_transform) = k_node.world_transform() else {
-                        continue;
-                    };
+                    let camera_transform = k_node.get_global_isometry();
                     let camera_transform: AlignedMatrix4<f32> =
                         camera_transform.to_homogeneous().cast::<f32>().into();
                     let Some(mut pcl_storage) = pcl_storage_channel.get_finished() else {

lunabotics/lunabot/src/main.rs

@@ -28,7 +28,8 @@ lumpur::define_configuration! {
             #[serde(default)]
             depth_cameras: FxHashMap<String, apps::DepthCameraInfo>,
             #[serde(default)]
-            apriltags: FxHashMap<String, apps::Apriltag>
+            apriltags: FxHashMap<String, apps::Apriltag>,
+            robot_layout: Option<String>
         },
         Sim {
             lunabase_address: SocketAddr,
@@ -90,6 +91,7 @@ fn main() {
             cameras,
             depth_cameras,
             apriltags,
+            robot_layout
         } => {
             apps::LunabotApp {
                 lunabase_address,
@@ -100,6 +102,7 @@ fn main() {
                 cameras,
                 depth_cameras,
                 apriltags,
+                robot_layout: robot_layout.unwrap_or_else(|| "robot-layout/lunabot.json".to_string())
             }
             .run();
             #[cfg(not(feature = "experimental"))]

robot-layout/lunabot.json

@@ -0,0 +1,16 @@
+{
+    "free_origin": [0.0, 0.0, 0.0],
+    "free_euler": [0.0, 0.0, 0.0],
+    "children": [
+        {
+            "name": "depth_camera",
+            "origin": [0.0, 0.573, -0.298],
+            "euler": [-30.0, 0.0, 0.0]
+        },
+        {
+            "name": "front_camera",
+            "origin": [0.0, 0.573, -0.298],
+            "euler": [-30.0, 0.0, 0.0]
+        }
+    ]
+}