diff --git a/CMakeLists.txt b/CMakeLists.txt index ee07d2f..e5f3935 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -22,6 +22,7 @@ find_package(ament_cmake REQUIRED) # find_package( REQUIRED) find_package(rclcpp REQUIRED) find_package(rclcpp_components REQUIRED) +find_package(rclpy REQUIRED) find_package(sensor_msgs REQUIRED) find_package(Poco REQUIRED COMPONENTS Foundation) @@ -54,6 +55,10 @@ install(DIRECTORY launch DESTINATION share/${PROJECT_NAME} ) +install(PROGRAMS scripts/timereference_fake.py + DESTINATION lib/${PROJECT_NAME} +) + if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) # the following line skips the linter which checks for copyrights diff --git a/package.xml b/package.xml index c8d4b0d..a59c98b 100644 --- a/package.xml +++ b/package.xml @@ -17,6 +17,7 @@ ament_cmake rclcpp + rclpy rclcpp_components sensor_msgs libpoco-dev diff --git a/scripts/timereference_fake.py b/scripts/timereference_fake.py new file mode 100644 index 0000000..c186fa4 --- /dev/null +++ b/scripts/timereference_fake.py @@ -0,0 +1,62 @@ +#!/usr/bin/env python3 +import rclpy +from rclpy.node import Node +from sensor_msgs.msg import TimeReference +import time + + +class FakeTimePublisher(Node): + def __init__(self): + super().__init__("fake_time_publisher") + + # Publisher to publish the fake time + self.publisher_ = self.create_publisher(TimeReference, "fake_time", 10) + self.declare_parameter("init_offset_time", 60.0) + + # Initialize start time + self.current_time = ( + time.time() + self.get_parameter("init_offset_time").value + ) + + # Timer to call the publish_time method every second + self.timer = self.create_timer(1.0, self.publish_time) + + def publish_time(self): + # Increment the current time by 1 second + self.current_time += 1 + + # Convert the current time to a human-readable format + readable_time = time.strftime( + "%Y-%m-%d %H:%M:%S", time.localtime(self.current_time) + ) + + # Create a message and publish it + msg = TimeReference() + msg.header.stamp = self.get_clock().now().to_msg() + msg.source = "fake_time_publisher" + msg.time_ref.sec = int(self.current_time) + msg.time_ref.nanosec = int((self.current_time - int(self.current_time)) * 1e9) + self.publisher_.publish(msg) + + # Log the published time + self.get_logger().info('Publishing: "%s"' % msg) + + +def main(args=None): + rclpy.init(args=args) + + # Create the node and spin it to keep it alive + fake_time_publisher = FakeTimePublisher() + + try: + rclpy.spin(fake_time_publisher) + except KeyboardInterrupt: + pass + finally: + # Shutdown the ROS 2 node gracefully + fake_time_publisher.destroy_node() + rclpy.shutdown() + + +if __name__ == "__main__": + main() diff --git a/src/NtpdShmDriver.cpp b/src/NtpdShmDriver.cpp index 0259ecc..3107363 100644 --- a/src/NtpdShmDriver.cpp +++ b/src/NtpdShmDriver.cpp @@ -129,7 +129,7 @@ void NtpdShmDriver::time_ref_cb(const sensor_msgs::msg::TimeReference::SharedPtr * date -d @1234567890: Sat Feb 14 02:31:30 MSK 2009 */ const rclcpp::Time magic_date(1234567890ULL, 0); - if (fixup_date_.as_bool() && clock->now() < magic_date) { + if (fixup_date_.as_bool() && clock->now().seconds() < magic_date.seconds()) { rclcpp::Time time_ref_(time_ref); set_system_time(time_ref_.seconds()); }