From 1321a1218e3f4fb773ca5d7efd82e402abff4b65 Mon Sep 17 00:00:00 2001 From: Jan-Niklas Burfeind Date: Tue, 9 Jul 2024 13:00:26 +0000 Subject: [PATCH] refactor: Do use rusts timestamp instead of the one provided by ROS2::node.get_clock(). This allows reducing create_joint_state_msg's interface in the next step. Co-authored-by: Philipp Caspers --- src/ros_publisher.rs | 13 ++++++++----- 1 file changed, 8 insertions(+), 5 deletions(-) diff --git a/src/ros_publisher.rs b/src/ros_publisher.rs index f379b3a..4c61732 100644 --- a/src/ros_publisher.rs +++ b/src/ros_publisher.rs @@ -1,6 +1,9 @@ use builtin_interfaces::msg::Time as TimeMsg; use sensor_msgs::msg::JointState as JointStateMsg; -use std::sync::Arc; +use std::{ + sync::Arc, + time::{SystemTime, UNIX_EPOCH}, +}; use std_msgs::msg::Header; use rclrs::{Node, Publisher, RclrsError, QOS_PROFILE_DEFAULT}; @@ -22,12 +25,12 @@ impl RosPublisher { } } -pub fn create_joint_state_msg(node: &Node, data: f64) -> JointStateMsg { - let time = node.get_clock().now().to_ros_msg().unwrap(); +pub fn create_joint_state_msg(_node: &Node, data: f64) -> JointStateMsg { + let system_timestamp = SystemTime::now().duration_since(UNIX_EPOCH).unwrap(); // Workaround for https://github.com/ros2-rust/ros2_rust/issues/385 let time_msgs = TimeMsg { - sec: time.sec, - nanosec: time.nanosec, + sec: i32::try_from(system_timestamp.as_secs()).expect("This function will break in 2038."), + nanosec: system_timestamp.subsec_nanos(), }; let joint_state_msg: JointStateMsg = JointStateMsg {