Skip to content
Permalink

Comparing changes

This is a direct comparison between two commits made in this repository or its related repositories. View the default comparison for this range or learn more about diff comparisons.

Open a pull request

Create a new pull request by comparing changes across two branches. If you need to, you can also . Learn more about diff comparisons here.
base repository: ros2/rclcpp
Failed to load repositories. Confirm that selected base ref is valid, then try again.
Loading
base: 22223f14f78641a2987caa69af77cfc8968d04a4
Choose a base ref
..
head repository: ros2/rclcpp
Failed to load repositories. Confirm that selected head ref is valid, then try again.
Loading
compare: 449c07a3fe6982ae2e6201830deda9d2855fde0a
Choose a head ref
Showing with 338 additions and 333 deletions.
  1. +3 −3 rclcpp/include/rclcpp/client.hpp
  2. +47 −71 rclcpp/include/rclcpp/executor.hpp
  3. +19 −33 rclcpp/include/rclcpp/executors.hpp
  4. +1 −1 rclcpp/include/rclcpp/future_return_code.hpp
  5. +3 −3 rclcpp/include/rclcpp/generic_client.hpp
  6. +1 −0 rclcpp/include/rclcpp/rclcpp.hpp
  7. +16 −16 rclcpp/src/rclcpp/parameter_client.cpp
  8. +1 −1 rclcpp/src/rclcpp/time_source.cpp
  9. +1 −1 rclcpp/test/benchmark/benchmark_client.cpp
  10. +14 −14 rclcpp/test/benchmark/benchmark_executor.cpp
  11. +1 −1 rclcpp/test/benchmark/benchmark_service.cpp
  12. +1 −1 rclcpp/test/rclcpp/executors/test_events_executor.cpp
  13. +29 −29 rclcpp/test/rclcpp/executors/test_executors.cpp
  14. +2 −2 rclcpp/test/rclcpp/executors/test_static_single_threaded_executor.cpp
  15. +3 −3 rclcpp/test/rclcpp/test_add_callback_groups_to_executor.cpp
  16. +1 −1 rclcpp/test/rclcpp/test_client_common.cpp
  17. +52 −10 rclcpp/test/rclcpp/test_executor.cpp
  18. +2 −2 rclcpp/test/rclcpp/test_intra_process_manager_with_allocators.cpp
  19. +6 −6 rclcpp/test/rclcpp/test_logger_service.cpp
  20. +23 −23 rclcpp/test/rclcpp/test_parameter_client.cpp
  21. +13 −13 rclcpp/test/rclcpp/test_qos_event.cpp
  22. +10 −10 rclcpp/test/rclcpp/test_rosout_subscription.cpp
  23. +9 −9 rclcpp/test/rclcpp/test_service_introspection.cpp
  24. +2 −2 rclcpp/test/rclcpp/topic_statistics/test_subscription_topic_statistics.cpp
  25. +8 −8 rclcpp_action/test/benchmark/benchmark_action_client.cpp
  26. +6 −6 rclcpp_action/test/benchmark/benchmark_action_server.cpp
  27. +41 −41 rclcpp_action/test/test_client.cpp
  28. +8 −8 rclcpp_action/test/test_server.cpp
  29. +15 −15 rclcpp_components/test/test_component_manager_api.cpp
6 changes: 3 additions & 3 deletions rclcpp/include/rclcpp/client.hpp
Original file line number Diff line number Diff line change
@@ -608,11 +608,11 @@ class Client : public ClientBase
/// Send a request to the service server.
/**
* This method returns a `FutureAndRequestId` instance
* that can be passed to Executor::spin_until_complete() to
* that can be passed to Executor::spin_until_future_complete() to
* wait until it has been completed.
*
* If the future never completes,
* e.g. the call to Executor::spin_until_complete() times out,
* e.g. the call to Executor::spin_until_future_complete() times out,
* Client::remove_pending_request() must be called to clean the client internal state.
* Not doing so will make the `Client` instance to use more memory each time a response is not
* received from the service server.
@@ -621,7 +621,7 @@ class Client : public ClientBase
* auto future = client->async_send_request(my_request);
* if (
* rclcpp::FutureReturnCode::TIMEOUT ==
* executor->spin_until_complete(future, timeout))
* executor->spin_until_future_complete(future, timeout))
* {
* client->remove_pending_request(future);
* // handle timeout
118 changes: 47 additions & 71 deletions rclcpp/include/rclcpp/executor.hpp
Original file line number Diff line number Diff line change
@@ -25,7 +25,6 @@
#include <memory>
#include <mutex>
#include <string>
#include <type_traits>
#include <vector>

#include "rcl/guard_condition.h"
@@ -351,37 +350,61 @@ class Executor
virtual void
spin_once(std::chrono::nanoseconds timeout = std::chrono::nanoseconds(-1));

/// Spin (blocking) until the condition is complete, it times out waiting,
/// or rclcpp is interrupted.
/// Spin (blocking) until the condition is complete, it times out waiting, or rclcpp is
/// interrupted.
/**
* \param[in] future The condition which can be callable or future type to wait on.
* If this function returns SUCCESS, the future can be
* accessed without blocking (though it may still throw an exception).
* \param[in] condition The callable condition to wait on.
* \param[in] timeout Optional timeout parameter, which gets passed to Executor::spin_node_once.
* `-1` is block forever, `0` is non-blocking.
* If the time spent inside the blocking loop exceeds this timeout, return a TIMEOUT return
* code.
* \return The return code, one of `SUCCESS`, `INTERRUPTED`, or `TIMEOUT`.
*/
template<typename ConditionT, typename DurationT = std::chrono::milliseconds>
template<typename DurationT = std::chrono::milliseconds>
FutureReturnCode
spin_until_complete(
const ConditionT & condition,
const std::function<bool(void)> condition,
DurationT timeout = DurationT(-1))
{
if constexpr (std::is_invocable_v<ConditionT>) {
using RetT = std::invoke_result_t<ConditionT>;
static_assert(
std::is_same_v<bool, RetT>,
"Conditional callable has to return boolean type");
return spin_until_complete_impl(condition, timeout);
} else {
auto check_future = [&condition]() {
return condition.wait_for(std::chrono::seconds(0)) ==
std::future_status::ready;
};
return spin_until_complete_impl(check_future, timeout);
auto end_time = std::chrono::steady_clock::now();
std::chrono::nanoseconds timeout_ns = std::chrono::duration_cast<std::chrono::nanoseconds>(
timeout);
if (timeout_ns > std::chrono::nanoseconds::zero()) {
end_time += timeout_ns;
}
std::chrono::nanoseconds timeout_left = timeout_ns;

// Preliminary check, finish if condition is done already.
if (condition()) {
return FutureReturnCode::SUCCESS;
}

if (spinning.exchange(true)) {
throw std::runtime_error("spin_until_complete() called while already spinning");
}
RCPPUTILS_SCOPE_EXIT(this->spinning.store(false); );
while (rclcpp::ok(this->context_) && spinning.load()) {
// Do one item of work.
spin_once_impl(timeout_left);

if (condition()) {
return FutureReturnCode::SUCCESS;
}
// If the original timeout is < 0, then this is blocking, never TIMEOUT.
if (timeout_ns < std::chrono::nanoseconds::zero()) {
continue;
}
// Otherwise check if we still have time to wait, return TIMEOUT if not.
auto now = std::chrono::steady_clock::now();
if (now >= end_time) {
return FutureReturnCode::TIMEOUT;
}
// Subtract the elapsed time from the original timeout.
timeout_left = std::chrono::duration_cast<std::chrono::nanoseconds>(end_time - now);
}

// The condition did not pass before ok() returned false, return INTERRUPTED.
return FutureReturnCode::INTERRUPTED;
}

/// Spin (blocking) for at least the given amount of duration.
@@ -407,13 +430,15 @@ class Executor
* \return The return code, one of `SUCCESS`, `INTERRUPTED`, or `TIMEOUT`.
*/
template<typename FutureT, typename TimeRepT = int64_t, typename TimeT = std::milli>
[[deprecated("use spin_until_complete(const ConditionT & condition, DurationT timeout) instead")]]
FutureReturnCode
spin_until_future_complete(
const FutureT & future,
std::chrono::duration<TimeRepT, TimeT> timeout = std::chrono::duration<TimeRepT, TimeT>(-1))
{
return spin_until_complete(future, timeout);
const auto condition = [&future]() {
return future.wait_for(std::chrono::seconds(0)) == std::future_status::ready;
};
return spin_until_complete(condition, timeout);
}

/// Cancel any running spin* function, causing it to return.
@@ -572,55 +597,6 @@ class Executor
virtual void
spin_once_impl(std::chrono::nanoseconds timeout);

protected:
// Implementation details, used by spin_until_complete and spin_for.
// Previous implementation of spin_until_future_complete.
template<typename ConditionT, typename DurationT>
FutureReturnCode
spin_until_complete_impl(ConditionT condition, DurationT timeout)
{
auto end_time = std::chrono::steady_clock::now();
std::chrono::nanoseconds timeout_ns = std::chrono::duration_cast<std::chrono::nanoseconds>(
timeout);
if (timeout_ns > std::chrono::nanoseconds::zero()) {
end_time += timeout_ns;
}
std::chrono::nanoseconds timeout_left = timeout_ns;

// Preliminary check, finish if conditon is done already.
if (condition()) {
return FutureReturnCode::SUCCESS;
}

if (spinning.exchange(true)) {
throw std::runtime_error("spin_until_complete() called while already spinning");
}
RCPPUTILS_SCOPE_EXIT(this->spinning.store(false); );
while (rclcpp::ok(this->context_) && spinning.load()) {
// Do one item of work.
spin_once_impl(timeout_left);

if (condition()) {
return FutureReturnCode::SUCCESS;
}
// If the original timeout is < 0, then this is blocking, never TIMEOUT.
if (timeout_ns < std::chrono::nanoseconds::zero()) {
continue;
}
// Otherwise check if we still have time to wait, return TIMEOUT if not.
auto now = std::chrono::steady_clock::now();
if (now >= end_time) {
return FutureReturnCode::TIMEOUT;
}
// Subtract the elapsed time from the original timeout.
timeout_left = std::chrono::duration_cast<std::chrono::nanoseconds>(end_time - now);
}

// The condition did not pass before ok() returned false, return INTERRUPTED.
return FutureReturnCode::INTERRUPTED;
}

public:
/// Waitable containing guard conditions controlling the executor flow.
/**
* This waitable contains the interrupt and shutdown guard condition, as well
52 changes: 19 additions & 33 deletions rclcpp/include/rclcpp/executors.hpp
Original file line number Diff line number Diff line change
@@ -71,20 +71,19 @@ using rclcpp::executors::SingleThreadedExecutor;
/**
* \param[in] executor The executor which will spin the node.
* \param[in] node_ptr The node to spin.
* \param[in] condition The callable or future to wait on. If `SUCCESS`, the condition is safe to
* access after this function
* \param[in] condition The callable condition to wait on.
* \param[in] timeout Optional timeout parameter, which gets passed to
* Executor::spin_node_once.
* `-1` is block forever, `0` is non-blocking.
* If the time spent inside the blocking loop exceeds this timeout, return a `TIMEOUT` return code.
* \return The return code, one of `SUCCESS`, `INTERRUPTED`, or `TIMEOUT`.
*/
template<typename ConditionT, typename DurationT = std::chrono::milliseconds>
template<typename DurationT = std::chrono::milliseconds>
rclcpp::FutureReturnCode
spin_node_until_complete(
rclcpp::Executor & executor,
rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr,
const ConditionT & condition,
const std::function<bool(void)> condition,
DurationT timeout = DurationT(-1))
{
// TODO(wjwwood): does not work recursively; can't call spin_node_until_complete
@@ -95,13 +94,12 @@ spin_node_until_complete(
return retcode;
}

template<typename NodeT = rclcpp::Node, typename ConditionT,
typename DurationT = std::chrono::milliseconds>
template<typename NodeT = rclcpp::Node, typename DurationT = std::chrono::milliseconds>
rclcpp::FutureReturnCode
spin_node_until_complete(
rclcpp::Executor & executor,
std::shared_ptr<NodeT> node_ptr,
const ConditionT & condition,
const std::function<bool(void)> & condition,
DurationT timeout = DurationT(-1))
{
return rclcpp::executors::spin_node_until_complete(
@@ -124,34 +122,31 @@ spin_node_until_complete(
* \return The return code, one of `SUCCESS`, `INTERRUPTED`, or `TIMEOUT`.
*/
template<typename FutureT, typename TimeRepT = int64_t, typename TimeT = std::milli>
[[deprecated(
"use spin_node_until_complete(Executor &, node_interfaces::NodeBaseInterface::SharedPtr, "
"const ConditionT &, DurationT) instead"
)]]
rclcpp::FutureReturnCode
spin_node_until_future_complete(
rclcpp::Executor & executor,
rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr,
const FutureT & future,
std::chrono::duration<TimeRepT, TimeT> timeout = std::chrono::duration<TimeRepT, TimeT>(-1))
{
return spin_until_complete(executor, node_ptr, future, timeout);
// TODO(wjwwood): does not work recursively; can't call spin_node_until_future_complete
// inside a callback executed by an executor.
executor.add_node(node_ptr);
auto retcode = executor.spin_until_future_complete(future, timeout);
executor.remove_node(node_ptr);
return retcode;
}

template<typename NodeT = rclcpp::Node, typename FutureT, typename TimeRepT = int64_t,
typename TimeT = std::milli>
[[deprecated(
"use spin_node_until_complete(Executor &, std::shared_ptr<NodeT>, "
"const ConditionT &, DurationT) instead"
)]]
rclcpp::FutureReturnCode
spin_node_until_future_complete(
rclcpp::Executor & executor,
std::shared_ptr<NodeT> node_ptr,
const FutureT & future,
std::chrono::duration<TimeRepT, TimeT> timeout = std::chrono::duration<TimeRepT, TimeT>(-1))
{
return rclcpp::executors::spin_node_until_complete(
return rclcpp::executors::spin_node_until_future_complete(
executor,
node_ptr->get_node_base_interface(),
future,
@@ -160,56 +155,47 @@ spin_node_until_future_complete(

} // namespace executors

template<typename ConditionT, typename DurationT = std::chrono::milliseconds>
template<typename DurationT = std::chrono::milliseconds>
rclcpp::FutureReturnCode
spin_until_complete(
rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr,
const ConditionT & condition,
const std::function<bool(void)> condition,
DurationT timeout = DurationT(-1))
{
rclcpp::executors::SingleThreadedExecutor executor;
return executors::spin_node_until_complete<ConditionT>(executor, node_ptr, condition, timeout);
return executors::spin_node_until_complete(executor, node_ptr, condition, timeout);
}

template<typename NodeT = rclcpp::Node, typename ConditionT,
typename DurationT = std::chrono::milliseconds>
template<typename NodeT = rclcpp::Node, typename DurationT = std::chrono::milliseconds>
rclcpp::FutureReturnCode
spin_until_complete(
std::shared_ptr<NodeT> node_ptr,
const ConditionT & condition,
const std::function<bool(void)> condition,
DurationT timeout = DurationT(-1))
{
return rclcpp::spin_until_complete(node_ptr->get_node_base_interface(), condition, timeout);
}

template<typename FutureT, typename TimeRepT = int64_t, typename TimeT = std::milli>
[[deprecated(
"use spin_until_complete(node_interfaces::NodeBaseInterface::SharedPtr, "
"const ConditionT &,DurationT) instead"
)]]
rclcpp::FutureReturnCode
spin_until_future_complete(
rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr,
const FutureT & future,
std::chrono::duration<TimeRepT, TimeT> timeout = std::chrono::duration<TimeRepT, TimeT>(-1))
{
rclcpp::executors::SingleThreadedExecutor executor;
return executors::spin_node_until_complete<FutureT>(executor, node_ptr, future, timeout);
return executors::spin_node_until_future_complete<FutureT>(executor, node_ptr, future, timeout);
}

template<typename NodeT = rclcpp::Node, typename FutureT, typename TimeRepT = int64_t,
typename TimeT = std::milli>
[[deprecated(
"use spin_until_complete(std::shared_ptr<NodeT>, const ConditionT &, "
"DurationT) instead"
)]]
rclcpp::FutureReturnCode
spin_until_future_complete(
std::shared_ptr<NodeT> node_ptr,
const FutureT & future,
std::chrono::duration<TimeRepT, TimeT> timeout = std::chrono::duration<TimeRepT, TimeT>(-1))
{
return rclcpp::spin_until_complete(node_ptr->get_node_base_interface(), future, timeout);
return rclcpp::spin_until_future_complete(node_ptr->get_node_base_interface(), future, timeout);
}

} // namespace rclcpp
2 changes: 1 addition & 1 deletion rclcpp/include/rclcpp/future_return_code.hpp
Original file line number Diff line number Diff line change
@@ -23,7 +23,7 @@
namespace rclcpp
{

/// Return codes to be used with spin_until_complete.
/// Return codes to be used with spin_until_future_complete.
/**
* SUCCESS: The future is complete and can be accessed with "get" without blocking.
* This does not indicate that the operation succeeded; "get" may still throw an exception.
6 changes: 3 additions & 3 deletions rclcpp/include/rclcpp/generic_client.hpp
Original file line number Diff line number Diff line change
@@ -100,11 +100,11 @@ class GenericClient : public ClientBase
/// Send a request to the service server.
/**
* This method returns a `FutureAndRequestId` instance
* that can be passed to Executor::spin_until_complete() to
* that can be passed to Executor::spin_until_future_complete() to
* wait until it has been completed.
*
* If the future never completes,
* e.g. the call to Executor::spin_until_complete() times out,
* e.g. the call to Executor::spin_until_future_complete() times out,
* GenericClient::remove_pending_request() must be called to clean the client internal state.
* Not doing so will make the `Client` instance to use more memory each time a response is not
* received from the service server.
@@ -113,7 +113,7 @@ class GenericClient : public ClientBase
* auto future = client->async_send_request(my_request);
* if (
* rclcpp::FutureReturnCode::TIMEOUT ==
* executor->spin_until_complete(future, timeout))
* executor->spin_until_future_complete(future, timeout))
* {
* client->remove_pending_request(future);
* // handle timeout
1 change: 1 addition & 0 deletions rclcpp/include/rclcpp/rclcpp.hpp
Original file line number Diff line number Diff line change
@@ -70,6 +70,7 @@
* - rclcpp::spin()
* - rclcpp::spin_some()
* - rclcpp::spin_until_complete()
* - rclcpp::spin_until_future_complete()
* - rclcpp::executors::SingleThreadedExecutor
* - rclcpp::executors::SingleThreadedExecutor::add_node()
* - rclcpp::executors::SingleThreadedExecutor::spin()
Loading