Skip to content

Commit

Permalink
Add spin_all instead of extending spin_some signature
Browse files Browse the repository at this point in the history
Signed-off-by: Ivan Santiago Paunovic <ivanpauno@ekumenlabs.com>
  • Loading branch information
ivanpauno committed Jun 11, 2020
1 parent 05625f0 commit 58ac8c9
Show file tree
Hide file tree
Showing 2 changed files with 42 additions and 9 deletions.
34 changes: 26 additions & 8 deletions rclcpp/include/rclcpp/executor.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -159,7 +159,7 @@ class Executor
void
spin_node_some(std::shared_ptr<rclcpp::Node> node);

/// Complete all available queued work without blocking.
/// Complete some available queued work without blocking.
/**
* This function can be overridden. The default implementation is suitable for a
* single-threaded model of execution.
Expand All @@ -169,16 +169,30 @@ class Executor
* \param[in] max_duration The maximum amount of time to spend executing work, or 0 for no limit.
* Note that spin_some() may take longer than this time as it only returns once max_duration has
* been exceeded.
* \param[in] exhaustive When true, it will try to collect entities again after executing executables
* until there is no more ready entities or the specified duration has been reached.
* When there are subscriptions/clients with long history queues, this will ensure all messages/requestes
* available are handled.
*/
RCLCPP_PUBLIC
virtual void
spin_some(
std::chrono::nanoseconds max_duration = std::chrono::nanoseconds(0),
bool exhaustive = false);
spin_some(std::chrono::nanoseconds max_duration = std::chrono::nanoseconds(0));

/// Complete all available queued work without blocking.
/**
* This function can be overridden. The default implementation is suitable for a
* single-threaded model of execution.
* Adding subscriptions, timers, services, etc. with blocking callbacks will cause this function
* to block (which may have unintended consequences).
*
* To ensure all available queued work is executed, this function have to check if there is
* available work repeatedly.
* Thus, if the time that waitables take to be executed is longer than the period on which new waitables
* become ready, this will execute work repeatedly until `max_duration` has elapsed.
*
* \param[in] max_duration The maximum amount of time to spend executing work. Must be positive.
* Note that spin_some() may take longer than this time as it only returns once max_duration has
* been exceeded.
*/
RCLCPP_PUBLIC
virtual void
spin_all(std::chrono::nanoseconds max_duration);

RCLCPP_PUBLIC
virtual void
Expand Down Expand Up @@ -270,6 +284,10 @@ class Executor
rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node,
std::chrono::nanoseconds timeout);

RCLCPP_PUBLIC
void
spin_some_impl(std::chrono::nanoseconds max_duration, bool exhaustive);

/// Find the next available executable and do the work associated with it.
/**
* \param[in] any_exec Union structure that can hold any executable type (timer, subscription,
Expand Down
17 changes: 16 additions & 1 deletion rclcpp/src/rclcpp/executor.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -28,6 +28,8 @@

#include "rcutils/logging_macros.h"

using namespace std::chrono_literals;

using rclcpp::exceptions::throw_from_rcl_error;
using rclcpp::AnyExecutable;
using rclcpp::Executor;
Expand Down Expand Up @@ -212,8 +214,21 @@ Executor::spin_node_some(std::shared_ptr<rclcpp::Node> node)
this->spin_node_some(node->get_node_base_interface());
}

void Executor::spin_some(std::chrono::nanoseconds max_duration)
{
return this->spin_some_impl(max_duration, false);
}

void Executor::spin_all(std::chrono::nanoseconds max_duration)
{
if (max_duration <= 0ns) {
throw std::invalid_argument("max_duration must be positive");
}
return this->spin_some_impl(max_duration, true);
}

void
Executor::spin_some(std::chrono::nanoseconds max_duration, bool exhaustive)
Executor::spin_some_impl(std::chrono::nanoseconds max_duration, bool exhaustive)
{
auto start = std::chrono::steady_clock::now();
auto max_duration_not_elapsed = [max_duration, start]() {
Expand Down

0 comments on commit 58ac8c9

Please sign in to comment.