Skip to content
Snippets Groups Projects
Commit 3af56259 authored by bpwilcox's avatar bpwilcox Committed by Carl Delsey
Browse files

check bool in rclcpp node thread to stop spinning

add back blank line
parent 6aa4daa1
No related branches found
No related tags found
No related merge requests found
......@@ -51,6 +51,7 @@ protected:
// When creating a local node, this class will launch a separate thread created to spin the node
std::unique_ptr<std::thread> rclcpp_thread_;
std::atomic<bool> stop_rclcpp_thread_{false};
};
} // namespace nav2_util
......
......@@ -47,9 +47,11 @@ LifecycleNode::LifecycleNode(
use_rclcpp_node_(use_rclcpp_node)
{
if (use_rclcpp_node_) {
stop_rclcpp_thread_ = false;
rclcpp_node_ = std::make_shared<rclcpp::Node>(node_name + "_rclcpp_node", namespace_);
rclcpp_thread_ = std::make_unique<std::thread>(
[](rclcpp::Node::SharedPtr node) {rclcpp::spin(node);}, rclcpp_node_
[&](rclcpp::Node::SharedPtr node) {
while (!stop_rclcpp_thread_ && rclcpp::ok()) {rclcpp::spin_some(node);}}, rclcpp_node_
);
}
}
......@@ -63,6 +65,7 @@ LifecycleNode::~LifecycleNode()
}
if (use_rclcpp_node_) {
stop_rclcpp_thread_ = true;
rclcpp_thread_->join();
}
}
......
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment