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

Merge pull request #58 from ros2/fix_spin_once_and_some

Browse source 
fix executor spin_once and spin_some
This commit is contained in:
Dirk Thomas 2015-07-16 22:33:43 -07:00
commit b62ab6a8c4
1 changed files with 19 additions and 15 deletions
Download patch file Download diff file Expand all files Collapse all files

34
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);
// Interrupt waiting to handle new node if (notify) {
rmw_ret_t status = rmw_trigger_guard_condition(interrupt_guard_condition_); // Interrupt waiting to handle new node
if (status != RMW_RET_OK) { rmw_ret_t status = rmw_trigger_guard_condition(interrupt_guard_condition_);
throw std::runtime_error(rmw_get_error_string_safe()); if (status != RMW_RET_OK) {
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,34 +93,36 @@ public:
node_removed |= matched; node_removed |= matched;
return matched; return matched;
})); }));
// If the node was matched and removed, interrupt waiting if (notify) {
if (node_removed) { // If the node was matched and removed, interrupt waiting
rmw_ret_t status = rmw_trigger_guard_condition(interrupt_guard_condition_); if (node_removed) {
if (status != RMW_RET_OK) { rmw_ret_t status = rmw_trigger_guard_condition(interrupt_guard_condition_);
throw std::runtime_error(rmw_get_error_string_safe()); if (status != RMW_RET_OK) {
throw std::runtime_error(rmw_get_error_string_safe());
}
} }
} }
} }
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