diff --git a/src/config.hpp b/src/config.hpp index d1d2f15..16ac5d2 100644 --- a/src/config.hpp +++ b/src/config.hpp @@ -66,6 +66,16 @@ * SLAM建图文件 */ constexpr const char *SLAM_PCD_FILE = "/unitree/module/graph_pid_ws/config_files/lio_sam_config/maps/pcd/default/GlobalMap.pcd"; + + /** + * SLAM建图脚本路径 + */ + constexpr const char *SLAM_SCRIPT_FILE = "/unitree/module/graph_pid_ws/0_unitree_slam.sh"; + + /** + * SLAM建图log路径 + */ + constexpr const char *SLAM_LOG_FILE = "/tmp/unitree_slam_service.log"; } #endif //CONFIG_HPP diff --git a/src/config.hpp b/src/config.hpp index d1d2f15..16ac5d2 100644 --- a/src/config.hpp +++ b/src/config.hpp @@ -66,6 +66,16 @@ * SLAM建图文件 */ constexpr const char *SLAM_PCD_FILE = "/unitree/module/graph_pid_ws/config_files/lio_sam_config/maps/pcd/default/GlobalMap.pcd"; + + /** + * SLAM建图脚本路径 + */ + constexpr const char *SLAM_SCRIPT_FILE = "/unitree/module/graph_pid_ws/0_unitree_slam.sh"; + + /** + * SLAM建图log路径 + */ + constexpr const char *SLAM_LOG_FILE = "/tmp/unitree_slam_service.log"; } #endif //CONFIG_HPP diff --git a/src/message_handler.cpp b/src/message_handler.cpp index 21c8099..2e44bd4 100644 --- a/src/message_handler.cpp +++ b/src/message_handler.cpp @@ -1,6 +1,8 @@ -#include +#include +#include #include #include +#include #include "tcp_service.hpp" #include "tcp_client.hpp" @@ -10,9 +12,13 @@ int main() { //启动TCP服务端 std::thread service_thread([] { - boost::asio::io_service io; - TcpService::getInstance().start(Config::TCP_SERVICE_LOCALE_PORT); - io.run(); + try { + boost::asio::io_service io; + TcpService::getInstance().start(Config::TCP_SERVICE_LOCALE_PORT); + io.run(); + } catch (const boost::wrapexcept &e) { + std::cerr << "[WARN] Boost exception: " << e.what() << std::endl; + } }); // 启动TCP客户端 @@ -36,9 +42,38 @@ } }); + // 拉起第三方脚本 + std::thread script_thread([] { + const auto cmd = ( + boost::format("setsid sh -c '%1%' > %2% 2>&1 &") + % Config::SLAM_SCRIPT_FILE + % Config::SLAM_LOG_FILE + ).str(); + std::system(cmd.c_str()); + + // 等几秒钟,扫一遍日志确认启动成功 + std::this_thread::sleep_for(std::chrono::seconds(2)); + std::ifstream log(Config::SLAM_LOG_FILE); + std::string line; + bool ok = false; + while (std::getline(log, line)) { + // 确认启动是否成功 + if (line.find("process started") != std::string::npos) { + ok = true; + break; + } + } + if (ok) { + std::cout << "Unitree Slam service init success" << std::endl; + } else { + std::cerr << "[WARN] Unitree Slam service may not be ready yet" << std::endl; + } + }); + // 等待所有线程完成 service_thread.join(); client_thread.join(); gas_serial_port_thread.join(); + script_thread.join(); return 0; } diff --git a/src/config.hpp b/src/config.hpp index d1d2f15..16ac5d2 100644 --- a/src/config.hpp +++ b/src/config.hpp @@ -66,6 +66,16 @@ * SLAM建图文件 */ constexpr const char *SLAM_PCD_FILE = "/unitree/module/graph_pid_ws/config_files/lio_sam_config/maps/pcd/default/GlobalMap.pcd"; + + /** + * SLAM建图脚本路径 + */ + constexpr const char *SLAM_SCRIPT_FILE = "/unitree/module/graph_pid_ws/0_unitree_slam.sh"; + + /** + * SLAM建图log路径 + */ + constexpr const char *SLAM_LOG_FILE = "/tmp/unitree_slam_service.log"; } #endif //CONFIG_HPP diff --git a/src/message_handler.cpp b/src/message_handler.cpp index 21c8099..2e44bd4 100644 --- a/src/message_handler.cpp +++ b/src/message_handler.cpp @@ -1,6 +1,8 @@ -#include +#include +#include #include #include +#include #include "tcp_service.hpp" #include "tcp_client.hpp" @@ -10,9 +12,13 @@ int main() { //启动TCP服务端 std::thread service_thread([] { - boost::asio::io_service io; - TcpService::getInstance().start(Config::TCP_SERVICE_LOCALE_PORT); - io.run(); + try { + boost::asio::io_service io; + TcpService::getInstance().start(Config::TCP_SERVICE_LOCALE_PORT); + io.run(); + } catch (const boost::wrapexcept &e) { + std::cerr << "[WARN] Boost exception: " << e.what() << std::endl; + } }); // 启动TCP客户端 @@ -36,9 +42,38 @@ } }); + // 拉起第三方脚本 + std::thread script_thread([] { + const auto cmd = ( + boost::format("setsid sh -c '%1%' > %2% 2>&1 &") + % Config::SLAM_SCRIPT_FILE + % Config::SLAM_LOG_FILE + ).str(); + std::system(cmd.c_str()); + + // 等几秒钟,扫一遍日志确认启动成功 + std::this_thread::sleep_for(std::chrono::seconds(2)); + std::ifstream log(Config::SLAM_LOG_FILE); + std::string line; + bool ok = false; + while (std::getline(log, line)) { + // 确认启动是否成功 + if (line.find("process started") != std::string::npos) { + ok = true; + break; + } + } + if (ok) { + std::cout << "Unitree Slam service init success" << std::endl; + } else { + std::cerr << "[WARN] Unitree Slam service may not be ready yet" << std::endl; + } + }); + // 等待所有线程完成 service_thread.join(); client_thread.join(); gas_serial_port_thread.join(); + script_thread.join(); return 0; } diff --git a/src/slam_wrapper.cpp b/src/slam_wrapper.cpp index 249e83b..8a2580a 100644 --- a/src/slam_wrapper.cpp +++ b/src/slam_wrapper.cpp @@ -113,13 +113,17 @@ SlamWrapper::SlamWrapper() { ChannelFactory::Instance()->Init(0, Config::SLAM_NET_PORT); - std::thread serial_port_thread([this] { - //初始化机械臂串口 - boost::asio::io_service io; - _serialPortPtr = new RoboticArmSerialPort(io, Config::ROBOTIC_ARM_PORT, Config::BAUD_RATE); - io.run(); - }); - serial_port_thread.detach(); + try { + std::thread serial_port_thread([this] { + //初始化机械臂串口 + boost::asio::io_service io; + _serialPortPtr = new RoboticArmSerialPort(io, Config::ROBOTIC_ARM_PORT, Config::BAUD_RATE); + io.run(); + }); + serial_port_thread.detach(); + } catch (const boost::wrapexcept &e) { + std::cerr << "Boost exception: " << e.what() << std::endl; + } // pub pubQtCommand->InitChannel(); diff --git a/src/config.hpp b/src/config.hpp index d1d2f15..16ac5d2 100644 --- a/src/config.hpp +++ b/src/config.hpp @@ -66,6 +66,16 @@ * SLAM建图文件 */ constexpr const char *SLAM_PCD_FILE = "/unitree/module/graph_pid_ws/config_files/lio_sam_config/maps/pcd/default/GlobalMap.pcd"; + + /** + * SLAM建图脚本路径 + */ + constexpr const char *SLAM_SCRIPT_FILE = "/unitree/module/graph_pid_ws/0_unitree_slam.sh"; + + /** + * SLAM建图log路径 + */ + constexpr const char *SLAM_LOG_FILE = "/tmp/unitree_slam_service.log"; } #endif //CONFIG_HPP diff --git a/src/message_handler.cpp b/src/message_handler.cpp index 21c8099..2e44bd4 100644 --- a/src/message_handler.cpp +++ b/src/message_handler.cpp @@ -1,6 +1,8 @@ -#include +#include +#include #include #include +#include #include "tcp_service.hpp" #include "tcp_client.hpp" @@ -10,9 +12,13 @@ int main() { //启动TCP服务端 std::thread service_thread([] { - boost::asio::io_service io; - TcpService::getInstance().start(Config::TCP_SERVICE_LOCALE_PORT); - io.run(); + try { + boost::asio::io_service io; + TcpService::getInstance().start(Config::TCP_SERVICE_LOCALE_PORT); + io.run(); + } catch (const boost::wrapexcept &e) { + std::cerr << "[WARN] Boost exception: " << e.what() << std::endl; + } }); // 启动TCP客户端 @@ -36,9 +42,38 @@ } }); + // 拉起第三方脚本 + std::thread script_thread([] { + const auto cmd = ( + boost::format("setsid sh -c '%1%' > %2% 2>&1 &") + % Config::SLAM_SCRIPT_FILE + % Config::SLAM_LOG_FILE + ).str(); + std::system(cmd.c_str()); + + // 等几秒钟,扫一遍日志确认启动成功 + std::this_thread::sleep_for(std::chrono::seconds(2)); + std::ifstream log(Config::SLAM_LOG_FILE); + std::string line; + bool ok = false; + while (std::getline(log, line)) { + // 确认启动是否成功 + if (line.find("process started") != std::string::npos) { + ok = true; + break; + } + } + if (ok) { + std::cout << "Unitree Slam service init success" << std::endl; + } else { + std::cerr << "[WARN] Unitree Slam service may not be ready yet" << std::endl; + } + }); + // 等待所有线程完成 service_thread.join(); client_thread.join(); gas_serial_port_thread.join(); + script_thread.join(); return 0; } diff --git a/src/slam_wrapper.cpp b/src/slam_wrapper.cpp index 249e83b..8a2580a 100644 --- a/src/slam_wrapper.cpp +++ b/src/slam_wrapper.cpp @@ -113,13 +113,17 @@ SlamWrapper::SlamWrapper() { ChannelFactory::Instance()->Init(0, Config::SLAM_NET_PORT); - std::thread serial_port_thread([this] { - //初始化机械臂串口 - boost::asio::io_service io; - _serialPortPtr = new RoboticArmSerialPort(io, Config::ROBOTIC_ARM_PORT, Config::BAUD_RATE); - io.run(); - }); - serial_port_thread.detach(); + try { + std::thread serial_port_thread([this] { + //初始化机械臂串口 + boost::asio::io_service io; + _serialPortPtr = new RoboticArmSerialPort(io, Config::ROBOTIC_ARM_PORT, Config::BAUD_RATE); + io.run(); + }); + serial_port_thread.detach(); + } catch (const boost::wrapexcept &e) { + std::cerr << "Boost exception: " << e.what() << std::endl; + } // pub pubQtCommand->InitChannel(); diff --git a/src/tcp_service.cpp b/src/tcp_service.cpp index 32304e4..490f703 100644 --- a/src/tcp_service.cpp +++ b/src/tcp_service.cpp @@ -107,7 +107,7 @@ client_thread.join(); } } catch (const std::exception &e) { - std::cerr << "exception in tcp_server::start: " << e.what() << std::endl; + std::cerr << "[WARN] TcpService start " << e.what() << std::endl; } }