diff --git a/src/message_handler.cpp b/src/message_handler.cpp index bbbbcaf..e479c49 100644 --- a/src/message_handler.cpp +++ b/src/message_handler.cpp @@ -3,30 +3,21 @@ #include #include "tcp_client.hpp" -#include "tcp_service.hpp" #include "serial_port_wrapper.hpp" int main() { - // 创建一个共享的 io 对象 - boost::asio::io_service io; - auto service = new TcpService(io, "/dev/ttyACM0", 115200); //机械臂串口 - // 启动TCP客户端，接收来自Web端的命令，控制机器狗运动 - std::thread client_thread([&io,&service] { - TcpClient client("eth0", service); + std::thread client_thread([] { + boost::asio::io_service io; + TcpClient client; client.connect("192.168.123.18", 9001); io.run(); }); - // 启动TCP服务，接收AI结果并通过串口控制机械臂 - std::thread service_thread([&io,&service] { - service->start(8888); - io.run(); - }); - // 启动甲烷数据查询串口线程，定时发送查询指令 - std::thread gas_serial_port_thread([&io] { + std::thread gas_serial_port_thread([] { try { + boost::asio::io_service io; //打开串口 SerialPortWrapper wrapper(io, "/dev/ttyUSB0", 115200); //甲烷串口 wrapper.read_from_port(); @@ -38,7 +29,6 @@ // 等待所有线程完成 client_thread.join(); - service_thread.join(); gas_serial_port_thread.join(); return 0; } diff --git a/src/message_handler.cpp b/src/message_handler.cpp index bbbbcaf..e479c49 100644 --- a/src/message_handler.cpp +++ b/src/message_handler.cpp @@ -3,30 +3,21 @@ #include #include "tcp_client.hpp" -#include "tcp_service.hpp" #include "serial_port_wrapper.hpp" int main() { - // 创建一个共享的 io 对象 - boost::asio::io_service io; - auto service = new TcpService(io, "/dev/ttyACM0", 115200); //机械臂串口 - // 启动TCP客户端，接收来自Web端的命令，控制机器狗运动 - std::thread client_thread([&io,&service] { - TcpClient client("eth0", service); + std::thread client_thread([] { + boost::asio::io_service io; + TcpClient client; client.connect("192.168.123.18", 9001); io.run(); }); - // 启动TCP服务，接收AI结果并通过串口控制机械臂 - std::thread service_thread([&io,&service] { - service->start(8888); - io.run(); - }); - // 启动甲烷数据查询串口线程，定时发送查询指令 - std::thread gas_serial_port_thread([&io] { + std::thread gas_serial_port_thread([] { try { + boost::asio::io_service io; //打开串口 SerialPortWrapper wrapper(io, "/dev/ttyUSB0", 115200); //甲烷串口 wrapper.read_from_port(); @@ -38,7 +29,6 @@ // 等待所有线程完成 client_thread.join(); - service_thread.join(); gas_serial_port_thread.join(); return 0; } diff --git a/src/message_observer.hpp b/src/message_observer.hpp new file mode 100644 index 0000000..b6b514d --- /dev/null +++ b/src/message_observer.hpp @@ -0,0 +1,17 @@ +// +// Created by pengx on 2025/3/7. +// + +#ifndef MESSAGE_OBSERVER_HPP +#define MESSAGE_OBSERVER_HPP + +#include +#include + +class MessageObserver { +public: + virtual ~MessageObserver() = default; + virtual void on_message_received(const std::vector &command) = 0; +}; + +#endif //MESSAGE_OBSERVER_HPP diff --git a/src/message_handler.cpp b/src/message_handler.cpp index bbbbcaf..e479c49 100644 --- a/src/message_handler.cpp +++ b/src/message_handler.cpp @@ -3,30 +3,21 @@ #include #include "tcp_client.hpp" -#include "tcp_service.hpp" #include "serial_port_wrapper.hpp" int main() { - // 创建一个共享的 io 对象 - boost::asio::io_service io; - auto service = new TcpService(io, "/dev/ttyACM0", 115200); //机械臂串口 - // 启动TCP客户端，接收来自Web端的命令，控制机器狗运动 - std::thread client_thread([&io,&service] { - TcpClient client("eth0", service); + std::thread client_thread([] { + boost::asio::io_service io; + TcpClient client; client.connect("192.168.123.18", 9001); io.run(); }); - // 启动TCP服务，接收AI结果并通过串口控制机械臂 - std::thread service_thread([&io,&service] { - service->start(8888); - io.run(); - }); - // 启动甲烷数据查询串口线程，定时发送查询指令 - std::thread gas_serial_port_thread([&io] { + std::thread gas_serial_port_thread([] { try { + boost::asio::io_service io; //打开串口 SerialPortWrapper wrapper(io, "/dev/ttyUSB0", 115200); //甲烷串口 wrapper.read_from_port(); @@ -38,7 +29,6 @@ // 等待所有线程完成 client_thread.join(); - service_thread.join(); gas_serial_port_thread.join(); return 0; } diff --git a/src/message_observer.hpp b/src/message_observer.hpp new file mode 100644 index 0000000..b6b514d --- /dev/null +++ b/src/message_observer.hpp @@ -0,0 +1,17 @@ +// +// Created by pengx on 2025/3/7. +// + +#ifndef MESSAGE_OBSERVER_HPP +#define MESSAGE_OBSERVER_HPP + +#include +#include + +class MessageObserver { +public: + virtual ~MessageObserver() = default; + virtual void on_message_received(const std::vector &command) = 0; +}; + +#endif //MESSAGE_OBSERVER_HPP diff --git a/src/slam_wrapper.cpp b/src/slam_wrapper.cpp index ba616c1..a99fb45 100644 --- a/src/slam_wrapper.cpp +++ b/src/slam_wrapper.cpp @@ -7,8 +7,8 @@ #include #include -SlamWrapper::SlamWrapper(const char *networkInterface, TcpService *tcp_service): _tcp_service(tcp_service) { - ChannelFactory::Instance()->Init(0, networkInterface); +SlamWrapper::SlamWrapper() { + ChannelFactory::Instance()->Init(0, "eth0"); // pub pubQtCommand->InitChannel(); @@ -20,6 +20,34 @@ subOdometer->InitChannel(std::bind(&SlamWrapper::odometer_handler, this, std::placeholders::_1), 1); } +void SlamWrapper::register_observer(const std::shared_ptr &observer) { + observers.push_back(observer); +} + +void SlamWrapper::notify_observers(const std::vector &command) { + for (const auto &observer: observers) { + observer->on_message_received(command); + } +} + +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 + }; + const std::vector right_command = { + 0xFE, 0xFE, 0x0F, 0x3C, 0x0A, 0x40, 0x0A, 0x89, 0x03, 0xDE, 0x08, 0x43, 0x09, 0xA8, 0x07, 0xE1, 0x1E, 0xFA + }; + + notify_observers(init_command); + std::this_thread::sleep_for(std::chrono::seconds(3)); + notify_observers(left_command); + std::this_thread::sleep_for(std::chrono::seconds(3)); + notify_observers(right_command); + std::this_thread::sleep_for(std::chrono::seconds(3)); + notify_observers(init_command); + std::cout << "Control robotic arm end" << std::endl; +} + void SlamWrapper::qt_notice_handler(const void *message) { const auto seq = static_cast(message); std::string str_; @@ -58,7 +86,7 @@ //TODO 控制机械臂转动到预设的角度 //朝右下方 std::cout << "发送机械臂转动指令，倒计时开始，请等待10秒" << std::endl; - _tcp_service->send_command({ + notify_observers({ 0xFE, 0xFE, 0x0F, 0x3C, 0x0B, 0x71, 0x08, 0x25, 0x06, 0xFB, 0x08, 0x2E, 0x0A, 0xCB, 0x08, 0x28, @@ -71,13 +99,7 @@ std::this_thread::sleep_for(std::chrono::seconds(10)); recover_navigation(); std::cout << "倒计时结束，恢复机械臂初始位置，恢复巡检"; - //水平朝前方向 - _tcp_service->send_command({ - 0xFE, 0xFE, 0x0F, 0x3C, - 0x08, 0x00, 0x08, 0x00, 0x08, 0x00, - 0x08, 0x00, 0x08, 0x00, 0x08, 0x00, - 0x14, 0xFA - }); + notify_observers(init_command); } catch (const std::exception &e) { std::cerr << "线程执行过程中发生异常: " << e.what() << std::endl; } diff --git a/src/message_handler.cpp b/src/message_handler.cpp index bbbbcaf..e479c49 100644 --- a/src/message_handler.cpp +++ b/src/message_handler.cpp @@ -3,30 +3,21 @@ #include #include "tcp_client.hpp" -#include "tcp_service.hpp" #include "serial_port_wrapper.hpp" int main() { - // 创建一个共享的 io 对象 - boost::asio::io_service io; - auto service = new TcpService(io, "/dev/ttyACM0", 115200); //机械臂串口 - // 启动TCP客户端，接收来自Web端的命令，控制机器狗运动 - std::thread client_thread([&io,&service] { - TcpClient client("eth0", service); + std::thread client_thread([] { + boost::asio::io_service io; + TcpClient client; client.connect("192.168.123.18", 9001); io.run(); }); - // 启动TCP服务，接收AI结果并通过串口控制机械臂 - std::thread service_thread([&io,&service] { - service->start(8888); - io.run(); - }); - // 启动甲烷数据查询串口线程，定时发送查询指令 - std::thread gas_serial_port_thread([&io] { + std::thread gas_serial_port_thread([] { try { + boost::asio::io_service io; //打开串口 SerialPortWrapper wrapper(io, "/dev/ttyUSB0", 115200); //甲烷串口 wrapper.read_from_port(); @@ -38,7 +29,6 @@ // 等待所有线程完成 client_thread.join(); - service_thread.join(); gas_serial_port_thread.join(); return 0; } diff --git a/src/message_observer.hpp b/src/message_observer.hpp new file mode 100644 index 0000000..b6b514d --- /dev/null +++ b/src/message_observer.hpp @@ -0,0 +1,17 @@ +// +// Created by pengx on 2025/3/7. +// + +#ifndef MESSAGE_OBSERVER_HPP +#define MESSAGE_OBSERVER_HPP + +#include +#include + +class MessageObserver { +public: + virtual ~MessageObserver() = default; + virtual void on_message_received(const std::vector &command) = 0; +}; + +#endif //MESSAGE_OBSERVER_HPP diff --git a/src/slam_wrapper.cpp b/src/slam_wrapper.cpp index ba616c1..a99fb45 100644 --- a/src/slam_wrapper.cpp +++ b/src/slam_wrapper.cpp @@ -7,8 +7,8 @@ #include #include -SlamWrapper::SlamWrapper(const char *networkInterface, TcpService *tcp_service): _tcp_service(tcp_service) { - ChannelFactory::Instance()->Init(0, networkInterface); +SlamWrapper::SlamWrapper() { + ChannelFactory::Instance()->Init(0, "eth0"); // pub pubQtCommand->InitChannel(); @@ -20,6 +20,34 @@ subOdometer->InitChannel(std::bind(&SlamWrapper::odometer_handler, this, std::placeholders::_1), 1); } +void SlamWrapper::register_observer(const std::shared_ptr &observer) { + observers.push_back(observer); +} + +void SlamWrapper::notify_observers(const std::vector &command) { + for (const auto &observer: observers) { + observer->on_message_received(command); + } +} + +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 + }; + const std::vector right_command = { + 0xFE, 0xFE, 0x0F, 0x3C, 0x0A, 0x40, 0x0A, 0x89, 0x03, 0xDE, 0x08, 0x43, 0x09, 0xA8, 0x07, 0xE1, 0x1E, 0xFA + }; + + notify_observers(init_command); + std::this_thread::sleep_for(std::chrono::seconds(3)); + notify_observers(left_command); + std::this_thread::sleep_for(std::chrono::seconds(3)); + notify_observers(right_command); + std::this_thread::sleep_for(std::chrono::seconds(3)); + notify_observers(init_command); + std::cout << "Control robotic arm end" << std::endl; +} + void SlamWrapper::qt_notice_handler(const void *message) { const auto seq = static_cast(message); std::string str_; @@ -58,7 +86,7 @@ //TODO 控制机械臂转动到预设的角度 //朝右下方 std::cout << "发送机械臂转动指令，倒计时开始，请等待10秒" << std::endl; - _tcp_service->send_command({ + notify_observers({ 0xFE, 0xFE, 0x0F, 0x3C, 0x0B, 0x71, 0x08, 0x25, 0x06, 0xFB, 0x08, 0x2E, 0x0A, 0xCB, 0x08, 0x28, @@ -71,13 +99,7 @@ std::this_thread::sleep_for(std::chrono::seconds(10)); recover_navigation(); std::cout << "倒计时结束，恢复机械臂初始位置，恢复巡检"; - //水平朝前方向 - _tcp_service->send_command({ - 0xFE, 0xFE, 0x0F, 0x3C, - 0x08, 0x00, 0x08, 0x00, 0x08, 0x00, - 0x08, 0x00, 0x08, 0x00, 0x08, 0x00, - 0x14, 0xFA - }); + notify_observers(init_command); } catch (const std::exception &e) { std::cerr << "线程执行过程中发生异常: " << e.what() << std::endl; } diff --git a/src/slam_wrapper.hpp b/src/slam_wrapper.hpp index bc07ed4..d8369a8 100644 --- a/src/slam_wrapper.hpp +++ b/src/slam_wrapper.hpp @@ -44,36 +44,18 @@ }; class SlamWrapper { -private: - int index = 0; - const Odometry_ *currentOdom{}; - TcpService *_tcp_service; - - // pub - ChannelPublisherPtr pubQtCommand = ChannelPublisherPtr( - new ChannelPublisher(COMMANDTOPIC) - ); - ChannelPublisherPtr pubQtNode = ChannelPublisherPtr( - new ChannelPublisher(ADDNODETOPIC) - ); - ChannelPublisherPtr pubQtEdge = ChannelPublisherPtr( - new ChannelPublisher(ADDEDGETOPIC) - ); - - // sub - ChannelSubscriberPtr subQtNotice = ChannelSubscriberPtr( - new ChannelSubscriber(NOTICETOPIC) - ); - ChannelSubscriberPtr subOdometer = ChannelSubscriberPtr( - new ChannelSubscriber(ODOMTOPIC) - ); - public: std::vector node_attributes; std::vector edge_attributes; u_int16_t node_name = 0; - explicit SlamWrapper(const char *networkInterface, TcpService *tcp_service); + void register_observer(const std::shared_ptr &observer); + + void notify_observers(const std::vector &command); + + void control_robotic_arm(); + + explicit SlamWrapper(); void qt_notice_handler(const void *message); @@ -106,6 +88,33 @@ void pause_navigation(); void recover_navigation(); + +private: + int index = 0; + const Odometry_ *currentOdom{}; + std::vector > observers; + const std::vector init_command = { + 0xFE, 0xFE, 0x0F, 0x3C, 0x08, 0x00, 0x08, 0x00, 0x08, 0x00, 0x08, 0x00, 0x08, 0x00, 0x08, 0x00, 0x14, 0xFA + }; + + // pub + ChannelPublisherPtr pubQtCommand = ChannelPublisherPtr( + new ChannelPublisher(COMMANDTOPIC) + ); + ChannelPublisherPtr pubQtNode = ChannelPublisherPtr( + new ChannelPublisher(ADDNODETOPIC) + ); + ChannelPublisherPtr pubQtEdge = ChannelPublisherPtr( + new ChannelPublisher(ADDEDGETOPIC) + ); + + // sub + ChannelSubscriberPtr subQtNotice = ChannelSubscriberPtr( + new ChannelSubscriber(NOTICETOPIC) + ); + ChannelSubscriberPtr subOdometer = ChannelSubscriberPtr( + new ChannelSubscriber(ODOMTOPIC) + ); }; #endif //SLAM_WRAPPER_HPP diff --git a/src/message_handler.cpp b/src/message_handler.cpp index bbbbcaf..e479c49 100644 --- a/src/message_handler.cpp +++ b/src/message_handler.cpp @@ -3,30 +3,21 @@ #include #include "tcp_client.hpp" -#include "tcp_service.hpp" #include "serial_port_wrapper.hpp" int main() { - // 创建一个共享的 io 对象 - boost::asio::io_service io; - auto service = new TcpService(io, "/dev/ttyACM0", 115200); //机械臂串口 - // 启动TCP客户端，接收来自Web端的命令，控制机器狗运动 - std::thread client_thread([&io,&service] { - TcpClient client("eth0", service); + std::thread client_thread([] { + boost::asio::io_service io; + TcpClient client; client.connect("192.168.123.18", 9001); io.run(); }); - // 启动TCP服务，接收AI结果并通过串口控制机械臂 - std::thread service_thread([&io,&service] { - service->start(8888); - io.run(); - }); - // 启动甲烷数据查询串口线程，定时发送查询指令 - std::thread gas_serial_port_thread([&io] { + std::thread gas_serial_port_thread([] { try { + boost::asio::io_service io; //打开串口 SerialPortWrapper wrapper(io, "/dev/ttyUSB0", 115200); //甲烷串口 wrapper.read_from_port(); @@ -38,7 +29,6 @@ // 等待所有线程完成 client_thread.join(); - service_thread.join(); gas_serial_port_thread.join(); return 0; } diff --git a/src/message_observer.hpp b/src/message_observer.hpp new file mode 100644 index 0000000..b6b514d --- /dev/null +++ b/src/message_observer.hpp @@ -0,0 +1,17 @@ +// +// Created by pengx on 2025/3/7. +// + +#ifndef MESSAGE_OBSERVER_HPP +#define MESSAGE_OBSERVER_HPP + +#include +#include + +class MessageObserver { +public: + virtual ~MessageObserver() = default; + virtual void on_message_received(const std::vector &command) = 0; +}; + +#endif //MESSAGE_OBSERVER_HPP diff --git a/src/slam_wrapper.cpp b/src/slam_wrapper.cpp index ba616c1..a99fb45 100644 --- a/src/slam_wrapper.cpp +++ b/src/slam_wrapper.cpp @@ -7,8 +7,8 @@ #include #include -SlamWrapper::SlamWrapper(const char *networkInterface, TcpService *tcp_service): _tcp_service(tcp_service) { - ChannelFactory::Instance()->Init(0, networkInterface); +SlamWrapper::SlamWrapper() { + ChannelFactory::Instance()->Init(0, "eth0"); // pub pubQtCommand->InitChannel(); @@ -20,6 +20,34 @@ subOdometer->InitChannel(std::bind(&SlamWrapper::odometer_handler, this, std::placeholders::_1), 1); } +void SlamWrapper::register_observer(const std::shared_ptr &observer) { + observers.push_back(observer); +} + +void SlamWrapper::notify_observers(const std::vector &command) { + for (const auto &observer: observers) { + observer->on_message_received(command); + } +} + +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 + }; + const std::vector right_command = { + 0xFE, 0xFE, 0x0F, 0x3C, 0x0A, 0x40, 0x0A, 0x89, 0x03, 0xDE, 0x08, 0x43, 0x09, 0xA8, 0x07, 0xE1, 0x1E, 0xFA + }; + + notify_observers(init_command); + std::this_thread::sleep_for(std::chrono::seconds(3)); + notify_observers(left_command); + std::this_thread::sleep_for(std::chrono::seconds(3)); + notify_observers(right_command); + std::this_thread::sleep_for(std::chrono::seconds(3)); + notify_observers(init_command); + std::cout << "Control robotic arm end" << std::endl; +} + void SlamWrapper::qt_notice_handler(const void *message) { const auto seq = static_cast(message); std::string str_; @@ -58,7 +86,7 @@ //TODO 控制机械臂转动到预设的角度 //朝右下方 std::cout << "发送机械臂转动指令，倒计时开始，请等待10秒" << std::endl; - _tcp_service->send_command({ + notify_observers({ 0xFE, 0xFE, 0x0F, 0x3C, 0x0B, 0x71, 0x08, 0x25, 0x06, 0xFB, 0x08, 0x2E, 0x0A, 0xCB, 0x08, 0x28, @@ -71,13 +99,7 @@ std::this_thread::sleep_for(std::chrono::seconds(10)); recover_navigation(); std::cout << "倒计时结束，恢复机械臂初始位置，恢复巡检"; - //水平朝前方向 - _tcp_service->send_command({ - 0xFE, 0xFE, 0x0F, 0x3C, - 0x08, 0x00, 0x08, 0x00, 0x08, 0x00, - 0x08, 0x00, 0x08, 0x00, 0x08, 0x00, - 0x14, 0xFA - }); + notify_observers(init_command); } catch (const std::exception &e) { std::cerr << "线程执行过程中发生异常: " << e.what() << std::endl; } diff --git a/src/slam_wrapper.hpp b/src/slam_wrapper.hpp index bc07ed4..d8369a8 100644 --- a/src/slam_wrapper.hpp +++ b/src/slam_wrapper.hpp @@ -44,36 +44,18 @@ }; class SlamWrapper { -private: - int index = 0; - const Odometry_ *currentOdom{}; - TcpService *_tcp_service; - - // pub - ChannelPublisherPtr pubQtCommand = ChannelPublisherPtr( - new ChannelPublisher(COMMANDTOPIC) - ); - ChannelPublisherPtr pubQtNode = ChannelPublisherPtr( - new ChannelPublisher(ADDNODETOPIC) - ); - ChannelPublisherPtr pubQtEdge = ChannelPublisherPtr( - new ChannelPublisher(ADDEDGETOPIC) - ); - - // sub - ChannelSubscriberPtr subQtNotice = ChannelSubscriberPtr( - new ChannelSubscriber(NOTICETOPIC) - ); - ChannelSubscriberPtr subOdometer = ChannelSubscriberPtr( - new ChannelSubscriber(ODOMTOPIC) - ); - public: std::vector node_attributes; std::vector edge_attributes; u_int16_t node_name = 0; - explicit SlamWrapper(const char *networkInterface, TcpService *tcp_service); + void register_observer(const std::shared_ptr &observer); + + void notify_observers(const std::vector &command); + + void control_robotic_arm(); + + explicit SlamWrapper(); void qt_notice_handler(const void *message); @@ -106,6 +88,33 @@ void pause_navigation(); void recover_navigation(); + +private: + int index = 0; + const Odometry_ *currentOdom{}; + std::vector > observers; + const std::vector init_command = { + 0xFE, 0xFE, 0x0F, 0x3C, 0x08, 0x00, 0x08, 0x00, 0x08, 0x00, 0x08, 0x00, 0x08, 0x00, 0x08, 0x00, 0x14, 0xFA + }; + + // pub + ChannelPublisherPtr pubQtCommand = ChannelPublisherPtr( + new ChannelPublisher(COMMANDTOPIC) + ); + ChannelPublisherPtr pubQtNode = ChannelPublisherPtr( + new ChannelPublisher(ADDNODETOPIC) + ); + ChannelPublisherPtr pubQtEdge = ChannelPublisherPtr( + new ChannelPublisher(ADDEDGETOPIC) + ); + + // sub + ChannelSubscriberPtr subQtNotice = ChannelSubscriberPtr( + new ChannelSubscriber(NOTICETOPIC) + ); + ChannelSubscriberPtr subOdometer = ChannelSubscriberPtr( + new ChannelSubscriber(ODOMTOPIC) + ); }; #endif //SLAM_WRAPPER_HPP diff --git a/src/tcp_client.cpp b/src/tcp_client.cpp index 2506db4..15786d8 100644 --- a/src/tcp_client.cpp +++ b/src/tcp_client.cpp @@ -3,13 +3,26 @@ // #include "tcp_client.hpp" +#include "tcp_service.hpp" +#include "slam_wrapper.hpp" + #include #include #include #include #include -TcpClient::TcpClient(const char *networkInterface, TcpService *tcp_service): _slam(networkInterface, tcp_service) { +TcpClient::TcpClient() { + _slam = new SlamWrapper(); + // 启动TCP服务，接收AI结果并通过串口控制机械臂 + std::thread service_thread([this] { + boost::asio::io_service io; + const auto service = std::make_shared(io, "/dev/ttyACM0", 115200); // 机械臂串口 + _slam->register_observer(service); + service->start(8888); + io.run(); + }); + service_thread.detach(); std::cout << "TcpClient::init success" << std::endl; } @@ -18,53 +31,56 @@ std::cout << "handle_data >>>> " << received_string << std::endl; if (received_string == "q") { std::cout << "Close all nodes" << std::endl; - _slam.close_all_node(); + _slam->close_all_node(); } else if (received_string == "w") { std::cout << "Start mapping (default to clearing node/edge information)" << std::endl; - _slam.delete_all_node(); - _slam.delete_all_edge(); - _slam.node_attributes.clear(); - _slam.edge_attributes.clear(); - _slam.start_mapping(); - _slam.node_name = 0; + _slam->delete_all_node(); + _slam->delete_all_edge(); + _slam->node_attributes.clear(); + _slam->edge_attributes.clear(); + _slam->start_mapping(); + _slam->node_name = 0; } else if (received_string == "e") { std::cout << "End mapping" << std::endl; - _slam.end_mapping(); + _slam->end_mapping(); } else if (received_string == "a") { std::cout << "Start navigation (default)" << std::endl; - _slam.start_relocation(); - _slam.start_navigation(); - _slam.init_pose(); - _slam.default_navigation(); + _slam->start_relocation(); + _slam->start_navigation(); + _slam->init_pose(); + _slam->default_navigation(); } else if (received_string == "s") { std::cout << "Pause navigation" << std::endl; - _slam.pause_navigation(); + _slam->pause_navigation(); } else if (received_string == "d") { std::cout << "Recover navigation" << std::endl; - _slam.recover_navigation(); + _slam->recover_navigation(); } else if (received_string == "z") { std::cout << "Start relocation and navigation to prepare for collecting node/edge information. " << "(Default clearing of node/edge information)" << std::endl; - _slam.delete_all_node(); - _slam.delete_all_edge(); - _slam.start_relocation(); - _slam.start_navigation(); - _slam.init_pose(); + _slam->delete_all_node(); + _slam->delete_all_edge(); + _slam->start_relocation(); + _slam->start_navigation(); + _slam->init_pose(); } else if (received_string == "x") { std::cout << "Collect node/edge information" << std::endl; - _slam.add_node_and_edge(); + _slam->add_node_and_edge(); } else if (received_string == "c") { std::cout << "Save node/edge information" << std::endl; - _slam.save_node_and_edge(); - _slam.node_attributes.clear(); - _slam.edge_attributes.clear(); - _slam.node_name = 0; + _slam->save_node_and_edge(); + _slam->node_attributes.clear(); + _slam->edge_attributes.clear(); + _slam->node_name = 0; } else if (received_string == "v") { std::cout << "Clear node/edge information" << std::endl; - _slam.delete_all_node(); - _slam.delete_all_edge(); - _slam.node_name = 0; + _slam->delete_all_node(); + _slam->delete_all_edge(); + _slam->node_name = 0; + } else if (received_string == "scan") { + std::cout << "Control robotic arm start" << std::endl; + _slam->control_robotic_arm(); } else { std::cout << "Unknown command" << std::endl; } @@ -97,7 +113,7 @@ received_data.append(temp_buffer, bytes_received); - // TODO 查找结束标志 + // 查找结束标志 while (true) { const size_t pos = received_data.find('/'); if (pos == std::string::npos) { diff --git a/src/message_handler.cpp b/src/message_handler.cpp index bbbbcaf..e479c49 100644 --- a/src/message_handler.cpp +++ b/src/message_handler.cpp @@ -3,30 +3,21 @@ #include #include "tcp_client.hpp" -#include "tcp_service.hpp" #include "serial_port_wrapper.hpp" int main() { - // 创建一个共享的 io 对象 - boost::asio::io_service io; - auto service = new TcpService(io, "/dev/ttyACM0", 115200); //机械臂串口 - // 启动TCP客户端，接收来自Web端的命令，控制机器狗运动 - std::thread client_thread([&io,&service] { - TcpClient client("eth0", service); + std::thread client_thread([] { + boost::asio::io_service io; + TcpClient client; client.connect("192.168.123.18", 9001); io.run(); }); - // 启动TCP服务，接收AI结果并通过串口控制机械臂 - std::thread service_thread([&io,&service] { - service->start(8888); - io.run(); - }); - // 启动甲烷数据查询串口线程，定时发送查询指令 - std::thread gas_serial_port_thread([&io] { + std::thread gas_serial_port_thread([] { try { + boost::asio::io_service io; //打开串口 SerialPortWrapper wrapper(io, "/dev/ttyUSB0", 115200); //甲烷串口 wrapper.read_from_port(); @@ -38,7 +29,6 @@ // 等待所有线程完成 client_thread.join(); - service_thread.join(); gas_serial_port_thread.join(); return 0; } diff --git a/src/message_observer.hpp b/src/message_observer.hpp new file mode 100644 index 0000000..b6b514d --- /dev/null +++ b/src/message_observer.hpp @@ -0,0 +1,17 @@ +// +// Created by pengx on 2025/3/7. +// + +#ifndef MESSAGE_OBSERVER_HPP +#define MESSAGE_OBSERVER_HPP + +#include +#include + +class MessageObserver { +public: + virtual ~MessageObserver() = default; + virtual void on_message_received(const std::vector &command) = 0; +}; + +#endif //MESSAGE_OBSERVER_HPP diff --git a/src/slam_wrapper.cpp b/src/slam_wrapper.cpp index ba616c1..a99fb45 100644 --- a/src/slam_wrapper.cpp +++ b/src/slam_wrapper.cpp @@ -7,8 +7,8 @@ #include #include -SlamWrapper::SlamWrapper(const char *networkInterface, TcpService *tcp_service): _tcp_service(tcp_service) { - ChannelFactory::Instance()->Init(0, networkInterface); +SlamWrapper::SlamWrapper() { + ChannelFactory::Instance()->Init(0, "eth0"); // pub pubQtCommand->InitChannel(); @@ -20,6 +20,34 @@ subOdometer->InitChannel(std::bind(&SlamWrapper::odometer_handler, this, std::placeholders::_1), 1); } +void SlamWrapper::register_observer(const std::shared_ptr &observer) { + observers.push_back(observer); +} + +void SlamWrapper::notify_observers(const std::vector &command) { + for (const auto &observer: observers) { + observer->on_message_received(command); + } +} + +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 + }; + const std::vector right_command = { + 0xFE, 0xFE, 0x0F, 0x3C, 0x0A, 0x40, 0x0A, 0x89, 0x03, 0xDE, 0x08, 0x43, 0x09, 0xA8, 0x07, 0xE1, 0x1E, 0xFA + }; + + notify_observers(init_command); + std::this_thread::sleep_for(std::chrono::seconds(3)); + notify_observers(left_command); + std::this_thread::sleep_for(std::chrono::seconds(3)); + notify_observers(right_command); + std::this_thread::sleep_for(std::chrono::seconds(3)); + notify_observers(init_command); + std::cout << "Control robotic arm end" << std::endl; +} + void SlamWrapper::qt_notice_handler(const void *message) { const auto seq = static_cast(message); std::string str_; @@ -58,7 +86,7 @@ //TODO 控制机械臂转动到预设的角度 //朝右下方 std::cout << "发送机械臂转动指令，倒计时开始，请等待10秒" << std::endl; - _tcp_service->send_command({ + notify_observers({ 0xFE, 0xFE, 0x0F, 0x3C, 0x0B, 0x71, 0x08, 0x25, 0x06, 0xFB, 0x08, 0x2E, 0x0A, 0xCB, 0x08, 0x28, @@ -71,13 +99,7 @@ std::this_thread::sleep_for(std::chrono::seconds(10)); recover_navigation(); std::cout << "倒计时结束，恢复机械臂初始位置，恢复巡检"; - //水平朝前方向 - _tcp_service->send_command({ - 0xFE, 0xFE, 0x0F, 0x3C, - 0x08, 0x00, 0x08, 0x00, 0x08, 0x00, - 0x08, 0x00, 0x08, 0x00, 0x08, 0x00, - 0x14, 0xFA - }); + notify_observers(init_command); } catch (const std::exception &e) { std::cerr << "线程执行过程中发生异常: " << e.what() << std::endl; } diff --git a/src/slam_wrapper.hpp b/src/slam_wrapper.hpp index bc07ed4..d8369a8 100644 --- a/src/slam_wrapper.hpp +++ b/src/slam_wrapper.hpp @@ -44,36 +44,18 @@ }; class SlamWrapper { -private: - int index = 0; - const Odometry_ *currentOdom{}; - TcpService *_tcp_service; - - // pub - ChannelPublisherPtr pubQtCommand = ChannelPublisherPtr( - new ChannelPublisher(COMMANDTOPIC) - ); - ChannelPublisherPtr pubQtNode = ChannelPublisherPtr( - new ChannelPublisher(ADDNODETOPIC) - ); - ChannelPublisherPtr pubQtEdge = ChannelPublisherPtr( - new ChannelPublisher(ADDEDGETOPIC) - ); - - // sub - ChannelSubscriberPtr subQtNotice = ChannelSubscriberPtr( - new ChannelSubscriber(NOTICETOPIC) - ); - ChannelSubscriberPtr subOdometer = ChannelSubscriberPtr( - new ChannelSubscriber(ODOMTOPIC) - ); - public: std::vector node_attributes; std::vector edge_attributes; u_int16_t node_name = 0; - explicit SlamWrapper(const char *networkInterface, TcpService *tcp_service); + void register_observer(const std::shared_ptr &observer); + + void notify_observers(const std::vector &command); + + void control_robotic_arm(); + + explicit SlamWrapper(); void qt_notice_handler(const void *message); @@ -106,6 +88,33 @@ void pause_navigation(); void recover_navigation(); + +private: + int index = 0; + const Odometry_ *currentOdom{}; + std::vector > observers; + const std::vector init_command = { + 0xFE, 0xFE, 0x0F, 0x3C, 0x08, 0x00, 0x08, 0x00, 0x08, 0x00, 0x08, 0x00, 0x08, 0x00, 0x08, 0x00, 0x14, 0xFA + }; + + // pub + ChannelPublisherPtr pubQtCommand = ChannelPublisherPtr( + new ChannelPublisher(COMMANDTOPIC) + ); + ChannelPublisherPtr pubQtNode = ChannelPublisherPtr( + new ChannelPublisher(ADDNODETOPIC) + ); + ChannelPublisherPtr pubQtEdge = ChannelPublisherPtr( + new ChannelPublisher(ADDEDGETOPIC) + ); + + // sub + ChannelSubscriberPtr subQtNotice = ChannelSubscriberPtr( + new ChannelSubscriber(NOTICETOPIC) + ); + ChannelSubscriberPtr subOdometer = ChannelSubscriberPtr( + new ChannelSubscriber(ODOMTOPIC) + ); }; #endif //SLAM_WRAPPER_HPP diff --git a/src/tcp_client.cpp b/src/tcp_client.cpp index 2506db4..15786d8 100644 --- a/src/tcp_client.cpp +++ b/src/tcp_client.cpp @@ -3,13 +3,26 @@ // #include "tcp_client.hpp" +#include "tcp_service.hpp" +#include "slam_wrapper.hpp" + #include #include #include #include #include -TcpClient::TcpClient(const char *networkInterface, TcpService *tcp_service): _slam(networkInterface, tcp_service) { +TcpClient::TcpClient() { + _slam = new SlamWrapper(); + // 启动TCP服务，接收AI结果并通过串口控制机械臂 + std::thread service_thread([this] { + boost::asio::io_service io; + const auto service = std::make_shared(io, "/dev/ttyACM0", 115200); // 机械臂串口 + _slam->register_observer(service); + service->start(8888); + io.run(); + }); + service_thread.detach(); std::cout << "TcpClient::init success" << std::endl; } @@ -18,53 +31,56 @@ std::cout << "handle_data >>>> " << received_string << std::endl; if (received_string == "q") { std::cout << "Close all nodes" << std::endl; - _slam.close_all_node(); + _slam->close_all_node(); } else if (received_string == "w") { std::cout << "Start mapping (default to clearing node/edge information)" << std::endl; - _slam.delete_all_node(); - _slam.delete_all_edge(); - _slam.node_attributes.clear(); - _slam.edge_attributes.clear(); - _slam.start_mapping(); - _slam.node_name = 0; + _slam->delete_all_node(); + _slam->delete_all_edge(); + _slam->node_attributes.clear(); + _slam->edge_attributes.clear(); + _slam->start_mapping(); + _slam->node_name = 0; } else if (received_string == "e") { std::cout << "End mapping" << std::endl; - _slam.end_mapping(); + _slam->end_mapping(); } else if (received_string == "a") { std::cout << "Start navigation (default)" << std::endl; - _slam.start_relocation(); - _slam.start_navigation(); - _slam.init_pose(); - _slam.default_navigation(); + _slam->start_relocation(); + _slam->start_navigation(); + _slam->init_pose(); + _slam->default_navigation(); } else if (received_string == "s") { std::cout << "Pause navigation" << std::endl; - _slam.pause_navigation(); + _slam->pause_navigation(); } else if (received_string == "d") { std::cout << "Recover navigation" << std::endl; - _slam.recover_navigation(); + _slam->recover_navigation(); } else if (received_string == "z") { std::cout << "Start relocation and navigation to prepare for collecting node/edge information. " << "(Default clearing of node/edge information)" << std::endl; - _slam.delete_all_node(); - _slam.delete_all_edge(); - _slam.start_relocation(); - _slam.start_navigation(); - _slam.init_pose(); + _slam->delete_all_node(); + _slam->delete_all_edge(); + _slam->start_relocation(); + _slam->start_navigation(); + _slam->init_pose(); } else if (received_string == "x") { std::cout << "Collect node/edge information" << std::endl; - _slam.add_node_and_edge(); + _slam->add_node_and_edge(); } else if (received_string == "c") { std::cout << "Save node/edge information" << std::endl; - _slam.save_node_and_edge(); - _slam.node_attributes.clear(); - _slam.edge_attributes.clear(); - _slam.node_name = 0; + _slam->save_node_and_edge(); + _slam->node_attributes.clear(); + _slam->edge_attributes.clear(); + _slam->node_name = 0; } else if (received_string == "v") { std::cout << "Clear node/edge information" << std::endl; - _slam.delete_all_node(); - _slam.delete_all_edge(); - _slam.node_name = 0; + _slam->delete_all_node(); + _slam->delete_all_edge(); + _slam->node_name = 0; + } else if (received_string == "scan") { + std::cout << "Control robotic arm start" << std::endl; + _slam->control_robotic_arm(); } else { std::cout << "Unknown command" << std::endl; } @@ -97,7 +113,7 @@ received_data.append(temp_buffer, bytes_received); - // TODO 查找结束标志 + // 查找结束标志 while (true) { const size_t pos = received_data.find('/'); if (pos == std::string::npos) { diff --git a/src/tcp_client.hpp b/src/tcp_client.hpp index 8f5bdb8..afa15f4 100644 --- a/src/tcp_client.hpp +++ b/src/tcp_client.hpp @@ -11,11 +11,10 @@ #include #include #include "slam_wrapper.hpp" -#include "tcp_service.hpp" class TcpClient { public: - explicit TcpClient(const char *networkInterface, TcpService *tcp_service); + TcpClient(); ~TcpClient() { if (_client_socket != -1) { @@ -33,8 +32,7 @@ int _client_socket{-1}; int _max_retries = 12; int _retry_interval = 5000; //重连间隔 - SlamWrapper _slam; - TcpService *_tcp_service; + SlamWrapper *_slam; void receive_data(); diff --git a/src/message_handler.cpp b/src/message_handler.cpp index bbbbcaf..e479c49 100644 --- a/src/message_handler.cpp +++ b/src/message_handler.cpp @@ -3,30 +3,21 @@ #include #include "tcp_client.hpp" -#include "tcp_service.hpp" #include "serial_port_wrapper.hpp" int main() { - // 创建一个共享的 io 对象 - boost::asio::io_service io; - auto service = new TcpService(io, "/dev/ttyACM0", 115200); //机械臂串口 - // 启动TCP客户端，接收来自Web端的命令，控制机器狗运动 - std::thread client_thread([&io,&service] { - TcpClient client("eth0", service); + std::thread client_thread([] { + boost::asio::io_service io; + TcpClient client; client.connect("192.168.123.18", 9001); io.run(); }); - // 启动TCP服务，接收AI结果并通过串口控制机械臂 - std::thread service_thread([&io,&service] { - service->start(8888); - io.run(); - }); - // 启动甲烷数据查询串口线程，定时发送查询指令 - std::thread gas_serial_port_thread([&io] { + std::thread gas_serial_port_thread([] { try { + boost::asio::io_service io; //打开串口 SerialPortWrapper wrapper(io, "/dev/ttyUSB0", 115200); //甲烷串口 wrapper.read_from_port(); @@ -38,7 +29,6 @@ // 等待所有线程完成 client_thread.join(); - service_thread.join(); gas_serial_port_thread.join(); return 0; } diff --git a/src/message_observer.hpp b/src/message_observer.hpp new file mode 100644 index 0000000..b6b514d --- /dev/null +++ b/src/message_observer.hpp @@ -0,0 +1,17 @@ +// +// Created by pengx on 2025/3/7. +// + +#ifndef MESSAGE_OBSERVER_HPP +#define MESSAGE_OBSERVER_HPP + +#include +#include + +class MessageObserver { +public: + virtual ~MessageObserver() = default; + virtual void on_message_received(const std::vector &command) = 0; +}; + +#endif //MESSAGE_OBSERVER_HPP diff --git a/src/slam_wrapper.cpp b/src/slam_wrapper.cpp index ba616c1..a99fb45 100644 --- a/src/slam_wrapper.cpp +++ b/src/slam_wrapper.cpp @@ -7,8 +7,8 @@ #include #include -SlamWrapper::SlamWrapper(const char *networkInterface, TcpService *tcp_service): _tcp_service(tcp_service) { - ChannelFactory::Instance()->Init(0, networkInterface); +SlamWrapper::SlamWrapper() { + ChannelFactory::Instance()->Init(0, "eth0"); // pub pubQtCommand->InitChannel(); @@ -20,6 +20,34 @@ subOdometer->InitChannel(std::bind(&SlamWrapper::odometer_handler, this, std::placeholders::_1), 1); } +void SlamWrapper::register_observer(const std::shared_ptr &observer) { + observers.push_back(observer); +} + +void SlamWrapper::notify_observers(const std::vector &command) { + for (const auto &observer: observers) { + observer->on_message_received(command); + } +} + +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 + }; + const std::vector right_command = { + 0xFE, 0xFE, 0x0F, 0x3C, 0x0A, 0x40, 0x0A, 0x89, 0x03, 0xDE, 0x08, 0x43, 0x09, 0xA8, 0x07, 0xE1, 0x1E, 0xFA + }; + + notify_observers(init_command); + std::this_thread::sleep_for(std::chrono::seconds(3)); + notify_observers(left_command); + std::this_thread::sleep_for(std::chrono::seconds(3)); + notify_observers(right_command); + std::this_thread::sleep_for(std::chrono::seconds(3)); + notify_observers(init_command); + std::cout << "Control robotic arm end" << std::endl; +} + void SlamWrapper::qt_notice_handler(const void *message) { const auto seq = static_cast(message); std::string str_; @@ -58,7 +86,7 @@ //TODO 控制机械臂转动到预设的角度 //朝右下方 std::cout << "发送机械臂转动指令，倒计时开始，请等待10秒" << std::endl; - _tcp_service->send_command({ + notify_observers({ 0xFE, 0xFE, 0x0F, 0x3C, 0x0B, 0x71, 0x08, 0x25, 0x06, 0xFB, 0x08, 0x2E, 0x0A, 0xCB, 0x08, 0x28, @@ -71,13 +99,7 @@ std::this_thread::sleep_for(std::chrono::seconds(10)); recover_navigation(); std::cout << "倒计时结束，恢复机械臂初始位置，恢复巡检"; - //水平朝前方向 - _tcp_service->send_command({ - 0xFE, 0xFE, 0x0F, 0x3C, - 0x08, 0x00, 0x08, 0x00, 0x08, 0x00, - 0x08, 0x00, 0x08, 0x00, 0x08, 0x00, - 0x14, 0xFA - }); + notify_observers(init_command); } catch (const std::exception &e) { std::cerr << "线程执行过程中发生异常: " << e.what() << std::endl; } diff --git a/src/slam_wrapper.hpp b/src/slam_wrapper.hpp index bc07ed4..d8369a8 100644 --- a/src/slam_wrapper.hpp +++ b/src/slam_wrapper.hpp @@ -44,36 +44,18 @@ }; class SlamWrapper { -private: - int index = 0; - const Odometry_ *currentOdom{}; - TcpService *_tcp_service; - - // pub - ChannelPublisherPtr pubQtCommand = ChannelPublisherPtr( - new ChannelPublisher(COMMANDTOPIC) - ); - ChannelPublisherPtr pubQtNode = ChannelPublisherPtr( - new ChannelPublisher(ADDNODETOPIC) - ); - ChannelPublisherPtr pubQtEdge = ChannelPublisherPtr( - new ChannelPublisher(ADDEDGETOPIC) - ); - - // sub - ChannelSubscriberPtr subQtNotice = ChannelSubscriberPtr( - new ChannelSubscriber(NOTICETOPIC) - ); - ChannelSubscriberPtr subOdometer = ChannelSubscriberPtr( - new ChannelSubscriber(ODOMTOPIC) - ); - public: std::vector node_attributes; std::vector edge_attributes; u_int16_t node_name = 0; - explicit SlamWrapper(const char *networkInterface, TcpService *tcp_service); + void register_observer(const std::shared_ptr &observer); + + void notify_observers(const std::vector &command); + + void control_robotic_arm(); + + explicit SlamWrapper(); void qt_notice_handler(const void *message); @@ -106,6 +88,33 @@ void pause_navigation(); void recover_navigation(); + +private: + int index = 0; + const Odometry_ *currentOdom{}; + std::vector > observers; + const std::vector init_command = { + 0xFE, 0xFE, 0x0F, 0x3C, 0x08, 0x00, 0x08, 0x00, 0x08, 0x00, 0x08, 0x00, 0x08, 0x00, 0x08, 0x00, 0x14, 0xFA + }; + + // pub + ChannelPublisherPtr pubQtCommand = ChannelPublisherPtr( + new ChannelPublisher(COMMANDTOPIC) + ); + ChannelPublisherPtr pubQtNode = ChannelPublisherPtr( + new ChannelPublisher(ADDNODETOPIC) + ); + ChannelPublisherPtr pubQtEdge = ChannelPublisherPtr( + new ChannelPublisher(ADDEDGETOPIC) + ); + + // sub + ChannelSubscriberPtr subQtNotice = ChannelSubscriberPtr( + new ChannelSubscriber(NOTICETOPIC) + ); + ChannelSubscriberPtr subOdometer = ChannelSubscriberPtr( + new ChannelSubscriber(ODOMTOPIC) + ); }; #endif //SLAM_WRAPPER_HPP diff --git a/src/tcp_client.cpp b/src/tcp_client.cpp index 2506db4..15786d8 100644 --- a/src/tcp_client.cpp +++ b/src/tcp_client.cpp @@ -3,13 +3,26 @@ // #include "tcp_client.hpp" +#include "tcp_service.hpp" +#include "slam_wrapper.hpp" + #include #include #include #include #include -TcpClient::TcpClient(const char *networkInterface, TcpService *tcp_service): _slam(networkInterface, tcp_service) { +TcpClient::TcpClient() { + _slam = new SlamWrapper(); + // 启动TCP服务，接收AI结果并通过串口控制机械臂 + std::thread service_thread([this] { + boost::asio::io_service io; + const auto service = std::make_shared(io, "/dev/ttyACM0", 115200); // 机械臂串口 + _slam->register_observer(service); + service->start(8888); + io.run(); + }); + service_thread.detach(); std::cout << "TcpClient::init success" << std::endl; } @@ -18,53 +31,56 @@ std::cout << "handle_data >>>> " << received_string << std::endl; if (received_string == "q") { std::cout << "Close all nodes" << std::endl; - _slam.close_all_node(); + _slam->close_all_node(); } else if (received_string == "w") { std::cout << "Start mapping (default to clearing node/edge information)" << std::endl; - _slam.delete_all_node(); - _slam.delete_all_edge(); - _slam.node_attributes.clear(); - _slam.edge_attributes.clear(); - _slam.start_mapping(); - _slam.node_name = 0; + _slam->delete_all_node(); + _slam->delete_all_edge(); + _slam->node_attributes.clear(); + _slam->edge_attributes.clear(); + _slam->start_mapping(); + _slam->node_name = 0; } else if (received_string == "e") { std::cout << "End mapping" << std::endl; - _slam.end_mapping(); + _slam->end_mapping(); } else if (received_string == "a") { std::cout << "Start navigation (default)" << std::endl; - _slam.start_relocation(); - _slam.start_navigation(); - _slam.init_pose(); - _slam.default_navigation(); + _slam->start_relocation(); + _slam->start_navigation(); + _slam->init_pose(); + _slam->default_navigation(); } else if (received_string == "s") { std::cout << "Pause navigation" << std::endl; - _slam.pause_navigation(); + _slam->pause_navigation(); } else if (received_string == "d") { std::cout << "Recover navigation" << std::endl; - _slam.recover_navigation(); + _slam->recover_navigation(); } else if (received_string == "z") { std::cout << "Start relocation and navigation to prepare for collecting node/edge information. " << "(Default clearing of node/edge information)" << std::endl; - _slam.delete_all_node(); - _slam.delete_all_edge(); - _slam.start_relocation(); - _slam.start_navigation(); - _slam.init_pose(); + _slam->delete_all_node(); + _slam->delete_all_edge(); + _slam->start_relocation(); + _slam->start_navigation(); + _slam->init_pose(); } else if (received_string == "x") { std::cout << "Collect node/edge information" << std::endl; - _slam.add_node_and_edge(); + _slam->add_node_and_edge(); } else if (received_string == "c") { std::cout << "Save node/edge information" << std::endl; - _slam.save_node_and_edge(); - _slam.node_attributes.clear(); - _slam.edge_attributes.clear(); - _slam.node_name = 0; + _slam->save_node_and_edge(); + _slam->node_attributes.clear(); + _slam->edge_attributes.clear(); + _slam->node_name = 0; } else if (received_string == "v") { std::cout << "Clear node/edge information" << std::endl; - _slam.delete_all_node(); - _slam.delete_all_edge(); - _slam.node_name = 0; + _slam->delete_all_node(); + _slam->delete_all_edge(); + _slam->node_name = 0; + } else if (received_string == "scan") { + std::cout << "Control robotic arm start" << std::endl; + _slam->control_robotic_arm(); } else { std::cout << "Unknown command" << std::endl; } @@ -97,7 +113,7 @@ received_data.append(temp_buffer, bytes_received); - // TODO 查找结束标志 + // 查找结束标志 while (true) { const size_t pos = received_data.find('/'); if (pos == std::string::npos) { diff --git a/src/tcp_client.hpp b/src/tcp_client.hpp index 8f5bdb8..afa15f4 100644 --- a/src/tcp_client.hpp +++ b/src/tcp_client.hpp @@ -11,11 +11,10 @@ #include #include #include "slam_wrapper.hpp" -#include "tcp_service.hpp" class TcpClient { public: - explicit TcpClient(const char *networkInterface, TcpService *tcp_service); + TcpClient(); ~TcpClient() { if (_client_socket != -1) { @@ -33,8 +32,7 @@ int _client_socket{-1}; int _max_retries = 12; int _retry_interval = 5000; //重连间隔 - SlamWrapper _slam; - TcpService *_tcp_service; + SlamWrapper *_slam; void receive_data(); diff --git a/src/tcp_service.cpp b/src/tcp_service.cpp index b874a23..45ac332 100644 --- a/src/tcp_service.cpp +++ b/src/tcp_service.cpp @@ -14,7 +14,7 @@ #include #include -void TcpService::send_command(const std::vector &command) { +void TcpService::on_message_received(const std::vector &command) { boost::asio::write(_port, boost::asio::buffer(command)); } diff --git a/src/message_handler.cpp b/src/message_handler.cpp index bbbbcaf..e479c49 100644 --- a/src/message_handler.cpp +++ b/src/message_handler.cpp @@ -3,30 +3,21 @@ #include #include "tcp_client.hpp" -#include "tcp_service.hpp" #include "serial_port_wrapper.hpp" int main() { - // 创建一个共享的 io 对象 - boost::asio::io_service io; - auto service = new TcpService(io, "/dev/ttyACM0", 115200); //机械臂串口 - // 启动TCP客户端，接收来自Web端的命令，控制机器狗运动 - std::thread client_thread([&io,&service] { - TcpClient client("eth0", service); + std::thread client_thread([] { + boost::asio::io_service io; + TcpClient client; client.connect("192.168.123.18", 9001); io.run(); }); - // 启动TCP服务，接收AI结果并通过串口控制机械臂 - std::thread service_thread([&io,&service] { - service->start(8888); - io.run(); - }); - // 启动甲烷数据查询串口线程，定时发送查询指令 - std::thread gas_serial_port_thread([&io] { + std::thread gas_serial_port_thread([] { try { + boost::asio::io_service io; //打开串口 SerialPortWrapper wrapper(io, "/dev/ttyUSB0", 115200); //甲烷串口 wrapper.read_from_port(); @@ -38,7 +29,6 @@ // 等待所有线程完成 client_thread.join(); - service_thread.join(); gas_serial_port_thread.join(); return 0; } diff --git a/src/message_observer.hpp b/src/message_observer.hpp new file mode 100644 index 0000000..b6b514d --- /dev/null +++ b/src/message_observer.hpp @@ -0,0 +1,17 @@ +// +// Created by pengx on 2025/3/7. +// + +#ifndef MESSAGE_OBSERVER_HPP +#define MESSAGE_OBSERVER_HPP + +#include +#include + +class MessageObserver { +public: + virtual ~MessageObserver() = default; + virtual void on_message_received(const std::vector &command) = 0; +}; + +#endif //MESSAGE_OBSERVER_HPP diff --git a/src/slam_wrapper.cpp b/src/slam_wrapper.cpp index ba616c1..a99fb45 100644 --- a/src/slam_wrapper.cpp +++ b/src/slam_wrapper.cpp @@ -7,8 +7,8 @@ #include #include -SlamWrapper::SlamWrapper(const char *networkInterface, TcpService *tcp_service): _tcp_service(tcp_service) { - ChannelFactory::Instance()->Init(0, networkInterface); +SlamWrapper::SlamWrapper() { + ChannelFactory::Instance()->Init(0, "eth0"); // pub pubQtCommand->InitChannel(); @@ -20,6 +20,34 @@ subOdometer->InitChannel(std::bind(&SlamWrapper::odometer_handler, this, std::placeholders::_1), 1); } +void SlamWrapper::register_observer(const std::shared_ptr &observer) { + observers.push_back(observer); +} + +void SlamWrapper::notify_observers(const std::vector &command) { + for (const auto &observer: observers) { + observer->on_message_received(command); + } +} + +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 + }; + const std::vector right_command = { + 0xFE, 0xFE, 0x0F, 0x3C, 0x0A, 0x40, 0x0A, 0x89, 0x03, 0xDE, 0x08, 0x43, 0x09, 0xA8, 0x07, 0xE1, 0x1E, 0xFA + }; + + notify_observers(init_command); + std::this_thread::sleep_for(std::chrono::seconds(3)); + notify_observers(left_command); + std::this_thread::sleep_for(std::chrono::seconds(3)); + notify_observers(right_command); + std::this_thread::sleep_for(std::chrono::seconds(3)); + notify_observers(init_command); + std::cout << "Control robotic arm end" << std::endl; +} + void SlamWrapper::qt_notice_handler(const void *message) { const auto seq = static_cast(message); std::string str_; @@ -58,7 +86,7 @@ //TODO 控制机械臂转动到预设的角度 //朝右下方 std::cout << "发送机械臂转动指令，倒计时开始，请等待10秒" << std::endl; - _tcp_service->send_command({ + notify_observers({ 0xFE, 0xFE, 0x0F, 0x3C, 0x0B, 0x71, 0x08, 0x25, 0x06, 0xFB, 0x08, 0x2E, 0x0A, 0xCB, 0x08, 0x28, @@ -71,13 +99,7 @@ std::this_thread::sleep_for(std::chrono::seconds(10)); recover_navigation(); std::cout << "倒计时结束，恢复机械臂初始位置，恢复巡检"; - //水平朝前方向 - _tcp_service->send_command({ - 0xFE, 0xFE, 0x0F, 0x3C, - 0x08, 0x00, 0x08, 0x00, 0x08, 0x00, - 0x08, 0x00, 0x08, 0x00, 0x08, 0x00, - 0x14, 0xFA - }); + notify_observers(init_command); } catch (const std::exception &e) { std::cerr << "线程执行过程中发生异常: " << e.what() << std::endl; } diff --git a/src/slam_wrapper.hpp b/src/slam_wrapper.hpp index bc07ed4..d8369a8 100644 --- a/src/slam_wrapper.hpp +++ b/src/slam_wrapper.hpp @@ -44,36 +44,18 @@ }; class SlamWrapper { -private: - int index = 0; - const Odometry_ *currentOdom{}; - TcpService *_tcp_service; - - // pub - ChannelPublisherPtr pubQtCommand = ChannelPublisherPtr( - new ChannelPublisher(COMMANDTOPIC) - ); - ChannelPublisherPtr pubQtNode = ChannelPublisherPtr( - new ChannelPublisher(ADDNODETOPIC) - ); - ChannelPublisherPtr pubQtEdge = ChannelPublisherPtr( - new ChannelPublisher(ADDEDGETOPIC) - ); - - // sub - ChannelSubscriberPtr subQtNotice = ChannelSubscriberPtr( - new ChannelSubscriber(NOTICETOPIC) - ); - ChannelSubscriberPtr subOdometer = ChannelSubscriberPtr( - new ChannelSubscriber(ODOMTOPIC) - ); - public: std::vector node_attributes; std::vector edge_attributes; u_int16_t node_name = 0; - explicit SlamWrapper(const char *networkInterface, TcpService *tcp_service); + void register_observer(const std::shared_ptr &observer); + + void notify_observers(const std::vector &command); + + void control_robotic_arm(); + + explicit SlamWrapper(); void qt_notice_handler(const void *message); @@ -106,6 +88,33 @@ void pause_navigation(); void recover_navigation(); + +private: + int index = 0; + const Odometry_ *currentOdom{}; + std::vector > observers; + const std::vector init_command = { + 0xFE, 0xFE, 0x0F, 0x3C, 0x08, 0x00, 0x08, 0x00, 0x08, 0x00, 0x08, 0x00, 0x08, 0x00, 0x08, 0x00, 0x14, 0xFA + }; + + // pub + ChannelPublisherPtr pubQtCommand = ChannelPublisherPtr( + new ChannelPublisher(COMMANDTOPIC) + ); + ChannelPublisherPtr pubQtNode = ChannelPublisherPtr( + new ChannelPublisher(ADDNODETOPIC) + ); + ChannelPublisherPtr pubQtEdge = ChannelPublisherPtr( + new ChannelPublisher(ADDEDGETOPIC) + ); + + // sub + ChannelSubscriberPtr subQtNotice = ChannelSubscriberPtr( + new ChannelSubscriber(NOTICETOPIC) + ); + ChannelSubscriberPtr subOdometer = ChannelSubscriberPtr( + new ChannelSubscriber(ODOMTOPIC) + ); }; #endif //SLAM_WRAPPER_HPP diff --git a/src/tcp_client.cpp b/src/tcp_client.cpp index 2506db4..15786d8 100644 --- a/src/tcp_client.cpp +++ b/src/tcp_client.cpp @@ -3,13 +3,26 @@ // #include "tcp_client.hpp" +#include "tcp_service.hpp" +#include "slam_wrapper.hpp" + #include #include #include #include #include -TcpClient::TcpClient(const char *networkInterface, TcpService *tcp_service): _slam(networkInterface, tcp_service) { +TcpClient::TcpClient() { + _slam = new SlamWrapper(); + // 启动TCP服务，接收AI结果并通过串口控制机械臂 + std::thread service_thread([this] { + boost::asio::io_service io; + const auto service = std::make_shared(io, "/dev/ttyACM0", 115200); // 机械臂串口 + _slam->register_observer(service); + service->start(8888); + io.run(); + }); + service_thread.detach(); std::cout << "TcpClient::init success" << std::endl; } @@ -18,53 +31,56 @@ std::cout << "handle_data >>>> " << received_string << std::endl; if (received_string == "q") { std::cout << "Close all nodes" << std::endl; - _slam.close_all_node(); + _slam->close_all_node(); } else if (received_string == "w") { std::cout << "Start mapping (default to clearing node/edge information)" << std::endl; - _slam.delete_all_node(); - _slam.delete_all_edge(); - _slam.node_attributes.clear(); - _slam.edge_attributes.clear(); - _slam.start_mapping(); - _slam.node_name = 0; + _slam->delete_all_node(); + _slam->delete_all_edge(); + _slam->node_attributes.clear(); + _slam->edge_attributes.clear(); + _slam->start_mapping(); + _slam->node_name = 0; } else if (received_string == "e") { std::cout << "End mapping" << std::endl; - _slam.end_mapping(); + _slam->end_mapping(); } else if (received_string == "a") { std::cout << "Start navigation (default)" << std::endl; - _slam.start_relocation(); - _slam.start_navigation(); - _slam.init_pose(); - _slam.default_navigation(); + _slam->start_relocation(); + _slam->start_navigation(); + _slam->init_pose(); + _slam->default_navigation(); } else if (received_string == "s") { std::cout << "Pause navigation" << std::endl; - _slam.pause_navigation(); + _slam->pause_navigation(); } else if (received_string == "d") { std::cout << "Recover navigation" << std::endl; - _slam.recover_navigation(); + _slam->recover_navigation(); } else if (received_string == "z") { std::cout << "Start relocation and navigation to prepare for collecting node/edge information. " << "(Default clearing of node/edge information)" << std::endl; - _slam.delete_all_node(); - _slam.delete_all_edge(); - _slam.start_relocation(); - _slam.start_navigation(); - _slam.init_pose(); + _slam->delete_all_node(); + _slam->delete_all_edge(); + _slam->start_relocation(); + _slam->start_navigation(); + _slam->init_pose(); } else if (received_string == "x") { std::cout << "Collect node/edge information" << std::endl; - _slam.add_node_and_edge(); + _slam->add_node_and_edge(); } else if (received_string == "c") { std::cout << "Save node/edge information" << std::endl; - _slam.save_node_and_edge(); - _slam.node_attributes.clear(); - _slam.edge_attributes.clear(); - _slam.node_name = 0; + _slam->save_node_and_edge(); + _slam->node_attributes.clear(); + _slam->edge_attributes.clear(); + _slam->node_name = 0; } else if (received_string == "v") { std::cout << "Clear node/edge information" << std::endl; - _slam.delete_all_node(); - _slam.delete_all_edge(); - _slam.node_name = 0; + _slam->delete_all_node(); + _slam->delete_all_edge(); + _slam->node_name = 0; + } else if (received_string == "scan") { + std::cout << "Control robotic arm start" << std::endl; + _slam->control_robotic_arm(); } else { std::cout << "Unknown command" << std::endl; } @@ -97,7 +113,7 @@ received_data.append(temp_buffer, bytes_received); - // TODO 查找结束标志 + // 查找结束标志 while (true) { const size_t pos = received_data.find('/'); if (pos == std::string::npos) { diff --git a/src/tcp_client.hpp b/src/tcp_client.hpp index 8f5bdb8..afa15f4 100644 --- a/src/tcp_client.hpp +++ b/src/tcp_client.hpp @@ -11,11 +11,10 @@ #include #include #include "slam_wrapper.hpp" -#include "tcp_service.hpp" class TcpClient { public: - explicit TcpClient(const char *networkInterface, TcpService *tcp_service); + TcpClient(); ~TcpClient() { if (_client_socket != -1) { @@ -33,8 +32,7 @@ int _client_socket{-1}; int _max_retries = 12; int _retry_interval = 5000; //重连间隔 - SlamWrapper _slam; - TcpService *_tcp_service; + SlamWrapper *_slam; void receive_data(); diff --git a/src/tcp_service.cpp b/src/tcp_service.cpp index b874a23..45ac332 100644 --- a/src/tcp_service.cpp +++ b/src/tcp_service.cpp @@ -14,7 +14,7 @@ #include #include -void TcpService::send_command(const std::vector &command) { +void TcpService::on_message_received(const std::vector &command) { boost::asio::write(_port, boost::asio::buffer(command)); } diff --git a/src/tcp_service.hpp b/src/tcp_service.hpp index ec5037e..cfe903f 100644 --- a/src/tcp_service.hpp +++ b/src/tcp_service.hpp @@ -8,9 +8,11 @@ #include #include +#include "message_observer.hpp" + using boost::asio::serial_port_base; -class TcpService { +class TcpService : public MessageObserver { public: explicit TcpService(boost::asio::io_context &io_context, const std::string &port_name, int baud_rate); @@ -18,7 +20,7 @@ void init_serial_port(const std::string &port_name, int baud_rate); - void send_command(const std::vector &command); + void on_message_received(const std::vector &command) override; private: int _socket_fd;