diff --git a/src/message_queue.cpp b/src/message_queue.cpp index b1569b0..ce03b73 100644 --- a/src/message_queue.cpp +++ b/src/message_queue.cpp @@ -4,16 +4,16 @@ #include "message_queue.hpp" -void MessageQueue::enqueue(const std::vector &command) { +void MessageQueue::enqueue(const std::vector &command) { std::lock_guard lock(mutex_); queue_.push(command); cond_.notify_one(); } -std::vector MessageQueue::dequeue() { +std::vector MessageQueue::dequeue() { std::unique_lock lock(mutex_); cond_.wait(lock, [this] { return !queue_.empty(); }); - std::vector msg = queue_.front(); + std::vector msg = queue_.front(); queue_.pop(); return msg; } diff --git a/src/message_queue.cpp b/src/message_queue.cpp index b1569b0..ce03b73 100644 --- a/src/message_queue.cpp +++ b/src/message_queue.cpp @@ -4,16 +4,16 @@ #include "message_queue.hpp" -void MessageQueue::enqueue(const std::vector &command) { +void MessageQueue::enqueue(const std::vector &command) { std::lock_guard lock(mutex_); queue_.push(command); cond_.notify_one(); } -std::vector MessageQueue::dequeue() { +std::vector MessageQueue::dequeue() { std::unique_lock lock(mutex_); cond_.wait(lock, [this] { return !queue_.empty(); }); - std::vector msg = queue_.front(); + std::vector msg = queue_.front(); queue_.pop(); return msg; } diff --git a/src/message_queue.hpp b/src/message_queue.hpp index 4538ffd..69075dc 100644 --- a/src/message_queue.hpp +++ b/src/message_queue.hpp @@ -11,12 +11,12 @@ class MessageQueue { public: - void enqueue(const std::vector &command); + void enqueue(const std::vector &command); - std::vector dequeue(); + std::vector dequeue(); private: - std::queue > queue_; + std::queue > queue_; std::mutex mutex_; std::condition_variable cond_; }; diff --git a/src/message_queue.cpp b/src/message_queue.cpp index b1569b0..ce03b73 100644 --- a/src/message_queue.cpp +++ b/src/message_queue.cpp @@ -4,16 +4,16 @@ #include "message_queue.hpp" -void MessageQueue::enqueue(const std::vector &command) { +void MessageQueue::enqueue(const std::vector &command) { std::lock_guard lock(mutex_); queue_.push(command); cond_.notify_one(); } -std::vector MessageQueue::dequeue() { +std::vector MessageQueue::dequeue() { std::unique_lock lock(mutex_); cond_.wait(lock, [this] { return !queue_.empty(); }); - std::vector msg = queue_.front(); + std::vector msg = queue_.front(); queue_.pop(); return msg; } diff --git a/src/message_queue.hpp b/src/message_queue.hpp index 4538ffd..69075dc 100644 --- a/src/message_queue.hpp +++ b/src/message_queue.hpp @@ -11,12 +11,12 @@ class MessageQueue { public: - void enqueue(const std::vector &command); + void enqueue(const std::vector &command); - std::vector dequeue(); + std::vector dequeue(); private: - std::queue > queue_; + std::queue > queue_; std::mutex mutex_; std::condition_variable cond_; }; diff --git a/src/slam_wrapper.cpp b/src/slam_wrapper.cpp index 281dea3..0724569 100644 --- a/src/slam_wrapper.cpp +++ b/src/slam_wrapper.cpp @@ -69,28 +69,23 @@ //TODO 控制机械臂转动到预设的角度 g_messageQueue.enqueue({ - static_cast(0xFE), static_cast(0xFE), 0x0F, 0x3C, - 0x0b, 0x71, 0x08, 0x25, 0x06, 0xfb, - 0x08, 0x2e, 0x0a, 0xcb, 0x08, 0x28, + 0xFE, 0xFE, 0x0F, 0x3C, + 0x0b, 0x71, 0x08, 0x25, 0x06, 0xFB, + 0x08, 0x2E, 0x0A, 0xCB, 0x08, 0x28, 0x14, 0xFA }); //倒计时结束后恢复巡检 - // std::thread countdown_thread(count_down, 10); - // countdown_thread.join(); + std::thread countdown_thread([this] { + std::this_thread::sleep_for(std::chrono::seconds(10)); + std::cout << "倒计时结束,恢复巡检"; + recover_navigation(); + }); + countdown_thread.detach(); } } } -void SlamWrapper::count_down(std::atomic &seconds) { - // while (seconds) { - // std::this_thread::sleep_for(std::chrono::seconds(1)); - // --seconds; - // } - // std::cout << "count down end, recover_navigation"; - // recover_navigation(); -} - void SlamWrapper::odometer_handler(const void *message) { currentOdom = static_cast(message); } diff --git a/src/message_queue.cpp b/src/message_queue.cpp index b1569b0..ce03b73 100644 --- a/src/message_queue.cpp +++ b/src/message_queue.cpp @@ -4,16 +4,16 @@ #include "message_queue.hpp" -void MessageQueue::enqueue(const std::vector &command) { +void MessageQueue::enqueue(const std::vector &command) { std::lock_guard lock(mutex_); queue_.push(command); cond_.notify_one(); } -std::vector MessageQueue::dequeue() { +std::vector MessageQueue::dequeue() { std::unique_lock lock(mutex_); cond_.wait(lock, [this] { return !queue_.empty(); }); - std::vector msg = queue_.front(); + std::vector msg = queue_.front(); queue_.pop(); return msg; } diff --git a/src/message_queue.hpp b/src/message_queue.hpp index 4538ffd..69075dc 100644 --- a/src/message_queue.hpp +++ b/src/message_queue.hpp @@ -11,12 +11,12 @@ class MessageQueue { public: - void enqueue(const std::vector &command); + void enqueue(const std::vector &command); - std::vector dequeue(); + std::vector dequeue(); private: - std::queue > queue_; + std::queue > queue_; std::mutex mutex_; std::condition_variable cond_; }; diff --git a/src/slam_wrapper.cpp b/src/slam_wrapper.cpp index 281dea3..0724569 100644 --- a/src/slam_wrapper.cpp +++ b/src/slam_wrapper.cpp @@ -69,28 +69,23 @@ //TODO 控制机械臂转动到预设的角度 g_messageQueue.enqueue({ - static_cast(0xFE), static_cast(0xFE), 0x0F, 0x3C, - 0x0b, 0x71, 0x08, 0x25, 0x06, 0xfb, - 0x08, 0x2e, 0x0a, 0xcb, 0x08, 0x28, + 0xFE, 0xFE, 0x0F, 0x3C, + 0x0b, 0x71, 0x08, 0x25, 0x06, 0xFB, + 0x08, 0x2E, 0x0A, 0xCB, 0x08, 0x28, 0x14, 0xFA }); //倒计时结束后恢复巡检 - // std::thread countdown_thread(count_down, 10); - // countdown_thread.join(); + std::thread countdown_thread([this] { + std::this_thread::sleep_for(std::chrono::seconds(10)); + std::cout << "倒计时结束,恢复巡检"; + recover_navigation(); + }); + countdown_thread.detach(); } } } -void SlamWrapper::count_down(std::atomic &seconds) { - // while (seconds) { - // std::this_thread::sleep_for(std::chrono::seconds(1)); - // --seconds; - // } - // std::cout << "count down end, recover_navigation"; - // recover_navigation(); -} - void SlamWrapper::odometer_handler(const void *message) { currentOdom = static_cast(message); } diff --git a/src/slam_wrapper.hpp b/src/slam_wrapper.hpp index 2237c02..80fb1f6 100644 --- a/src/slam_wrapper.hpp +++ b/src/slam_wrapper.hpp @@ -103,8 +103,6 @@ void pause_navigation(); void recover_navigation(); - - void count_down(std::atomic &seconds); }; #endif //SLAM_WRAPPER_HPP