diff --git a/src/message_observer.hpp b/src/message_observer.hpp index b6b514d..67f121f 100644 --- a/src/message_observer.hpp +++ b/src/message_observer.hpp @@ -12,6 +12,7 @@ public: virtual ~MessageObserver() = default; virtual void on_message_received(const std::vector &command) = 0; + virtual void on_node_arrived(const int &node) = 0; }; #endif //MESSAGE_OBSERVER_HPP diff --git a/src/message_observer.hpp b/src/message_observer.hpp index b6b514d..67f121f 100644 --- a/src/message_observer.hpp +++ b/src/message_observer.hpp @@ -12,6 +12,7 @@ public: virtual ~MessageObserver() = default; virtual void on_message_received(const std::vector &command) = 0; + virtual void on_node_arrived(const int &node) = 0; }; #endif //MESSAGE_OBSERVER_HPP diff --git a/src/slam_wrapper.cpp b/src/slam_wrapper.cpp index 5f6a3e6..4f22353 100644 --- a/src/slam_wrapper.cpp +++ b/src/slam_wrapper.cpp @@ -30,6 +30,12 @@ } } +void SlamWrapper::notify_observers(const int &node) { + for (const auto &observer: observers) { + observer->on_node_arrived(node); + } +} + void SlamWrapper::control_robotic_arm() { const std::vector left_command = { 0xFE, 0xFE, 0x0F, 0x3C, 0x05, 0x1F, 0x09, 0x70, 0x05, 0x04, 0x08, 0x67, 0x08, 0x6E, 0x07, 0xDF, 0x1E, 0xFA @@ -60,7 +66,6 @@ begin_ = seq->data().find("notice:", 0); // Prompt message end_ = seq->data().find(";", begin_); const std::string notice_ = seq->data().substr(begin_ + 7, end_ - begin_ - 7); - if (index_ <= 10000) { // Command execution feedback begin_ = seq->data().find("feedback:", 0); @@ -79,6 +84,7 @@ str_ = seq->data().substr(begin_ + 7, end_ - begin_ - 7); const int arrive_ = atoi(str_.c_str()); std::cout << "I arrived node " << arrive_ << ". " << notice_ << std::endl; + notify_observers(arrive_); if (arrive_ == 2) { //发送暂停指令 pause_navigation(); diff --git a/src/message_observer.hpp b/src/message_observer.hpp index b6b514d..67f121f 100644 --- a/src/message_observer.hpp +++ b/src/message_observer.hpp @@ -12,6 +12,7 @@ public: virtual ~MessageObserver() = default; virtual void on_message_received(const std::vector &command) = 0; + virtual void on_node_arrived(const int &node) = 0; }; #endif //MESSAGE_OBSERVER_HPP diff --git a/src/slam_wrapper.cpp b/src/slam_wrapper.cpp index 5f6a3e6..4f22353 100644 --- a/src/slam_wrapper.cpp +++ b/src/slam_wrapper.cpp @@ -30,6 +30,12 @@ } } +void SlamWrapper::notify_observers(const int &node) { + for (const auto &observer: observers) { + observer->on_node_arrived(node); + } +} + void SlamWrapper::control_robotic_arm() { const std::vector left_command = { 0xFE, 0xFE, 0x0F, 0x3C, 0x05, 0x1F, 0x09, 0x70, 0x05, 0x04, 0x08, 0x67, 0x08, 0x6E, 0x07, 0xDF, 0x1E, 0xFA @@ -60,7 +66,6 @@ begin_ = seq->data().find("notice:", 0); // Prompt message end_ = seq->data().find(";", begin_); const std::string notice_ = seq->data().substr(begin_ + 7, end_ - begin_ - 7); - if (index_ <= 10000) { // Command execution feedback begin_ = seq->data().find("feedback:", 0); @@ -79,6 +84,7 @@ str_ = seq->data().substr(begin_ + 7, end_ - begin_ - 7); const int arrive_ = atoi(str_.c_str()); std::cout << "I arrived node " << arrive_ << ". " << notice_ << std::endl; + notify_observers(arrive_); if (arrive_ == 2) { //发送暂停指令 pause_navigation(); diff --git a/src/slam_wrapper.hpp b/src/slam_wrapper.hpp index d8369a8..d1eff0f 100644 --- a/src/slam_wrapper.hpp +++ b/src/slam_wrapper.hpp @@ -53,6 +53,8 @@ void notify_observers(const std::vector &command); + void notify_observers(const int &node); + void control_robotic_arm(); explicit SlamWrapper(); diff --git a/src/message_observer.hpp b/src/message_observer.hpp index b6b514d..67f121f 100644 --- a/src/message_observer.hpp +++ b/src/message_observer.hpp @@ -12,6 +12,7 @@ public: virtual ~MessageObserver() = default; virtual void on_message_received(const std::vector &command) = 0; + virtual void on_node_arrived(const int &node) = 0; }; #endif //MESSAGE_OBSERVER_HPP diff --git a/src/slam_wrapper.cpp b/src/slam_wrapper.cpp index 5f6a3e6..4f22353 100644 --- a/src/slam_wrapper.cpp +++ b/src/slam_wrapper.cpp @@ -30,6 +30,12 @@ } } +void SlamWrapper::notify_observers(const int &node) { + for (const auto &observer: observers) { + observer->on_node_arrived(node); + } +} + void SlamWrapper::control_robotic_arm() { const std::vector left_command = { 0xFE, 0xFE, 0x0F, 0x3C, 0x05, 0x1F, 0x09, 0x70, 0x05, 0x04, 0x08, 0x67, 0x08, 0x6E, 0x07, 0xDF, 0x1E, 0xFA @@ -60,7 +66,6 @@ begin_ = seq->data().find("notice:", 0); // Prompt message end_ = seq->data().find(";", begin_); const std::string notice_ = seq->data().substr(begin_ + 7, end_ - begin_ - 7); - if (index_ <= 10000) { // Command execution feedback begin_ = seq->data().find("feedback:", 0); @@ -79,6 +84,7 @@ str_ = seq->data().substr(begin_ + 7, end_ - begin_ - 7); const int arrive_ = atoi(str_.c_str()); std::cout << "I arrived node " << arrive_ << ". " << notice_ << std::endl; + notify_observers(arrive_); if (arrive_ == 2) { //发送暂停指令 pause_navigation(); diff --git a/src/slam_wrapper.hpp b/src/slam_wrapper.hpp index d8369a8..d1eff0f 100644 --- a/src/slam_wrapper.hpp +++ b/src/slam_wrapper.hpp @@ -53,6 +53,8 @@ void notify_observers(const std::vector &command); + void notify_observers(const int &node); + void control_robotic_arm(); explicit SlamWrapper(); diff --git a/src/tcp_service.cpp b/src/tcp_service.cpp index 45ac332..6578f68 100644 --- a/src/tcp_service.cpp +++ b/src/tcp_service.cpp @@ -18,6 +18,17 @@ boost::asio::write(_port, boost::asio::buffer(command)); } +void TcpService::on_node_arrived(const int &node) { + //回消息给已连接的TCP客户端 + const std::string message = std::to_string(node); + std::cout << "send message to client: " << message << std::endl; + for (const int client_socket: _client_sockets) { + if (send(client_socket, message.c_str(), message.size(), 0) == -1) { + std::cerr << "Failed to send message to client: " << strerror(errno) << std::endl; + } + } +} + void TcpService::init_serial_port(const std::string &port_name, const int baud_rate) { _port.open(port_name); if (_port.is_open()) { @@ -139,6 +150,7 @@ << std::endl; // 处理连接 + _client_sockets.push_back(client_socket_fd); handle_client(client_socket_fd); } } catch (const std::exception &e) { diff --git a/src/message_observer.hpp b/src/message_observer.hpp index b6b514d..67f121f 100644 --- a/src/message_observer.hpp +++ b/src/message_observer.hpp @@ -12,6 +12,7 @@ public: virtual ~MessageObserver() = default; virtual void on_message_received(const std::vector &command) = 0; + virtual void on_node_arrived(const int &node) = 0; }; #endif //MESSAGE_OBSERVER_HPP diff --git a/src/slam_wrapper.cpp b/src/slam_wrapper.cpp index 5f6a3e6..4f22353 100644 --- a/src/slam_wrapper.cpp +++ b/src/slam_wrapper.cpp @@ -30,6 +30,12 @@ } } +void SlamWrapper::notify_observers(const int &node) { + for (const auto &observer: observers) { + observer->on_node_arrived(node); + } +} + void SlamWrapper::control_robotic_arm() { const std::vector left_command = { 0xFE, 0xFE, 0x0F, 0x3C, 0x05, 0x1F, 0x09, 0x70, 0x05, 0x04, 0x08, 0x67, 0x08, 0x6E, 0x07, 0xDF, 0x1E, 0xFA @@ -60,7 +66,6 @@ begin_ = seq->data().find("notice:", 0); // Prompt message end_ = seq->data().find(";", begin_); const std::string notice_ = seq->data().substr(begin_ + 7, end_ - begin_ - 7); - if (index_ <= 10000) { // Command execution feedback begin_ = seq->data().find("feedback:", 0); @@ -79,6 +84,7 @@ str_ = seq->data().substr(begin_ + 7, end_ - begin_ - 7); const int arrive_ = atoi(str_.c_str()); std::cout << "I arrived node " << arrive_ << ". " << notice_ << std::endl; + notify_observers(arrive_); if (arrive_ == 2) { //发送暂停指令 pause_navigation(); diff --git a/src/slam_wrapper.hpp b/src/slam_wrapper.hpp index d8369a8..d1eff0f 100644 --- a/src/slam_wrapper.hpp +++ b/src/slam_wrapper.hpp @@ -53,6 +53,8 @@ void notify_observers(const std::vector &command); + void notify_observers(const int &node); + void control_robotic_arm(); explicit SlamWrapper(); diff --git a/src/tcp_service.cpp b/src/tcp_service.cpp index 45ac332..6578f68 100644 --- a/src/tcp_service.cpp +++ b/src/tcp_service.cpp @@ -18,6 +18,17 @@ boost::asio::write(_port, boost::asio::buffer(command)); } +void TcpService::on_node_arrived(const int &node) { + //回消息给已连接的TCP客户端 + const std::string message = std::to_string(node); + std::cout << "send message to client: " << message << std::endl; + for (const int client_socket: _client_sockets) { + if (send(client_socket, message.c_str(), message.size(), 0) == -1) { + std::cerr << "Failed to send message to client: " << strerror(errno) << std::endl; + } + } +} + void TcpService::init_serial_port(const std::string &port_name, const int baud_rate) { _port.open(port_name); if (_port.is_open()) { @@ -139,6 +150,7 @@ << std::endl; // 处理连接 + _client_sockets.push_back(client_socket_fd); handle_client(client_socket_fd); } } catch (const std::exception &e) { diff --git a/src/tcp_service.hpp b/src/tcp_service.hpp index cfe903f..7a8328f 100644 --- a/src/tcp_service.hpp +++ b/src/tcp_service.hpp @@ -12,7 +12,7 @@ using boost::asio::serial_port_base; -class TcpService : public MessageObserver { +class TcpService final : public MessageObserver { public: explicit TcpService(boost::asio::io_context &io_context, const std::string &port_name, int baud_rate); @@ -22,8 +22,11 @@ void on_message_received(const std::vector &command) override; + void on_node_arrived(const int &node) override; + private: int _socket_fd; + std::vector _client_sockets; boost::asio::io_context &_context; boost::asio::serial_port _port;