From 0c826497f1a8184a68c1a2fa69823b5feef8d598 Mon Sep 17 00:00:00 2001 From: William Woodall Date: Thu, 3 Mar 2016 18:13:46 -0800 Subject: [PATCH 1/3] convert enum to enum class and provide to_string --- rclcpp/include/rclcpp/executor.hpp | 9 ++++++++- rclcpp/src/rclcpp/executor.cpp | 25 +++++++++++++++++++++++++ 2 files changed, 33 insertions(+), 1 deletion(-) diff --git a/rclcpp/include/rclcpp/executor.hpp b/rclcpp/include/rclcpp/executor.hpp index e7f4e4e..2c22333 100644 --- a/rclcpp/include/rclcpp/executor.hpp +++ b/rclcpp/include/rclcpp/executor.hpp @@ -17,6 +17,7 @@ #include #include +#include #include #include #include @@ -42,7 +43,13 @@ namespace executor * INTERRUPTED: The future is not complete, spinning was interrupted by Ctrl-C or another error. * TIMEOUT: Spinning timed out. */ -enum FutureReturnCode {SUCCESS, INTERRUPTED, TIMEOUT}; +enum class FutureReturnCode {SUCCESS, INTERRUPTED, TIMEOUT}; + +std::ostream & +operator << (std::ostream & os, const FutureReturnCode & future_return_code); + +std::string +to_string(const FutureReturnCode & future_return_code); /// /** diff --git a/rclcpp/src/rclcpp/executor.cpp b/rclcpp/src/rclcpp/executor.cpp index 3be7e7d..19bb620 100644 --- a/rclcpp/src/rclcpp/executor.cpp +++ b/rclcpp/src/rclcpp/executor.cpp @@ -12,6 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include + #include "rclcpp/executor.hpp" #include "rclcpp/scope_exit.hpp" @@ -20,6 +22,7 @@ using rclcpp::executor::AnyExecutable; using rclcpp::executor::Executor; using rclcpp::executor::ExecutorArgs; +using rclcpp::executor::FutureReturnCode; Executor::Executor(const ExecutorArgs & args) : spinning(false), @@ -569,3 +572,25 @@ Executor::get_next_executable(std::chrono::nanoseconds timeout) } return any_exec; } + +std::ostream & +rclcpp::executor::operator << (std::ostream & os, const FutureReturnCode & future_return_code) +{ + return os << to_string(future_return_code); +} + +std::string +rclcpp::executor::to_string(const FutureReturnCode & future_return_code) +{ + using enum_type = std::underlying_type::type; + using std::string; + using std::to_string; + switch (future_return_code) { + case FutureReturnCode::SUCCESS: + return string("SUCCESS (" + to_string(static_cast(future_return_code)) + ")"); + case FutureReturnCode::INTERRUPTED: + return string("INTERRUPTED (" + to_string(static_cast(future_return_code)) + ")"); + case FutureReturnCode::TIMEOUT: + return string("TIMEOUT (" + to_string(static_cast(future_return_code)) + ")"); + } +} From e8f9344015317abe0bd888e36a6bf2e9a3c18e10 Mon Sep 17 00:00:00 2001 From: William Woodall Date: Thu, 3 Mar 2016 18:14:18 -0800 Subject: [PATCH 2/3] refactor executor.spin_until_future_complete --- rclcpp/include/rclcpp/executor.hpp | 35 ++++++++++++++++++------------ 1 file changed, 21 insertions(+), 14 deletions(-) diff --git a/rclcpp/include/rclcpp/executor.hpp b/rclcpp/include/rclcpp/executor.hpp index 2c22333..cc1b51c 100644 --- a/rclcpp/include/rclcpp/executor.hpp +++ b/rclcpp/include/rclcpp/executor.hpp @@ -184,23 +184,30 @@ public: // Check the future before entering the while loop. // If the future is already complete, don't try to spin. std::future_status status = future.wait_for(std::chrono::seconds(0)); - - auto start_time = std::chrono::system_clock::now(); - - while (status != std::future_status::ready && rclcpp::utilities::ok()) { - spin_once(timeout); - if (timeout.count() >= 0) { - if (start_time + timeout < std::chrono::system_clock::now()) { - return TIMEOUT; - } - } - status = future.wait_for(std::chrono::seconds(0)); - } - - // If the future completed, and we weren't interrupted by ctrl-C, return the response if (status == std::future_status::ready) { return FutureReturnCode::SUCCESS; } + + auto start_time = std::chrono::steady_clock::now(); + + while (rclcpp::utilities::ok()) { + // Do one item of work. + spin_once(timeout); + // Check if the future is set, return SUCCESS if it is. + status = future.wait_for(std::chrono::seconds(0)); + if (status == std::future_status::ready) { + return FutureReturnCode::SUCCESS; + } + // Otherwise check if we still have time to wait, return TIMEOUT if not. + auto elapsed_time = std::chrono::steady_clock::now() - start_time; + if (elapsed_time > timeout) { + return FutureReturnCode::TIMEOUT; + } + // Subtract the elapsed time from the original timeout. + timeout -= std::chrono::duration_cast>(elapsed_time); + } + + // The future did not complete before ok() returned false, return INTERRUPTED. return FutureReturnCode::INTERRUPTED; } From 82139f1a12c29f84211b7119268fc546eaae505d Mon Sep 17 00:00:00 2001 From: William Woodall Date: Fri, 4 Mar 2016 16:37:42 -0800 Subject: [PATCH 3/3] refactor and test spin_until_future_complete --- rclcpp/include/rclcpp/executor.hpp | 27 ++++++++++++++++++++------- rclcpp/src/rclcpp/executor.cpp | 17 +++++++++++------ 2 files changed, 31 insertions(+), 13 deletions(-) diff --git a/rclcpp/include/rclcpp/executor.hpp b/rclcpp/include/rclcpp/executor.hpp index cc1b51c..ae2dbdb 100644 --- a/rclcpp/include/rclcpp/executor.hpp +++ b/rclcpp/include/rclcpp/executor.hpp @@ -22,6 +22,7 @@ #include #include #include +#include #include #include "rclcpp/any_executable.hpp" @@ -45,9 +46,11 @@ namespace executor */ enum class FutureReturnCode {SUCCESS, INTERRUPTED, TIMEOUT}; +RCLCPP_PUBLIC std::ostream & -operator << (std::ostream & os, const FutureReturnCode & future_return_code); +operator<<(std::ostream & os, const FutureReturnCode & future_return_code); +RCLCPP_PUBLIC std::string to_string(const FutureReturnCode & future_return_code); @@ -166,7 +169,8 @@ public: /** * \param[in] executor The executor which will spin the node. * \param[in] node_ptr The node to spin. - * \param[in] future The future to wait on. If SUCCESS, the future is safe to access after this function + * \param[in] future The future to wait on. If SUCCESS, the future is safe to access after this + function. * \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. @@ -188,23 +192,32 @@ public: return FutureReturnCode::SUCCESS; } - auto start_time = std::chrono::steady_clock::now(); + auto end_time = std::chrono::steady_clock::now(); + if (timeout > std::chrono::nanoseconds::zero()) { + end_time += timeout; + } + auto timeout_left = timeout; while (rclcpp::utilities::ok()) { // Do one item of work. - spin_once(timeout); + spin_once(timeout_left); // Check if the future is set, return SUCCESS if it is. status = future.wait_for(std::chrono::seconds(0)); if (status == std::future_status::ready) { return FutureReturnCode::SUCCESS; } + // If the original timeout is < 0, then this is blocking, never TIMEOUT. + if (timeout < std::chrono::nanoseconds::zero()) { + continue; + } // Otherwise check if we still have time to wait, return TIMEOUT if not. - auto elapsed_time = std::chrono::steady_clock::now() - start_time; - if (elapsed_time > timeout) { + auto now = std::chrono::steady_clock::now(); + if (now >= end_time) { return FutureReturnCode::TIMEOUT; } // Subtract the elapsed time from the original timeout. - timeout -= std::chrono::duration_cast>(elapsed_time); + using duration_type = std::chrono::duration; + timeout_left = std::chrono::duration_cast(end_time - now); } // The future did not complete before ok() returned false, return INTERRUPTED. diff --git a/rclcpp/src/rclcpp/executor.cpp b/rclcpp/src/rclcpp/executor.cpp index 19bb620..e734f48 100644 --- a/rclcpp/src/rclcpp/executor.cpp +++ b/rclcpp/src/rclcpp/executor.cpp @@ -12,6 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include #include #include "rclcpp/executor.hpp" @@ -574,7 +575,7 @@ Executor::get_next_executable(std::chrono::nanoseconds timeout) } std::ostream & -rclcpp::executor::operator << (std::ostream & os, const FutureReturnCode & future_return_code) +rclcpp::executor::operator<<(std::ostream & os, const FutureReturnCode & future_return_code) { return os << to_string(future_return_code); } @@ -583,14 +584,18 @@ std::string rclcpp::executor::to_string(const FutureReturnCode & future_return_code) { using enum_type = std::underlying_type::type; - using std::string; - using std::to_string; + std::string prefix = "Unknown enum value ("; + std::string ret_as_string = std::to_string(static_cast(future_return_code)); switch (future_return_code) { case FutureReturnCode::SUCCESS: - return string("SUCCESS (" + to_string(static_cast(future_return_code)) + ")"); + prefix = "SUCCESS ("; + break; case FutureReturnCode::INTERRUPTED: - return string("INTERRUPTED (" + to_string(static_cast(future_return_code)) + ")"); + prefix = "INTERRUPTED ("; + break; case FutureReturnCode::TIMEOUT: - return string("TIMEOUT (" + to_string(static_cast(future_return_code)) + ")"); + prefix = "TIMEOUT ("; + break; } + return prefix + ret_as_string + ")"; }