Skip to content

Commit

Permalink
Cleanup time source object lifetimes (#1867)
Browse files Browse the repository at this point in the history 
* Small cleanups in TimeSource.

Simplify some code, and also make sure to throw an exception
when use_sim_time is not of type bool.  Also add a test for
the latter.

* Remove the sim_time_cb_handler.

It was originally used to make sure that someone didn't change
the 'use_sim_time' type from boolean to something else.  But
since the parameters interface now does that automatically for
us, we don't need the explicit check here.

I can think of one situation that this allows that wasn't allowed
before.  If the user defined 'use_sim_time' as a parameter override
when constructing a node, and the type is bool, and they also
defined the parameters as 'dynamic_typing', then this callback
will still have effect.  But presumably if the user went out of
their way to change the parameter to dynamic_typing, they are
trying to do something esoteric and so we should just let them.

* ClocksState does not need to be a pointer.

Instead, make it a regular member variable.  That lets us get
rid of all of the special handling for when it is a weak_ptr
or not.  It's lifetime is now just that of NodeState.

* Stop using NodeState as a weak_ptr in the captures.

This ended up causing the lifetime of the object to be weird.
Instead, just capture 'this' which is sufficient.

* Make sure destroy_clock_sub is first.

* Switch to using just a regular object.

Windows objects to trying to do "make_shared" in the RCLCPP
macro, so just switch to a normal object here.

Signed-off-by: Chris Lalancette <clalancette@openrobotics.org>
  • Loading branch information
@clalancette
clalancette committed Jan 20, 2022
1 parent bca3fd7 commit c54a6f1
Show file tree
Hide file tree
Showing 3 changed files with 70 additions and 100 deletions.
3 changes: 0 additions & 3 deletions rclcpp/include/rclcpp/time_source.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -147,9 +147,6 @@ class TimeSource
~TimeSource();

private:
class ClocksState;
std::shared_ptr<ClocksState> clocks_state_;

class NodeState;
std::shared_ptr<NodeState> node_state_;

Expand Down
144 changes: 51 additions & 93 deletions rclcpp/src/rclcpp/time_source.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -33,7 +33,7 @@
namespace rclcpp
{

class TimeSource::ClocksState : public std::enable_shared_from_this<ClocksState>
class ClocksState final
{
public:
ClocksState()
Expand Down Expand Up @@ -182,12 +182,11 @@ class TimeSource::ClocksState : public std::enable_shared_from_this<ClocksState>
std::shared_ptr<const rosgraph_msgs::msg::Clock> last_msg_set_;
};

class TimeSource::NodeState : public std::enable_shared_from_this<NodeState>
class TimeSource::NodeState final
{
public:
NodeState(std::weak_ptr<ClocksState> clocks_state, const rclcpp::QoS & qos, bool use_clock_thread)
: clocks_state_(std::move(clocks_state)),
use_clock_thread_(use_clock_thread),
NodeState(const rclcpp::QoS & qos, bool use_clock_thread)
: use_clock_thread_(use_clock_thread),
logger_(rclcpp::get_logger("rclcpp")),
qos_(qos)
{
Expand Down Expand Up @@ -250,94 +249,77 @@ class TimeSource::NodeState : public std::enable_shared_from_this<NodeState>
if (!node_parameters_->has_parameter(use_sim_time_name)) {
use_sim_time_param = node_parameters_->declare_parameter(
use_sim_time_name,
rclcpp::ParameterValue(false),
rcl_interfaces::msg::ParameterDescriptor());
rclcpp::ParameterValue(false));
} else {
use_sim_time_param = node_parameters_->get_parameter(use_sim_time_name).get_parameter_value();
}
if (use_sim_time_param.get_type() == rclcpp::PARAMETER_BOOL) {
if (use_sim_time_param.get<bool>()) {
parameter_state_ = SET_TRUE;
// There should be no way to call this attachNode when the clocks_state_ pointer is not
// valid because it means the TimeSource is being destroyed
if (auto clocks_state_ptr = clocks_state_.lock()) {
clocks_state_ptr->enable_ros_time();
create_clock_sub();
}
clocks_state_.enable_ros_time();
create_clock_sub();
}
} else {
RCLCPP_ERROR(
logger_, "Invalid type '%s' for parameter 'use_sim_time', should be 'bool'",
rclcpp::to_string(use_sim_time_param.get_type()).c_str());
throw std::invalid_argument("Invalid type for parameter 'use_sim_time', should be 'bool'");
}
sim_time_cb_handler_ = node_parameters_->add_on_set_parameters_callback(
[use_sim_time_name](const std::vector<rclcpp::Parameter> & parameters) {
rcl_interfaces::msg::SetParametersResult result;
result.successful = true;
for (const auto & parameter : parameters) {
if (
parameter.get_name() == use_sim_time_name &&
parameter.get_type() != rclcpp::PARAMETER_BOOL)
{
result.successful = false;
result.reason = "'" + use_sim_time_name + "' must be a bool";
break;
}
}
return result;
});

// TODO(tfoote) use parameters interface not subscribe to events via topic ticketed #609
parameter_subscription_ = rclcpp::AsyncParametersClient::on_parameter_event(
node_topics_,
[state = std::weak_ptr<NodeState>(this->shared_from_this())](
std::shared_ptr<const rcl_interfaces::msg::ParameterEvent> event) {
if (auto state_ptr = state.lock()) {
state_ptr->on_parameter_event(event);
[this](std::shared_ptr<const rcl_interfaces::msg::ParameterEvent> event) {
if (node_base_ != nullptr) {
this->on_parameter_event(event);
}
// Do nothing if the pointer could not be locked because it means the TimeSource is now
// Do nothing if node_base_ is nullptr because it means the TimeSource is now
// without an attached node
});
}

// Detach the attached node
void detachNode()
{
// There should be no way to call detachNode when the clocks_state_ pointer is not valid
// because it means the TimeSource is being destroyed
if (auto clocks_state_ptr = clocks_state_.lock()) {
clocks_state_ptr->disable_ros_time();
}
// destroy_clock_sub() *must* be first here, to ensure that the executor
// can't possibly call any of the callbacks as we are cleaning up.
destroy_clock_sub();
clocks_state_.disable_ros_time();
parameter_subscription_.reset();
node_base_.reset();
node_topics_.reset();
node_graph_.reset();
node_services_.reset();
node_logging_.reset();
node_clock_.reset();
if (sim_time_cb_handler_ && node_parameters_) {
node_parameters_->remove_on_set_parameters_callback(sim_time_cb_handler_.get());
}
sim_time_cb_handler_.reset();
node_parameters_.reset();
}

void attachClock(std::shared_ptr<rclcpp::Clock> clock)
{
clocks_state_.attachClock(std::move(clock));
}

void detachClock(std::shared_ptr<rclcpp::Clock> clock)
{
clocks_state_.detachClock(std::move(clock));
}

private:
std::weak_ptr<ClocksState> clocks_state_;
ClocksState clocks_state_;

// Dedicated thread for clock subscription.
bool use_clock_thread_;
std::thread clock_executor_thread_;

// Preserve the node reference
rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base_;
rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr node_topics_;
rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph_;
rclcpp::node_interfaces::NodeServicesInterface::SharedPtr node_services_;
rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging_;
rclcpp::node_interfaces::NodeClockInterface::SharedPtr node_clock_;
rclcpp::node_interfaces::NodeParametersInterface::SharedPtr node_parameters_;
rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base_{nullptr};
rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr node_topics_{nullptr};
rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph_{nullptr};
rclcpp::node_interfaces::NodeServicesInterface::SharedPtr node_services_{nullptr};
rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging_{nullptr};
rclcpp::node_interfaces::NodeClockInterface::SharedPtr node_clock_{nullptr};
rclcpp::node_interfaces::NodeParametersInterface::SharedPtr node_parameters_{nullptr};

// Store (and update on node attach) logger for logging.
Logger logger_;
Expand All @@ -346,9 +328,7 @@ class TimeSource::NodeState : public std::enable_shared_from_this<NodeState>
rclcpp::QoS qos_;

// The subscription for the clock callback
using MessageT = rosgraph_msgs::msg::Clock;
using Alloc = std::allocator<void>;
using SubscriptionT = rclcpp::Subscription<MessageT, Alloc>;
using SubscriptionT = rclcpp::Subscription<rosgraph_msgs::msg::Clock>;
std::shared_ptr<SubscriptionT> clock_subscription_{nullptr};
std::mutex clock_sub_lock_;
rclcpp::CallbackGroup::SharedPtr clock_callback_group_;
Expand All @@ -358,21 +338,15 @@ class TimeSource::NodeState : public std::enable_shared_from_this<NodeState>
// The clock callback itself
void clock_cb(std::shared_ptr<const rosgraph_msgs::msg::Clock> msg)
{
auto clocks_state_ptr = clocks_state_.lock();
if (!clocks_state_ptr) {
// The clock_state_ pointer is no longer valid, implying that the TimeSource object is being
// destroyed, so do nothing
return;
}
if (!clocks_state_ptr->is_ros_time_active() && SET_TRUE == this->parameter_state_) {
clocks_state_ptr->enable_ros_time();
if (!clocks_state_.is_ros_time_active() && SET_TRUE == this->parameter_state_) {
clocks_state_.enable_ros_time();
}
// Cache the last message in case a new clock is attached.
clocks_state_ptr->cache_last_msg(msg);
clocks_state_.cache_last_msg(msg);
auto time_msg = std::make_shared<builtin_interfaces::msg::Time>(msg->clock);

if (SET_TRUE == this->parameter_state_) {
clocks_state_ptr->set_all_clocks(time_msg, true);
clocks_state_.set_all_clocks(time_msg, true);
}
}

Expand Down Expand Up @@ -421,13 +395,12 @@ class TimeSource::NodeState : public std::enable_shared_from_this<NodeState>
node_topics_,
"/clock",
qos_,
[state = std::weak_ptr<NodeState>(this->shared_from_this())](
std::shared_ptr<const rosgraph_msgs::msg::Clock> msg) {
if (auto state_ptr = state.lock()) {
state_ptr->clock_cb(msg);
[this](std::shared_ptr<const rosgraph_msgs::msg::Clock> msg) {
// We are using node_base_ as an indication if there is a node attached.
// Only call the clock_cb if that is the case.
if (node_base_ != nullptr) {
clock_cb(msg);
}
// Do nothing if the pointer could not be locked because it means the TimeSource is now
// without an attached node
},
options
);
Expand All @@ -447,19 +420,12 @@ class TimeSource::NodeState : public std::enable_shared_from_this<NodeState>
}

// Parameter Event subscription
using ParamMessageT = rcl_interfaces::msg::ParameterEvent;
using ParamSubscriptionT = rclcpp::Subscription<ParamMessageT, Alloc>;
using ParamSubscriptionT = rclcpp::Subscription<rcl_interfaces::msg::ParameterEvent>;
std::shared_ptr<ParamSubscriptionT> parameter_subscription_;

// Callback for parameter updates
void on_parameter_event(std::shared_ptr<const rcl_interfaces::msg::ParameterEvent> event)
{
auto clocks_state_ptr = clocks_state_.lock();
if (!clocks_state_ptr) {
// The clock_state_ pointer is no longer valid, implying that the TimeSource object is being
// destroyed, so do nothing
return;
}
// Filter out events on 'use_sim_time' parameter instances in other nodes.
if (event->node != node_base_->get_fully_qualified_name()) {
return;
Expand All @@ -475,41 +441,35 @@ class TimeSource::NodeState : public std::enable_shared_from_this<NodeState>
}
if (it.second->value.bool_value) {
parameter_state_ = SET_TRUE;
clocks_state_ptr->enable_ros_time();
clocks_state_.enable_ros_time();
create_clock_sub();
} else {
parameter_state_ = SET_FALSE;
clocks_state_ptr->disable_ros_time();
destroy_clock_sub();
clocks_state_.disable_ros_time();
}
}
// Handle the case that use_sim_time was deleted.
rclcpp::ParameterEventsFilter deleted(event, {"use_sim_time"},
{rclcpp::ParameterEventsFilter::EventType::DELETED});
for (auto & it : deleted.get_events()) {
(void) it; // if there is a match it's already matched, don't bother reading it.
// If the parameter is deleted mark it as unset but dont' change state.
// If the parameter is deleted mark it as unset but don't change state.
parameter_state_ = UNSET;
}
}

// An enum to hold the parameter state
enum UseSimTimeParameterState {UNSET, SET_TRUE, SET_FALSE};
UseSimTimeParameterState parameter_state_;

// A handler for the use_sim_time parameter callback.
rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr sim_time_cb_handler_{nullptr};
};

TimeSource::TimeSource(
std::shared_ptr<rclcpp::Node> node,
const rclcpp::QoS & qos,
bool use_clock_thread)
: constructed_use_clock_thread_(use_clock_thread),
constructed_qos_(qos)
: TimeSource(qos, use_clock_thread)
{
clocks_state_ = std::make_shared<ClocksState>();
node_state_ = std::make_shared<NodeState>(clocks_state_->weak_from_this(), qos, use_clock_thread);
attachNode(node);
}

Expand All @@ -519,8 +479,7 @@ TimeSource::TimeSource(
: constructed_use_clock_thread_(use_clock_thread),
constructed_qos_(qos)
{
clocks_state_ = std::make_shared<ClocksState>();
node_state_ = std::make_shared<NodeState>(clocks_state_->weak_from_this(), qos, use_clock_thread);
node_state_ = std::make_shared<NodeState>(qos, use_clock_thread);
}

void TimeSource::attachNode(rclcpp::Node::SharedPtr node)
Expand Down Expand Up @@ -559,19 +518,18 @@ void TimeSource::detachNode()
{
node_state_.reset();
node_state_ = std::make_shared<NodeState>(
clocks_state_->weak_from_this(),
constructed_qos_,
constructed_use_clock_thread_);
}

void TimeSource::attachClock(std::shared_ptr<rclcpp::Clock> clock)
{
clocks_state_->attachClock(std::move(clock));
node_state_->attachClock(std::move(clock));
}

void TimeSource::detachClock(std::shared_ptr<rclcpp::Clock> clock)
{
clocks_state_->detachClock(std::move(clock));
node_state_->detachClock(std::move(clock));
}

bool TimeSource::get_use_clock_thread()
Expand Down
23 changes: 19 additions & 4 deletions rclcpp/test/rclcpp/test_time_source.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -27,6 +27,8 @@
#include "rclcpp/time.hpp"
#include "rclcpp/time_source.hpp"

#include "../utils/rclcpp_gtest_macros.hpp"

using namespace std::chrono_literals;

class TestTimeSource : public ::testing::Test
Expand Down Expand Up @@ -246,11 +248,25 @@ TEST_F(TestTimeSource, ROS_time_valid_sim_time) {
}

TEST_F(TestTimeSource, ROS_invalid_sim_time) {
rclcpp::TimeSource ts;
ts.attachNode(node);
rclcpp::TimeSource ts(node);
EXPECT_FALSE(node->set_parameter(rclcpp::Parameter("use_sim_time", "not boolean")).successful);
}

TEST(TimeSource, invalid_sim_time_parameter_override)
{
rclcpp::init(0, nullptr);

rclcpp::NodeOptions options;
options.automatically_declare_parameters_from_overrides(true);
options.append_parameter_override("use_sim_time", "not boolean");

RCLCPP_EXPECT_THROW_EQ(
rclcpp::Node("my_node", options),
std::invalid_argument("Invalid type for parameter 'use_sim_time', should be 'bool'"));

rclcpp::shutdown();
}

TEST_F(TestTimeSource, clock) {
rclcpp::TimeSource ts(node);
auto ros_clock = std::make_shared<rclcpp::Clock>(RCL_ROS_TIME);
Expand Down Expand Up @@ -749,8 +765,7 @@ TEST_F(TestTimeSource, clock_sleep_until_with_ros_time_basic) {

node->set_parameter({"use_sim_time", true});
auto clock = std::make_shared<rclcpp::Clock>(RCL_ROS_TIME);
rclcpp::TimeSource time_source;
time_source.attachNode(node);
rclcpp::TimeSource time_source(node);
time_source.attachClock(clock);

// Wait until time source has definitely received a first ROS time from the pub node
Expand Down

0 comments on commit c54a6f1

Please sign in to comment.