support ROS_TIME for create_timer

Signed-off-by: Koichi <koichi.imai.2@tier4.jp>

Author: Koichi98

src/agnocast_sample_application/CMakeLists.txt

@@ -90,6 +90,12 @@ target_include_directories(no_rclcpp_client PRIVATE
   ${agnocastlib_INCLUDE_DIRS}
 )
 
+add_executable(sim_time_timer src/sim_time_timer.cpp)
+ament_target_dependencies(sim_time_timer rclcpp agnocastlib)
+target_include_directories(sim_time_timer PRIVATE
+  ${agnocastlib_INCLUDE_DIRS}
+)
+
 add_library(listener_component SHARED src/minimal_subscriber.cpp)
 ament_target_dependencies(listener_component rclcpp rclcpp_components agnocastlib agnocast_sample_interfaces)
 target_include_directories(listener_component PRIVATE ${agnocastlib_INCLUDE_DIRS})
@@ -151,6 +157,9 @@ install(TARGETS no_rclcpp_server
 install(TARGETS no_rclcpp_client
   DESTINATION lib/${PROJECT_NAME})
 
+install(TARGETS sim_time_timer
+  DESTINATION lib/${PROJECT_NAME})
+
 install(DIRECTORY launch
   DESTINATION share/${PROJECT_NAME}/
 )

src/agnocast_sample_application/src/sim_time_timer.cpp

@@ -0,0 +1,73 @@
+/**
+ * @brief Sample application to demonstrate create_timer with use_sim_time support
+ *
+ * This sample creates a timer using agnocast::Node::create_timer() which supports
+ * ROS_TIME (simulation time). When use_sim_time:=true, the timer will use the
+ * /clock topic time instead of wall clock time.
+ *
+ * Usage:
+ *   # Terminal 1: Run with use_sim_time enabled
+ *   ros2 run agnocast_sample_application sim_time_timer --ros-args -p use_sim_time:=true
+ *
+ *   # Terminal 2: Publish clock messages
+ *   ros2 topic pub /clock rosgraph_msgs/msg/Clock "{clock: {sec: 0, nanosec: 0}}" --once
+ *   ros2 topic pub /clock rosgraph_msgs/msg/Clock "{clock: {sec: 1, nanosec: 0}}" --once
+ *   ros2 topic pub /clock rosgraph_msgs/msg/Clock "{clock: {sec: 2, nanosec: 0}}" --once
+ *
+ *   # Or run without use_sim_time (uses wall clock)
+ *   ros2 run agnocast_sample_application sim_time_timer
+ */
+
+#include "agnocast/agnocast.hpp"
+
+#include <chrono>
+
+using namespace std::chrono_literals;
+
+class SimTimeTimerNode : public agnocast::Node
+{
+public:
+  SimTimeTimerNode() : agnocast::Node("sim_time_timer_node")
+  {
+    // Log whether we're using sim time
+    const bool use_sim_time = this->get_parameter("use_sim_time").as_bool();
+    RCLCPP_INFO(
+      this->get_logger(), "Starting timer node (use_sim_time: %s)",
+      use_sim_time ? "true" : "false");
+
+    // Create timer using create_timer() which supports ROS_TIME
+    // This timer will respect use_sim_time parameter
+    timer_ = this->create_timer(500ms, std::bind(&SimTimeTimerNode::timer_callback, this));
+
+    RCLCPP_INFO(
+      this->get_logger(), "Timer created with period 500ms, clock type: %s",
+      timer_->is_steady() ? "STEADY_TIME" : "ROS_TIME");
+  }
+
+private:
+  void timer_callback()
+  {
+    const auto now = this->now();
+    const int64_t sec = now.seconds();
+    const int64_t nsec = now.nanoseconds() % 1000000000;
+
+    RCLCPP_INFO(
+      this->get_logger(), "Timer callback! Current time: %ld.%09ld (count: %d)", sec, nsec,
+      callback_count_++);
+  }
+
+  agnocast::TimerBase::SharedPtr timer_;
+  int callback_count_ = 0;
+};
+
+int main(int argc, char * argv[])
+{
+  agnocast::init(argc, argv);
+
+  agnocast::AgnocastOnlySingleThreadedExecutor executor;
+  auto node = std::make_shared<SimTimeTimerNode>();
+  executor.add_node(node);
+  executor.spin();
+
+  return 0;
+}

src/agnocastlib/include/agnocast/agnocast_epoll.hpp

@@ -16,7 +16,8 @@ struct AgnocastExecutable;
 extern std::atomic<bool> need_epoll_updates;
 
 constexpr uint32_t TIMER_EVENT_FLAG = 0x80000000;
-constexpr uint32_t SHUTDOWN_EVENT_FLAG = 0x40000000;
+constexpr uint32_t CLOCK_EVENT_FLAG = 0x40000000;     // For clock_eventfd events (ROS_TIME timers)
+constexpr uint32_t SHUTDOWN_EVENT_FLAG = 0x20000000;  // For shutdown events (AgnocastOnlyExecutor)
 
 /// @return true if shutdown event detected, false otherwise
 bool wait_and_handle_epoll_event(
@@ -81,14 +82,34 @@ void prepare_epoll_impl(
         continue;
       }
 
-      struct epoll_event ev = {};
-      ev.events = EPOLLIN;
-      ev.data.u32 = timer_id | TIMER_EVENT_FLAG;
+      // Register timerfd (wall clock based firing)
+      if (timer_info.timer_fd >= 0) {
+        struct epoll_event ev = {};
+        ev.events = EPOLLIN;
+        ev.data.u32 = timer_id | TIMER_EVENT_FLAG;
+
+        if (epoll_ctl(epoll_fd, EPOLL_CTL_ADD, timer_info.timer_fd, &ev) == -1) {
+          if (errno != EEXIST) {  // EEXIST means already registered, which is fine
+            RCLCPP_ERROR(logger, "epoll_ctl failed for timer: %s", strerror(errno));
+            close(agnocast_fd);
+            exit(EXIT_FAILURE);
+          }
+        }
+      }
 
-      if (epoll_ctl(epoll_fd, EPOLL_CTL_ADD, timer_info.timer_fd, &ev) == -1) {
-        RCLCPP_ERROR(logger, "epoll_ctl failed for timer: %s", strerror(errno));
-        close(agnocast_fd);
-        exit(EXIT_FAILURE);
+      // Register clock_eventfd for ROS_TIME timers (simulation time support)
+      if (timer_info.clock_eventfd >= 0) {
+        struct epoll_event clock_ev = {};
+        clock_ev.events = EPOLLIN;
+        clock_ev.data.u32 = timer_id | CLOCK_EVENT_FLAG;
+
+        if (epoll_ctl(epoll_fd, EPOLL_CTL_ADD, timer_info.clock_eventfd, &clock_ev) == -1) {
+          if (errno != EEXIST) {  // EEXIST means already registered, which is fine
+            RCLCPP_ERROR(logger, "epoll_ctl failed for clock_eventfd: %s", strerror(errno));
+            close(agnocast_fd);
+            exit(EXIT_FAILURE);
+          }
+        }
       }
 
       timer_info.need_epoll_update = false;

src/agnocastlib/include/agnocast/agnocast_timer_info.hpp

@@ -19,25 +19,33 @@ struct TimerInfo
 {
   ~TimerInfo();
 
+  uint32_t timer_id = 0;
   int timer_fd = -1;
+  int clock_eventfd = -1;  // eventfd to wake epoll on clock updates (ROS_TIME only)
   std::weak_ptr<TimerBase> timer;
   rclcpp::CallbackGroup::SharedPtr callback_group;
   std::atomic<int64_t> last_call_time_ns;
   std::atomic<int64_t> next_call_time_ns;
+  std::atomic<int64_t> time_credit{
+    0};  // Credit for time elapsed before ROS time is activated/deactivated
   std::chrono::nanoseconds period;
   bool need_epoll_update = true;
 
   rclcpp::Clock::SharedPtr clock;
+  rclcpp::JumpHandler::SharedPtr jump_handler;
 };
 
 extern std::mutex id2_timer_info_mtx;
 extern std::unordered_map<uint32_t, std::shared_ptr<TimerInfo>> id2_timer_info;
 extern std::atomic<uint32_t> next_timer_id;
 
-int create_timer_fd(uint32_t timer_id, std::chrono::nanoseconds period);
+int create_timer_fd(
+  uint32_t timer_id, std::chrono::nanoseconds period, rcl_clock_type_t clock_type);
 
 void handle_timer_event(TimerInfo & timer_info);
 
+void handle_clock_event(TimerInfo & timer_info);
+
 uint32_t allocate_timer_id();
 
 void register_timer_info(

src/agnocastlib/src/agnocast_epoll.cpp

@@ -40,7 +40,7 @@ bool wait_and_handle_epoll_event(
   }
 
   if ((event_id & TIMER_EVENT_FLAG) != 0U) {
-    // Timer event
+    // Timer event (timerfd fired)
     const uint32_t timer_id = event_id & ~TIMER_EVENT_FLAG;
     rclcpp::CallbackGroup::SharedPtr callback_group;
 
@@ -64,6 +64,35 @@ bool wait_and_handle_epoll_event(
     const std::shared_ptr<std::function<void()>> callable =
       std::make_shared<std::function<void()>>([timer_info]() { handle_timer_event(*timer_info); });
 
+    {
+      std::lock_guard<std::mutex> ready_lock{ready_agnocast_executables_mutex};
+      ready_agnocast_executables.emplace_back(AgnocastExecutable{callable, callback_group});
+    }
+  } else if ((event_id & CLOCK_EVENT_FLAG) != 0U) {
+    // Clock event (ROS_TIME clock updated via time jump callback)
+    const uint32_t timer_id = event_id & ~CLOCK_EVENT_FLAG;
+    rclcpp::CallbackGroup::SharedPtr callback_group;
+
+    std::shared_ptr<TimerInfo> timer_info;
+    {
+      std::lock_guard<std::mutex> lock(id2_timer_info_mtx);
+      const auto it = id2_timer_info.find(timer_id);
+      if (it == id2_timer_info.end()) {
+        RCLCPP_ERROR(logger, "Agnocast internal implementation error: timer info cannot be found");
+        close(agnocast_fd);
+        exit(EXIT_FAILURE);
+      }
+      timer_info = it->second;
+      if (!timer_info->timer.lock()) {
+        return;  // Timer object has been destroyed
+      }
+      callback_group = timer_info->callback_group;
+    }
+
+    // Create a callable that handles the clock event
+    const std::shared_ptr<std::function<void()>> callable =
+      std::make_shared<std::function<void()>>([timer_info]() { handle_clock_event(*timer_info); });
+
     {
       std::lock_guard<std::mutex> ready_lock{ready_agnocast_executables_mutex};
       ready_agnocast_executables.emplace_back(AgnocastExecutable{callable, callback_group});