From 32cbe4bbf8359425a79d0f0f9095f96902b47162 Mon Sep 17 00:00:00 2001 From: Vladimir Ermakov Date: Wed, 6 Aug 2014 00:36:26 +0400 Subject: [PATCH] plugins: C++11 chrono want time by ref, return *_DT Fix #80. --- src/plugins/command.cpp | 7 +++++-- src/plugins/param.cpp | 4 +++- src/plugins/waypoint.cpp | 6 ++++-- 3 files changed, 12 insertions(+), 5 deletions(-) diff --git a/src/plugins/command.cpp b/src/plugins/command.cpp index 1ac3b0fa4..7624f9208 100644 --- a/src/plugins/command.cpp +++ b/src/plugins/command.cpp @@ -58,7 +58,8 @@ class CommandTransaction { class CommandPlugin : public MavRosPlugin { public: CommandPlugin() : - uas(nullptr) + uas(nullptr), + ACK_TIMEOUT_DT(ACK_TIMEOUT_MS / 1000.0) { }; void initialize(UAS &uas_, @@ -114,12 +115,14 @@ class CommandPlugin : public MavRosPlugin { std::list ack_waiting_list; static constexpr int ACK_TIMEOUT_MS = 5000; + const ros::Duration ACK_TIMEOUT_DT; + /* -*- mid-level functions -*- */ bool wait_ack_for(CommandTransaction *tr) { std::unique_lock lock(tr->cond_mutex); - return tr->ack.wait_for(lock, std::chrono::milliseconds(ACK_TIMEOUT_MS)) + return tr->ack.wait_for(lock, std::chrono::nanoseconds(ACK_TIMEOUT_DT.toNSec())) == std::cv_status::no_timeout; } diff --git a/src/plugins/param.cpp b/src/plugins/param.cpp index c98860d60..3af3fc508 100644 --- a/src/plugins/param.cpp +++ b/src/plugins/param.cpp @@ -344,6 +344,7 @@ class ParamPlugin : public MavRosPlugin { is_timedout(false), param_rx_retries(RETRIES_COUNT), BOOTUP_TIME_DT(BOOTUP_TIME_MS / 1000.0), + LIST_TIMEOUT_DT(LIST_TIMEOUT_MS / 1000.0), PARAM_TIMEOUT_DT(PARAM_TIMEOUT_MS / 1000.0) { }; @@ -480,6 +481,7 @@ class ParamPlugin : public MavRosPlugin { static constexpr int RETRIES_COUNT = 3; const ros::Duration BOOTUP_TIME_DT; + const ros::Duration LIST_TIMEOUT_DT; const ros::Duration PARAM_TIMEOUT_DT; std::map parameters; @@ -684,7 +686,7 @@ class ParamPlugin : public MavRosPlugin { bool wait_fetch_all() { std::unique_lock lock(list_cond_mutex); - return list_receiving.wait_for(lock, std::chrono::milliseconds(LIST_TIMEOUT_MS)) + return list_receiving.wait_for(lock, std::chrono::nanoseconds(LIST_TIMEOUT_DT.toNSec())) == std::cv_status::no_timeout && !is_timedout; } diff --git a/src/plugins/waypoint.cpp b/src/plugins/waypoint.cpp index f9e7f064f..ab27468af 100644 --- a/src/plugins/waypoint.cpp +++ b/src/plugins/waypoint.cpp @@ -156,6 +156,7 @@ class WaypointPlugin : public MavRosPlugin { do_pull_after_gcs(false), reshedule_pull(false), BOOTUP_TIME_DT(BOOTUP_TIME_MS / 1000.0), + LIST_TIMEOUT_DT(LIST_TIMEOUT_MS / 1000.0), WP_TIMEOUT_DT(WP_TIMEOUT_MS/ 1000.0), RESHEDULE_DT(RESHEDULE_MS / 1000.0) { }; @@ -287,6 +288,7 @@ class WaypointPlugin : public MavRosPlugin { static constexpr int RETRIES_COUNT = 3; const ros::Duration BOOTUP_TIME_DT; + const ros::Duration LIST_TIMEOUT_DT; const ros::Duration WP_TIMEOUT_DT; const ros::Duration RESHEDULE_DT; @@ -615,7 +617,7 @@ class WaypointPlugin : public MavRosPlugin { bool wait_fetch_all() { std::unique_lock lock(recv_cond_mutex); - return list_receiving.wait_for(lock, std::chrono::milliseconds(LIST_TIMEOUT_MS)) + return list_receiving.wait_for(lock, std::chrono::nanoseconds(LIST_TIMEOUT_DT.toNSec())) == std::cv_status::no_timeout && !is_timedout; } @@ -623,7 +625,7 @@ class WaypointPlugin : public MavRosPlugin { bool wait_push_all() { std::unique_lock lock(send_cond_mutex); - return list_sending.wait_for(lock, std::chrono::milliseconds(LIST_TIMEOUT_MS)) + return list_sending.wait_for(lock, std::chrono::nanoseconds(LIST_TIMEOUT_DT.toNSec())) == std::cv_status::no_timeout && !is_timedout; }