niklas/rclcpp
1
0
Fork 0
Code Issues Pull requests Projects Releases Packages Wiki Activity Actions

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

Browse source 
* Add failing tests for partial jump handlers

* Code deduplication

* Check callbacks before calling them
This commit is contained in:
dhood 2018-07-18 07:11:35 +10:00 • committed by GitHub
parent 3067a72a2a
commit 8f2052d65a
No known key found for this signature in database
GPG key ID: 4AEE18F83AFDEB23
2 changed files with 50 additions and 62 deletions
Download patch file Download diff file Expand all files Collapse all files

8
rclcpp/src/rclcpp/clock.cpp
View file

@ -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);
}
}
}

104
rclcpp/test/test_time_source.cpp
View file

@ -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",
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();
}
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<rosgraph_msgs::msg::Clock>("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<rosgraph_msgs::msg::Clock>();
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<rosgraph_msgs::msg::Clock>();
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<rosgraph_msgs::msg::Clock>("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<void(rclcpp::TimeJump)>(),
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<rosgraph_msgs::msg::Clock>();
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<void()>(),
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_);
}