diff --git a/src/constant_config.hpp b/src/constant_config.hpp index 3b8cbb6..5b9300e 100644 --- a/src/constant_config.hpp +++ b/src/constant_config.hpp @@ -18,6 +18,7 @@ constexpr int BAUD_RATE = 115200; constexpr const char *API_RECEIVE_DATA = "/device/receiveData"; + constexpr const char *API_UPLOAD_PCD_FILE = "/device/receiveFile"; constexpr const char *HTTP_PORT = "30028"; constexpr const char *DOG_CODE = "B42D4000OCAD3N82"; @@ -26,6 +27,8 @@ constexpr const char *ROBOTIC_ARM_PORT = "/dev/ttyACM0"; constexpr const char *ROBOTIC_ARM_COMMAND_CONFIG = "/home/unitree/Desktop/robotic_arm_commands.conf"; + constexpr const char *SLAM_PCD_FILE = + "/unitree/module/graph_pid_ws/config_files/lio_sam_config/maps/pcd/default/GlobalMap.pcd"; } #endif //CONSTANT_CONFIG_HPP diff --git a/src/constant_config.hpp b/src/constant_config.hpp index 3b8cbb6..5b9300e 100644 --- a/src/constant_config.hpp +++ b/src/constant_config.hpp @@ -18,6 +18,7 @@ constexpr int BAUD_RATE = 115200; constexpr const char *API_RECEIVE_DATA = "/device/receiveData"; + constexpr const char *API_UPLOAD_PCD_FILE = "/device/receiveFile"; constexpr const char *HTTP_PORT = "30028"; constexpr const char *DOG_CODE = "B42D4000OCAD3N82"; @@ -26,6 +27,8 @@ constexpr const char *ROBOTIC_ARM_PORT = "/dev/ttyACM0"; constexpr const char *ROBOTIC_ARM_COMMAND_CONFIG = "/home/unitree/Desktop/robotic_arm_commands.conf"; + constexpr const char *SLAM_PCD_FILE = + "/unitree/module/graph_pid_ws/config_files/lio_sam_config/maps/pcd/default/GlobalMap.pcd"; } #endif //CONSTANT_CONFIG_HPP diff --git a/src/slam_wrapper.cpp b/src/slam_wrapper.cpp index 8d9c373..0a46d7b 100644 --- a/src/slam_wrapper.cpp +++ b/src/slam_wrapper.cpp @@ -10,6 +10,15 @@ #include #include #include +#include +#include +#include +#include +#include +#include + +namespace http = boost::beast::http; +using tcp = boost::asio::ip::tcp; static std::string trim(const std::string &s) { auto start = s.begin(); @@ -23,6 +32,83 @@ return std::string(start, end + 1); } +static std::string generate_multipart_form(const std::string &file_path, const std::string &boundary, + std::string &content_type) { + std::ifstream file(file_path, std::ios::binary); + if (!file) { + throw std::runtime_error("无法打开文件: " + file_path); + } + + // 提取文件名（避免暴露完整路径） + std::string file_name = file_path.substr(file_path.find_last_of("/\\") + 1); + + // 读取文件内容 + std::ostringstream content_stream; + content_stream << file.rdbuf(); + std::string file_content = content_stream.str(); + + // 构建 multipart/form-data 内容 + std::ostringstream body; + body << "--" << boundary << "\r\n"; + body << R"(Content-Disposition: form-data; name="file"; filename=")" << file_name << "\"\r\n"; + body << "Content-Type: application/octet-stream\r\n\r\n"; + body << file_content; + body << "\r\n--" << boundary << "--\r\n"; + + content_type = "multipart/form-data; boundary=" + boundary; + return body.str(); +} + +static void upload_pcd_file() { + std::cout << "upload_pcd_file start" << std::endl; + try { + // 设置 Boost.Asio 上下文 + boost::asio::io_context ioc; + tcp::resolver resolver(ioc); + tcp::socket socket(ioc); + + // 解析并连接服务器 + auto const results = resolver.resolve(ConstantConfig::SERVICE_ADDRESS, ConstantConfig::HTTP_PORT); + boost::asio::connect(socket, results.begin(), results.end()); + + // 构建 HTTP 请求 + http::request req{http::verb::post, ConstantConfig::API_UPLOAD_PCD_FILE, 11}; + req.set(http::field::host, ConstantConfig::SERVICE_ADDRESS); + req.set(http::field::user_agent, BOOST_BEAST_VERSION_STRING); + + // 构建 multipart/form-data 数据 + std::string boundary = "---------------------------boundary_example_" + std::to_string(std::time(nullptr)); + std::string contentType; + std::string body = generate_multipart_form(ConstantConfig::SLAM_PCD_FILE, boundary, contentType); + if (body.empty()) { + throw std::runtime_error("Failed to generate multipart form data"); + } + req.body() = body; + req.prepare_payload(); + req.set(http::field::content_type, contentType); + http::write(socket, req); + + // 接收响应 + boost::beast::flat_buffer buffer; + http::response res; + http::read(socket, buffer, res); + std::cout << "状态码: " << res.result_int() << "\n"; + + // 清空响应体以避免残留数据 + ostream(res.body()) << ""; + + // 关闭 socket + boost::system::error_code ec; + socket.shutdown(tcp::socket::shutdown_both, ec); + if (ec) { + std::cerr << "Socket shutdown error: " << ec.message() << std::endl; + } + } catch (std::exception const &e) { + std::cerr << "错误: " << e.what() << std::endl; + } + std::cout << "upload_pcd_file end" << std::endl; +} + SlamWrapper::SlamWrapper() { ChannelFactory::Instance()->Init(0, ConstantConfig::SLAM_NET_PORT); @@ -102,17 +188,7 @@ std::cout << "SlamWrapper init success, has " << arm_commands_.size() << " arm commands " << std::endl; } -std::vector SlamWrapper::get_command(const int index) { - const std::string section = std::to_string(index); - const std::string full_key = section + ".command"; - - const auto it = arm_commands_.find(full_key); - if (it != arm_commands_.end() && it->second.enabled) { - return it->second.command; - } - return {}; -} - +//casic start void SlamWrapper::control_robotic_arm() { const std::vector left_command = { 0XFE, 0XFE, 0X0F, 0X3C, 0X05, 0XB3, 0X08, 0X19, 0X07, 0X5E, 0X08, 0X73, 0X08, 0X1D, 0X08, 0XA6, 0X14, 0XFA @@ -167,6 +243,19 @@ std::cout << "move_to_o end" << std::endl; } +std::vector SlamWrapper::get_command(const int index) { + const std::string section = std::to_string(index); + const std::string full_key = section + ".command"; + + const auto it = arm_commands_.find(full_key); + if (it != arm_commands_.end() && it->second.enabled) { + return it->second.command; + } + return {}; +} + +//casic end + void SlamWrapper::qt_notice_handler(const void *message) { const auto seq = static_cast(message); std::string str_; @@ -250,6 +339,7 @@ send_msg.floor_index_().push_back(0); // Floor number，just give a fixed value of 0. send_msg.pcdmap_index_().push_back(0); // PCD Map number，just give a fixed value of 0. pubQtCommand->Write(send_msg); + upload_pcd_file(); } void SlamWrapper::start_relocation() { diff --git a/src/constant_config.hpp b/src/constant_config.hpp index 3b8cbb6..5b9300e 100644 --- a/src/constant_config.hpp +++ b/src/constant_config.hpp @@ -18,6 +18,7 @@ constexpr int BAUD_RATE = 115200; constexpr const char *API_RECEIVE_DATA = "/device/receiveData"; + constexpr const char *API_UPLOAD_PCD_FILE = "/device/receiveFile"; constexpr const char *HTTP_PORT = "30028"; constexpr const char *DOG_CODE = "B42D4000OCAD3N82"; @@ -26,6 +27,8 @@ constexpr const char *ROBOTIC_ARM_PORT = "/dev/ttyACM0"; constexpr const char *ROBOTIC_ARM_COMMAND_CONFIG = "/home/unitree/Desktop/robotic_arm_commands.conf"; + constexpr const char *SLAM_PCD_FILE = + "/unitree/module/graph_pid_ws/config_files/lio_sam_config/maps/pcd/default/GlobalMap.pcd"; } #endif //CONSTANT_CONFIG_HPP diff --git a/src/slam_wrapper.cpp b/src/slam_wrapper.cpp index 8d9c373..0a46d7b 100644 --- a/src/slam_wrapper.cpp +++ b/src/slam_wrapper.cpp @@ -10,6 +10,15 @@ #include #include #include +#include +#include +#include +#include +#include +#include + +namespace http = boost::beast::http; +using tcp = boost::asio::ip::tcp; static std::string trim(const std::string &s) { auto start = s.begin(); @@ -23,6 +32,83 @@ return std::string(start, end + 1); } +static std::string generate_multipart_form(const std::string &file_path, const std::string &boundary, + std::string &content_type) { + std::ifstream file(file_path, std::ios::binary); + if (!file) { + throw std::runtime_error("无法打开文件: " + file_path); + } + + // 提取文件名（避免暴露完整路径） + std::string file_name = file_path.substr(file_path.find_last_of("/\\") + 1); + + // 读取文件内容 + std::ostringstream content_stream; + content_stream << file.rdbuf(); + std::string file_content = content_stream.str(); + + // 构建 multipart/form-data 内容 + std::ostringstream body; + body << "--" << boundary << "\r\n"; + body << R"(Content-Disposition: form-data; name="file"; filename=")" << file_name << "\"\r\n"; + body << "Content-Type: application/octet-stream\r\n\r\n"; + body << file_content; + body << "\r\n--" << boundary << "--\r\n"; + + content_type = "multipart/form-data; boundary=" + boundary; + return body.str(); +} + +static void upload_pcd_file() { + std::cout << "upload_pcd_file start" << std::endl; + try { + // 设置 Boost.Asio 上下文 + boost::asio::io_context ioc; + tcp::resolver resolver(ioc); + tcp::socket socket(ioc); + + // 解析并连接服务器 + auto const results = resolver.resolve(ConstantConfig::SERVICE_ADDRESS, ConstantConfig::HTTP_PORT); + boost::asio::connect(socket, results.begin(), results.end()); + + // 构建 HTTP 请求 + http::request req{http::verb::post, ConstantConfig::API_UPLOAD_PCD_FILE, 11}; + req.set(http::field::host, ConstantConfig::SERVICE_ADDRESS); + req.set(http::field::user_agent, BOOST_BEAST_VERSION_STRING); + + // 构建 multipart/form-data 数据 + std::string boundary = "---------------------------boundary_example_" + std::to_string(std::time(nullptr)); + std::string contentType; + std::string body = generate_multipart_form(ConstantConfig::SLAM_PCD_FILE, boundary, contentType); + if (body.empty()) { + throw std::runtime_error("Failed to generate multipart form data"); + } + req.body() = body; + req.prepare_payload(); + req.set(http::field::content_type, contentType); + http::write(socket, req); + + // 接收响应 + boost::beast::flat_buffer buffer; + http::response res; + http::read(socket, buffer, res); + std::cout << "状态码: " << res.result_int() << "\n"; + + // 清空响应体以避免残留数据 + ostream(res.body()) << ""; + + // 关闭 socket + boost::system::error_code ec; + socket.shutdown(tcp::socket::shutdown_both, ec); + if (ec) { + std::cerr << "Socket shutdown error: " << ec.message() << std::endl; + } + } catch (std::exception const &e) { + std::cerr << "错误: " << e.what() << std::endl; + } + std::cout << "upload_pcd_file end" << std::endl; +} + SlamWrapper::SlamWrapper() { ChannelFactory::Instance()->Init(0, ConstantConfig::SLAM_NET_PORT); @@ -102,17 +188,7 @@ std::cout << "SlamWrapper init success, has " << arm_commands_.size() << " arm commands " << std::endl; } -std::vector SlamWrapper::get_command(const int index) { - const std::string section = std::to_string(index); - const std::string full_key = section + ".command"; - - const auto it = arm_commands_.find(full_key); - if (it != arm_commands_.end() && it->second.enabled) { - return it->second.command; - } - return {}; -} - +//casic start void SlamWrapper::control_robotic_arm() { const std::vector left_command = { 0XFE, 0XFE, 0X0F, 0X3C, 0X05, 0XB3, 0X08, 0X19, 0X07, 0X5E, 0X08, 0X73, 0X08, 0X1D, 0X08, 0XA6, 0X14, 0XFA @@ -167,6 +243,19 @@ std::cout << "move_to_o end" << std::endl; } +std::vector SlamWrapper::get_command(const int index) { + const std::string section = std::to_string(index); + const std::string full_key = section + ".command"; + + const auto it = arm_commands_.find(full_key); + if (it != arm_commands_.end() && it->second.enabled) { + return it->second.command; + } + return {}; +} + +//casic end + void SlamWrapper::qt_notice_handler(const void *message) { const auto seq = static_cast(message); std::string str_; @@ -250,6 +339,7 @@ send_msg.floor_index_().push_back(0); // Floor number，just give a fixed value of 0. send_msg.pcdmap_index_().push_back(0); // PCD Map number，just give a fixed value of 0. pubQtCommand->Write(send_msg); + upload_pcd_file(); } void SlamWrapper::start_relocation() { diff --git a/src/slam_wrapper.hpp b/src/slam_wrapper.hpp index 2c76b60..0fec4c3 100644 --- a/src/slam_wrapper.hpp +++ b/src/slam_wrapper.hpp @@ -57,7 +57,7 @@ explicit SlamWrapper(); - //test start + //casic start void control_robotic_arm(); void move_to_i(); @@ -65,8 +65,7 @@ void move_to_o(); std::vector get_command(int index); - - //test end + //casic end void qt_notice_handler(const void *message);