diff --git a/CMakeLists.txt b/CMakeLists.txt index 4ca6761..b604b89 100755 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -12,6 +12,7 @@ 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 4ca6761..b604b89 100755 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -12,6 +12,7 @@ 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 new file mode 100644 index 0000000..b1569b0 --- /dev/null +++ b/src/message_queue.cpp @@ -0,0 +1,19 @@ +// +// 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 4ca6761..b604b89 100755 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -12,6 +12,7 @@ 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 new file mode 100644 index 0000000..b1569b0 --- /dev/null +++ b/src/message_queue.cpp @@ -0,0 +1,19 @@ +// +// 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 new file mode 100644 index 0000000..4538ffd --- /dev/null +++ b/src/message_queue.hpp @@ -0,0 +1,25 @@ +// +// 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 4ca6761..b604b89 100755 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -12,6 +12,7 @@ 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 new file mode 100644 index 0000000..b1569b0 --- /dev/null +++ b/src/message_queue.cpp @@ -0,0 +1,19 @@ +// +// 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 new file mode 100644 index 0000000..4538ffd --- /dev/null +++ b/src/message_queue.hpp @@ -0,0 +1,25 @@ +// +// 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/slam_wrapper.cpp b/src/slam_wrapper.cpp index 3700351..03453bc 100644 --- a/src/slam_wrapper.cpp +++ b/src/slam_wrapper.cpp @@ -3,8 +3,11 @@ // #include "slam_wrapper.hpp" +#include "message_queue.hpp" #include +extern MessageQueue g_messageQueue; + SlamWrapper::SlamWrapper(const char *networkInterface) { ChannelFactory::Instance()->Init(0, networkInterface); @@ -52,7 +55,7 @@ begin_ = seq->data().find("arrive:", 0); end_ = seq->data().find(";", begin_); str_ = seq->data().substr(begin_ + 7, end_ - begin_ - 7); - // TODO 巡检到达的位置。判断到达的是第一个点，然后发送暂停指令，控制机械臂转动到预设的角度，几秒后恢复巡检 + // 巡检到达的位置，判断到达的是第几个点 const long arrive = strtol(str_.c_str(), &endptr, 10); if (*endptr != '\0') { std::cerr << "Error converting arrive to integer" << std::endl; @@ -60,7 +63,13 @@ } std::cout << "index:" << index << " arrive:" << arrive << " notice:" << notice_ << std::endl; if (arrive == 1) { + //发送暂停指令 pause_navigation(); + + //TODO 控制机械臂转动到预设的角度 + g_messageQueue.enqueue({}); + + //倒计时结束后恢复巡检 } } } @@ -160,7 +169,7 @@ } void SlamWrapper::add_edge(const u_int16_t edge_name, const u_int16_t start_node, u_int16_t end_node) { - edge_attribute edgeTemp{}; + EdgeAttribute edgeTemp{}; edgeTemp.edgeName = edge_name; edgeTemp.edgeStart = start_node; diff --git a/CMakeLists.txt b/CMakeLists.txt index 4ca6761..b604b89 100755 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -12,6 +12,7 @@ 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 new file mode 100644 index 0000000..b1569b0 --- /dev/null +++ b/src/message_queue.cpp @@ -0,0 +1,19 @@ +// +// 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 new file mode 100644 index 0000000..4538ffd --- /dev/null +++ b/src/message_queue.hpp @@ -0,0 +1,25 @@ +// +// 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/slam_wrapper.cpp b/src/slam_wrapper.cpp index 3700351..03453bc 100644 --- a/src/slam_wrapper.cpp +++ b/src/slam_wrapper.cpp @@ -3,8 +3,11 @@ // #include "slam_wrapper.hpp" +#include "message_queue.hpp" #include +extern MessageQueue g_messageQueue; + SlamWrapper::SlamWrapper(const char *networkInterface) { ChannelFactory::Instance()->Init(0, networkInterface); @@ -52,7 +55,7 @@ begin_ = seq->data().find("arrive:", 0); end_ = seq->data().find(";", begin_); str_ = seq->data().substr(begin_ + 7, end_ - begin_ - 7); - // TODO 巡检到达的位置。判断到达的是第一个点，然后发送暂停指令，控制机械臂转动到预设的角度，几秒后恢复巡检 + // 巡检到达的位置，判断到达的是第几个点 const long arrive = strtol(str_.c_str(), &endptr, 10); if (*endptr != '\0') { std::cerr << "Error converting arrive to integer" << std::endl; @@ -60,7 +63,13 @@ } std::cout << "index:" << index << " arrive:" << arrive << " notice:" << notice_ << std::endl; if (arrive == 1) { + //发送暂停指令 pause_navigation(); + + //TODO 控制机械臂转动到预设的角度 + g_messageQueue.enqueue({}); + + //倒计时结束后恢复巡检 } } } @@ -160,7 +169,7 @@ } void SlamWrapper::add_edge(const u_int16_t edge_name, const u_int16_t start_node, u_int16_t end_node) { - edge_attribute edgeTemp{}; + EdgeAttribute edgeTemp{}; edgeTemp.edgeName = edge_name; edgeTemp.edgeStart = start_node; diff --git a/src/slam_wrapper.hpp b/src/slam_wrapper.hpp index 971a382..80fb1f6 100644 --- a/src/slam_wrapper.hpp +++ b/src/slam_wrapper.hpp @@ -27,7 +27,7 @@ using namespace std_msgs::msg::dds_; using namespace nav_msgs::msg::dds_; -struct node_attribute { +struct NodeAttribute { u_int16_t nodeName; float nodeX; float nodeY; @@ -35,7 +35,7 @@ float nodeYaw; }; -struct edge_attribute { +struct EdgeAttribute { u_int16_t edgeName; u_int16_t edgeStart; u_int16_t edgeEnd; @@ -66,8 +66,8 @@ ); public: - std::vector node_attributes; - std::vector edge_attributes; + std::vector node_attributes; + std::vector edge_attributes; u_int16_t node_name = 0; explicit SlamWrapper(const char *networkInterface); diff --git a/CMakeLists.txt b/CMakeLists.txt index 4ca6761..b604b89 100755 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -12,6 +12,7 @@ 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 new file mode 100644 index 0000000..b1569b0 --- /dev/null +++ b/src/message_queue.cpp @@ -0,0 +1,19 @@ +// +// 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 new file mode 100644 index 0000000..4538ffd --- /dev/null +++ b/src/message_queue.hpp @@ -0,0 +1,25 @@ +// +// 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/slam_wrapper.cpp b/src/slam_wrapper.cpp index 3700351..03453bc 100644 --- a/src/slam_wrapper.cpp +++ b/src/slam_wrapper.cpp @@ -3,8 +3,11 @@ // #include "slam_wrapper.hpp" +#include "message_queue.hpp" #include +extern MessageQueue g_messageQueue; + SlamWrapper::SlamWrapper(const char *networkInterface) { ChannelFactory::Instance()->Init(0, networkInterface); @@ -52,7 +55,7 @@ begin_ = seq->data().find("arrive:", 0); end_ = seq->data().find(";", begin_); str_ = seq->data().substr(begin_ + 7, end_ - begin_ - 7); - // TODO 巡检到达的位置。判断到达的是第一个点，然后发送暂停指令，控制机械臂转动到预设的角度，几秒后恢复巡检 + // 巡检到达的位置，判断到达的是第几个点 const long arrive = strtol(str_.c_str(), &endptr, 10); if (*endptr != '\0') { std::cerr << "Error converting arrive to integer" << std::endl; @@ -60,7 +63,13 @@ } std::cout << "index:" << index << " arrive:" << arrive << " notice:" << notice_ << std::endl; if (arrive == 1) { + //发送暂停指令 pause_navigation(); + + //TODO 控制机械臂转动到预设的角度 + g_messageQueue.enqueue({}); + + //倒计时结束后恢复巡检 } } } @@ -160,7 +169,7 @@ } void SlamWrapper::add_edge(const u_int16_t edge_name, const u_int16_t start_node, u_int16_t end_node) { - edge_attribute edgeTemp{}; + EdgeAttribute edgeTemp{}; edgeTemp.edgeName = edge_name; edgeTemp.edgeStart = start_node; diff --git a/src/slam_wrapper.hpp b/src/slam_wrapper.hpp index 971a382..80fb1f6 100644 --- a/src/slam_wrapper.hpp +++ b/src/slam_wrapper.hpp @@ -27,7 +27,7 @@ using namespace std_msgs::msg::dds_; using namespace nav_msgs::msg::dds_; -struct node_attribute { +struct NodeAttribute { u_int16_t nodeName; float nodeX; float nodeY; @@ -35,7 +35,7 @@ float nodeYaw; }; -struct edge_attribute { +struct EdgeAttribute { u_int16_t edgeName; u_int16_t edgeStart; u_int16_t edgeEnd; @@ -66,8 +66,8 @@ ); public: - std::vector node_attributes; - std::vector edge_attributes; + std::vector node_attributes; + std::vector edge_attributes; u_int16_t node_name = 0; explicit SlamWrapper(const char *networkInterface); diff --git a/src/tcp_service.cpp b/src/tcp_service.cpp index 0ced084..4945ba5 100644 --- a/src/tcp_service.cpp +++ b/src/tcp_service.cpp @@ -3,6 +3,7 @@ // #include "tcp_service.hpp" +#include "message_queue.hpp" #include #include #include @@ -14,6 +15,15 @@ #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 4ca6761..b604b89 100755 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -12,6 +12,7 @@ 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 new file mode 100644 index 0000000..b1569b0 --- /dev/null +++ b/src/message_queue.cpp @@ -0,0 +1,19 @@ +// +// 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 new file mode 100644 index 0000000..4538ffd --- /dev/null +++ b/src/message_queue.hpp @@ -0,0 +1,25 @@ +// +// 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/slam_wrapper.cpp b/src/slam_wrapper.cpp index 3700351..03453bc 100644 --- a/src/slam_wrapper.cpp +++ b/src/slam_wrapper.cpp @@ -3,8 +3,11 @@ // #include "slam_wrapper.hpp" +#include "message_queue.hpp" #include +extern MessageQueue g_messageQueue; + SlamWrapper::SlamWrapper(const char *networkInterface) { ChannelFactory::Instance()->Init(0, networkInterface); @@ -52,7 +55,7 @@ begin_ = seq->data().find("arrive:", 0); end_ = seq->data().find(";", begin_); str_ = seq->data().substr(begin_ + 7, end_ - begin_ - 7); - // TODO 巡检到达的位置。判断到达的是第一个点，然后发送暂停指令，控制机械臂转动到预设的角度，几秒后恢复巡检 + // 巡检到达的位置，判断到达的是第几个点 const long arrive = strtol(str_.c_str(), &endptr, 10); if (*endptr != '\0') { std::cerr << "Error converting arrive to integer" << std::endl; @@ -60,7 +63,13 @@ } std::cout << "index:" << index << " arrive:" << arrive << " notice:" << notice_ << std::endl; if (arrive == 1) { + //发送暂停指令 pause_navigation(); + + //TODO 控制机械臂转动到预设的角度 + g_messageQueue.enqueue({}); + + //倒计时结束后恢复巡检 } } } @@ -160,7 +169,7 @@ } void SlamWrapper::add_edge(const u_int16_t edge_name, const u_int16_t start_node, u_int16_t end_node) { - edge_attribute edgeTemp{}; + EdgeAttribute edgeTemp{}; edgeTemp.edgeName = edge_name; edgeTemp.edgeStart = start_node; diff --git a/src/slam_wrapper.hpp b/src/slam_wrapper.hpp index 971a382..80fb1f6 100644 --- a/src/slam_wrapper.hpp +++ b/src/slam_wrapper.hpp @@ -27,7 +27,7 @@ using namespace std_msgs::msg::dds_; using namespace nav_msgs::msg::dds_; -struct node_attribute { +struct NodeAttribute { u_int16_t nodeName; float nodeX; float nodeY; @@ -35,7 +35,7 @@ float nodeYaw; }; -struct edge_attribute { +struct EdgeAttribute { u_int16_t edgeName; u_int16_t edgeStart; u_int16_t edgeEnd; @@ -66,8 +66,8 @@ ); public: - std::vector node_attributes; - std::vector edge_attributes; + std::vector node_attributes; + std::vector edge_attributes; u_int16_t node_name = 0; explicit SlamWrapper(const char *networkInterface); diff --git a/src/tcp_service.cpp b/src/tcp_service.cpp index 0ced084..4945ba5 100644 --- a/src/tcp_service.cpp +++ b/src/tcp_service.cpp @@ -3,6 +3,7 @@ // #include "tcp_service.hpp" +#include "message_queue.hpp" #include #include #include @@ -14,6 +15,15 @@ #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 c6a2479..19429ea 100644 --- a/src/tcp_service.hpp +++ b/src/tcp_service.hpp @@ -26,6 +26,8 @@ void handle_data_packet(std::vector &data); void handle_client(int client_socket); + + void process_messages(); };