Support jump handlers with only pre- or post-jump callback (#517)

* Add failing tests for partial jump handlers

* Code deduplication

* Check callbacks before calling them

rclcpp/src/rclcpp/clock.cpp

@@ -153,7 +153,9 @@ Clock::invoke_prejump_callbacks(
   const std::vector<JumpHandler::SharedPtr> & callbacks)
 {
   for (const auto cb : callbacks) {
-    cb->pre_callback();
+    if (cb->pre_callback != nullptr) {
+      cb->pre_callback();
+    }
   }
 }
 
@@ -163,7 +165,9 @@ Clock::invoke_postjump_callbacks(
   const TimeJump & jump)
 {
   for (auto cb : callbacks) {
-    cb->post_callback(jump);
+    if (cb->post_callback != nullptr) {
+      cb->post_callback(jump);
+    }
   }
 }
 

rclcpp/test/test_time_source.cpp

@@ -50,6 +50,29 @@ protected:
   rclcpp::Node::SharedPtr node;
 };
 
+void trigger_clock_changes(
+  rclcpp::Node::SharedPtr node)
+{
+  auto clock_pub = node->create_publisher<rosgraph_msgs::msg::Clock>("clock",
+      rmw_qos_profile_default);
+
+  rclcpp::executors::SingleThreadedExecutor executor;
+  executor.add_node(node);
+
+  rclcpp::WallRate loop_rate(50);
+  for (int i = 0; i < 5; ++i) {
+    if (!rclcpp::ok()) {
+      break;  // Break for ctrl-c
+    }
+    auto msg = std::make_shared<rosgraph_msgs::msg::Clock>();
+    msg->clock.sec = i;
+    msg->clock.nanosec = 1000;
+    clock_pub->publish(msg);
+    executor.spin_once(1000000ns);
+    loop_rate.sleep();
+  }
+}
+
 TEST_F(TestTimeSource, detachUnattached) {
   rclcpp::TimeSource ts;
 
@@ -92,20 +115,8 @@ TEST_F(TestTimeSource, clock) {
   ts.attachClock(ros_clock);
   EXPECT_FALSE(ros_clock->ros_time_is_active());
 
-  auto clock_pub = node->create_publisher<rosgraph_msgs::msg::Clock>("clock",
+  trigger_clock_changes(node);
-      rmw_qos_profile_default);
+
-  rclcpp::WallRate loop_rate(50);
-  for (int i = 0; i < 5; ++i) {
-    if (!rclcpp::ok()) {
-      break;  // Break for ctrl-c
-    }
-    auto msg = std::make_shared<rosgraph_msgs::msg::Clock>();
-    msg->clock.sec = i;
-    msg->clock.nanosec = 1000;
-    clock_pub->publish(msg);
-    rclcpp::spin_some(node);
-    loop_rate.sleep();
-  }
   auto t_low = rclcpp::Time(1, 0, RCL_ROS_TIME);
   auto t_high = rclcpp::Time(10, 100000, RCL_ROS_TIME);
 
@@ -161,21 +172,7 @@ TEST_F(TestTimeSource, callbacks) {
   ts.attachClock(ros_clock);
   EXPECT_FALSE(ros_clock->ros_time_is_active());
 
-  auto clock_pub = node->create_publisher<rosgraph_msgs::msg::Clock>("clock",
+  trigger_clock_changes(node);
-      rmw_qos_profile_default);
-
-  rclcpp::WallRate loop_rate(50);
-  for (int i = 0; i < 5; ++i) {
-    if (!rclcpp::ok()) {
-      break;  // Break for ctrl-c
-    }
-    auto msg = std::make_shared<rosgraph_msgs::msg::Clock>();
-    msg->clock.sec = i;
-    msg->clock.nanosec = 1000;
-    clock_pub->publish(msg);
-    rclcpp::spin_some(node);
-    loop_rate.sleep();
-  }
   auto t_low = rclcpp::Time(1, 0, RCL_ROS_TIME);
   auto t_high = rclcpp::Time(10, 100000, RCL_ROS_TIME);
 
@@ -198,17 +195,7 @@ TEST_F(TestTimeSource, callbacks) {
     std::bind(&CallbackObject::post_callback, &cbo, std::placeholders::_1, 2),
     jump_threshold);
 
-  for (int i = 0; i < 5; ++i) {
+  trigger_clock_changes(node);
-    if (!rclcpp::ok()) {
-      break;  // Break for ctrl-c
-    }
-    auto msg = std::make_shared<rosgraph_msgs::msg::Clock>();
-    msg->clock.sec = i;
-    msg->clock.nanosec = 2000;
-    clock_pub->publish(msg);
-    rclcpp::spin_some(node);
-    loop_rate.sleep();
-  }
 
   EXPECT_EQ(2, cbo.last_precallback_id_);
   EXPECT_EQ(2, cbo.last_postcallback_id_);
@@ -221,29 +208,26 @@ TEST_F(TestTimeSource, callbacks) {
   EXPECT_NE(0L, t_out.nanoseconds());
   EXPECT_LT(t_low.nanoseconds(), t_out.nanoseconds());
   EXPECT_GT(t_high.nanoseconds(), t_out.nanoseconds());
-}
 
-void trigger_clock_changes(
+  // Register a callback handler with only pre_callback
-  rclcpp::Node::SharedPtr node)
+  rclcpp::JumpHandler::SharedPtr callback_handler3 = ros_clock->create_jump_callback(
-{
+    std::bind(&CallbackObject::pre_callback, &cbo, 3),
-  auto clock_pub = node->create_publisher<rosgraph_msgs::msg::Clock>("clock",
+    std::function<void(rclcpp::TimeJump)>(),
-      rmw_qos_profile_default);
+    jump_threshold);
 
-  rclcpp::executors::SingleThreadedExecutor executor;
+  trigger_clock_changes(node);
-  executor.add_node(node);
+  EXPECT_EQ(3, cbo.last_precallback_id_);
+  EXPECT_EQ(2, cbo.last_postcallback_id_);
 
-  rclcpp::WallRate loop_rate(50);
+  // Register a callback handler with only post_callback
-  for (int i = 0; i < 5; ++i) {
+  rclcpp::JumpHandler::SharedPtr callback_handler4 = ros_clock->create_jump_callback(
-    if (!rclcpp::ok()) {
+    std::function<void()>(),
-      break;  // Break for ctrl-c
+    std::bind(&CallbackObject::post_callback, &cbo, std::placeholders::_1, 4),
-    }
+    jump_threshold);
-    auto msg = std::make_shared<rosgraph_msgs::msg::Clock>();
+
-    msg->clock.sec = i;
+  trigger_clock_changes(node);
-    msg->clock.nanosec = 1000;
+  EXPECT_EQ(3, cbo.last_precallback_id_);
-    clock_pub->publish(msg);
+  EXPECT_EQ(4, cbo.last_postcallback_id_);
-    executor.spin_once(1000000ns);
-    loop_rate.sleep();
-  }
 }