diff --git a/CMakeLists.txt b/CMakeLists.txt index eb70a26..229ed37 100755 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -12,10 +12,10 @@ add_executable(message_handler src/message_handler.cpp - src/slam_wrapper.cpp - src/tcp_client.cpp src/tcp_service.cpp - src/methane_serial_port.cpp + src/tcp_client.cpp + src/slam_wrapper.cpp src/robotic_arm_serial_port.cpp + src/methane_serial_port.cpp ) target_link_libraries(message_handler unitree_sdk2) \ No newline at end of file diff --git a/CMakeLists.txt b/CMakeLists.txt index eb70a26..229ed37 100755 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -12,10 +12,10 @@ add_executable(message_handler src/message_handler.cpp - src/slam_wrapper.cpp - src/tcp_client.cpp src/tcp_service.cpp - src/methane_serial_port.cpp + src/tcp_client.cpp + src/slam_wrapper.cpp src/robotic_arm_serial_port.cpp + src/methane_serial_port.cpp ) target_link_libraries(message_handler unitree_sdk2) \ No newline at end of file diff --git a/src/message_handler.cpp b/src/message_handler.cpp index e2f6f47..3c38cdc 100644 --- a/src/message_handler.cpp +++ b/src/message_handler.cpp @@ -2,11 +2,19 @@ #include #include +#include "tcp_service.hpp" #include "tcp_client.hpp" #include "methane_serial_port.hpp" int main() { - // 启动TCP客户端，接收来自Web端的命令，控制机器狗运动 + //启动TCP服务端 + std::thread service_thread([] { + boost::asio::io_service io; + TcpService::getInstance().start(8888); + io.run(); + }); + + // 启动TCP客户端 std::thread client_thread([] { boost::asio::io_service io; TcpClient client; @@ -15,7 +23,7 @@ io.run(); }); - // 启动甲烷数据查询串口线程，定时发送查询指令 + //启动甲烷数据查询串口线程 std::thread gas_serial_port_thread([] { try { boost::asio::io_service io; @@ -29,6 +37,7 @@ }); // 等待所有线程完成 + service_thread.join(); client_thread.join(); gas_serial_port_thread.join(); return 0; diff --git a/CMakeLists.txt b/CMakeLists.txt index eb70a26..229ed37 100755 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -12,10 +12,10 @@ add_executable(message_handler src/message_handler.cpp - src/slam_wrapper.cpp - src/tcp_client.cpp src/tcp_service.cpp - src/methane_serial_port.cpp + src/tcp_client.cpp + src/slam_wrapper.cpp src/robotic_arm_serial_port.cpp + src/methane_serial_port.cpp ) target_link_libraries(message_handler unitree_sdk2) \ No newline at end of file diff --git a/src/message_handler.cpp b/src/message_handler.cpp index e2f6f47..3c38cdc 100644 --- a/src/message_handler.cpp +++ b/src/message_handler.cpp @@ -2,11 +2,19 @@ #include #include +#include "tcp_service.hpp" #include "tcp_client.hpp" #include "methane_serial_port.hpp" int main() { - // 启动TCP客户端，接收来自Web端的命令，控制机器狗运动 + //启动TCP服务端 + std::thread service_thread([] { + boost::asio::io_service io; + TcpService::getInstance().start(8888); + io.run(); + }); + + // 启动TCP客户端 std::thread client_thread([] { boost::asio::io_service io; TcpClient client; @@ -15,7 +23,7 @@ io.run(); }); - // 启动甲烷数据查询串口线程，定时发送查询指令 + //启动甲烷数据查询串口线程 std::thread gas_serial_port_thread([] { try { boost::asio::io_service io; @@ -29,6 +37,7 @@ }); // 等待所有线程完成 + service_thread.join(); client_thread.join(); gas_serial_port_thread.join(); return 0; diff --git a/src/message_observer.hpp b/src/message_observer.hpp deleted file mode 100644 index 67f121f..0000000 --- a/src/message_observer.hpp +++ /dev/null @@ -1,18 +0,0 @@ -// -// 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; - virtual void on_node_arrived(const int &node) = 0; -}; - -#endif //MESSAGE_OBSERVER_HPP diff --git a/CMakeLists.txt b/CMakeLists.txt index eb70a26..229ed37 100755 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -12,10 +12,10 @@ add_executable(message_handler src/message_handler.cpp - src/slam_wrapper.cpp - src/tcp_client.cpp src/tcp_service.cpp - src/methane_serial_port.cpp + src/tcp_client.cpp + src/slam_wrapper.cpp src/robotic_arm_serial_port.cpp + src/methane_serial_port.cpp ) target_link_libraries(message_handler unitree_sdk2) \ No newline at end of file diff --git a/src/message_handler.cpp b/src/message_handler.cpp index e2f6f47..3c38cdc 100644 --- a/src/message_handler.cpp +++ b/src/message_handler.cpp @@ -2,11 +2,19 @@ #include #include +#include "tcp_service.hpp" #include "tcp_client.hpp" #include "methane_serial_port.hpp" int main() { - // 启动TCP客户端，接收来自Web端的命令，控制机器狗运动 + //启动TCP服务端 + std::thread service_thread([] { + boost::asio::io_service io; + TcpService::getInstance().start(8888); + io.run(); + }); + + // 启动TCP客户端 std::thread client_thread([] { boost::asio::io_service io; TcpClient client; @@ -15,7 +23,7 @@ io.run(); }); - // 启动甲烷数据查询串口线程，定时发送查询指令 + //启动甲烷数据查询串口线程 std::thread gas_serial_port_thread([] { try { boost::asio::io_service io; @@ -29,6 +37,7 @@ }); // 等待所有线程完成 + service_thread.join(); client_thread.join(); gas_serial_port_thread.join(); return 0; diff --git a/src/message_observer.hpp b/src/message_observer.hpp deleted file mode 100644 index 67f121f..0000000 --- a/src/message_observer.hpp +++ /dev/null @@ -1,18 +0,0 @@ -// -// 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; - virtual void on_node_arrived(const int &node) = 0; -}; - -#endif //MESSAGE_OBSERVER_HPP diff --git a/src/methane_serial_port.cpp b/src/methane_serial_port.cpp index a55c912..5f811f8 100644 --- a/src/methane_serial_port.cpp +++ b/src/methane_serial_port.cpp @@ -6,14 +6,15 @@ #include #include -void handle_data(const std::vector &buffer) { +#include "tcp_service.hpp" + +void handle_response(const std::vector &buffer) { const auto x = buffer[2] & 0xFFFF; const auto y = buffer[3] & 0xFFF; const auto z = buffer[4] & 0xFF; const auto w = buffer[5]; const auto result = x + y + z + w; - std::cout << "methane concentration: " << result << " ppm·m" << std::endl; - //http上传数据 + TcpService::getInstance().update_gas_value(result); } MethaneSerialPort::MethaneSerialPort( @@ -24,6 +25,8 @@ port_.set_option(serial_port_base::parity(serial_port_base::parity::none)); port_.set_option(serial_port_base::stop_bits(serial_port_base::stop_bits::one)); + std::cout << "MethaneSerialPort init success" << std::endl; + start_timer(); } @@ -44,23 +47,23 @@ if (!error) { // 获取接收到的数据 std::istream is(&buffer_); - std::vector received_data(static_cast(bytes_transferred)); + std::vector response(static_cast(bytes_transferred)); is.read( - reinterpret_cast(received_data.data()), static_cast(bytes_transferred) + reinterpret_cast(response.data()), static_cast(bytes_transferred) ); - // std::cout << "received " << bytes_transferred << " bytes: "; - // for (size_t i = 0; i < bytes_transferred; ++i) { - // std::cout << std::hex - // << std::uppercase - // << std::setw(2) - // << std::setfill('0') - // << static_cast(received_data[i]) - // << " "; - // } - // std::cout << std::endl; + std::cout << "received " << bytes_transferred << " bytes: "; + for (size_t i = 0; i < bytes_transferred; ++i) { + std::cout << std::hex + << std::uppercase + << std::setw(2) + << std::setfill('0') + << static_cast(response[i]) + << " "; + } + std::cout << std::endl; - handle_data(received_data); + handle_response(response); start_timer(); } else { @@ -68,4 +71,4 @@ start_timer(); } }); -} \ No newline at end of file +} diff --git a/CMakeLists.txt b/CMakeLists.txt index eb70a26..229ed37 100755 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -12,10 +12,10 @@ add_executable(message_handler src/message_handler.cpp - src/slam_wrapper.cpp - src/tcp_client.cpp src/tcp_service.cpp - src/methane_serial_port.cpp + src/tcp_client.cpp + src/slam_wrapper.cpp src/robotic_arm_serial_port.cpp + src/methane_serial_port.cpp ) target_link_libraries(message_handler unitree_sdk2) \ No newline at end of file diff --git a/src/message_handler.cpp b/src/message_handler.cpp index e2f6f47..3c38cdc 100644 --- a/src/message_handler.cpp +++ b/src/message_handler.cpp @@ -2,11 +2,19 @@ #include #include +#include "tcp_service.hpp" #include "tcp_client.hpp" #include "methane_serial_port.hpp" int main() { - // 启动TCP客户端，接收来自Web端的命令，控制机器狗运动 + //启动TCP服务端 + std::thread service_thread([] { + boost::asio::io_service io; + TcpService::getInstance().start(8888); + io.run(); + }); + + // 启动TCP客户端 std::thread client_thread([] { boost::asio::io_service io; TcpClient client; @@ -15,7 +23,7 @@ io.run(); }); - // 启动甲烷数据查询串口线程，定时发送查询指令 + //启动甲烷数据查询串口线程 std::thread gas_serial_port_thread([] { try { boost::asio::io_service io; @@ -29,6 +37,7 @@ }); // 等待所有线程完成 + service_thread.join(); client_thread.join(); gas_serial_port_thread.join(); return 0; diff --git a/src/message_observer.hpp b/src/message_observer.hpp deleted file mode 100644 index 67f121f..0000000 --- a/src/message_observer.hpp +++ /dev/null @@ -1,18 +0,0 @@ -// -// 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; - virtual void on_node_arrived(const int &node) = 0; -}; - -#endif //MESSAGE_OBSERVER_HPP diff --git a/src/methane_serial_port.cpp b/src/methane_serial_port.cpp index a55c912..5f811f8 100644 --- a/src/methane_serial_port.cpp +++ b/src/methane_serial_port.cpp @@ -6,14 +6,15 @@ #include #include -void handle_data(const std::vector &buffer) { +#include "tcp_service.hpp" + +void handle_response(const std::vector &buffer) { const auto x = buffer[2] & 0xFFFF; const auto y = buffer[3] & 0xFFF; const auto z = buffer[4] & 0xFF; const auto w = buffer[5]; const auto result = x + y + z + w; - std::cout << "methane concentration: " << result << " ppm·m" << std::endl; - //http上传数据 + TcpService::getInstance().update_gas_value(result); } MethaneSerialPort::MethaneSerialPort( @@ -24,6 +25,8 @@ port_.set_option(serial_port_base::parity(serial_port_base::parity::none)); port_.set_option(serial_port_base::stop_bits(serial_port_base::stop_bits::one)); + std::cout << "MethaneSerialPort init success" << std::endl; + start_timer(); } @@ -44,23 +47,23 @@ if (!error) { // 获取接收到的数据 std::istream is(&buffer_); - std::vector received_data(static_cast(bytes_transferred)); + std::vector response(static_cast(bytes_transferred)); is.read( - reinterpret_cast(received_data.data()), static_cast(bytes_transferred) + reinterpret_cast(response.data()), static_cast(bytes_transferred) ); - // std::cout << "received " << bytes_transferred << " bytes: "; - // for (size_t i = 0; i < bytes_transferred; ++i) { - // std::cout << std::hex - // << std::uppercase - // << std::setw(2) - // << std::setfill('0') - // << static_cast(received_data[i]) - // << " "; - // } - // std::cout << std::endl; + std::cout << "received " << bytes_transferred << " bytes: "; + for (size_t i = 0; i < bytes_transferred; ++i) { + std::cout << std::hex + << std::uppercase + << std::setw(2) + << std::setfill('0') + << static_cast(response[i]) + << " "; + } + std::cout << std::endl; - handle_data(received_data); + handle_response(response); start_timer(); } else { @@ -68,4 +71,4 @@ start_timer(); } }); -} \ No newline at end of file +} diff --git a/src/robotic_arm_serial_port.cpp b/src/robotic_arm_serial_port.cpp index 17516ef..c6b850b 100644 --- a/src/robotic_arm_serial_port.cpp +++ b/src/robotic_arm_serial_port.cpp @@ -6,9 +6,6 @@ #include #include -void handle_data(const std::vector &buffer) { -} - RoboticArmSerialPort::RoboticArmSerialPort( boost::asio::io_service &io_service, const std::string &port_name, const int baud_rate ): io_service_(io_service), port_(io_service, port_name) { @@ -42,10 +39,21 @@ << " "; } std::cout << std::endl; - - handle_data(received_data); } else { std::cerr << "error: " << error.message() << std::endl; } }); } + +void RoboticArmSerialPort::write(std::vector command) { + for (const uint8_t i: command) { + std::cout << std::hex + << std::uppercase + << std::setw(2) + << std::setfill('0') + << static_cast(static_cast(i)) + << " "; + } + std::cout << std::endl; + boost::asio::write(port_, boost::asio::buffer(command)); +} \ No newline at end of file diff --git a/CMakeLists.txt b/CMakeLists.txt index eb70a26..229ed37 100755 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -12,10 +12,10 @@ add_executable(message_handler src/message_handler.cpp - src/slam_wrapper.cpp - src/tcp_client.cpp src/tcp_service.cpp - src/methane_serial_port.cpp + src/tcp_client.cpp + src/slam_wrapper.cpp src/robotic_arm_serial_port.cpp + src/methane_serial_port.cpp ) target_link_libraries(message_handler unitree_sdk2) \ No newline at end of file diff --git a/src/message_handler.cpp b/src/message_handler.cpp index e2f6f47..3c38cdc 100644 --- a/src/message_handler.cpp +++ b/src/message_handler.cpp @@ -2,11 +2,19 @@ #include #include +#include "tcp_service.hpp" #include "tcp_client.hpp" #include "methane_serial_port.hpp" int main() { - // 启动TCP客户端，接收来自Web端的命令，控制机器狗运动 + //启动TCP服务端 + std::thread service_thread([] { + boost::asio::io_service io; + TcpService::getInstance().start(8888); + io.run(); + }); + + // 启动TCP客户端 std::thread client_thread([] { boost::asio::io_service io; TcpClient client; @@ -15,7 +23,7 @@ io.run(); }); - // 启动甲烷数据查询串口线程，定时发送查询指令 + //启动甲烷数据查询串口线程 std::thread gas_serial_port_thread([] { try { boost::asio::io_service io; @@ -29,6 +37,7 @@ }); // 等待所有线程完成 + service_thread.join(); client_thread.join(); gas_serial_port_thread.join(); return 0; diff --git a/src/message_observer.hpp b/src/message_observer.hpp deleted file mode 100644 index 67f121f..0000000 --- a/src/message_observer.hpp +++ /dev/null @@ -1,18 +0,0 @@ -// -// 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; - virtual void on_node_arrived(const int &node) = 0; -}; - -#endif //MESSAGE_OBSERVER_HPP diff --git a/src/methane_serial_port.cpp b/src/methane_serial_port.cpp index a55c912..5f811f8 100644 --- a/src/methane_serial_port.cpp +++ b/src/methane_serial_port.cpp @@ -6,14 +6,15 @@ #include #include -void handle_data(const std::vector &buffer) { +#include "tcp_service.hpp" + +void handle_response(const std::vector &buffer) { const auto x = buffer[2] & 0xFFFF; const auto y = buffer[3] & 0xFFF; const auto z = buffer[4] & 0xFF; const auto w = buffer[5]; const auto result = x + y + z + w; - std::cout << "methane concentration: " << result << " ppm·m" << std::endl; - //http上传数据 + TcpService::getInstance().update_gas_value(result); } MethaneSerialPort::MethaneSerialPort( @@ -24,6 +25,8 @@ port_.set_option(serial_port_base::parity(serial_port_base::parity::none)); port_.set_option(serial_port_base::stop_bits(serial_port_base::stop_bits::one)); + std::cout << "MethaneSerialPort init success" << std::endl; + start_timer(); } @@ -44,23 +47,23 @@ if (!error) { // 获取接收到的数据 std::istream is(&buffer_); - std::vector received_data(static_cast(bytes_transferred)); + std::vector response(static_cast(bytes_transferred)); is.read( - reinterpret_cast(received_data.data()), static_cast(bytes_transferred) + reinterpret_cast(response.data()), static_cast(bytes_transferred) ); - // std::cout << "received " << bytes_transferred << " bytes: "; - // for (size_t i = 0; i < bytes_transferred; ++i) { - // std::cout << std::hex - // << std::uppercase - // << std::setw(2) - // << std::setfill('0') - // << static_cast(received_data[i]) - // << " "; - // } - // std::cout << std::endl; + std::cout << "received " << bytes_transferred << " bytes: "; + for (size_t i = 0; i < bytes_transferred; ++i) { + std::cout << std::hex + << std::uppercase + << std::setw(2) + << std::setfill('0') + << static_cast(response[i]) + << " "; + } + std::cout << std::endl; - handle_data(received_data); + handle_response(response); start_timer(); } else { @@ -68,4 +71,4 @@ start_timer(); } }); -} \ No newline at end of file +} diff --git a/src/robotic_arm_serial_port.cpp b/src/robotic_arm_serial_port.cpp index 17516ef..c6b850b 100644 --- a/src/robotic_arm_serial_port.cpp +++ b/src/robotic_arm_serial_port.cpp @@ -6,9 +6,6 @@ #include #include -void handle_data(const std::vector &buffer) { -} - RoboticArmSerialPort::RoboticArmSerialPort( boost::asio::io_service &io_service, const std::string &port_name, const int baud_rate ): io_service_(io_service), port_(io_service, port_name) { @@ -42,10 +39,21 @@ << " "; } std::cout << std::endl; - - handle_data(received_data); } else { std::cerr << "error: " << error.message() << std::endl; } }); } + +void RoboticArmSerialPort::write(std::vector command) { + for (const uint8_t i: command) { + std::cout << std::hex + << std::uppercase + << std::setw(2) + << std::setfill('0') + << static_cast(static_cast(i)) + << " "; + } + std::cout << std::endl; + boost::asio::write(port_, boost::asio::buffer(command)); +} \ No newline at end of file diff --git a/src/robotic_arm_serial_port.hpp b/src/robotic_arm_serial_port.hpp index 835539c..ac77fc6 100644 --- a/src/robotic_arm_serial_port.hpp +++ b/src/robotic_arm_serial_port.hpp @@ -15,6 +15,8 @@ boost::asio::serial_port &get() { return port_; } + void write(std::vector command); + private: boost::asio::io_service &io_service_; boost::asio::serial_port port_; diff --git a/CMakeLists.txt b/CMakeLists.txt index eb70a26..229ed37 100755 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -12,10 +12,10 @@ add_executable(message_handler src/message_handler.cpp - src/slam_wrapper.cpp - src/tcp_client.cpp src/tcp_service.cpp - src/methane_serial_port.cpp + src/tcp_client.cpp + src/slam_wrapper.cpp src/robotic_arm_serial_port.cpp + src/methane_serial_port.cpp ) target_link_libraries(message_handler unitree_sdk2) \ No newline at end of file diff --git a/src/message_handler.cpp b/src/message_handler.cpp index e2f6f47..3c38cdc 100644 --- a/src/message_handler.cpp +++ b/src/message_handler.cpp @@ -2,11 +2,19 @@ #include #include +#include "tcp_service.hpp" #include "tcp_client.hpp" #include "methane_serial_port.hpp" int main() { - // 启动TCP客户端，接收来自Web端的命令，控制机器狗运动 + //启动TCP服务端 + std::thread service_thread([] { + boost::asio::io_service io; + TcpService::getInstance().start(8888); + io.run(); + }); + + // 启动TCP客户端 std::thread client_thread([] { boost::asio::io_service io; TcpClient client; @@ -15,7 +23,7 @@ io.run(); }); - // 启动甲烷数据查询串口线程，定时发送查询指令 + //启动甲烷数据查询串口线程 std::thread gas_serial_port_thread([] { try { boost::asio::io_service io; @@ -29,6 +37,7 @@ }); // 等待所有线程完成 + service_thread.join(); client_thread.join(); gas_serial_port_thread.join(); return 0; diff --git a/src/message_observer.hpp b/src/message_observer.hpp deleted file mode 100644 index 67f121f..0000000 --- a/src/message_observer.hpp +++ /dev/null @@ -1,18 +0,0 @@ -// -// 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; - virtual void on_node_arrived(const int &node) = 0; -}; - -#endif //MESSAGE_OBSERVER_HPP diff --git a/src/methane_serial_port.cpp b/src/methane_serial_port.cpp index a55c912..5f811f8 100644 --- a/src/methane_serial_port.cpp +++ b/src/methane_serial_port.cpp @@ -6,14 +6,15 @@ #include #include -void handle_data(const std::vector &buffer) { +#include "tcp_service.hpp" + +void handle_response(const std::vector &buffer) { const auto x = buffer[2] & 0xFFFF; const auto y = buffer[3] & 0xFFF; const auto z = buffer[4] & 0xFF; const auto w = buffer[5]; const auto result = x + y + z + w; - std::cout << "methane concentration: " << result << " ppm·m" << std::endl; - //http上传数据 + TcpService::getInstance().update_gas_value(result); } MethaneSerialPort::MethaneSerialPort( @@ -24,6 +25,8 @@ port_.set_option(serial_port_base::parity(serial_port_base::parity::none)); port_.set_option(serial_port_base::stop_bits(serial_port_base::stop_bits::one)); + std::cout << "MethaneSerialPort init success" << std::endl; + start_timer(); } @@ -44,23 +47,23 @@ if (!error) { // 获取接收到的数据 std::istream is(&buffer_); - std::vector received_data(static_cast(bytes_transferred)); + std::vector response(static_cast(bytes_transferred)); is.read( - reinterpret_cast(received_data.data()), static_cast(bytes_transferred) + reinterpret_cast(response.data()), static_cast(bytes_transferred) ); - // std::cout << "received " << bytes_transferred << " bytes: "; - // for (size_t i = 0; i < bytes_transferred; ++i) { - // std::cout << std::hex - // << std::uppercase - // << std::setw(2) - // << std::setfill('0') - // << static_cast(received_data[i]) - // << " "; - // } - // std::cout << std::endl; + std::cout << "received " << bytes_transferred << " bytes: "; + for (size_t i = 0; i < bytes_transferred; ++i) { + std::cout << std::hex + << std::uppercase + << std::setw(2) + << std::setfill('0') + << static_cast(response[i]) + << " "; + } + std::cout << std::endl; - handle_data(received_data); + handle_response(response); start_timer(); } else { @@ -68,4 +71,4 @@ start_timer(); } }); -} \ No newline at end of file +} diff --git a/src/robotic_arm_serial_port.cpp b/src/robotic_arm_serial_port.cpp index 17516ef..c6b850b 100644 --- a/src/robotic_arm_serial_port.cpp +++ b/src/robotic_arm_serial_port.cpp @@ -6,9 +6,6 @@ #include #include -void handle_data(const std::vector &buffer) { -} - RoboticArmSerialPort::RoboticArmSerialPort( boost::asio::io_service &io_service, const std::string &port_name, const int baud_rate ): io_service_(io_service), port_(io_service, port_name) { @@ -42,10 +39,21 @@ << " "; } std::cout << std::endl; - - handle_data(received_data); } else { std::cerr << "error: " << error.message() << std::endl; } }); } + +void RoboticArmSerialPort::write(std::vector command) { + for (const uint8_t i: command) { + std::cout << std::hex + << std::uppercase + << std::setw(2) + << std::setfill('0') + << static_cast(static_cast(i)) + << " "; + } + std::cout << std::endl; + boost::asio::write(port_, boost::asio::buffer(command)); +} \ No newline at end of file diff --git a/src/robotic_arm_serial_port.hpp b/src/robotic_arm_serial_port.hpp index 835539c..ac77fc6 100644 --- a/src/robotic_arm_serial_port.hpp +++ b/src/robotic_arm_serial_port.hpp @@ -15,6 +15,8 @@ boost::asio::serial_port &get() { return port_; } + void write(std::vector command); + private: boost::asio::io_service &io_service_; boost::asio::serial_port port_; diff --git a/src/slam_wrapper.cpp b/src/slam_wrapper.cpp index 4f22353..cefeef1 100644 --- a/src/slam_wrapper.cpp +++ b/src/slam_wrapper.cpp @@ -10,6 +10,14 @@ SlamWrapper::SlamWrapper() { ChannelFactory::Instance()->Init(0, "eth0"); + std::thread serial_port_thread([this] { + //初始化机械臂串口 + boost::asio::io_service io; + serial_port_ = new RoboticArmSerialPort(io, "/dev/ttyACM0", 115200); + io.run(); + }); + serial_port_thread.detach(); + // pub pubQtCommand->InitChannel(); pubQtNode->InitChannel(); @@ -18,22 +26,8 @@ // sub subQtNotice->InitChannel(std::bind(&SlamWrapper::qt_notice_handler, this, std::placeholders::_1), 10); 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::notify_observers(const int &node) { - for (const auto &observer: observers) { - observer->on_node_arrived(node); - } + std::cout << "SlamWrapper init success" << std::endl; } void SlamWrapper::control_robotic_arm() { @@ -44,13 +38,13 @@ 0xFE, 0xFE, 0x0F, 0x3C, 0x0A, 0x40, 0x0A, 0x89, 0x03, 0xDE, 0x08, 0x43, 0x09, 0xA8, 0x07, 0xE1, 0x1E, 0xFA }; - notify_observers(init_command); + serial_port_->write(init_command); std::this_thread::sleep_for(std::chrono::seconds(3)); - notify_observers(left_command); + serial_port_->write(left_command); std::this_thread::sleep_for(std::chrono::seconds(3)); - notify_observers(right_command); + serial_port_->write(right_command); std::this_thread::sleep_for(std::chrono::seconds(3)); - notify_observers(init_command); + serial_port_->write(init_command); std::cout << "Control robotic arm end" << std::endl; } @@ -84,7 +78,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_); + TcpService::getInstance().update_node(arrive_); if (arrive_ == 2) { //发送暂停指令 pause_navigation(); @@ -92,7 +86,7 @@ //TODO 控制机械臂转动到预设的角度 //朝右下方 std::cout << "发送机械臂转动指令，倒计时开始，请等待10秒" << std::endl; - notify_observers({ + serial_port_->write({ 0xFE, 0xFE, 0x0F, 0x3C, 0x0B, 0x71, 0x08, 0x25, 0x06, 0xFB, 0x08, 0x2E, 0x0A, 0xCB, 0x08, 0x28, @@ -105,7 +99,7 @@ std::this_thread::sleep_for(std::chrono::seconds(10)); recover_navigation(); std::cout << "倒计时结束，恢复机械臂初始位置，恢复巡检"; - notify_observers(init_command); + serial_port_->write(init_command); } catch (const std::exception &e) { std::cerr << "线程执行过程中发生异常: " << e.what() << std::endl; } diff --git a/CMakeLists.txt b/CMakeLists.txt index eb70a26..229ed37 100755 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -12,10 +12,10 @@ add_executable(message_handler src/message_handler.cpp - src/slam_wrapper.cpp - src/tcp_client.cpp src/tcp_service.cpp - src/methane_serial_port.cpp + src/tcp_client.cpp + src/slam_wrapper.cpp src/robotic_arm_serial_port.cpp + src/methane_serial_port.cpp ) target_link_libraries(message_handler unitree_sdk2) \ No newline at end of file diff --git a/src/message_handler.cpp b/src/message_handler.cpp index e2f6f47..3c38cdc 100644 --- a/src/message_handler.cpp +++ b/src/message_handler.cpp @@ -2,11 +2,19 @@ #include #include +#include "tcp_service.hpp" #include "tcp_client.hpp" #include "methane_serial_port.hpp" int main() { - // 启动TCP客户端，接收来自Web端的命令，控制机器狗运动 + //启动TCP服务端 + std::thread service_thread([] { + boost::asio::io_service io; + TcpService::getInstance().start(8888); + io.run(); + }); + + // 启动TCP客户端 std::thread client_thread([] { boost::asio::io_service io; TcpClient client; @@ -15,7 +23,7 @@ io.run(); }); - // 启动甲烷数据查询串口线程，定时发送查询指令 + //启动甲烷数据查询串口线程 std::thread gas_serial_port_thread([] { try { boost::asio::io_service io; @@ -29,6 +37,7 @@ }); // 等待所有线程完成 + service_thread.join(); client_thread.join(); gas_serial_port_thread.join(); return 0; diff --git a/src/message_observer.hpp b/src/message_observer.hpp deleted file mode 100644 index 67f121f..0000000 --- a/src/message_observer.hpp +++ /dev/null @@ -1,18 +0,0 @@ -// -// 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; - virtual void on_node_arrived(const int &node) = 0; -}; - -#endif //MESSAGE_OBSERVER_HPP diff --git a/src/methane_serial_port.cpp b/src/methane_serial_port.cpp index a55c912..5f811f8 100644 --- a/src/methane_serial_port.cpp +++ b/src/methane_serial_port.cpp @@ -6,14 +6,15 @@ #include #include -void handle_data(const std::vector &buffer) { +#include "tcp_service.hpp" + +void handle_response(const std::vector &buffer) { const auto x = buffer[2] & 0xFFFF; const auto y = buffer[3] & 0xFFF; const auto z = buffer[4] & 0xFF; const auto w = buffer[5]; const auto result = x + y + z + w; - std::cout << "methane concentration: " << result << " ppm·m" << std::endl; - //http上传数据 + TcpService::getInstance().update_gas_value(result); } MethaneSerialPort::MethaneSerialPort( @@ -24,6 +25,8 @@ port_.set_option(serial_port_base::parity(serial_port_base::parity::none)); port_.set_option(serial_port_base::stop_bits(serial_port_base::stop_bits::one)); + std::cout << "MethaneSerialPort init success" << std::endl; + start_timer(); } @@ -44,23 +47,23 @@ if (!error) { // 获取接收到的数据 std::istream is(&buffer_); - std::vector received_data(static_cast(bytes_transferred)); + std::vector response(static_cast(bytes_transferred)); is.read( - reinterpret_cast(received_data.data()), static_cast(bytes_transferred) + reinterpret_cast(response.data()), static_cast(bytes_transferred) ); - // std::cout << "received " << bytes_transferred << " bytes: "; - // for (size_t i = 0; i < bytes_transferred; ++i) { - // std::cout << std::hex - // << std::uppercase - // << std::setw(2) - // << std::setfill('0') - // << static_cast(received_data[i]) - // << " "; - // } - // std::cout << std::endl; + std::cout << "received " << bytes_transferred << " bytes: "; + for (size_t i = 0; i < bytes_transferred; ++i) { + std::cout << std::hex + << std::uppercase + << std::setw(2) + << std::setfill('0') + << static_cast(response[i]) + << " "; + } + std::cout << std::endl; - handle_data(received_data); + handle_response(response); start_timer(); } else { @@ -68,4 +71,4 @@ start_timer(); } }); -} \ No newline at end of file +} diff --git a/src/robotic_arm_serial_port.cpp b/src/robotic_arm_serial_port.cpp index 17516ef..c6b850b 100644 --- a/src/robotic_arm_serial_port.cpp +++ b/src/robotic_arm_serial_port.cpp @@ -6,9 +6,6 @@ #include #include -void handle_data(const std::vector &buffer) { -} - RoboticArmSerialPort::RoboticArmSerialPort( boost::asio::io_service &io_service, const std::string &port_name, const int baud_rate ): io_service_(io_service), port_(io_service, port_name) { @@ -42,10 +39,21 @@ << " "; } std::cout << std::endl; - - handle_data(received_data); } else { std::cerr << "error: " << error.message() << std::endl; } }); } + +void RoboticArmSerialPort::write(std::vector command) { + for (const uint8_t i: command) { + std::cout << std::hex + << std::uppercase + << std::setw(2) + << std::setfill('0') + << static_cast(static_cast(i)) + << " "; + } + std::cout << std::endl; + boost::asio::write(port_, boost::asio::buffer(command)); +} \ No newline at end of file diff --git a/src/robotic_arm_serial_port.hpp b/src/robotic_arm_serial_port.hpp index 835539c..ac77fc6 100644 --- a/src/robotic_arm_serial_port.hpp +++ b/src/robotic_arm_serial_port.hpp @@ -15,6 +15,8 @@ boost::asio::serial_port &get() { return port_; } + void write(std::vector command); + private: boost::asio::io_service &io_service_; boost::asio::serial_port port_; diff --git a/src/slam_wrapper.cpp b/src/slam_wrapper.cpp index 4f22353..cefeef1 100644 --- a/src/slam_wrapper.cpp +++ b/src/slam_wrapper.cpp @@ -10,6 +10,14 @@ SlamWrapper::SlamWrapper() { ChannelFactory::Instance()->Init(0, "eth0"); + std::thread serial_port_thread([this] { + //初始化机械臂串口 + boost::asio::io_service io; + serial_port_ = new RoboticArmSerialPort(io, "/dev/ttyACM0", 115200); + io.run(); + }); + serial_port_thread.detach(); + // pub pubQtCommand->InitChannel(); pubQtNode->InitChannel(); @@ -18,22 +26,8 @@ // sub subQtNotice->InitChannel(std::bind(&SlamWrapper::qt_notice_handler, this, std::placeholders::_1), 10); 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::notify_observers(const int &node) { - for (const auto &observer: observers) { - observer->on_node_arrived(node); - } + std::cout << "SlamWrapper init success" << std::endl; } void SlamWrapper::control_robotic_arm() { @@ -44,13 +38,13 @@ 0xFE, 0xFE, 0x0F, 0x3C, 0x0A, 0x40, 0x0A, 0x89, 0x03, 0xDE, 0x08, 0x43, 0x09, 0xA8, 0x07, 0xE1, 0x1E, 0xFA }; - notify_observers(init_command); + serial_port_->write(init_command); std::this_thread::sleep_for(std::chrono::seconds(3)); - notify_observers(left_command); + serial_port_->write(left_command); std::this_thread::sleep_for(std::chrono::seconds(3)); - notify_observers(right_command); + serial_port_->write(right_command); std::this_thread::sleep_for(std::chrono::seconds(3)); - notify_observers(init_command); + serial_port_->write(init_command); std::cout << "Control robotic arm end" << std::endl; } @@ -84,7 +78,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_); + TcpService::getInstance().update_node(arrive_); if (arrive_ == 2) { //发送暂停指令 pause_navigation(); @@ -92,7 +86,7 @@ //TODO 控制机械臂转动到预设的角度 //朝右下方 std::cout << "发送机械臂转动指令，倒计时开始，请等待10秒" << std::endl; - notify_observers({ + serial_port_->write({ 0xFE, 0xFE, 0x0F, 0x3C, 0x0B, 0x71, 0x08, 0x25, 0x06, 0xFB, 0x08, 0x2E, 0x0A, 0xCB, 0x08, 0x28, @@ -105,7 +99,7 @@ std::this_thread::sleep_for(std::chrono::seconds(10)); recover_navigation(); std::cout << "倒计时结束，恢复机械臂初始位置，恢复巡检"; - notify_observers(init_command); + serial_port_->write(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 d1eff0f..94b245d 100644 --- a/src/slam_wrapper.hpp +++ b/src/slam_wrapper.hpp @@ -22,6 +22,7 @@ #define ADDNODETOPIC "rt/qt_add_node" #define ADDEDGETOPIC "rt/qt_add_edge" +#include "robotic_arm_serial_port.hpp" #include "tcp_service.hpp" using namespace unitree::robot; @@ -49,16 +50,10 @@ std::vector edge_attributes; u_int16_t node_name = 0; - void register_observer(const std::shared_ptr &observer); - - void notify_observers(const std::vector &command); - - void notify_observers(const int &node); + explicit SlamWrapper(); void control_robotic_arm(); - explicit SlamWrapper(); - void qt_notice_handler(const void *message); void odometer_handler(const void *message); @@ -94,7 +89,7 @@ private: int index = 0; const Odometry_ *currentOdom{}; - std::vector > observers; + RoboticArmSerialPort *serial_port_; const std::vector init_command = { 0xFE, 0xFE, 0x0F, 0x3C, 0x08, 0x00, 0x08, 0x00, 0x08, 0x00, 0x08, 0x00, 0x08, 0x00, 0x08, 0x00, 0x14, 0xFA }; diff --git a/CMakeLists.txt b/CMakeLists.txt index eb70a26..229ed37 100755 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -12,10 +12,10 @@ add_executable(message_handler src/message_handler.cpp - src/slam_wrapper.cpp - src/tcp_client.cpp src/tcp_service.cpp - src/methane_serial_port.cpp + src/tcp_client.cpp + src/slam_wrapper.cpp src/robotic_arm_serial_port.cpp + src/methane_serial_port.cpp ) target_link_libraries(message_handler unitree_sdk2) \ No newline at end of file diff --git a/src/message_handler.cpp b/src/message_handler.cpp index e2f6f47..3c38cdc 100644 --- a/src/message_handler.cpp +++ b/src/message_handler.cpp @@ -2,11 +2,19 @@ #include #include +#include "tcp_service.hpp" #include "tcp_client.hpp" #include "methane_serial_port.hpp" int main() { - // 启动TCP客户端，接收来自Web端的命令，控制机器狗运动 + //启动TCP服务端 + std::thread service_thread([] { + boost::asio::io_service io; + TcpService::getInstance().start(8888); + io.run(); + }); + + // 启动TCP客户端 std::thread client_thread([] { boost::asio::io_service io; TcpClient client; @@ -15,7 +23,7 @@ io.run(); }); - // 启动甲烷数据查询串口线程，定时发送查询指令 + //启动甲烷数据查询串口线程 std::thread gas_serial_port_thread([] { try { boost::asio::io_service io; @@ -29,6 +37,7 @@ }); // 等待所有线程完成 + service_thread.join(); client_thread.join(); gas_serial_port_thread.join(); return 0; diff --git a/src/message_observer.hpp b/src/message_observer.hpp deleted file mode 100644 index 67f121f..0000000 --- a/src/message_observer.hpp +++ /dev/null @@ -1,18 +0,0 @@ -// -// 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; - virtual void on_node_arrived(const int &node) = 0; -}; - -#endif //MESSAGE_OBSERVER_HPP diff --git a/src/methane_serial_port.cpp b/src/methane_serial_port.cpp index a55c912..5f811f8 100644 --- a/src/methane_serial_port.cpp +++ b/src/methane_serial_port.cpp @@ -6,14 +6,15 @@ #include #include -void handle_data(const std::vector &buffer) { +#include "tcp_service.hpp" + +void handle_response(const std::vector &buffer) { const auto x = buffer[2] & 0xFFFF; const auto y = buffer[3] & 0xFFF; const auto z = buffer[4] & 0xFF; const auto w = buffer[5]; const auto result = x + y + z + w; - std::cout << "methane concentration: " << result << " ppm·m" << std::endl; - //http上传数据 + TcpService::getInstance().update_gas_value(result); } MethaneSerialPort::MethaneSerialPort( @@ -24,6 +25,8 @@ port_.set_option(serial_port_base::parity(serial_port_base::parity::none)); port_.set_option(serial_port_base::stop_bits(serial_port_base::stop_bits::one)); + std::cout << "MethaneSerialPort init success" << std::endl; + start_timer(); } @@ -44,23 +47,23 @@ if (!error) { // 获取接收到的数据 std::istream is(&buffer_); - std::vector received_data(static_cast(bytes_transferred)); + std::vector response(static_cast(bytes_transferred)); is.read( - reinterpret_cast(received_data.data()), static_cast(bytes_transferred) + reinterpret_cast(response.data()), static_cast(bytes_transferred) ); - // std::cout << "received " << bytes_transferred << " bytes: "; - // for (size_t i = 0; i < bytes_transferred; ++i) { - // std::cout << std::hex - // << std::uppercase - // << std::setw(2) - // << std::setfill('0') - // << static_cast(received_data[i]) - // << " "; - // } - // std::cout << std::endl; + std::cout << "received " << bytes_transferred << " bytes: "; + for (size_t i = 0; i < bytes_transferred; ++i) { + std::cout << std::hex + << std::uppercase + << std::setw(2) + << std::setfill('0') + << static_cast(response[i]) + << " "; + } + std::cout << std::endl; - handle_data(received_data); + handle_response(response); start_timer(); } else { @@ -68,4 +71,4 @@ start_timer(); } }); -} \ No newline at end of file +} diff --git a/src/robotic_arm_serial_port.cpp b/src/robotic_arm_serial_port.cpp index 17516ef..c6b850b 100644 --- a/src/robotic_arm_serial_port.cpp +++ b/src/robotic_arm_serial_port.cpp @@ -6,9 +6,6 @@ #include #include -void handle_data(const std::vector &buffer) { -} - RoboticArmSerialPort::RoboticArmSerialPort( boost::asio::io_service &io_service, const std::string &port_name, const int baud_rate ): io_service_(io_service), port_(io_service, port_name) { @@ -42,10 +39,21 @@ << " "; } std::cout << std::endl; - - handle_data(received_data); } else { std::cerr << "error: " << error.message() << std::endl; } }); } + +void RoboticArmSerialPort::write(std::vector command) { + for (const uint8_t i: command) { + std::cout << std::hex + << std::uppercase + << std::setw(2) + << std::setfill('0') + << static_cast(static_cast(i)) + << " "; + } + std::cout << std::endl; + boost::asio::write(port_, boost::asio::buffer(command)); +} \ No newline at end of file diff --git a/src/robotic_arm_serial_port.hpp b/src/robotic_arm_serial_port.hpp index 835539c..ac77fc6 100644 --- a/src/robotic_arm_serial_port.hpp +++ b/src/robotic_arm_serial_port.hpp @@ -15,6 +15,8 @@ boost::asio::serial_port &get() { return port_; } + void write(std::vector command); + private: boost::asio::io_service &io_service_; boost::asio::serial_port port_; diff --git a/src/slam_wrapper.cpp b/src/slam_wrapper.cpp index 4f22353..cefeef1 100644 --- a/src/slam_wrapper.cpp +++ b/src/slam_wrapper.cpp @@ -10,6 +10,14 @@ SlamWrapper::SlamWrapper() { ChannelFactory::Instance()->Init(0, "eth0"); + std::thread serial_port_thread([this] { + //初始化机械臂串口 + boost::asio::io_service io; + serial_port_ = new RoboticArmSerialPort(io, "/dev/ttyACM0", 115200); + io.run(); + }); + serial_port_thread.detach(); + // pub pubQtCommand->InitChannel(); pubQtNode->InitChannel(); @@ -18,22 +26,8 @@ // sub subQtNotice->InitChannel(std::bind(&SlamWrapper::qt_notice_handler, this, std::placeholders::_1), 10); 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::notify_observers(const int &node) { - for (const auto &observer: observers) { - observer->on_node_arrived(node); - } + std::cout << "SlamWrapper init success" << std::endl; } void SlamWrapper::control_robotic_arm() { @@ -44,13 +38,13 @@ 0xFE, 0xFE, 0x0F, 0x3C, 0x0A, 0x40, 0x0A, 0x89, 0x03, 0xDE, 0x08, 0x43, 0x09, 0xA8, 0x07, 0xE1, 0x1E, 0xFA }; - notify_observers(init_command); + serial_port_->write(init_command); std::this_thread::sleep_for(std::chrono::seconds(3)); - notify_observers(left_command); + serial_port_->write(left_command); std::this_thread::sleep_for(std::chrono::seconds(3)); - notify_observers(right_command); + serial_port_->write(right_command); std::this_thread::sleep_for(std::chrono::seconds(3)); - notify_observers(init_command); + serial_port_->write(init_command); std::cout << "Control robotic arm end" << std::endl; } @@ -84,7 +78,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_); + TcpService::getInstance().update_node(arrive_); if (arrive_ == 2) { //发送暂停指令 pause_navigation(); @@ -92,7 +86,7 @@ //TODO 控制机械臂转动到预设的角度 //朝右下方 std::cout << "发送机械臂转动指令，倒计时开始，请等待10秒" << std::endl; - notify_observers({ + serial_port_->write({ 0xFE, 0xFE, 0x0F, 0x3C, 0x0B, 0x71, 0x08, 0x25, 0x06, 0xFB, 0x08, 0x2E, 0x0A, 0xCB, 0x08, 0x28, @@ -105,7 +99,7 @@ std::this_thread::sleep_for(std::chrono::seconds(10)); recover_navigation(); std::cout << "倒计时结束，恢复机械臂初始位置，恢复巡检"; - notify_observers(init_command); + serial_port_->write(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 d1eff0f..94b245d 100644 --- a/src/slam_wrapper.hpp +++ b/src/slam_wrapper.hpp @@ -22,6 +22,7 @@ #define ADDNODETOPIC "rt/qt_add_node" #define ADDEDGETOPIC "rt/qt_add_edge" +#include "robotic_arm_serial_port.hpp" #include "tcp_service.hpp" using namespace unitree::robot; @@ -49,16 +50,10 @@ std::vector edge_attributes; u_int16_t node_name = 0; - void register_observer(const std::shared_ptr &observer); - - void notify_observers(const std::vector &command); - - void notify_observers(const int &node); + explicit SlamWrapper(); void control_robotic_arm(); - explicit SlamWrapper(); - void qt_notice_handler(const void *message); void odometer_handler(const void *message); @@ -94,7 +89,7 @@ private: int index = 0; const Odometry_ *currentOdom{}; - std::vector > observers; + RoboticArmSerialPort *serial_port_; const std::vector init_command = { 0xFE, 0xFE, 0x0F, 0x3C, 0x08, 0x00, 0x08, 0x00, 0x08, 0x00, 0x08, 0x00, 0x08, 0x00, 0x08, 0x00, 0x14, 0xFA }; diff --git a/src/tcp_client.cpp b/src/tcp_client.cpp index 46e3469..644f473 100644 --- a/src/tcp_client.cpp +++ b/src/tcp_client.cpp @@ -13,16 +13,7 @@ #include TcpClient::TcpClient() { - // 启动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; + std::cout << "TcpClient init success" << std::endl; } void TcpClient::handle_data(const std::vector &buffer) { @@ -54,9 +45,7 @@ std::cout << "Recover navigation" << std::endl; _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; + std::cout << "Start relocation and navigation to prepare for collecting node/edge information. " << std::endl; _slam.delete_all_node(); _slam.delete_all_edge(); _slam.start_relocation(); @@ -154,8 +143,8 @@ << ", waiting for command ..." << std::endl; //连接成功后把自己的编号发送给Tcp Service - // const std::string dog_code = "B42D4000OCAD3N82"; - // send(_client_socket, dog_code.c_str(), dog_code.size(), 0); + const std::string dog_code = "B42D4000OCAD3N82"; + send(_client_socket, dog_code.c_str(), dog_code.size(), 0); // 启动接收数据的线程 std::thread receive_thread(&TcpClient::receive_data, this); diff --git a/CMakeLists.txt b/CMakeLists.txt index eb70a26..229ed37 100755 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -12,10 +12,10 @@ add_executable(message_handler src/message_handler.cpp - src/slam_wrapper.cpp - src/tcp_client.cpp src/tcp_service.cpp - src/methane_serial_port.cpp + src/tcp_client.cpp + src/slam_wrapper.cpp src/robotic_arm_serial_port.cpp + src/methane_serial_port.cpp ) target_link_libraries(message_handler unitree_sdk2) \ No newline at end of file diff --git a/src/message_handler.cpp b/src/message_handler.cpp index e2f6f47..3c38cdc 100644 --- a/src/message_handler.cpp +++ b/src/message_handler.cpp @@ -2,11 +2,19 @@ #include #include +#include "tcp_service.hpp" #include "tcp_client.hpp" #include "methane_serial_port.hpp" int main() { - // 启动TCP客户端，接收来自Web端的命令，控制机器狗运动 + //启动TCP服务端 + std::thread service_thread([] { + boost::asio::io_service io; + TcpService::getInstance().start(8888); + io.run(); + }); + + // 启动TCP客户端 std::thread client_thread([] { boost::asio::io_service io; TcpClient client; @@ -15,7 +23,7 @@ io.run(); }); - // 启动甲烷数据查询串口线程，定时发送查询指令 + //启动甲烷数据查询串口线程 std::thread gas_serial_port_thread([] { try { boost::asio::io_service io; @@ -29,6 +37,7 @@ }); // 等待所有线程完成 + service_thread.join(); client_thread.join(); gas_serial_port_thread.join(); return 0; diff --git a/src/message_observer.hpp b/src/message_observer.hpp deleted file mode 100644 index 67f121f..0000000 --- a/src/message_observer.hpp +++ /dev/null @@ -1,18 +0,0 @@ -// -// 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; - virtual void on_node_arrived(const int &node) = 0; -}; - -#endif //MESSAGE_OBSERVER_HPP diff --git a/src/methane_serial_port.cpp b/src/methane_serial_port.cpp index a55c912..5f811f8 100644 --- a/src/methane_serial_port.cpp +++ b/src/methane_serial_port.cpp @@ -6,14 +6,15 @@ #include #include -void handle_data(const std::vector &buffer) { +#include "tcp_service.hpp" + +void handle_response(const std::vector &buffer) { const auto x = buffer[2] & 0xFFFF; const auto y = buffer[3] & 0xFFF; const auto z = buffer[4] & 0xFF; const auto w = buffer[5]; const auto result = x + y + z + w; - std::cout << "methane concentration: " << result << " ppm·m" << std::endl; - //http上传数据 + TcpService::getInstance().update_gas_value(result); } MethaneSerialPort::MethaneSerialPort( @@ -24,6 +25,8 @@ port_.set_option(serial_port_base::parity(serial_port_base::parity::none)); port_.set_option(serial_port_base::stop_bits(serial_port_base::stop_bits::one)); + std::cout << "MethaneSerialPort init success" << std::endl; + start_timer(); } @@ -44,23 +47,23 @@ if (!error) { // 获取接收到的数据 std::istream is(&buffer_); - std::vector received_data(static_cast(bytes_transferred)); + std::vector response(static_cast(bytes_transferred)); is.read( - reinterpret_cast(received_data.data()), static_cast(bytes_transferred) + reinterpret_cast(response.data()), static_cast(bytes_transferred) ); - // std::cout << "received " << bytes_transferred << " bytes: "; - // for (size_t i = 0; i < bytes_transferred; ++i) { - // std::cout << std::hex - // << std::uppercase - // << std::setw(2) - // << std::setfill('0') - // << static_cast(received_data[i]) - // << " "; - // } - // std::cout << std::endl; + std::cout << "received " << bytes_transferred << " bytes: "; + for (size_t i = 0; i < bytes_transferred; ++i) { + std::cout << std::hex + << std::uppercase + << std::setw(2) + << std::setfill('0') + << static_cast(response[i]) + << " "; + } + std::cout << std::endl; - handle_data(received_data); + handle_response(response); start_timer(); } else { @@ -68,4 +71,4 @@ start_timer(); } }); -} \ No newline at end of file +} diff --git a/src/robotic_arm_serial_port.cpp b/src/robotic_arm_serial_port.cpp index 17516ef..c6b850b 100644 --- a/src/robotic_arm_serial_port.cpp +++ b/src/robotic_arm_serial_port.cpp @@ -6,9 +6,6 @@ #include #include -void handle_data(const std::vector &buffer) { -} - RoboticArmSerialPort::RoboticArmSerialPort( boost::asio::io_service &io_service, const std::string &port_name, const int baud_rate ): io_service_(io_service), port_(io_service, port_name) { @@ -42,10 +39,21 @@ << " "; } std::cout << std::endl; - - handle_data(received_data); } else { std::cerr << "error: " << error.message() << std::endl; } }); } + +void RoboticArmSerialPort::write(std::vector command) { + for (const uint8_t i: command) { + std::cout << std::hex + << std::uppercase + << std::setw(2) + << std::setfill('0') + << static_cast(static_cast(i)) + << " "; + } + std::cout << std::endl; + boost::asio::write(port_, boost::asio::buffer(command)); +} \ No newline at end of file diff --git a/src/robotic_arm_serial_port.hpp b/src/robotic_arm_serial_port.hpp index 835539c..ac77fc6 100644 --- a/src/robotic_arm_serial_port.hpp +++ b/src/robotic_arm_serial_port.hpp @@ -15,6 +15,8 @@ boost::asio::serial_port &get() { return port_; } + void write(std::vector command); + private: boost::asio::io_service &io_service_; boost::asio::serial_port port_; diff --git a/src/slam_wrapper.cpp b/src/slam_wrapper.cpp index 4f22353..cefeef1 100644 --- a/src/slam_wrapper.cpp +++ b/src/slam_wrapper.cpp @@ -10,6 +10,14 @@ SlamWrapper::SlamWrapper() { ChannelFactory::Instance()->Init(0, "eth0"); + std::thread serial_port_thread([this] { + //初始化机械臂串口 + boost::asio::io_service io; + serial_port_ = new RoboticArmSerialPort(io, "/dev/ttyACM0", 115200); + io.run(); + }); + serial_port_thread.detach(); + // pub pubQtCommand->InitChannel(); pubQtNode->InitChannel(); @@ -18,22 +26,8 @@ // sub subQtNotice->InitChannel(std::bind(&SlamWrapper::qt_notice_handler, this, std::placeholders::_1), 10); 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::notify_observers(const int &node) { - for (const auto &observer: observers) { - observer->on_node_arrived(node); - } + std::cout << "SlamWrapper init success" << std::endl; } void SlamWrapper::control_robotic_arm() { @@ -44,13 +38,13 @@ 0xFE, 0xFE, 0x0F, 0x3C, 0x0A, 0x40, 0x0A, 0x89, 0x03, 0xDE, 0x08, 0x43, 0x09, 0xA8, 0x07, 0xE1, 0x1E, 0xFA }; - notify_observers(init_command); + serial_port_->write(init_command); std::this_thread::sleep_for(std::chrono::seconds(3)); - notify_observers(left_command); + serial_port_->write(left_command); std::this_thread::sleep_for(std::chrono::seconds(3)); - notify_observers(right_command); + serial_port_->write(right_command); std::this_thread::sleep_for(std::chrono::seconds(3)); - notify_observers(init_command); + serial_port_->write(init_command); std::cout << "Control robotic arm end" << std::endl; } @@ -84,7 +78,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_); + TcpService::getInstance().update_node(arrive_); if (arrive_ == 2) { //发送暂停指令 pause_navigation(); @@ -92,7 +86,7 @@ //TODO 控制机械臂转动到预设的角度 //朝右下方 std::cout << "发送机械臂转动指令，倒计时开始，请等待10秒" << std::endl; - notify_observers({ + serial_port_->write({ 0xFE, 0xFE, 0x0F, 0x3C, 0x0B, 0x71, 0x08, 0x25, 0x06, 0xFB, 0x08, 0x2E, 0x0A, 0xCB, 0x08, 0x28, @@ -105,7 +99,7 @@ std::this_thread::sleep_for(std::chrono::seconds(10)); recover_navigation(); std::cout << "倒计时结束，恢复机械臂初始位置，恢复巡检"; - notify_observers(init_command); + serial_port_->write(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 d1eff0f..94b245d 100644 --- a/src/slam_wrapper.hpp +++ b/src/slam_wrapper.hpp @@ -22,6 +22,7 @@ #define ADDNODETOPIC "rt/qt_add_node" #define ADDEDGETOPIC "rt/qt_add_edge" +#include "robotic_arm_serial_port.hpp" #include "tcp_service.hpp" using namespace unitree::robot; @@ -49,16 +50,10 @@ std::vector edge_attributes; u_int16_t node_name = 0; - void register_observer(const std::shared_ptr &observer); - - void notify_observers(const std::vector &command); - - void notify_observers(const int &node); + explicit SlamWrapper(); void control_robotic_arm(); - explicit SlamWrapper(); - void qt_notice_handler(const void *message); void odometer_handler(const void *message); @@ -94,7 +89,7 @@ private: int index = 0; const Odometry_ *currentOdom{}; - std::vector > observers; + RoboticArmSerialPort *serial_port_; const std::vector init_command = { 0xFE, 0xFE, 0x0F, 0x3C, 0x08, 0x00, 0x08, 0x00, 0x08, 0x00, 0x08, 0x00, 0x08, 0x00, 0x08, 0x00, 0x14, 0xFA }; diff --git a/src/tcp_client.cpp b/src/tcp_client.cpp index 46e3469..644f473 100644 --- a/src/tcp_client.cpp +++ b/src/tcp_client.cpp @@ -13,16 +13,7 @@ #include TcpClient::TcpClient() { - // 启动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; + std::cout << "TcpClient init success" << std::endl; } void TcpClient::handle_data(const std::vector &buffer) { @@ -54,9 +45,7 @@ std::cout << "Recover navigation" << std::endl; _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; + std::cout << "Start relocation and navigation to prepare for collecting node/edge information. " << std::endl; _slam.delete_all_node(); _slam.delete_all_edge(); _slam.start_relocation(); @@ -154,8 +143,8 @@ << ", waiting for command ..." << std::endl; //连接成功后把自己的编号发送给Tcp Service - // const std::string dog_code = "B42D4000OCAD3N82"; - // send(_client_socket, dog_code.c_str(), dog_code.size(), 0); + const std::string dog_code = "B42D4000OCAD3N82"; + send(_client_socket, dog_code.c_str(), dog_code.size(), 0); // 启动接收数据的线程 std::thread receive_thread(&TcpClient::receive_data, this); diff --git a/src/tcp_service.cpp b/src/tcp_service.cpp index 3f33706..450075c 100644 --- a/src/tcp_service.cpp +++ b/src/tcp_service.cpp @@ -14,47 +14,7 @@ #include #include -/** - * @link SlamWrapper---> @link TcpService - * @param command - */ -void TcpService::on_message_received(const std::vector &command) { - boost::asio::write(_port, boost::asio::buffer(command)); -} - -/** - * 回消息给已连接的TCP客户端。需要拼接数据包。（机器狗当前位置node，经纬度，浓度） - * @param node - */ -void TcpService::on_node_arrived(const int &node) { - const std::string message = std::to_string(node); - std::cout << "send message to client: " << message << std::endl; - for (const int client_socket: _client_sockets) { - send(client_socket, message.c_str(), message.size(), 0); - } -} - -void TcpService::init_serial_port(const std::string &port_name, const int baud_rate) { - _port.open(port_name); - if (_port.is_open()) { - _port.set_option(serial_port_base::baud_rate(baud_rate)); - _port.set_option(serial_port_base::character_size(8)); - _port.set_option(serial_port_base::stop_bits(serial_port_base::stop_bits::one)); - _port.set_option(serial_port_base::parity(serial_port_base::parity::none)); - _port.set_option(serial_port_base::flow_control(serial_port_base::flow_control::none)); - } else { - std::cerr << "Failed to open serial port" << std::endl; - } -} - -TcpService::TcpService( - boost::asio::io_context &io_context, const std::string &port_name, const int baud_rate -): _context(io_context), _port(io_context) { - std::cout << "TcpService::init success" << std::endl; - init_serial_port(port_name, baud_rate); -} - -void TcpService::handle_data_packet(std::vector &data) { +static void handle_data_packet(std::vector &data) { for (const uint8_t i: data) { std::cout << std::hex << std::uppercase @@ -67,10 +27,9 @@ //转为std::vector并发给机械臂串口 std::vector command(data.begin(), data.end()); - boost::asio::write(_port, boost::asio::buffer(command)); } -void TcpService::handle_client(const int client_socket) { +static void handle_client(const int client_socket) { std::vector buffer; char byte; while (true) { @@ -147,3 +106,22 @@ std::cerr << "exception in tcp_server::start: " << e.what() << std::endl; } } + +void TcpService::update_node(const int node) { + std::stringstream ss; + ss << node << "," + << gas_value << "," + << "116.296975" << "," + << "39.990889" + << std::endl; + const std::string message = ss.str(); + std::cout << "TcpService update node: " << message << std::endl; + for (const auto &client_socket: _client_sockets) { + send(client_socket, message.c_str(), message.size(), 0); + } +} + +void TcpService::update_gas_value(const int value) { + gas_value = value; + std::cout << "TcpService update gas value: " << gas_value << " ppm·m" << std::endl; +} diff --git a/CMakeLists.txt b/CMakeLists.txt index eb70a26..229ed37 100755 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -12,10 +12,10 @@ add_executable(message_handler src/message_handler.cpp - src/slam_wrapper.cpp - src/tcp_client.cpp src/tcp_service.cpp - src/methane_serial_port.cpp + src/tcp_client.cpp + src/slam_wrapper.cpp src/robotic_arm_serial_port.cpp + src/methane_serial_port.cpp ) target_link_libraries(message_handler unitree_sdk2) \ No newline at end of file diff --git a/src/message_handler.cpp b/src/message_handler.cpp index e2f6f47..3c38cdc 100644 --- a/src/message_handler.cpp +++ b/src/message_handler.cpp @@ -2,11 +2,19 @@ #include #include +#include "tcp_service.hpp" #include "tcp_client.hpp" #include "methane_serial_port.hpp" int main() { - // 启动TCP客户端，接收来自Web端的命令，控制机器狗运动 + //启动TCP服务端 + std::thread service_thread([] { + boost::asio::io_service io; + TcpService::getInstance().start(8888); + io.run(); + }); + + // 启动TCP客户端 std::thread client_thread([] { boost::asio::io_service io; TcpClient client; @@ -15,7 +23,7 @@ io.run(); }); - // 启动甲烷数据查询串口线程，定时发送查询指令 + //启动甲烷数据查询串口线程 std::thread gas_serial_port_thread([] { try { boost::asio::io_service io; @@ -29,6 +37,7 @@ }); // 等待所有线程完成 + service_thread.join(); client_thread.join(); gas_serial_port_thread.join(); return 0; diff --git a/src/message_observer.hpp b/src/message_observer.hpp deleted file mode 100644 index 67f121f..0000000 --- a/src/message_observer.hpp +++ /dev/null @@ -1,18 +0,0 @@ -// -// 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; - virtual void on_node_arrived(const int &node) = 0; -}; - -#endif //MESSAGE_OBSERVER_HPP diff --git a/src/methane_serial_port.cpp b/src/methane_serial_port.cpp index a55c912..5f811f8 100644 --- a/src/methane_serial_port.cpp +++ b/src/methane_serial_port.cpp @@ -6,14 +6,15 @@ #include #include -void handle_data(const std::vector &buffer) { +#include "tcp_service.hpp" + +void handle_response(const std::vector &buffer) { const auto x = buffer[2] & 0xFFFF; const auto y = buffer[3] & 0xFFF; const auto z = buffer[4] & 0xFF; const auto w = buffer[5]; const auto result = x + y + z + w; - std::cout << "methane concentration: " << result << " ppm·m" << std::endl; - //http上传数据 + TcpService::getInstance().update_gas_value(result); } MethaneSerialPort::MethaneSerialPort( @@ -24,6 +25,8 @@ port_.set_option(serial_port_base::parity(serial_port_base::parity::none)); port_.set_option(serial_port_base::stop_bits(serial_port_base::stop_bits::one)); + std::cout << "MethaneSerialPort init success" << std::endl; + start_timer(); } @@ -44,23 +47,23 @@ if (!error) { // 获取接收到的数据 std::istream is(&buffer_); - std::vector received_data(static_cast(bytes_transferred)); + std::vector response(static_cast(bytes_transferred)); is.read( - reinterpret_cast(received_data.data()), static_cast(bytes_transferred) + reinterpret_cast(response.data()), static_cast(bytes_transferred) ); - // std::cout << "received " << bytes_transferred << " bytes: "; - // for (size_t i = 0; i < bytes_transferred; ++i) { - // std::cout << std::hex - // << std::uppercase - // << std::setw(2) - // << std::setfill('0') - // << static_cast(received_data[i]) - // << " "; - // } - // std::cout << std::endl; + std::cout << "received " << bytes_transferred << " bytes: "; + for (size_t i = 0; i < bytes_transferred; ++i) { + std::cout << std::hex + << std::uppercase + << std::setw(2) + << std::setfill('0') + << static_cast(response[i]) + << " "; + } + std::cout << std::endl; - handle_data(received_data); + handle_response(response); start_timer(); } else { @@ -68,4 +71,4 @@ start_timer(); } }); -} \ No newline at end of file +} diff --git a/src/robotic_arm_serial_port.cpp b/src/robotic_arm_serial_port.cpp index 17516ef..c6b850b 100644 --- a/src/robotic_arm_serial_port.cpp +++ b/src/robotic_arm_serial_port.cpp @@ -6,9 +6,6 @@ #include #include -void handle_data(const std::vector &buffer) { -} - RoboticArmSerialPort::RoboticArmSerialPort( boost::asio::io_service &io_service, const std::string &port_name, const int baud_rate ): io_service_(io_service), port_(io_service, port_name) { @@ -42,10 +39,21 @@ << " "; } std::cout << std::endl; - - handle_data(received_data); } else { std::cerr << "error: " << error.message() << std::endl; } }); } + +void RoboticArmSerialPort::write(std::vector command) { + for (const uint8_t i: command) { + std::cout << std::hex + << std::uppercase + << std::setw(2) + << std::setfill('0') + << static_cast(static_cast(i)) + << " "; + } + std::cout << std::endl; + boost::asio::write(port_, boost::asio::buffer(command)); +} \ No newline at end of file diff --git a/src/robotic_arm_serial_port.hpp b/src/robotic_arm_serial_port.hpp index 835539c..ac77fc6 100644 --- a/src/robotic_arm_serial_port.hpp +++ b/src/robotic_arm_serial_port.hpp @@ -15,6 +15,8 @@ boost::asio::serial_port &get() { return port_; } + void write(std::vector command); + private: boost::asio::io_service &io_service_; boost::asio::serial_port port_; diff --git a/src/slam_wrapper.cpp b/src/slam_wrapper.cpp index 4f22353..cefeef1 100644 --- a/src/slam_wrapper.cpp +++ b/src/slam_wrapper.cpp @@ -10,6 +10,14 @@ SlamWrapper::SlamWrapper() { ChannelFactory::Instance()->Init(0, "eth0"); + std::thread serial_port_thread([this] { + //初始化机械臂串口 + boost::asio::io_service io; + serial_port_ = new RoboticArmSerialPort(io, "/dev/ttyACM0", 115200); + io.run(); + }); + serial_port_thread.detach(); + // pub pubQtCommand->InitChannel(); pubQtNode->InitChannel(); @@ -18,22 +26,8 @@ // sub subQtNotice->InitChannel(std::bind(&SlamWrapper::qt_notice_handler, this, std::placeholders::_1), 10); 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::notify_observers(const int &node) { - for (const auto &observer: observers) { - observer->on_node_arrived(node); - } + std::cout << "SlamWrapper init success" << std::endl; } void SlamWrapper::control_robotic_arm() { @@ -44,13 +38,13 @@ 0xFE, 0xFE, 0x0F, 0x3C, 0x0A, 0x40, 0x0A, 0x89, 0x03, 0xDE, 0x08, 0x43, 0x09, 0xA8, 0x07, 0xE1, 0x1E, 0xFA }; - notify_observers(init_command); + serial_port_->write(init_command); std::this_thread::sleep_for(std::chrono::seconds(3)); - notify_observers(left_command); + serial_port_->write(left_command); std::this_thread::sleep_for(std::chrono::seconds(3)); - notify_observers(right_command); + serial_port_->write(right_command); std::this_thread::sleep_for(std::chrono::seconds(3)); - notify_observers(init_command); + serial_port_->write(init_command); std::cout << "Control robotic arm end" << std::endl; } @@ -84,7 +78,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_); + TcpService::getInstance().update_node(arrive_); if (arrive_ == 2) { //发送暂停指令 pause_navigation(); @@ -92,7 +86,7 @@ //TODO 控制机械臂转动到预设的角度 //朝右下方 std::cout << "发送机械臂转动指令，倒计时开始，请等待10秒" << std::endl; - notify_observers({ + serial_port_->write({ 0xFE, 0xFE, 0x0F, 0x3C, 0x0B, 0x71, 0x08, 0x25, 0x06, 0xFB, 0x08, 0x2E, 0x0A, 0xCB, 0x08, 0x28, @@ -105,7 +99,7 @@ std::this_thread::sleep_for(std::chrono::seconds(10)); recover_navigation(); std::cout << "倒计时结束，恢复机械臂初始位置，恢复巡检"; - notify_observers(init_command); + serial_port_->write(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 d1eff0f..94b245d 100644 --- a/src/slam_wrapper.hpp +++ b/src/slam_wrapper.hpp @@ -22,6 +22,7 @@ #define ADDNODETOPIC "rt/qt_add_node" #define ADDEDGETOPIC "rt/qt_add_edge" +#include "robotic_arm_serial_port.hpp" #include "tcp_service.hpp" using namespace unitree::robot; @@ -49,16 +50,10 @@ std::vector edge_attributes; u_int16_t node_name = 0; - void register_observer(const std::shared_ptr &observer); - - void notify_observers(const std::vector &command); - - void notify_observers(const int &node); + explicit SlamWrapper(); void control_robotic_arm(); - explicit SlamWrapper(); - void qt_notice_handler(const void *message); void odometer_handler(const void *message); @@ -94,7 +89,7 @@ private: int index = 0; const Odometry_ *currentOdom{}; - std::vector > observers; + RoboticArmSerialPort *serial_port_; const std::vector init_command = { 0xFE, 0xFE, 0x0F, 0x3C, 0x08, 0x00, 0x08, 0x00, 0x08, 0x00, 0x08, 0x00, 0x08, 0x00, 0x08, 0x00, 0x14, 0xFA }; diff --git a/src/tcp_client.cpp b/src/tcp_client.cpp index 46e3469..644f473 100644 --- a/src/tcp_client.cpp +++ b/src/tcp_client.cpp @@ -13,16 +13,7 @@ #include TcpClient::TcpClient() { - // 启动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; + std::cout << "TcpClient init success" << std::endl; } void TcpClient::handle_data(const std::vector &buffer) { @@ -54,9 +45,7 @@ std::cout << "Recover navigation" << std::endl; _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; + std::cout << "Start relocation and navigation to prepare for collecting node/edge information. " << std::endl; _slam.delete_all_node(); _slam.delete_all_edge(); _slam.start_relocation(); @@ -154,8 +143,8 @@ << ", waiting for command ..." << std::endl; //连接成功后把自己的编号发送给Tcp Service - // const std::string dog_code = "B42D4000OCAD3N82"; - // send(_client_socket, dog_code.c_str(), dog_code.size(), 0); + const std::string dog_code = "B42D4000OCAD3N82"; + send(_client_socket, dog_code.c_str(), dog_code.size(), 0); // 启动接收数据的线程 std::thread receive_thread(&TcpClient::receive_data, this); diff --git a/src/tcp_service.cpp b/src/tcp_service.cpp index 3f33706..450075c 100644 --- a/src/tcp_service.cpp +++ b/src/tcp_service.cpp @@ -14,47 +14,7 @@ #include #include -/** - * @link SlamWrapper---> @link TcpService - * @param command - */ -void TcpService::on_message_received(const std::vector &command) { - boost::asio::write(_port, boost::asio::buffer(command)); -} - -/** - * 回消息给已连接的TCP客户端。需要拼接数据包。（机器狗当前位置node，经纬度，浓度） - * @param node - */ -void TcpService::on_node_arrived(const int &node) { - const std::string message = std::to_string(node); - std::cout << "send message to client: " << message << std::endl; - for (const int client_socket: _client_sockets) { - send(client_socket, message.c_str(), message.size(), 0); - } -} - -void TcpService::init_serial_port(const std::string &port_name, const int baud_rate) { - _port.open(port_name); - if (_port.is_open()) { - _port.set_option(serial_port_base::baud_rate(baud_rate)); - _port.set_option(serial_port_base::character_size(8)); - _port.set_option(serial_port_base::stop_bits(serial_port_base::stop_bits::one)); - _port.set_option(serial_port_base::parity(serial_port_base::parity::none)); - _port.set_option(serial_port_base::flow_control(serial_port_base::flow_control::none)); - } else { - std::cerr << "Failed to open serial port" << std::endl; - } -} - -TcpService::TcpService( - boost::asio::io_context &io_context, const std::string &port_name, const int baud_rate -): _context(io_context), _port(io_context) { - std::cout << "TcpService::init success" << std::endl; - init_serial_port(port_name, baud_rate); -} - -void TcpService::handle_data_packet(std::vector &data) { +static void handle_data_packet(std::vector &data) { for (const uint8_t i: data) { std::cout << std::hex << std::uppercase @@ -67,10 +27,9 @@ //转为std::vector并发给机械臂串口 std::vector command(data.begin(), data.end()); - boost::asio::write(_port, boost::asio::buffer(command)); } -void TcpService::handle_client(const int client_socket) { +static void handle_client(const int client_socket) { std::vector buffer; char byte; while (true) { @@ -147,3 +106,22 @@ std::cerr << "exception in tcp_server::start: " << e.what() << std::endl; } } + +void TcpService::update_node(const int node) { + std::stringstream ss; + ss << node << "," + << gas_value << "," + << "116.296975" << "," + << "39.990889" + << std::endl; + const std::string message = ss.str(); + std::cout << "TcpService update node: " << message << std::endl; + for (const auto &client_socket: _client_sockets) { + send(client_socket, message.c_str(), message.size(), 0); + } +} + +void TcpService::update_gas_value(const int value) { + gas_value = value; + std::cout << "TcpService update gas value: " << gas_value << " ppm·m" << std::endl; +} diff --git a/src/tcp_service.hpp b/src/tcp_service.hpp index 7a8328f..b2bcc2f 100644 --- a/src/tcp_service.hpp +++ b/src/tcp_service.hpp @@ -8,31 +8,41 @@ #include #include -#include "message_observer.hpp" - using boost::asio::serial_port_base; -class TcpService final : public MessageObserver { +class TcpService { public: - explicit TcpService(boost::asio::io_context &io_context, const std::string &port_name, int baud_rate); + static TcpService &getInstance() { + static TcpService instance; + return instance; + } + + TcpService(const TcpService &) = delete; + + TcpService &operator=(const TcpService &) = delete; void start(int port); - void init_serial_port(const std::string &port_name, int baud_rate); + /** + * 更新节点信息 + * @param node + */ + void update_node(int node); - void on_message_received(const std::vector &command) override; - - void on_node_arrived(const int &node) override; + /** + * 更新甲烷浓度数据 + * @param value + */ + void update_gas_value(int value); private: - int _socket_fd; - std::vector _client_sockets; - boost::asio::io_context &_context; - boost::asio::serial_port _port; + TcpService() = default; - void handle_data_packet(std::vector &data); + ~TcpService() = default; - void handle_client(int client_socket); + int _socket_fd = -1; + std::vector _client_sockets{}; + int gas_value = 0; };