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

Merge pull request #66 from ros2/wait_timeout

Browse source 
Pass timeout duration to invocation of rmw_wait
This commit is contained in:
Jackie Kay 2015-08-04 19:24:41 -07:00
commit 2418947a3e
1 changed files with 27 additions and 8 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -107,11 +107,13 @@ public:
} }
} }
void spin_node_once(rclcpp::node::Node::SharedPtr & node, bool nonblocking = false) template<typename T = std::milli>
void spin_node_once(rclcpp::node::Node::SharedPtr & node,
std::chrono::duration<int64_t, T> timeout = std::chrono::duration<int64_t, T>(-1))
{ {
this->add_node(node, false); this->add_node(node, false);
// non-blocking = true // non-blocking = true
auto any_exec = get_next_executable(nonblocking); auto any_exec = get_next_executable(timeout);
if (any_exec) { if (any_exec) {
execute_any_executable(any_exec); execute_any_executable(any_exec);
} }
@ -122,7 +124,9 @@ public:
{ {
this->add_node(node, false); 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(std::chrono::milliseconds::zero()))
{
execute_any_executable(any_exec); execute_any_executable(any_exec);
} }
this->remove_node(node, false); this->remove_node(node, false);
@ -240,8 +244,9 @@ protected:
/*** Populate class storage from stored weak node pointers and wait. ***/ /*** Populate class storage from stored weak node pointers and wait. ***/
template<typename T = std::milli>
void void
wait_for_work(bool nonblocking) wait_for_work(std::chrono::duration<int64_t, T> timeout = std::chrono::duration<int64_t, T>(-1))
{ {
// Collect the subscriptions and timers to be waited on // Collect the subscriptions and timers to be waited on
bool has_invalid_weak_nodes = false; bool has_invalid_weak_nodes = false;
@ -371,14 +376,26 @@ protected:
guard_cond_handle_index += 1; guard_cond_handle_index += 1;
} }
rmw_time_t * wait_timeout = NULL;
rmw_time_t rmw_timeout;
if (timeout >= std::chrono::duration<int64_t, T>::zero()) {
// Convert timeout representation to rmw_time
auto timeout_sec = std::chrono::duration_cast<std::chrono::seconds>(timeout);
rmw_timeout.sec = timeout_sec.count();
rmw_timeout.nsec = std::chrono::duration_cast<std::chrono::nanoseconds>(timeout).count() %
(1000 * 1000 * 1000);
wait_timeout = &rmw_timeout;
}
// Now wait on the waitable subscriptions and timers // Now wait on the waitable subscriptions and timers
rmw_ret_t status = rmw_wait( rmw_ret_t status = rmw_wait(
&subscriber_handles, &subscriber_handles,
&guard_condition_handles, &guard_condition_handles,
&service_handles, &service_handles,
&client_handles, &client_handles,
nonblocking); wait_timeout);
if (status != RMW_RET_OK) { if (status != RMW_RET_OK && status != RMW_RET_TIMEOUT) {
throw std::runtime_error(rmw_get_error_string_safe()); throw std::runtime_error(rmw_get_error_string_safe());
} }
// If ctrl-c guard condition, return directly // If ctrl-c guard condition, return directly
@ -824,8 +841,10 @@ protected:
return any_exec; return any_exec;
} }
template<typename T = std::milli>
AnyExecutable::SharedPtr AnyExecutable::SharedPtr
get_next_executable(bool nonblocking = false) get_next_executable(std::chrono::duration<int64_t, T> timeout = std::chrono::duration<int64_t,
T>(-1))
{ {
// Check to see if there are any subscriptions or timers needing service // Check to see if there are any subscriptions or timers needing service
// TODO(wjwwood): improve run to run efficiency of this function // TODO(wjwwood): improve run to run efficiency of this function
@ -833,7 +852,7 @@ protected:
// If there are none // If there are none
if (!any_exec) { if (!any_exec) {
// Wait for subscriptions or timers to work on // Wait for subscriptions or timers to work on
wait_for_work(nonblocking); wait_for_work(timeout);
// Try again // Try again
any_exec = get_next_ready_executable(); any_exec = get_next_ready_executable();
} }