diff --git a/rclcpp/src/rclcpp/clock.cpp b/rclcpp/src/rclcpp/clock.cpp index 1ca8708..64ae080 100644 --- a/rclcpp/src/rclcpp/clock.cpp +++ b/rclcpp/src/rclcpp/clock.cpp @@ -153,7 +153,9 @@ Clock::invoke_prejump_callbacks( const std::vector & 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); + } } } diff --git a/rclcpp/test/test_time_source.cpp b/rclcpp/test/test_time_source.cpp index 0f80a2f..cd62974 100644 --- a/rclcpp/test/test_time_source.cpp +++ b/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("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(); + 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("clock", - 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(); - msg->clock.sec = i; - msg->clock.nanosec = 1000; - clock_pub->publish(msg); - rclcpp::spin_some(node); - loop_rate.sleep(); - } + trigger_clock_changes(node); + 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("clock", - 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(); - msg->clock.sec = i; - msg->clock.nanosec = 1000; - clock_pub->publish(msg); - rclcpp::spin_some(node); - loop_rate.sleep(); - } + trigger_clock_changes(node); 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) { - if (!rclcpp::ok()) { - break; // Break for ctrl-c - } - auto msg = std::make_shared(); - msg->clock.sec = i; - msg->clock.nanosec = 2000; - clock_pub->publish(msg); - rclcpp::spin_some(node); - loop_rate.sleep(); - } + trigger_clock_changes(node); 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( - rclcpp::Node::SharedPtr node) -{ - auto clock_pub = node->create_publisher("clock", - rmw_qos_profile_default); + // Register a callback handler with only pre_callback + rclcpp::JumpHandler::SharedPtr callback_handler3 = ros_clock->create_jump_callback( + std::bind(&CallbackObject::pre_callback, &cbo, 3), + std::function(), + jump_threshold); - rclcpp::executors::SingleThreadedExecutor executor; - executor.add_node(node); + trigger_clock_changes(node); + EXPECT_EQ(3, cbo.last_precallback_id_); + EXPECT_EQ(2, cbo.last_postcallback_id_); - 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(); - msg->clock.sec = i; - msg->clock.nanosec = 1000; - clock_pub->publish(msg); - executor.spin_once(1000000ns); - loop_rate.sleep(); - } + // Register a callback handler with only post_callback + rclcpp::JumpHandler::SharedPtr callback_handler4 = ros_clock->create_jump_callback( + std::function(), + std::bind(&CallbackObject::post_callback, &cbo, std::placeholders::_1, 4), + jump_threshold); + + trigger_clock_changes(node); + EXPECT_EQ(3, cbo.last_precallback_id_); + EXPECT_EQ(4, cbo.last_postcallback_id_); }