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

fix executor spin_once and spin_some

Browse source
This commit is contained in:
Dirk Thomas 2015-07-16 22:28:40 -07:00
parent bdafa54d8b
commit 60cb347807
1 changed files with 19 additions and 15 deletions
Download patch file Download diff file Expand all files Collapse all files

16
rclcpp/include/rclcpp/executor.hpp
View file

@ -60,7 +60,7 @@ public:
virtual void spin() = 0; virtual void spin() = 0;
virtual void virtual void
add_node(rclcpp::node::Node::SharedPtr & node_ptr) add_node(rclcpp::node::Node::SharedPtr & node_ptr, bool notify=true)
{ {
// Check to ensure node not already added // Check to ensure node not already added
for (auto & weak_node : weak_nodes_) { for (auto & weak_node : weak_nodes_) {
@ -71,15 +71,17 @@ public:
} }
} }
weak_nodes_.push_back(node_ptr); weak_nodes_.push_back(node_ptr);
if (notify) {
// Interrupt waiting to handle new node // Interrupt waiting to handle new node
rmw_ret_t status = rmw_trigger_guard_condition(interrupt_guard_condition_); rmw_ret_t status = rmw_trigger_guard_condition(interrupt_guard_condition_);
if (status != RMW_RET_OK) { if (status != RMW_RET_OK) {
throw std::runtime_error(rmw_get_error_string_safe()); throw std::runtime_error(rmw_get_error_string_safe());
} }
} }
}
virtual void virtual void
remove_node(rclcpp::node::Node::SharedPtr & node_ptr) remove_node(rclcpp::node::Node::SharedPtr & node_ptr, bool notify=true)
{ {
bool node_removed = false; bool node_removed = false;
weak_nodes_.erase( weak_nodes_.erase(
@ -91,6 +93,7 @@ public:
node_removed |= matched; node_removed |= matched;
return matched; return matched;
})); }));
if (notify) {
// If the node was matched and removed, interrupt waiting // If the node was matched and removed, interrupt waiting
if (node_removed) { if (node_removed) {
rmw_ret_t status = rmw_trigger_guard_condition(interrupt_guard_condition_); rmw_ret_t status = rmw_trigger_guard_condition(interrupt_guard_condition_);
@ -99,26 +102,27 @@ public:
} }
} }
} }
}
void spin_node_once(rclcpp::node::Node::SharedPtr & node, bool nonblocking = false) void spin_node_once(rclcpp::node::Node::SharedPtr & node, bool nonblocking = false)
{ {
this->add_node(node); this->add_node(node, false);
// non-blocking = true // non-blocking = true
auto any_exec = get_next_executable(nonblocking); auto any_exec = get_next_executable(nonblocking);
if (any_exec) { if (any_exec) {
execute_any_executable(any_exec); execute_any_executable(any_exec);
} }
this->remove_node(node); this->remove_node(node, false);
} }
void spin_node_some(rclcpp::node::Node::SharedPtr & node) void spin_node_some(rclcpp::node::Node::SharedPtr & node)
{ {
this->add_node(node); this->add_node(node, false);
// non-blocking = true // non-blocking = true
while (AnyExecutable::SharedPtr any_exec = get_next_executable(true)) { while (AnyExecutable::SharedPtr any_exec = get_next_executable(true)) {
execute_any_executable(any_exec); execute_any_executable(any_exec);
} }
this->remove_node(node); this->remove_node(node, false);
} }
// Support dynamic switching of memory strategy // Support dynamic switching of memory strategy