diff --git a/dimos/perception/depth2depth_cloud/rust/flake.nix b/dimos/perception/depth2depth_cloud/rust/flake.nix index 2901288ad19..2822a8acf08 100644 --- a/dimos/perception/depth2depth_cloud/rust/flake.nix +++ b/dimos/perception/depth2depth_cloud/rust/flake.nix @@ -34,8 +34,8 @@ name = "dimos-depth2depth-cloud"; moduleSource = nix-filter.lib { root = ./.; exclude = [ "target" "build" "result" "__pycache__" ]; }; sharedArchive = pkgs.fetchurl { - url = "https://codeload.github.com/dimensionalOS/dimos/tar.gz/b547716883d0f2619e53d241351357347400a40a"; - sha256 = "1ec7f63d9ff57a3e5f151009a2b71472b2c90a3c4f17c29f98a6215cbef18c6b"; + url = "https://codeload.github.com/dimensionalOS/dimos/tar.gz/b971b729786cc12dcf4769cfbc233b0e36f1feee"; + sha256 = "20abf8673b36a6fdea3c30b6949fe7e618c2836917c34f39425ef466609a5171"; }; sharedSource = pkgs.runCommand "dimos-depth-cloud-sdk-source" { nativeBuildInputs = [ pkgs.gnutar pkgs.gzip ]; @@ -52,6 +52,8 @@ mkdir -p $out/sdk cp -r ${sharedSource}/native/rust/dimos-module ${sharedSource}/native/rust/dimos-module-macros $out/sdk/ cp -r ${messageSource}/rust $out/messages + substituteInPlace $out/Cargo.toml --replace-fail '[workspace]' '[workspace] + exclude = ["sdk/dimos-module", "sdk/dimos-module-macros", "messages"]' substituteInPlace $out/Cargo.toml \ --replace-fail '../../../../native/rust/dimos-module' 'sdk/dimos-module' \ --replace-fail '../../../../build/message-source/rust' 'messages' diff --git a/dimos/perception/depth2depth_cloud/rust/src/module.rs b/dimos/perception/depth2depth_cloud/rust/src/module.rs index 12246ff5bd6..a36b21d1366 100644 --- a/dimos/perception/depth2depth_cloud/rust/src/module.rs +++ b/dimos/perception/depth2depth_cloud/rust/src/module.rs @@ -100,10 +100,10 @@ fn decode_tf(bytes: &[u8]) -> std::io::Result> "unnormalized TF stamp", )); } - Ok(dimos_module::tf::Transform::new( + Ok(dimos_module::tf::Transform::from_nanoseconds( edge.header.frame_id, edge.child_frame_id, - seconds(&edge.header.stamp), + edge.header.stamp.sec as i64 * 1_000_000_000 + edge.header.stamp.nanosec as i64, Isometry3::from_parts(Translation3::new(p.x, p.y, p.z), rotation), )) }) @@ -121,14 +121,12 @@ fn encode_tf(transforms: &[dimos_module::tf::Transform]) -> std::io::Result i64::MAX as f64 { - return Err(std::io::Error::new( + let ns = value.stamp_nanoseconds().ok_or_else(|| { + std::io::Error::new( std::io::ErrorKind::InvalidInput, "TF timestamp is outside range", - )); - } - let ns = ns as i64; + ) + })?; let sec = i32::try_from(ns.div_euclid(1_000_000_000)).map_err(|_| { std::io::Error::new( std::io::ErrorKind::InvalidInput, @@ -988,6 +986,35 @@ mod tests { assert!(decode_tf(&[0, 3, 0, 0]).is_err()); } + #[test] + fn tf_cdr_source_stamps_survive_epoch_nanoseconds_and_negative_times() { + use dimos_generated_messages::tf2_msgs::msg::tf_message::TFMessage; + for ns in [0, -1, 1_700_000_000_123_456_789] { + let value = dimos_module::tf::Transform::from_nanoseconds( + "world", + "camera", + ns, + Isometry3::translation(1., 2., 3.), + ); + let message: TFMessage = cdr::decode(&encode_tf(&[value]).unwrap()).unwrap(); + assert_eq!( + message.transforms[0].header.stamp.sec as i64, + ns.div_euclid(1_000_000_000) + ); + assert_eq!( + message.transforms[0].header.stamp.nanosec as i64, + ns.rem_euclid(1_000_000_000) + ); + for little in [false, true] { + let bytes = cdr::encode_endian(&message, little).unwrap(); + let decoded = decode_tf(&bytes).unwrap(); + assert_eq!(decoded[0].stamp_nanoseconds(), Some(ns)); + let output: TFMessage = cdr::decode(&encode_tf(&decoded).unwrap()).unwrap(); + assert_eq!(output, message); + } + } + } + #[test] fn a_stage_behind_takes_the_newest_item_and_counts_the_ones_it_missed() { let latest = Latest::default();