diff --git a/src/message_handler.cpp b/src/message_handler.cpp index d423476..71d60eb 100644 --- a/src/message_handler.cpp +++ b/src/message_handler.cpp @@ -13,7 +13,7 @@ try { boost::asio::io_service io; //打开串口 - serial_port_wrapper wrapper(io, "/dev/ttyUSB0", 115200); //改为甲烷串口 + SerialPortWrapper wrapper(io, "/dev/ttyUSB0", 115200); //改为甲烷串口 // 从串口读取数据 boost::asio::streambuf buffer; @@ -30,19 +30,19 @@ }); gas_serial_port_thread.join(); + // 启动TCP客户端，接收来自Web端的命令，控制机器狗运动 + std::thread client_thread([] { + TcpClient client("eth0"); + client.connect("192.168.123.18", 9001); + }); + client_thread.join(); + // 启动TCP服务，接收AI结果并通过串口控制机械臂 std::thread service_thread([] { boost::asio::io_context _context; - tcp_service service(_context, "/dev/ttyACM0", 115200); //机械臂串口 + TcpService service(_context, "/dev/ttyACM0", 115200); //机械臂串口 service.start(8888); }); service_thread.join(); - - // 启动TCP客户端，接收来自Web端的命令，控制机器狗运动 - std::thread client_thread([] { - tcp_client client("eth0"); - client.connect("192.168.123.18", 7777); - }); - client_thread.join(); return 0; } diff --git a/src/message_handler.cpp b/src/message_handler.cpp index d423476..71d60eb 100644 --- a/src/message_handler.cpp +++ b/src/message_handler.cpp @@ -13,7 +13,7 @@ try { boost::asio::io_service io; //打开串口 - serial_port_wrapper wrapper(io, "/dev/ttyUSB0", 115200); //改为甲烷串口 + SerialPortWrapper wrapper(io, "/dev/ttyUSB0", 115200); //改为甲烷串口 // 从串口读取数据 boost::asio::streambuf buffer; @@ -30,19 +30,19 @@ }); gas_serial_port_thread.join(); + // 启动TCP客户端，接收来自Web端的命令，控制机器狗运动 + std::thread client_thread([] { + TcpClient client("eth0"); + client.connect("192.168.123.18", 9001); + }); + client_thread.join(); + // 启动TCP服务，接收AI结果并通过串口控制机械臂 std::thread service_thread([] { boost::asio::io_context _context; - tcp_service service(_context, "/dev/ttyACM0", 115200); //机械臂串口 + TcpService service(_context, "/dev/ttyACM0", 115200); //机械臂串口 service.start(8888); }); service_thread.join(); - - // 启动TCP客户端，接收来自Web端的命令，控制机器狗运动 - std::thread client_thread([] { - tcp_client client("eth0"); - client.connect("192.168.123.18", 7777); - }); - client_thread.join(); return 0; } diff --git a/src/slam_wrapper.cpp b/src/slam_wrapper.cpp index 03453bc..281dea3 100644 --- a/src/slam_wrapper.cpp +++ b/src/slam_wrapper.cpp @@ -5,6 +5,7 @@ #include "slam_wrapper.hpp" #include "message_queue.hpp" #include +#include extern MessageQueue g_messageQueue; @@ -22,7 +23,7 @@ } void SlamWrapper::qt_notice_handler(const void *message) { - const std_msgs::msg::dds_::String_ *seq = (const std_msgs::msg::dds_::String_ *) message; + const auto seq = static_cast(message); std::string str_; int begin_ = seq->data().find("index:", 0); // Instruction unique identifier @@ -67,20 +68,36 @@ pause_navigation(); //TODO 控制机械臂转动到预设的角度 - g_messageQueue.enqueue({}); + g_messageQueue.enqueue({ + static_cast(0xFE), static_cast(0xFE), 0x0F, 0x3C, + 0x0b, 0x71, 0x08, 0x25, 0x06, 0xfb, + 0x08, 0x2e, 0x0a, 0xcb, 0x08, 0x28, + 0x14, 0xFA + }); //倒计时结束后恢复巡检 + // std::thread countdown_thread(count_down, 10); + // countdown_thread.join(); } } } +void SlamWrapper::count_down(std::atomic &seconds) { + // while (seconds) { + // std::this_thread::sleep_for(std::chrono::seconds(1)); + // --seconds; + // } + // std::cout << "count down end, recover_navigation"; + // recover_navigation(); +} + void SlamWrapper::odometer_handler(const void *message) { - currentOdom = (const nav_msgs::msg::dds_::Odometry_ *) message; + currentOdom = static_cast(message); } void SlamWrapper::start_mapping() { index++; - unitree_interfaces::msg::dds_::QtCommand_ send_msg; + QtCommand_ send_msg; send_msg.seq_().data() = "index:" + std::to_string(index) + ";"; send_msg.command_() = 3; // 3 is to start mapping command send_msg.attribute_() = 1; @@ -90,7 +107,7 @@ void SlamWrapper::end_mapping() { index++; // - unitree_interfaces::msg::dds_::QtCommand_ send_msg; + QtCommand_ send_msg; send_msg.command_() = 4; // 4 is the end mapping command send_msg.attribute_() = 0; send_msg.seq_().data() = "index:" + std::to_string(index) + ";"; @@ -101,7 +118,7 @@ void SlamWrapper::start_relocation() { index++; - unitree_interfaces::msg::dds_::QtCommand_ send_msg; + QtCommand_ send_msg; send_msg.seq_().data() = "index:" + std::to_string(index) + ";"; send_msg.command_() = 6; // 6 is to start relocation command send_msg.attribute_() = 1; @@ -111,7 +128,7 @@ void SlamWrapper::init_pose() { index++; - unitree_interfaces::msg::dds_::QtCommand_ send_msg; + QtCommand_ send_msg; send_msg.seq_().data() = "index:" + std::to_string(index) + ";"; send_msg.command_() = 7; // 7 is the pose initialization instruction send_msg.quaternion_x_() = 0; // Quaternion @@ -126,7 +143,7 @@ void SlamWrapper::start_navigation() { index++; - unitree_interfaces::msg::dds_::QtCommand_ send_msg; + QtCommand_ send_msg; send_msg.seq_().data() = "index:" + std::to_string(index) + ";"; send_msg.command_() = 8; // 8 is the command to start navigation pubQtCommand->Write(send_msg); @@ -134,25 +151,23 @@ void SlamWrapper::default_navigation() { index++; - unitree_interfaces::msg::dds_::QtCommand_ send_msg; + QtCommand_ send_msg; send_msg.seq_().data() = "index:" + std::to_string(index) + ";"; send_msg.command_() = 10; // 10 Multi nodes loop navigation(default) command pubQtCommand->Write(send_msg); } void SlamWrapper::add_node_and_edge() { - node_attribute nodeTemp{}; + NodeAttribute nodeTemp{}; - const float siny_cosp = 2 * (currentOdom - ->pose().pose().orientation().w() * currentOdom - ->pose().pose().orientation().z() + currentOdom - ->pose().pose().orientation().x() * currentOdom - ->pose().pose().orientation().y()); - const float cosy_cosp = 1 - 2 * (currentOdom - ->pose().pose().orientation().y() * currentOdom - ->pose().pose().orientation().y() + currentOdom - ->pose().pose().orientation().z() * currentOdom - ->pose().pose().orientation().z()); + const float siny_cosp = 2 * (currentOdom->pose().pose().orientation().w() + * currentOdom->pose().pose().orientation().z() + + currentOdom->pose().pose().orientation().x() + * currentOdom->pose().pose().orientation().y()); + const float cosy_cosp = 1 - 2 * (currentOdom->pose().pose().orientation().y() + * currentOdom->pose().pose().orientation().y() + + currentOdom->pose().pose().orientation().z() + * currentOdom->pose().pose().orientation().z()); node_name++; nodeTemp.nodeX = currentOdom->pose().pose().position().x(); @@ -178,8 +193,8 @@ } void SlamWrapper::save_node_and_edge() { - unitree_interfaces::msg::dds_::QtNode_ nodeMsg; - unitree_interfaces::msg::dds_::QtEdge_ edgeMsg; + QtNode_ nodeMsg; + QtEdge_ edgeMsg; if (edge_attributes.empty()) { return; } @@ -224,7 +239,7 @@ void SlamWrapper::close_all_node() { index++; - unitree_interfaces::msg::dds_::QtCommand_ send_msg; + QtCommand_ send_msg; send_msg.seq_().data() = "index:" + std::to_string(index) + ";"; send_msg.command_() = 99; // 99 Close all nodes command pubQtCommand->Write(send_msg); @@ -232,7 +247,7 @@ void SlamWrapper::delete_all_node() { index++; - unitree_interfaces::msg::dds_::QtCommand_ send_msg; + QtCommand_ send_msg; send_msg.seq_().data() = "index:" + std::to_string(index) + ";"; send_msg.command_() = 1; // 1 is delete instruction send_msg.attribute_() = 1; // 1 is select delete node @@ -242,7 +257,7 @@ void SlamWrapper::delete_all_edge() { index++; - unitree_interfaces::msg::dds_::QtCommand_ send_msg; + QtCommand_ send_msg; send_msg.seq_().data() = "index:" + std::to_string(index) + ";"; send_msg.command_() = 1; // 1 is delete instruction send_msg.attribute_() = 2; // 2 is select delete edge @@ -252,7 +267,7 @@ void SlamWrapper::pause_navigation() { index++; - unitree_interfaces::msg::dds_::QtCommand_ send_msg; + QtCommand_ send_msg; send_msg.seq_().data() = "index:" + std::to_string(index) + ";"; send_msg.command_() = 13; // 13 is the pause navigation command pubQtCommand->Write(send_msg); @@ -260,7 +275,7 @@ void SlamWrapper::recover_navigation() { index++; - unitree_interfaces::msg::dds_::QtCommand_ send_msg; + QtCommand_ send_msg; send_msg.seq_().data() = "index:" + std::to_string(index) + ";"; send_msg.command_() = 14; // 14 to recover navigation commands pubQtCommand->Write(send_msg); diff --git a/src/message_handler.cpp b/src/message_handler.cpp index d423476..71d60eb 100644 --- a/src/message_handler.cpp +++ b/src/message_handler.cpp @@ -13,7 +13,7 @@ try { boost::asio::io_service io; //打开串口 - serial_port_wrapper wrapper(io, "/dev/ttyUSB0", 115200); //改为甲烷串口 + SerialPortWrapper wrapper(io, "/dev/ttyUSB0", 115200); //改为甲烷串口 // 从串口读取数据 boost::asio::streambuf buffer; @@ -30,19 +30,19 @@ }); gas_serial_port_thread.join(); + // 启动TCP客户端，接收来自Web端的命令，控制机器狗运动 + std::thread client_thread([] { + TcpClient client("eth0"); + client.connect("192.168.123.18", 9001); + }); + client_thread.join(); + // 启动TCP服务，接收AI结果并通过串口控制机械臂 std::thread service_thread([] { boost::asio::io_context _context; - tcp_service service(_context, "/dev/ttyACM0", 115200); //机械臂串口 + TcpService service(_context, "/dev/ttyACM0", 115200); //机械臂串口 service.start(8888); }); service_thread.join(); - - // 启动TCP客户端，接收来自Web端的命令，控制机器狗运动 - std::thread client_thread([] { - tcp_client client("eth0"); - client.connect("192.168.123.18", 7777); - }); - client_thread.join(); return 0; } diff --git a/src/slam_wrapper.cpp b/src/slam_wrapper.cpp index 03453bc..281dea3 100644 --- a/src/slam_wrapper.cpp +++ b/src/slam_wrapper.cpp @@ -5,6 +5,7 @@ #include "slam_wrapper.hpp" #include "message_queue.hpp" #include +#include extern MessageQueue g_messageQueue; @@ -22,7 +23,7 @@ } void SlamWrapper::qt_notice_handler(const void *message) { - const std_msgs::msg::dds_::String_ *seq = (const std_msgs::msg::dds_::String_ *) message; + const auto seq = static_cast(message); std::string str_; int begin_ = seq->data().find("index:", 0); // Instruction unique identifier @@ -67,20 +68,36 @@ pause_navigation(); //TODO 控制机械臂转动到预设的角度 - g_messageQueue.enqueue({}); + g_messageQueue.enqueue({ + static_cast(0xFE), static_cast(0xFE), 0x0F, 0x3C, + 0x0b, 0x71, 0x08, 0x25, 0x06, 0xfb, + 0x08, 0x2e, 0x0a, 0xcb, 0x08, 0x28, + 0x14, 0xFA + }); //倒计时结束后恢复巡检 + // std::thread countdown_thread(count_down, 10); + // countdown_thread.join(); } } } +void SlamWrapper::count_down(std::atomic &seconds) { + // while (seconds) { + // std::this_thread::sleep_for(std::chrono::seconds(1)); + // --seconds; + // } + // std::cout << "count down end, recover_navigation"; + // recover_navigation(); +} + void SlamWrapper::odometer_handler(const void *message) { - currentOdom = (const nav_msgs::msg::dds_::Odometry_ *) message; + currentOdom = static_cast(message); } void SlamWrapper::start_mapping() { index++; - unitree_interfaces::msg::dds_::QtCommand_ send_msg; + QtCommand_ send_msg; send_msg.seq_().data() = "index:" + std::to_string(index) + ";"; send_msg.command_() = 3; // 3 is to start mapping command send_msg.attribute_() = 1; @@ -90,7 +107,7 @@ void SlamWrapper::end_mapping() { index++; // - unitree_interfaces::msg::dds_::QtCommand_ send_msg; + QtCommand_ send_msg; send_msg.command_() = 4; // 4 is the end mapping command send_msg.attribute_() = 0; send_msg.seq_().data() = "index:" + std::to_string(index) + ";"; @@ -101,7 +118,7 @@ void SlamWrapper::start_relocation() { index++; - unitree_interfaces::msg::dds_::QtCommand_ send_msg; + QtCommand_ send_msg; send_msg.seq_().data() = "index:" + std::to_string(index) + ";"; send_msg.command_() = 6; // 6 is to start relocation command send_msg.attribute_() = 1; @@ -111,7 +128,7 @@ void SlamWrapper::init_pose() { index++; - unitree_interfaces::msg::dds_::QtCommand_ send_msg; + QtCommand_ send_msg; send_msg.seq_().data() = "index:" + std::to_string(index) + ";"; send_msg.command_() = 7; // 7 is the pose initialization instruction send_msg.quaternion_x_() = 0; // Quaternion @@ -126,7 +143,7 @@ void SlamWrapper::start_navigation() { index++; - unitree_interfaces::msg::dds_::QtCommand_ send_msg; + QtCommand_ send_msg; send_msg.seq_().data() = "index:" + std::to_string(index) + ";"; send_msg.command_() = 8; // 8 is the command to start navigation pubQtCommand->Write(send_msg); @@ -134,25 +151,23 @@ void SlamWrapper::default_navigation() { index++; - unitree_interfaces::msg::dds_::QtCommand_ send_msg; + QtCommand_ send_msg; send_msg.seq_().data() = "index:" + std::to_string(index) + ";"; send_msg.command_() = 10; // 10 Multi nodes loop navigation(default) command pubQtCommand->Write(send_msg); } void SlamWrapper::add_node_and_edge() { - node_attribute nodeTemp{}; + NodeAttribute nodeTemp{}; - const float siny_cosp = 2 * (currentOdom - ->pose().pose().orientation().w() * currentOdom - ->pose().pose().orientation().z() + currentOdom - ->pose().pose().orientation().x() * currentOdom - ->pose().pose().orientation().y()); - const float cosy_cosp = 1 - 2 * (currentOdom - ->pose().pose().orientation().y() * currentOdom - ->pose().pose().orientation().y() + currentOdom - ->pose().pose().orientation().z() * currentOdom - ->pose().pose().orientation().z()); + const float siny_cosp = 2 * (currentOdom->pose().pose().orientation().w() + * currentOdom->pose().pose().orientation().z() + + currentOdom->pose().pose().orientation().x() + * currentOdom->pose().pose().orientation().y()); + const float cosy_cosp = 1 - 2 * (currentOdom->pose().pose().orientation().y() + * currentOdom->pose().pose().orientation().y() + + currentOdom->pose().pose().orientation().z() + * currentOdom->pose().pose().orientation().z()); node_name++; nodeTemp.nodeX = currentOdom->pose().pose().position().x(); @@ -178,8 +193,8 @@ } void SlamWrapper::save_node_and_edge() { - unitree_interfaces::msg::dds_::QtNode_ nodeMsg; - unitree_interfaces::msg::dds_::QtEdge_ edgeMsg; + QtNode_ nodeMsg; + QtEdge_ edgeMsg; if (edge_attributes.empty()) { return; } @@ -224,7 +239,7 @@ void SlamWrapper::close_all_node() { index++; - unitree_interfaces::msg::dds_::QtCommand_ send_msg; + QtCommand_ send_msg; send_msg.seq_().data() = "index:" + std::to_string(index) + ";"; send_msg.command_() = 99; // 99 Close all nodes command pubQtCommand->Write(send_msg); @@ -232,7 +247,7 @@ void SlamWrapper::delete_all_node() { index++; - unitree_interfaces::msg::dds_::QtCommand_ send_msg; + QtCommand_ send_msg; send_msg.seq_().data() = "index:" + std::to_string(index) + ";"; send_msg.command_() = 1; // 1 is delete instruction send_msg.attribute_() = 1; // 1 is select delete node @@ -242,7 +257,7 @@ void SlamWrapper::delete_all_edge() { index++; - unitree_interfaces::msg::dds_::QtCommand_ send_msg; + QtCommand_ send_msg; send_msg.seq_().data() = "index:" + std::to_string(index) + ";"; send_msg.command_() = 1; // 1 is delete instruction send_msg.attribute_() = 2; // 2 is select delete edge @@ -252,7 +267,7 @@ void SlamWrapper::pause_navigation() { index++; - unitree_interfaces::msg::dds_::QtCommand_ send_msg; + QtCommand_ send_msg; send_msg.seq_().data() = "index:" + std::to_string(index) + ";"; send_msg.command_() = 13; // 13 is the pause navigation command pubQtCommand->Write(send_msg); @@ -260,7 +275,7 @@ void SlamWrapper::recover_navigation() { index++; - unitree_interfaces::msg::dds_::QtCommand_ send_msg; + QtCommand_ send_msg; send_msg.seq_().data() = "index:" + std::to_string(index) + ";"; send_msg.command_() = 14; // 14 to recover navigation commands pubQtCommand->Write(send_msg); diff --git a/src/slam_wrapper.hpp b/src/slam_wrapper.hpp index 80fb1f6..2237c02 100644 --- a/src/slam_wrapper.hpp +++ b/src/slam_wrapper.hpp @@ -103,6 +103,8 @@ void pause_navigation(); void recover_navigation(); + + void count_down(std::atomic &seconds); }; #endif //SLAM_WRAPPER_HPP diff --git a/src/message_handler.cpp b/src/message_handler.cpp index d423476..71d60eb 100644 --- a/src/message_handler.cpp +++ b/src/message_handler.cpp @@ -13,7 +13,7 @@ try { boost::asio::io_service io; //打开串口 - serial_port_wrapper wrapper(io, "/dev/ttyUSB0", 115200); //改为甲烷串口 + SerialPortWrapper wrapper(io, "/dev/ttyUSB0", 115200); //改为甲烷串口 // 从串口读取数据 boost::asio::streambuf buffer; @@ -30,19 +30,19 @@ }); gas_serial_port_thread.join(); + // 启动TCP客户端，接收来自Web端的命令，控制机器狗运动 + std::thread client_thread([] { + TcpClient client("eth0"); + client.connect("192.168.123.18", 9001); + }); + client_thread.join(); + // 启动TCP服务，接收AI结果并通过串口控制机械臂 std::thread service_thread([] { boost::asio::io_context _context; - tcp_service service(_context, "/dev/ttyACM0", 115200); //机械臂串口 + TcpService service(_context, "/dev/ttyACM0", 115200); //机械臂串口 service.start(8888); }); service_thread.join(); - - // 启动TCP客户端，接收来自Web端的命令，控制机器狗运动 - std::thread client_thread([] { - tcp_client client("eth0"); - client.connect("192.168.123.18", 7777); - }); - client_thread.join(); return 0; } diff --git a/src/slam_wrapper.cpp b/src/slam_wrapper.cpp index 03453bc..281dea3 100644 --- a/src/slam_wrapper.cpp +++ b/src/slam_wrapper.cpp @@ -5,6 +5,7 @@ #include "slam_wrapper.hpp" #include "message_queue.hpp" #include +#include extern MessageQueue g_messageQueue; @@ -22,7 +23,7 @@ } void SlamWrapper::qt_notice_handler(const void *message) { - const std_msgs::msg::dds_::String_ *seq = (const std_msgs::msg::dds_::String_ *) message; + const auto seq = static_cast(message); std::string str_; int begin_ = seq->data().find("index:", 0); // Instruction unique identifier @@ -67,20 +68,36 @@ pause_navigation(); //TODO 控制机械臂转动到预设的角度 - g_messageQueue.enqueue({}); + g_messageQueue.enqueue({ + static_cast(0xFE), static_cast(0xFE), 0x0F, 0x3C, + 0x0b, 0x71, 0x08, 0x25, 0x06, 0xfb, + 0x08, 0x2e, 0x0a, 0xcb, 0x08, 0x28, + 0x14, 0xFA + }); //倒计时结束后恢复巡检 + // std::thread countdown_thread(count_down, 10); + // countdown_thread.join(); } } } +void SlamWrapper::count_down(std::atomic &seconds) { + // while (seconds) { + // std::this_thread::sleep_for(std::chrono::seconds(1)); + // --seconds; + // } + // std::cout << "count down end, recover_navigation"; + // recover_navigation(); +} + void SlamWrapper::odometer_handler(const void *message) { - currentOdom = (const nav_msgs::msg::dds_::Odometry_ *) message; + currentOdom = static_cast(message); } void SlamWrapper::start_mapping() { index++; - unitree_interfaces::msg::dds_::QtCommand_ send_msg; + QtCommand_ send_msg; send_msg.seq_().data() = "index:" + std::to_string(index) + ";"; send_msg.command_() = 3; // 3 is to start mapping command send_msg.attribute_() = 1; @@ -90,7 +107,7 @@ void SlamWrapper::end_mapping() { index++; // - unitree_interfaces::msg::dds_::QtCommand_ send_msg; + QtCommand_ send_msg; send_msg.command_() = 4; // 4 is the end mapping command send_msg.attribute_() = 0; send_msg.seq_().data() = "index:" + std::to_string(index) + ";"; @@ -101,7 +118,7 @@ void SlamWrapper::start_relocation() { index++; - unitree_interfaces::msg::dds_::QtCommand_ send_msg; + QtCommand_ send_msg; send_msg.seq_().data() = "index:" + std::to_string(index) + ";"; send_msg.command_() = 6; // 6 is to start relocation command send_msg.attribute_() = 1; @@ -111,7 +128,7 @@ void SlamWrapper::init_pose() { index++; - unitree_interfaces::msg::dds_::QtCommand_ send_msg; + QtCommand_ send_msg; send_msg.seq_().data() = "index:" + std::to_string(index) + ";"; send_msg.command_() = 7; // 7 is the pose initialization instruction send_msg.quaternion_x_() = 0; // Quaternion @@ -126,7 +143,7 @@ void SlamWrapper::start_navigation() { index++; - unitree_interfaces::msg::dds_::QtCommand_ send_msg; + QtCommand_ send_msg; send_msg.seq_().data() = "index:" + std::to_string(index) + ";"; send_msg.command_() = 8; // 8 is the command to start navigation pubQtCommand->Write(send_msg); @@ -134,25 +151,23 @@ void SlamWrapper::default_navigation() { index++; - unitree_interfaces::msg::dds_::QtCommand_ send_msg; + QtCommand_ send_msg; send_msg.seq_().data() = "index:" + std::to_string(index) + ";"; send_msg.command_() = 10; // 10 Multi nodes loop navigation(default) command pubQtCommand->Write(send_msg); } void SlamWrapper::add_node_and_edge() { - node_attribute nodeTemp{}; + NodeAttribute nodeTemp{}; - const float siny_cosp = 2 * (currentOdom - ->pose().pose().orientation().w() * currentOdom - ->pose().pose().orientation().z() + currentOdom - ->pose().pose().orientation().x() * currentOdom - ->pose().pose().orientation().y()); - const float cosy_cosp = 1 - 2 * (currentOdom - ->pose().pose().orientation().y() * currentOdom - ->pose().pose().orientation().y() + currentOdom - ->pose().pose().orientation().z() * currentOdom - ->pose().pose().orientation().z()); + const float siny_cosp = 2 * (currentOdom->pose().pose().orientation().w() + * currentOdom->pose().pose().orientation().z() + + currentOdom->pose().pose().orientation().x() + * currentOdom->pose().pose().orientation().y()); + const float cosy_cosp = 1 - 2 * (currentOdom->pose().pose().orientation().y() + * currentOdom->pose().pose().orientation().y() + + currentOdom->pose().pose().orientation().z() + * currentOdom->pose().pose().orientation().z()); node_name++; nodeTemp.nodeX = currentOdom->pose().pose().position().x(); @@ -178,8 +193,8 @@ } void SlamWrapper::save_node_and_edge() { - unitree_interfaces::msg::dds_::QtNode_ nodeMsg; - unitree_interfaces::msg::dds_::QtEdge_ edgeMsg; + QtNode_ nodeMsg; + QtEdge_ edgeMsg; if (edge_attributes.empty()) { return; } @@ -224,7 +239,7 @@ void SlamWrapper::close_all_node() { index++; - unitree_interfaces::msg::dds_::QtCommand_ send_msg; + QtCommand_ send_msg; send_msg.seq_().data() = "index:" + std::to_string(index) + ";"; send_msg.command_() = 99; // 99 Close all nodes command pubQtCommand->Write(send_msg); @@ -232,7 +247,7 @@ void SlamWrapper::delete_all_node() { index++; - unitree_interfaces::msg::dds_::QtCommand_ send_msg; + QtCommand_ send_msg; send_msg.seq_().data() = "index:" + std::to_string(index) + ";"; send_msg.command_() = 1; // 1 is delete instruction send_msg.attribute_() = 1; // 1 is select delete node @@ -242,7 +257,7 @@ void SlamWrapper::delete_all_edge() { index++; - unitree_interfaces::msg::dds_::QtCommand_ send_msg; + QtCommand_ send_msg; send_msg.seq_().data() = "index:" + std::to_string(index) + ";"; send_msg.command_() = 1; // 1 is delete instruction send_msg.attribute_() = 2; // 2 is select delete edge @@ -252,7 +267,7 @@ void SlamWrapper::pause_navigation() { index++; - unitree_interfaces::msg::dds_::QtCommand_ send_msg; + QtCommand_ send_msg; send_msg.seq_().data() = "index:" + std::to_string(index) + ";"; send_msg.command_() = 13; // 13 is the pause navigation command pubQtCommand->Write(send_msg); @@ -260,7 +275,7 @@ void SlamWrapper::recover_navigation() { index++; - unitree_interfaces::msg::dds_::QtCommand_ send_msg; + QtCommand_ send_msg; send_msg.seq_().data() = "index:" + std::to_string(index) + ";"; send_msg.command_() = 14; // 14 to recover navigation commands pubQtCommand->Write(send_msg); diff --git a/src/slam_wrapper.hpp b/src/slam_wrapper.hpp index 80fb1f6..2237c02 100644 --- a/src/slam_wrapper.hpp +++ b/src/slam_wrapper.hpp @@ -103,6 +103,8 @@ void pause_navigation(); void recover_navigation(); + + void count_down(std::atomic &seconds); }; #endif //SLAM_WRAPPER_HPP diff --git a/src/tcp_client.cpp b/src/tcp_client.cpp index 439c94d..433ceba 100644 --- a/src/tcp_client.cpp +++ b/src/tcp_client.cpp @@ -15,6 +15,7 @@ void TcpClient::handle_data(const std::vector &buffer) { const std::string received_string(buffer.begin(), buffer.end()); + std::cout << "handle_data >>>> " << received_string << std::endl; if (received_string == "q") { std::cout << "Close all nodes" << std::endl; _slam.close_all_node(); diff --git a/src/message_handler.cpp b/src/message_handler.cpp index d423476..71d60eb 100644 --- a/src/message_handler.cpp +++ b/src/message_handler.cpp @@ -13,7 +13,7 @@ try { boost::asio::io_service io; //打开串口 - serial_port_wrapper wrapper(io, "/dev/ttyUSB0", 115200); //改为甲烷串口 + SerialPortWrapper wrapper(io, "/dev/ttyUSB0", 115200); //改为甲烷串口 // 从串口读取数据 boost::asio::streambuf buffer; @@ -30,19 +30,19 @@ }); gas_serial_port_thread.join(); + // 启动TCP客户端，接收来自Web端的命令，控制机器狗运动 + std::thread client_thread([] { + TcpClient client("eth0"); + client.connect("192.168.123.18", 9001); + }); + client_thread.join(); + // 启动TCP服务，接收AI结果并通过串口控制机械臂 std::thread service_thread([] { boost::asio::io_context _context; - tcp_service service(_context, "/dev/ttyACM0", 115200); //机械臂串口 + TcpService service(_context, "/dev/ttyACM0", 115200); //机械臂串口 service.start(8888); }); service_thread.join(); - - // 启动TCP客户端，接收来自Web端的命令，控制机器狗运动 - std::thread client_thread([] { - tcp_client client("eth0"); - client.connect("192.168.123.18", 7777); - }); - client_thread.join(); return 0; } diff --git a/src/slam_wrapper.cpp b/src/slam_wrapper.cpp index 03453bc..281dea3 100644 --- a/src/slam_wrapper.cpp +++ b/src/slam_wrapper.cpp @@ -5,6 +5,7 @@ #include "slam_wrapper.hpp" #include "message_queue.hpp" #include +#include extern MessageQueue g_messageQueue; @@ -22,7 +23,7 @@ } void SlamWrapper::qt_notice_handler(const void *message) { - const std_msgs::msg::dds_::String_ *seq = (const std_msgs::msg::dds_::String_ *) message; + const auto seq = static_cast(message); std::string str_; int begin_ = seq->data().find("index:", 0); // Instruction unique identifier @@ -67,20 +68,36 @@ pause_navigation(); //TODO 控制机械臂转动到预设的角度 - g_messageQueue.enqueue({}); + g_messageQueue.enqueue({ + static_cast(0xFE), static_cast(0xFE), 0x0F, 0x3C, + 0x0b, 0x71, 0x08, 0x25, 0x06, 0xfb, + 0x08, 0x2e, 0x0a, 0xcb, 0x08, 0x28, + 0x14, 0xFA + }); //倒计时结束后恢复巡检 + // std::thread countdown_thread(count_down, 10); + // countdown_thread.join(); } } } +void SlamWrapper::count_down(std::atomic &seconds) { + // while (seconds) { + // std::this_thread::sleep_for(std::chrono::seconds(1)); + // --seconds; + // } + // std::cout << "count down end, recover_navigation"; + // recover_navigation(); +} + void SlamWrapper::odometer_handler(const void *message) { - currentOdom = (const nav_msgs::msg::dds_::Odometry_ *) message; + currentOdom = static_cast(message); } void SlamWrapper::start_mapping() { index++; - unitree_interfaces::msg::dds_::QtCommand_ send_msg; + QtCommand_ send_msg; send_msg.seq_().data() = "index:" + std::to_string(index) + ";"; send_msg.command_() = 3; // 3 is to start mapping command send_msg.attribute_() = 1; @@ -90,7 +107,7 @@ void SlamWrapper::end_mapping() { index++; // - unitree_interfaces::msg::dds_::QtCommand_ send_msg; + QtCommand_ send_msg; send_msg.command_() = 4; // 4 is the end mapping command send_msg.attribute_() = 0; send_msg.seq_().data() = "index:" + std::to_string(index) + ";"; @@ -101,7 +118,7 @@ void SlamWrapper::start_relocation() { index++; - unitree_interfaces::msg::dds_::QtCommand_ send_msg; + QtCommand_ send_msg; send_msg.seq_().data() = "index:" + std::to_string(index) + ";"; send_msg.command_() = 6; // 6 is to start relocation command send_msg.attribute_() = 1; @@ -111,7 +128,7 @@ void SlamWrapper::init_pose() { index++; - unitree_interfaces::msg::dds_::QtCommand_ send_msg; + QtCommand_ send_msg; send_msg.seq_().data() = "index:" + std::to_string(index) + ";"; send_msg.command_() = 7; // 7 is the pose initialization instruction send_msg.quaternion_x_() = 0; // Quaternion @@ -126,7 +143,7 @@ void SlamWrapper::start_navigation() { index++; - unitree_interfaces::msg::dds_::QtCommand_ send_msg; + QtCommand_ send_msg; send_msg.seq_().data() = "index:" + std::to_string(index) + ";"; send_msg.command_() = 8; // 8 is the command to start navigation pubQtCommand->Write(send_msg); @@ -134,25 +151,23 @@ void SlamWrapper::default_navigation() { index++; - unitree_interfaces::msg::dds_::QtCommand_ send_msg; + QtCommand_ send_msg; send_msg.seq_().data() = "index:" + std::to_string(index) + ";"; send_msg.command_() = 10; // 10 Multi nodes loop navigation(default) command pubQtCommand->Write(send_msg); } void SlamWrapper::add_node_and_edge() { - node_attribute nodeTemp{}; + NodeAttribute nodeTemp{}; - const float siny_cosp = 2 * (currentOdom - ->pose().pose().orientation().w() * currentOdom - ->pose().pose().orientation().z() + currentOdom - ->pose().pose().orientation().x() * currentOdom - ->pose().pose().orientation().y()); - const float cosy_cosp = 1 - 2 * (currentOdom - ->pose().pose().orientation().y() * currentOdom - ->pose().pose().orientation().y() + currentOdom - ->pose().pose().orientation().z() * currentOdom - ->pose().pose().orientation().z()); + const float siny_cosp = 2 * (currentOdom->pose().pose().orientation().w() + * currentOdom->pose().pose().orientation().z() + + currentOdom->pose().pose().orientation().x() + * currentOdom->pose().pose().orientation().y()); + const float cosy_cosp = 1 - 2 * (currentOdom->pose().pose().orientation().y() + * currentOdom->pose().pose().orientation().y() + + currentOdom->pose().pose().orientation().z() + * currentOdom->pose().pose().orientation().z()); node_name++; nodeTemp.nodeX = currentOdom->pose().pose().position().x(); @@ -178,8 +193,8 @@ } void SlamWrapper::save_node_and_edge() { - unitree_interfaces::msg::dds_::QtNode_ nodeMsg; - unitree_interfaces::msg::dds_::QtEdge_ edgeMsg; + QtNode_ nodeMsg; + QtEdge_ edgeMsg; if (edge_attributes.empty()) { return; } @@ -224,7 +239,7 @@ void SlamWrapper::close_all_node() { index++; - unitree_interfaces::msg::dds_::QtCommand_ send_msg; + QtCommand_ send_msg; send_msg.seq_().data() = "index:" + std::to_string(index) + ";"; send_msg.command_() = 99; // 99 Close all nodes command pubQtCommand->Write(send_msg); @@ -232,7 +247,7 @@ void SlamWrapper::delete_all_node() { index++; - unitree_interfaces::msg::dds_::QtCommand_ send_msg; + QtCommand_ send_msg; send_msg.seq_().data() = "index:" + std::to_string(index) + ";"; send_msg.command_() = 1; // 1 is delete instruction send_msg.attribute_() = 1; // 1 is select delete node @@ -242,7 +257,7 @@ void SlamWrapper::delete_all_edge() { index++; - unitree_interfaces::msg::dds_::QtCommand_ send_msg; + QtCommand_ send_msg; send_msg.seq_().data() = "index:" + std::to_string(index) + ";"; send_msg.command_() = 1; // 1 is delete instruction send_msg.attribute_() = 2; // 2 is select delete edge @@ -252,7 +267,7 @@ void SlamWrapper::pause_navigation() { index++; - unitree_interfaces::msg::dds_::QtCommand_ send_msg; + QtCommand_ send_msg; send_msg.seq_().data() = "index:" + std::to_string(index) + ";"; send_msg.command_() = 13; // 13 is the pause navigation command pubQtCommand->Write(send_msg); @@ -260,7 +275,7 @@ void SlamWrapper::recover_navigation() { index++; - unitree_interfaces::msg::dds_::QtCommand_ send_msg; + QtCommand_ send_msg; send_msg.seq_().data() = "index:" + std::to_string(index) + ";"; send_msg.command_() = 14; // 14 to recover navigation commands pubQtCommand->Write(send_msg); diff --git a/src/slam_wrapper.hpp b/src/slam_wrapper.hpp index 80fb1f6..2237c02 100644 --- a/src/slam_wrapper.hpp +++ b/src/slam_wrapper.hpp @@ -103,6 +103,8 @@ void pause_navigation(); void recover_navigation(); + + void count_down(std::atomic &seconds); }; #endif //SLAM_WRAPPER_HPP diff --git a/src/tcp_client.cpp b/src/tcp_client.cpp index 439c94d..433ceba 100644 --- a/src/tcp_client.cpp +++ b/src/tcp_client.cpp @@ -15,6 +15,7 @@ void TcpClient::handle_data(const std::vector &buffer) { const std::string received_string(buffer.begin(), buffer.end()); + std::cout << "handle_data >>>> " << received_string << std::endl; if (received_string == "q") { std::cout << "Close all nodes" << std::endl; _slam.close_all_node(); diff --git a/src/tcp_client.hpp b/src/tcp_client.hpp index 5397635..92b47e9 100644 --- a/src/tcp_client.hpp +++ b/src/tcp_client.hpp @@ -30,7 +30,7 @@ mutable std::atomic _should_stop{false}; mutable std::mutex _mutex; int _client_socket{-1}; - int _max_retries = 5; + int _max_retries = 12; int _retry_interval = 5000; //重连间隔 SlamWrapper _slam;