diff --git a/CMakeLists.txt b/CMakeLists.txt index b604b89..4ca6761 100755 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -12,7 +12,6 @@ add_executable(message_handler src/message_handler.cpp - src/message_queue.cpp src/serial_port_wrapper.cpp src/slam_wrapper.cpp src/tcp_client.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index b604b89..4ca6761 100755 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -12,7 +12,6 @@ add_executable(message_handler src/message_handler.cpp - src/message_queue.cpp src/serial_port_wrapper.cpp src/slam_wrapper.cpp src/tcp_client.cpp diff --git a/src/message_queue.cpp b/src/message_queue.cpp deleted file mode 100644 index ce03b73..0000000 --- a/src/message_queue.cpp +++ /dev/null @@ -1,19 +0,0 @@ -// -// Created by casic on 25-3-5. -// - -#include "message_queue.hpp" - -void MessageQueue::enqueue(const std::vector &command) { - std::lock_guard lock(mutex_); - queue_.push(command); - cond_.notify_one(); -} - -std::vector MessageQueue::dequeue() { - std::unique_lock lock(mutex_); - cond_.wait(lock, [this] { return !queue_.empty(); }); - std::vector msg = queue_.front(); - queue_.pop(); - return msg; -} diff --git a/CMakeLists.txt b/CMakeLists.txt index b604b89..4ca6761 100755 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -12,7 +12,6 @@ add_executable(message_handler src/message_handler.cpp - src/message_queue.cpp src/serial_port_wrapper.cpp src/slam_wrapper.cpp src/tcp_client.cpp diff --git a/src/message_queue.cpp b/src/message_queue.cpp deleted file mode 100644 index ce03b73..0000000 --- a/src/message_queue.cpp +++ /dev/null @@ -1,19 +0,0 @@ -// -// Created by casic on 25-3-5. -// - -#include "message_queue.hpp" - -void MessageQueue::enqueue(const std::vector &command) { - std::lock_guard lock(mutex_); - queue_.push(command); - cond_.notify_one(); -} - -std::vector MessageQueue::dequeue() { - std::unique_lock lock(mutex_); - cond_.wait(lock, [this] { return !queue_.empty(); }); - std::vector msg = queue_.front(); - queue_.pop(); - return msg; -} diff --git a/src/message_queue.hpp b/src/message_queue.hpp deleted file mode 100644 index 69075dc..0000000 --- a/src/message_queue.hpp +++ /dev/null @@ -1,25 +0,0 @@ -// -// Created by casic on 25-3-5. -// - -#ifndef MESSAGE_INTERFACE_HPP -#define MESSAGE_INTERFACE_HPP - -#include -#include -#include - -class MessageQueue { -public: - void enqueue(const std::vector &command); - - std::vector dequeue(); - -private: - std::queue > queue_; - std::mutex mutex_; - std::condition_variable cond_; -}; - - -#endif //MESSAGE_INTERFACE_HPP diff --git a/CMakeLists.txt b/CMakeLists.txt index b604b89..4ca6761 100755 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -12,7 +12,6 @@ add_executable(message_handler src/message_handler.cpp - src/message_queue.cpp src/serial_port_wrapper.cpp src/slam_wrapper.cpp src/tcp_client.cpp diff --git a/src/message_queue.cpp b/src/message_queue.cpp deleted file mode 100644 index ce03b73..0000000 --- a/src/message_queue.cpp +++ /dev/null @@ -1,19 +0,0 @@ -// -// Created by casic on 25-3-5. -// - -#include "message_queue.hpp" - -void MessageQueue::enqueue(const std::vector &command) { - std::lock_guard lock(mutex_); - queue_.push(command); - cond_.notify_one(); -} - -std::vector MessageQueue::dequeue() { - std::unique_lock lock(mutex_); - cond_.wait(lock, [this] { return !queue_.empty(); }); - std::vector msg = queue_.front(); - queue_.pop(); - return msg; -} diff --git a/src/message_queue.hpp b/src/message_queue.hpp deleted file mode 100644 index 69075dc..0000000 --- a/src/message_queue.hpp +++ /dev/null @@ -1,25 +0,0 @@ -// -// Created by casic on 25-3-5. -// - -#ifndef MESSAGE_INTERFACE_HPP -#define MESSAGE_INTERFACE_HPP - -#include -#include -#include - -class MessageQueue { -public: - void enqueue(const std::vector &command); - - std::vector dequeue(); - -private: - std::queue > queue_; - std::mutex mutex_; - std::condition_variable cond_; -}; - - -#endif //MESSAGE_INTERFACE_HPP diff --git a/src/serial_port_wrapper.cpp b/src/serial_port_wrapper.cpp index 277102a..2ce65d6 100644 --- a/src/serial_port_wrapper.cpp +++ b/src/serial_port_wrapper.cpp @@ -32,8 +32,6 @@ timer_.async_wait([this](const boost::system::error_code &error) { if (!error) { read_from_port(); - } else { - std::cerr << "Timer error: " << error.message() << std::endl; } }); } diff --git a/CMakeLists.txt b/CMakeLists.txt index b604b89..4ca6761 100755 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -12,7 +12,6 @@ add_executable(message_handler src/message_handler.cpp - src/message_queue.cpp src/serial_port_wrapper.cpp src/slam_wrapper.cpp src/tcp_client.cpp diff --git a/src/message_queue.cpp b/src/message_queue.cpp deleted file mode 100644 index ce03b73..0000000 --- a/src/message_queue.cpp +++ /dev/null @@ -1,19 +0,0 @@ -// -// Created by casic on 25-3-5. -// - -#include "message_queue.hpp" - -void MessageQueue::enqueue(const std::vector &command) { - std::lock_guard lock(mutex_); - queue_.push(command); - cond_.notify_one(); -} - -std::vector MessageQueue::dequeue() { - std::unique_lock lock(mutex_); - cond_.wait(lock, [this] { return !queue_.empty(); }); - std::vector msg = queue_.front(); - queue_.pop(); - return msg; -} diff --git a/src/message_queue.hpp b/src/message_queue.hpp deleted file mode 100644 index 69075dc..0000000 --- a/src/message_queue.hpp +++ /dev/null @@ -1,25 +0,0 @@ -// -// Created by casic on 25-3-5. -// - -#ifndef MESSAGE_INTERFACE_HPP -#define MESSAGE_INTERFACE_HPP - -#include -#include -#include - -class MessageQueue { -public: - void enqueue(const std::vector &command); - - std::vector dequeue(); - -private: - std::queue > queue_; - std::mutex mutex_; - std::condition_variable cond_; -}; - - -#endif //MESSAGE_INTERFACE_HPP diff --git a/src/serial_port_wrapper.cpp b/src/serial_port_wrapper.cpp index 277102a..2ce65d6 100644 --- a/src/serial_port_wrapper.cpp +++ b/src/serial_port_wrapper.cpp @@ -32,8 +32,6 @@ timer_.async_wait([this](const boost::system::error_code &error) { if (!error) { read_from_port(); - } else { - std::cerr << "Timer error: " << error.message() << std::endl; } }); } diff --git a/src/slam_wrapper.cpp b/src/slam_wrapper.cpp index 3157c63..8787f31 100644 --- a/src/slam_wrapper.cpp +++ b/src/slam_wrapper.cpp @@ -3,12 +3,9 @@ // #include "slam_wrapper.hpp" -#include "message_queue.hpp" #include #include -extern MessageQueue g_messageQueue; - SlamWrapper::SlamWrapper(const char *networkInterface) { ChannelFactory::Instance()->Init(0, networkInterface); @@ -57,17 +54,26 @@ pause_navigation(); //TODO 控制机械臂转动到预设的角度 - g_messageQueue.enqueue({ - 0xFE, 0xFE, 0x0F, 0x3C, - 0x0B, 0x71, 0x08, 0x25, 0x06, 0xFB, - 0x08, 0x2E, 0x0A, 0xCB, 0x08, 0x28, - 0x14, 0xFA - }); + //朝右下方 + // g_messageQueue.enqueue({ + // 0xFE, 0xFE, 0x0F, 0x3C, + // 0x0B, 0x71, 0x08, 0x25, 0x06, 0xFB, + // 0x08, 0x2E, 0x0A, 0xCB, 0x08, 0x28, + // 0x14, 0xFA + // }); //倒计时结束后恢复巡检 std::thread countdown_thread([this] { std::this_thread::sleep_for(std::chrono::seconds(10)); - std::cout << "倒计时结束，恢复巡检"; + std::cout << "倒计时结束，恢复机械臂初始位置，恢复巡检"; + //水平朝前方向 + // g_messageQueue.enqueue({ + // 0xFE, 0xFE, 0x0F, 0x3C, + // 0x08, 0x00, 0x08, 0x00, 0x08, 0x00, + // 0x08, 0x00, 0x08, 0x00, 0x08, 0x00, + // 0x14, 0xFA + // }); + recover_navigation(); }); countdown_thread.detach(); diff --git a/CMakeLists.txt b/CMakeLists.txt index b604b89..4ca6761 100755 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -12,7 +12,6 @@ add_executable(message_handler src/message_handler.cpp - src/message_queue.cpp src/serial_port_wrapper.cpp src/slam_wrapper.cpp src/tcp_client.cpp diff --git a/src/message_queue.cpp b/src/message_queue.cpp deleted file mode 100644 index ce03b73..0000000 --- a/src/message_queue.cpp +++ /dev/null @@ -1,19 +0,0 @@ -// -// Created by casic on 25-3-5. -// - -#include "message_queue.hpp" - -void MessageQueue::enqueue(const std::vector &command) { - std::lock_guard lock(mutex_); - queue_.push(command); - cond_.notify_one(); -} - -std::vector MessageQueue::dequeue() { - std::unique_lock lock(mutex_); - cond_.wait(lock, [this] { return !queue_.empty(); }); - std::vector msg = queue_.front(); - queue_.pop(); - return msg; -} diff --git a/src/message_queue.hpp b/src/message_queue.hpp deleted file mode 100644 index 69075dc..0000000 --- a/src/message_queue.hpp +++ /dev/null @@ -1,25 +0,0 @@ -// -// Created by casic on 25-3-5. -// - -#ifndef MESSAGE_INTERFACE_HPP -#define MESSAGE_INTERFACE_HPP - -#include -#include -#include - -class MessageQueue { -public: - void enqueue(const std::vector &command); - - std::vector dequeue(); - -private: - std::queue > queue_; - std::mutex mutex_; - std::condition_variable cond_; -}; - - -#endif //MESSAGE_INTERFACE_HPP diff --git a/src/serial_port_wrapper.cpp b/src/serial_port_wrapper.cpp index 277102a..2ce65d6 100644 --- a/src/serial_port_wrapper.cpp +++ b/src/serial_port_wrapper.cpp @@ -32,8 +32,6 @@ timer_.async_wait([this](const boost::system::error_code &error) { if (!error) { read_from_port(); - } else { - std::cerr << "Timer error: " << error.message() << std::endl; } }); } diff --git a/src/slam_wrapper.cpp b/src/slam_wrapper.cpp index 3157c63..8787f31 100644 --- a/src/slam_wrapper.cpp +++ b/src/slam_wrapper.cpp @@ -3,12 +3,9 @@ // #include "slam_wrapper.hpp" -#include "message_queue.hpp" #include #include -extern MessageQueue g_messageQueue; - SlamWrapper::SlamWrapper(const char *networkInterface) { ChannelFactory::Instance()->Init(0, networkInterface); @@ -57,17 +54,26 @@ pause_navigation(); //TODO 控制机械臂转动到预设的角度 - g_messageQueue.enqueue({ - 0xFE, 0xFE, 0x0F, 0x3C, - 0x0B, 0x71, 0x08, 0x25, 0x06, 0xFB, - 0x08, 0x2E, 0x0A, 0xCB, 0x08, 0x28, - 0x14, 0xFA - }); + //朝右下方 + // g_messageQueue.enqueue({ + // 0xFE, 0xFE, 0x0F, 0x3C, + // 0x0B, 0x71, 0x08, 0x25, 0x06, 0xFB, + // 0x08, 0x2E, 0x0A, 0xCB, 0x08, 0x28, + // 0x14, 0xFA + // }); //倒计时结束后恢复巡检 std::thread countdown_thread([this] { std::this_thread::sleep_for(std::chrono::seconds(10)); - std::cout << "倒计时结束，恢复巡检"; + std::cout << "倒计时结束，恢复机械臂初始位置，恢复巡检"; + //水平朝前方向 + // g_messageQueue.enqueue({ + // 0xFE, 0xFE, 0x0F, 0x3C, + // 0x08, 0x00, 0x08, 0x00, 0x08, 0x00, + // 0x08, 0x00, 0x08, 0x00, 0x08, 0x00, + // 0x14, 0xFA + // }); + recover_navigation(); }); countdown_thread.detach(); diff --git a/src/tcp_client.cpp b/src/tcp_client.cpp index 2d1e489..ac6f3c5 100644 --- a/src/tcp_client.cpp +++ b/src/tcp_client.cpp @@ -17,8 +17,30 @@ const std::string received_string(buffer.begin(), buffer.end()); std::cout << "handle_data >>>> " << received_string << std::endl; if (received_string == "q") { - std::cout << "Close all nodes" << std::endl; - _slam.close_all_node(); + // std::cout << "Close all nodes" << std::endl; + // _slam.close_all_node(); + + std::cout << "发送机械臂转动指令，倒计时开始，请等待10秒"; + _tcp_service->send_command({ + 0xFE, 0xFE, 0x0F, 0x3C, + 0x0B, 0x71, 0x08, 0x25, 0x06, 0xFB, + 0x08, 0x2E, 0x0A, 0xCB, 0x08, 0x28, + 0x14, 0xFA + }); + + //倒计时结束后恢复巡检 + std::thread countdown_thread([this] { + std::this_thread::sleep_for(std::chrono::seconds(10)); + std::cout << "倒计时结束，恢复机械臂初始位置，恢复巡检"; + //水平朝前方向 + _tcp_service->send_command({ + 0xFE, 0xFE, 0x0F, 0x3C, + 0x08, 0x00, 0x08, 0x00, 0x08, 0x00, + 0x08, 0x00, 0x08, 0x00, 0x08, 0x00, + 0x14, 0xFA + }); + }); + countdown_thread.detach(); } else if (received_string == "w") { std::cout << "Start mapping (default to clearing node/edge information)" << std::endl; _slam.delete_all_node(); diff --git a/CMakeLists.txt b/CMakeLists.txt index b604b89..4ca6761 100755 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -12,7 +12,6 @@ add_executable(message_handler src/message_handler.cpp - src/message_queue.cpp src/serial_port_wrapper.cpp src/slam_wrapper.cpp src/tcp_client.cpp diff --git a/src/message_queue.cpp b/src/message_queue.cpp deleted file mode 100644 index ce03b73..0000000 --- a/src/message_queue.cpp +++ /dev/null @@ -1,19 +0,0 @@ -// -// Created by casic on 25-3-5. -// - -#include "message_queue.hpp" - -void MessageQueue::enqueue(const std::vector &command) { - std::lock_guard lock(mutex_); - queue_.push(command); - cond_.notify_one(); -} - -std::vector MessageQueue::dequeue() { - std::unique_lock lock(mutex_); - cond_.wait(lock, [this] { return !queue_.empty(); }); - std::vector msg = queue_.front(); - queue_.pop(); - return msg; -} diff --git a/src/message_queue.hpp b/src/message_queue.hpp deleted file mode 100644 index 69075dc..0000000 --- a/src/message_queue.hpp +++ /dev/null @@ -1,25 +0,0 @@ -// -// Created by casic on 25-3-5. -// - -#ifndef MESSAGE_INTERFACE_HPP -#define MESSAGE_INTERFACE_HPP - -#include -#include -#include - -class MessageQueue { -public: - void enqueue(const std::vector &command); - - std::vector dequeue(); - -private: - std::queue > queue_; - std::mutex mutex_; - std::condition_variable cond_; -}; - - -#endif //MESSAGE_INTERFACE_HPP diff --git a/src/serial_port_wrapper.cpp b/src/serial_port_wrapper.cpp index 277102a..2ce65d6 100644 --- a/src/serial_port_wrapper.cpp +++ b/src/serial_port_wrapper.cpp @@ -32,8 +32,6 @@ timer_.async_wait([this](const boost::system::error_code &error) { if (!error) { read_from_port(); - } else { - std::cerr << "Timer error: " << error.message() << std::endl; } }); } diff --git a/src/slam_wrapper.cpp b/src/slam_wrapper.cpp index 3157c63..8787f31 100644 --- a/src/slam_wrapper.cpp +++ b/src/slam_wrapper.cpp @@ -3,12 +3,9 @@ // #include "slam_wrapper.hpp" -#include "message_queue.hpp" #include #include -extern MessageQueue g_messageQueue; - SlamWrapper::SlamWrapper(const char *networkInterface) { ChannelFactory::Instance()->Init(0, networkInterface); @@ -57,17 +54,26 @@ pause_navigation(); //TODO 控制机械臂转动到预设的角度 - g_messageQueue.enqueue({ - 0xFE, 0xFE, 0x0F, 0x3C, - 0x0B, 0x71, 0x08, 0x25, 0x06, 0xFB, - 0x08, 0x2E, 0x0A, 0xCB, 0x08, 0x28, - 0x14, 0xFA - }); + //朝右下方 + // g_messageQueue.enqueue({ + // 0xFE, 0xFE, 0x0F, 0x3C, + // 0x0B, 0x71, 0x08, 0x25, 0x06, 0xFB, + // 0x08, 0x2E, 0x0A, 0xCB, 0x08, 0x28, + // 0x14, 0xFA + // }); //倒计时结束后恢复巡检 std::thread countdown_thread([this] { std::this_thread::sleep_for(std::chrono::seconds(10)); - std::cout << "倒计时结束，恢复巡检"; + std::cout << "倒计时结束，恢复机械臂初始位置，恢复巡检"; + //水平朝前方向 + // g_messageQueue.enqueue({ + // 0xFE, 0xFE, 0x0F, 0x3C, + // 0x08, 0x00, 0x08, 0x00, 0x08, 0x00, + // 0x08, 0x00, 0x08, 0x00, 0x08, 0x00, + // 0x14, 0xFA + // }); + recover_navigation(); }); countdown_thread.detach(); diff --git a/src/tcp_client.cpp b/src/tcp_client.cpp index 2d1e489..ac6f3c5 100644 --- a/src/tcp_client.cpp +++ b/src/tcp_client.cpp @@ -17,8 +17,30 @@ const std::string received_string(buffer.begin(), buffer.end()); std::cout << "handle_data >>>> " << received_string << std::endl; if (received_string == "q") { - std::cout << "Close all nodes" << std::endl; - _slam.close_all_node(); + // std::cout << "Close all nodes" << std::endl; + // _slam.close_all_node(); + + std::cout << "发送机械臂转动指令，倒计时开始，请等待10秒"; + _tcp_service->send_command({ + 0xFE, 0xFE, 0x0F, 0x3C, + 0x0B, 0x71, 0x08, 0x25, 0x06, 0xFB, + 0x08, 0x2E, 0x0A, 0xCB, 0x08, 0x28, + 0x14, 0xFA + }); + + //倒计时结束后恢复巡检 + std::thread countdown_thread([this] { + std::this_thread::sleep_for(std::chrono::seconds(10)); + std::cout << "倒计时结束，恢复机械臂初始位置，恢复巡检"; + //水平朝前方向 + _tcp_service->send_command({ + 0xFE, 0xFE, 0x0F, 0x3C, + 0x08, 0x00, 0x08, 0x00, 0x08, 0x00, + 0x08, 0x00, 0x08, 0x00, 0x08, 0x00, + 0x14, 0xFA + }); + }); + countdown_thread.detach(); } else if (received_string == "w") { std::cout << "Start mapping (default to clearing node/edge information)" << std::endl; _slam.delete_all_node(); diff --git a/src/tcp_service.cpp b/src/tcp_service.cpp index 14f9840..c7d2876 100644 --- a/src/tcp_service.cpp +++ b/src/tcp_service.cpp @@ -3,7 +3,6 @@ // #include "tcp_service.hpp" -#include "message_queue.hpp" #include #include #include @@ -15,15 +14,6 @@ #include #include -MessageQueue g_messageQueue; - -void TcpService::process_messages() { - while (true) { - std::vector command = g_messageQueue.dequeue(); - handle_data_packet(command); - } -} - void TcpService::init_serial_port(const std::string &port_name, const int baud_rate) { _port.open(port_name); if (_port.is_open()) { diff --git a/CMakeLists.txt b/CMakeLists.txt index b604b89..4ca6761 100755 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -12,7 +12,6 @@ add_executable(message_handler src/message_handler.cpp - src/message_queue.cpp src/serial_port_wrapper.cpp src/slam_wrapper.cpp src/tcp_client.cpp diff --git a/src/message_queue.cpp b/src/message_queue.cpp deleted file mode 100644 index ce03b73..0000000 --- a/src/message_queue.cpp +++ /dev/null @@ -1,19 +0,0 @@ -// -// Created by casic on 25-3-5. -// - -#include "message_queue.hpp" - -void MessageQueue::enqueue(const std::vector &command) { - std::lock_guard lock(mutex_); - queue_.push(command); - cond_.notify_one(); -} - -std::vector MessageQueue::dequeue() { - std::unique_lock lock(mutex_); - cond_.wait(lock, [this] { return !queue_.empty(); }); - std::vector msg = queue_.front(); - queue_.pop(); - return msg; -} diff --git a/src/message_queue.hpp b/src/message_queue.hpp deleted file mode 100644 index 69075dc..0000000 --- a/src/message_queue.hpp +++ /dev/null @@ -1,25 +0,0 @@ -// -// Created by casic on 25-3-5. -// - -#ifndef MESSAGE_INTERFACE_HPP -#define MESSAGE_INTERFACE_HPP - -#include -#include -#include - -class MessageQueue { -public: - void enqueue(const std::vector &command); - - std::vector dequeue(); - -private: - std::queue > queue_; - std::mutex mutex_; - std::condition_variable cond_; -}; - - -#endif //MESSAGE_INTERFACE_HPP diff --git a/src/serial_port_wrapper.cpp b/src/serial_port_wrapper.cpp index 277102a..2ce65d6 100644 --- a/src/serial_port_wrapper.cpp +++ b/src/serial_port_wrapper.cpp @@ -32,8 +32,6 @@ timer_.async_wait([this](const boost::system::error_code &error) { if (!error) { read_from_port(); - } else { - std::cerr << "Timer error: " << error.message() << std::endl; } }); } diff --git a/src/slam_wrapper.cpp b/src/slam_wrapper.cpp index 3157c63..8787f31 100644 --- a/src/slam_wrapper.cpp +++ b/src/slam_wrapper.cpp @@ -3,12 +3,9 @@ // #include "slam_wrapper.hpp" -#include "message_queue.hpp" #include #include -extern MessageQueue g_messageQueue; - SlamWrapper::SlamWrapper(const char *networkInterface) { ChannelFactory::Instance()->Init(0, networkInterface); @@ -57,17 +54,26 @@ pause_navigation(); //TODO 控制机械臂转动到预设的角度 - g_messageQueue.enqueue({ - 0xFE, 0xFE, 0x0F, 0x3C, - 0x0B, 0x71, 0x08, 0x25, 0x06, 0xFB, - 0x08, 0x2E, 0x0A, 0xCB, 0x08, 0x28, - 0x14, 0xFA - }); + //朝右下方 + // g_messageQueue.enqueue({ + // 0xFE, 0xFE, 0x0F, 0x3C, + // 0x0B, 0x71, 0x08, 0x25, 0x06, 0xFB, + // 0x08, 0x2E, 0x0A, 0xCB, 0x08, 0x28, + // 0x14, 0xFA + // }); //倒计时结束后恢复巡检 std::thread countdown_thread([this] { std::this_thread::sleep_for(std::chrono::seconds(10)); - std::cout << "倒计时结束，恢复巡检"; + std::cout << "倒计时结束，恢复机械臂初始位置，恢复巡检"; + //水平朝前方向 + // g_messageQueue.enqueue({ + // 0xFE, 0xFE, 0x0F, 0x3C, + // 0x08, 0x00, 0x08, 0x00, 0x08, 0x00, + // 0x08, 0x00, 0x08, 0x00, 0x08, 0x00, + // 0x14, 0xFA + // }); + recover_navigation(); }); countdown_thread.detach(); diff --git a/src/tcp_client.cpp b/src/tcp_client.cpp index 2d1e489..ac6f3c5 100644 --- a/src/tcp_client.cpp +++ b/src/tcp_client.cpp @@ -17,8 +17,30 @@ const std::string received_string(buffer.begin(), buffer.end()); std::cout << "handle_data >>>> " << received_string << std::endl; if (received_string == "q") { - std::cout << "Close all nodes" << std::endl; - _slam.close_all_node(); + // std::cout << "Close all nodes" << std::endl; + // _slam.close_all_node(); + + std::cout << "发送机械臂转动指令，倒计时开始，请等待10秒"; + _tcp_service->send_command({ + 0xFE, 0xFE, 0x0F, 0x3C, + 0x0B, 0x71, 0x08, 0x25, 0x06, 0xFB, + 0x08, 0x2E, 0x0A, 0xCB, 0x08, 0x28, + 0x14, 0xFA + }); + + //倒计时结束后恢复巡检 + std::thread countdown_thread([this] { + std::this_thread::sleep_for(std::chrono::seconds(10)); + std::cout << "倒计时结束，恢复机械臂初始位置，恢复巡检"; + //水平朝前方向 + _tcp_service->send_command({ + 0xFE, 0xFE, 0x0F, 0x3C, + 0x08, 0x00, 0x08, 0x00, 0x08, 0x00, + 0x08, 0x00, 0x08, 0x00, 0x08, 0x00, + 0x14, 0xFA + }); + }); + countdown_thread.detach(); } else if (received_string == "w") { std::cout << "Start mapping (default to clearing node/edge information)" << std::endl; _slam.delete_all_node(); diff --git a/src/tcp_service.cpp b/src/tcp_service.cpp index 14f9840..c7d2876 100644 --- a/src/tcp_service.cpp +++ b/src/tcp_service.cpp @@ -3,7 +3,6 @@ // #include "tcp_service.hpp" -#include "message_queue.hpp" #include #include #include @@ -15,15 +14,6 @@ #include #include -MessageQueue g_messageQueue; - -void TcpService::process_messages() { - while (true) { - std::vector command = g_messageQueue.dequeue(); - handle_data_packet(command); - } -} - void TcpService::init_serial_port(const std::string &port_name, const int baud_rate) { _port.open(port_name); if (_port.is_open()) { diff --git a/src/tcp_service.hpp b/src/tcp_service.hpp index c03bc7b..999a2f7 100644 --- a/src/tcp_service.hpp +++ b/src/tcp_service.hpp @@ -26,8 +26,6 @@ void handle_data_packet(std::vector &data); void handle_client(int client_socket); - - void process_messages(); };