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

Don't auto-activate ROS time if clock topic is being published (#559)

Browse source 
* Don't auto-activate ROS time if clock topic is being published

* Destroy subscription when not needed, avoid re-creating it

* Additional tests

* Always reset pointer

* Initialise sub in initialiser list
This commit is contained in:
dhood 2018-09-25 08:34:25 -07:00 • committed by GitHub
parent f6c2f5ba0d
commit 6ff3ff43fe
No known key found for this signature in database
GPG key ID: 4AEE18F83AFDEB23
3 changed files with 111 additions and 89 deletions
Download patch file Download diff file Expand all files Collapse all files

7
rclcpp/include/rclcpp/time_source.hpp
View file

@ -80,10 +80,17 @@ private:
using Alloc = std::allocator<void>;
using SubscriptionT = rclcpp::Subscription<MessageT, Alloc>;
std::shared_ptr<SubscriptionT> clock_subscription_;
std::mutex clock_sub_lock_;
// The clock callback itself
void clock_cb(const rosgraph_msgs::msg::Clock::SharedPtr msg);
// Create the subscription for the clock topic
void create_clock_sub();
// Destroy the subscription for the clock topic
void destroy_clock_sub();
// Parameter Client pointer
std::shared_ptr<rclcpp::AsyncParametersClient> parameter_client_;

72
rclcpp/src/rclcpp/time_source.cpp
View file

@ -34,7 +34,8 @@ namespace rclcpp
{
TimeSource::TimeSource(std::shared_ptr<rclcpp::Node> node)
: ros_time_active_(false)
: clock_subscription_(nullptr),
ros_time_active_(false)
{
this->attachNode(node);
}
@ -65,28 +66,6 @@ void TimeSource::attachNode(
node_services_ = node_services_interface;
// TODO(tfoote): Update QOS
const std::string topic_name = "/clock";
rclcpp::callback_group::CallbackGroup::SharedPtr group;
using rclcpp::message_memory_strategy::MessageMemoryStrategy;
auto msg_mem_strat = MessageMemoryStrategy<MessageT, Alloc>::create_default();
auto allocator = std::make_shared<Alloc>();
auto cb = std::bind(&TimeSource::clock_cb, this, std::placeholders::_1);
clock_subscription_ = rclcpp::create_subscription<
MessageT, decltype(cb), Alloc, MessageT, SubscriptionT
>(
node_topics_.get(),
topic_name,
std::move(cb),
rmw_qos_profile_default,
group,
false,
false,
msg_mem_strat,
allocator);
parameter_client_ = std::make_shared<rclcpp::AsyncParametersClient>(
node_base_,
node_topics_,
@ -163,19 +142,56 @@ void TimeSource::set_clock(
void TimeSource::clock_cb(const rosgraph_msgs::msg::Clock::SharedPtr msg)
{
if (!this->ros_time_active_) {
if (!this->ros_time_active_ && SET_TRUE == this->parameter_state_) {
enable_ros_time();
}
// Cache the last message in case a new clock is attached.
last_msg_set_ = msg;
auto time_msg = std::make_shared<builtin_interfaces::msg::Time>(msg->clock);
std::lock_guard<std::mutex> guard(clock_list_lock_);
for (auto it = associated_clocks_.begin(); it != associated_clocks_.end(); ++it) {
set_clock(time_msg, true, *it);
if (SET_TRUE == this->parameter_state_) {
std::lock_guard<std::mutex> guard(clock_list_lock_);
for (auto it = associated_clocks_.begin(); it != associated_clocks_.end(); ++it) {
set_clock(time_msg, true, *it);
}
}
}
void TimeSource::create_clock_sub()
{
std::lock_guard<std::mutex> guard(clock_sub_lock_);
if (clock_subscription_) {
// Subscription already created.
return;
}
const std::string topic_name = "/clock";
rclcpp::callback_group::CallbackGroup::SharedPtr group;
using rclcpp::message_memory_strategy::MessageMemoryStrategy;
auto msg_mem_strat = MessageMemoryStrategy<MessageT, Alloc>::create_default();
auto allocator = std::make_shared<Alloc>();
auto cb = std::bind(&TimeSource::clock_cb, this, std::placeholders::_1);
clock_subscription_ = rclcpp::create_subscription<
MessageT, decltype(cb), Alloc, MessageT, SubscriptionT
>(
node_topics_.get(),
topic_name,
std::move(cb),
rmw_qos_profile_default,
group,
false,
false,
msg_mem_strat,
allocator);
}
void TimeSource::destroy_clock_sub()
{
std::lock_guard<std::mutex> guard(clock_sub_lock_);
clock_subscription_.reset();
}
void TimeSource::on_parameter_event(const rcl_interfaces::msg::ParameterEvent::SharedPtr event)
{
// Filter for only 'use_sim_time' being added or changed.
@ -190,9 +206,11 @@ void TimeSource::on_parameter_event(const rcl_interfaces::msg::ParameterEvent::S
if (it.second->value.bool_value) {
parameter_state_ = SET_TRUE;
enable_ros_time();
create_clock_sub();
} else {
parameter_state_ = SET_FALSE;
disable_ros_time();
destroy_clock_sub();
}
}
// Handle the case that use_sim_time was deleted.

121
rclcpp/test/test_time_source.cpp
View file

@ -73,6 +73,24 @@ void trigger_clock_changes(
}
}
void set_use_sim_time_parameter(rclcpp::Node::SharedPtr node, rclcpp::ParameterValue value)
{
auto parameters_client = std::make_shared<rclcpp::SyncParametersClient>(node);
using namespace std::chrono_literals;
EXPECT_TRUE(parameters_client->wait_for_service(2s));
auto set_parameters_results = parameters_client->set_parameters({
rclcpp::Parameter("use_sim_time", value)
});
for (auto & result : set_parameters_results) {
EXPECT_TRUE(result.successful);
}
// SyncParametersClient returns when parameters have been set on the node_parameters interface,
// but it doesn't mean the on_parameter_event subscription in TimeSource has been called.
// Spin some to handle that subscription.
rclcpp::spin_some(node);
}
TEST_F(TestTimeSource, detachUnattached) {
rclcpp::TimeSource ts;
@ -117,14 +135,21 @@ TEST_F(TestTimeSource, clock) {
trigger_clock_changes(node);
auto t_low = rclcpp::Time(1, 0, RCL_ROS_TIME);
auto t_high = rclcpp::Time(10, 100000, RCL_ROS_TIME);
// Even now that we've recieved a message, ROS time should still not be active since the
// parameter has not been explicitly set.
EXPECT_FALSE(ros_clock->ros_time_is_active());
// Now that we've recieved a message it should be active with parameter unset
// Activate ROS time.
set_use_sim_time_parameter(node, rclcpp::ParameterValue(true));
EXPECT_TRUE(ros_clock->ros_time_is_active());
trigger_clock_changes(node);
auto t_out = ros_clock->now();
// Time from clock should now reflect what was published on the /clock topic.
auto t_low = rclcpp::Time(1, 0, RCL_ROS_TIME);
auto t_high = rclcpp::Time(10, 100000, RCL_ROS_TIME);
EXPECT_NE(0L, t_out.nanoseconds());
EXPECT_LT(t_low.nanoseconds(), t_out.nanoseconds());
EXPECT_GT(t_high.nanoseconds(), t_out.nanoseconds());
@ -179,18 +204,25 @@ TEST_F(TestTimeSource, callbacks) {
auto t_low = rclcpp::Time(1, 0, RCL_ROS_TIME);
auto t_high = rclcpp::Time(10, 100000, RCL_ROS_TIME);
EXPECT_EQ(1, cbo.last_precallback_id_);
EXPECT_EQ(1, cbo.last_postcallback_id_);
// Callbacks will not be triggered since ROS time is not active.
EXPECT_EQ(0, cbo.last_precallback_id_);
EXPECT_EQ(0, cbo.last_postcallback_id_);
// Now that we've recieved a message it should be active with parameter unset
// Activate ROS time.
set_use_sim_time_parameter(node, rclcpp::ParameterValue(true));
EXPECT_TRUE(ros_clock->ros_time_is_active());
trigger_clock_changes(node);
auto t_out = ros_clock->now();
EXPECT_NE(0L, t_out.nanoseconds());
EXPECT_LT(t_low.nanoseconds(), t_out.nanoseconds());
EXPECT_GT(t_high.nanoseconds(), t_out.nanoseconds());
// Callbacks will now have been triggered since ROS time is active.
EXPECT_EQ(1, cbo.last_precallback_id_);
EXPECT_EQ(1, cbo.last_postcallback_id_);
// Change callbacks
rclcpp::JumpHandler::SharedPtr callback_handler2 = ros_clock->create_jump_callback(
@ -203,7 +235,6 @@ TEST_F(TestTimeSource, callbacks) {
EXPECT_EQ(2, cbo.last_precallback_id_);
EXPECT_EQ(2, cbo.last_postcallback_id_);
// Now that we've recieved a message it should be active with parameter unset
EXPECT_TRUE(ros_clock->ros_time_is_active());
t_out = ros_clock->now();
@ -258,24 +289,23 @@ TEST_F(TestTimeSource, callback_handler_erasure) {
std::bind(&CallbackObject::post_callback, &cbo, std::placeholders::_1, 1),
jump_threshold);
// Callbacks will not be triggered since ROS time is not active.
EXPECT_EQ(0, cbo.last_precallback_id_);
EXPECT_EQ(0, cbo.last_postcallback_id_);
EXPECT_FALSE(ros_clock->ros_time_is_active());
// Activate ROS time.
set_use_sim_time_parameter(node, rclcpp::ParameterValue(true));
EXPECT_TRUE(ros_clock->ros_time_is_active());
trigger_clock_changes(node);
auto t_low = rclcpp::Time(1, 0, RCL_ROS_TIME);
auto t_high = rclcpp::Time(10, 100000, RCL_ROS_TIME);
// Callbacks will now have been triggered since ROS time is active.
EXPECT_EQ(1, cbo.last_precallback_id_);
EXPECT_EQ(1, cbo.last_postcallback_id_);
// Now that we've recieved a message it should be active with parameter unset
EXPECT_TRUE(ros_clock->ros_time_is_active());
auto t_out = ros_clock->now();
EXPECT_NE(0L, t_out.nanoseconds());
@ -293,11 +323,9 @@ TEST_F(TestTimeSource, callback_handler_erasure) {
trigger_clock_changes(node);
EXPECT_EQ(2, cbo.last_precallback_id_);
EXPECT_EQ(2, cbo.last_postcallback_id_);
// Now that we've recieved a message it should be active with parameter unset
EXPECT_TRUE(ros_clock->ros_time_is_active());
t_out = ros_clock->now();
@ -316,48 +344,28 @@ TEST_F(TestTimeSource, parameter_activation) {
ts.attachClock(ros_clock);
EXPECT_FALSE(ros_clock->ros_time_is_active());
auto parameters_client = std::make_shared<rclcpp::SyncParametersClient>(node);
using namespace std::chrono_literals;
EXPECT_TRUE(parameters_client->wait_for_service(2s));
auto set_parameters_results = parameters_client->set_parameters({
rclcpp::Parameter("use_sim_time", true)
});
for (auto & result : set_parameters_results) {
EXPECT_TRUE(result.successful);
}
// SyncParametersClient returns when parameters have been set on the node_parameters interface,
// but it doesn't mean the on_parameter_event subscription in TimeSource has been called.
// Spin some to handle that subscription.
rclcpp::spin_some(node);
set_use_sim_time_parameter(node, rclcpp::ParameterValue(true));
EXPECT_TRUE(ros_clock->ros_time_is_active());
set_parameters_results = parameters_client->set_parameters({
rclcpp::Parameter("use_sim_time", rclcpp::ParameterType::PARAMETER_NOT_SET)
});
for (auto & result : set_parameters_results) {
EXPECT_TRUE(result.successful);
}
rclcpp::spin_some(node);
set_use_sim_time_parameter(
node, rclcpp::ParameterValue(rclcpp::ParameterType::PARAMETER_NOT_SET));
EXPECT_TRUE(ros_clock->ros_time_is_active());
set_parameters_results = parameters_client->set_parameters({
rclcpp::Parameter("use_sim_time", false)
});
for (auto & result : set_parameters_results) {
EXPECT_TRUE(result.successful);
}
rclcpp::spin_some(node);
set_use_sim_time_parameter(node, rclcpp::ParameterValue(false));
EXPECT_FALSE(ros_clock->ros_time_is_active());
set_parameters_results = parameters_client->set_parameters({
rclcpp::Parameter("use_sim_time", rclcpp::ParameterType::PARAMETER_NOT_SET)
});
for (auto & result : set_parameters_results) {
EXPECT_TRUE(result.successful);
}
rclcpp::spin_some(node);
set_use_sim_time_parameter(
node, rclcpp::ParameterValue(rclcpp::ParameterType::PARAMETER_NOT_SET));
EXPECT_FALSE(ros_clock->ros_time_is_active());
// If the use_sim_time parameter is not explicitly set to True, this clock's use of sim time
// should not be affected by the presence of a clock publisher.
trigger_clock_changes(node);
EXPECT_FALSE(ros_clock->ros_time_is_active());
set_use_sim_time_parameter(node, rclcpp::ParameterValue(false));
EXPECT_FALSE(ros_clock->ros_time_is_active());
set_use_sim_time_parameter(node, rclcpp::ParameterValue(true));
EXPECT_TRUE(ros_clock->ros_time_is_active());
}
TEST_F(TestTimeSource, no_pre_jump_callback) {
@ -381,18 +389,7 @@ TEST_F(TestTimeSource, no_pre_jump_callback) {
ts.attachClock(ros_clock);
// Activate ROS time
auto parameters_client = std::make_shared<rclcpp::SyncParametersClient>(node);
ASSERT_TRUE(parameters_client->wait_for_service(2s));
auto set_parameters_results = parameters_client->set_parameters({
rclcpp::Parameter("use_sim_time", true)
});
for (auto & result : set_parameters_results) {
EXPECT_TRUE(result.successful);
}
// SyncParametersClient returns when parameters have been set on the node_parameters interface,
// but it doesn't mean the on_parameter_event subscription in TimeSource has been called.
// Spin some to handle that subscription.
rclcpp::spin_some(node);
set_use_sim_time_parameter(node, rclcpp::ParameterValue(true));
ASSERT_TRUE(ros_clock->ros_time_is_active());
EXPECT_EQ(0, cbo.last_precallback_id_);