diff --git a/src/autonomy/fu/src/fu_node.cpp b/src/autonomy/fu/src/fu_node.cpp index c50d136..cffc7f5 100644 --- a/src/autonomy/fu/src/fu_node.cpp +++ b/src/autonomy/fu/src/fu_node.cpp @@ -750,6 +750,8 @@ class fu_node : public rclcpp::Node reliability = msg->reliability; located = msg->enter_range; is_start = msg->is_start; + + LOG_INFO("目标点位置: x -- %.2f , y -- %.2f", x, y); } void msg_callback_DetectLine(const sweeper_interfaces::msg::DetectLine::SharedPtr msg) @@ -1638,7 +1640,7 @@ class fu_node : public rclcpp::Node int main(int argc, char** argv) { // 初始化日志系统 - logger::Logger::Init("fu", "./log"); + logger::Logger::Init("fu", "./nodes_log"); rclcpp::init(argc, argv); auto node = std::make_shared("fu_node"); diff --git a/src/autonomy/pl/package.xml b/src/autonomy/pl/package.xml index 4c8f22b..54b0713 100644 --- a/src/autonomy/pl/package.xml +++ b/src/autonomy/pl/package.xml @@ -13,6 +13,7 @@ std_msgs sweeper_interfaces mc + logger ament_lint_auto ament_lint_common diff --git a/src/autonomy/pl/src/pl.cpp b/src/autonomy/pl/src/pl.cpp index f6d9c66..c9cd453 100644 --- a/src/autonomy/pl/src/pl.cpp +++ b/src/autonomy/pl/src/pl.cpp @@ -218,11 +218,6 @@ void PL_ProcThread() des_pos = Road_Planning_Find_Aim_Point(road_pos, 3); // 找到当前点后第 3 个点,作为目标点 direction_pos = Road_Planning_Find_Aim_Point(road_pos, 6); // 找到当前点后第 6 个点,判断直行/转弯 - // printf("当前位置: %d\n", road_pos); - // printf("目标位置: %d\n", des_pos); - // printf("direction_pos: %d\n", direction_pos); - // printf("\n"); - drive_mode = straight_or_turn(g_rtk.direction, GPSPointQueue[direction_pos].Heading, 12); // 新增距离判断逻辑 @@ -233,6 +228,12 @@ void PL_ProcThread() double target_lat = GPSPointQueue[des_pos].LatitudeInfo; double target_head = GPSPointQueue[des_pos].Heading; + LOG_INFO_THROTTLE(1000, "当前位置: %d 经度: %.6f 纬度: %.6f 朝向: %.1f", road_pos, current_lon, current_lat, + current_head); + LOG_INFO_THROTTLE(1000, "目标位置: %d 经度: %.6f 纬度: %.6f 朝向: %.1f", des_pos, target_lon, target_lat, + target_head); + LOG_INFO_THROTTLE(1000, "############################################################"); + // 计算当前定位点与目标点距离 double distance = ntzx_GPS_length(current_lon, current_lat, target_lon, target_lat); int nl_within_head = (abs(target_head - current_head) < 30) ? 1 : 0; diff --git a/src/autonomy/pl/src/pl_node.cpp b/src/autonomy/pl/src/pl_node.cpp index 03866f1..faa3b4a 100644 --- a/src/autonomy/pl/src/pl_node.cpp +++ b/src/autonomy/pl/src/pl_node.cpp @@ -229,7 +229,7 @@ int main(int argc, char** argv) initConfig(); rclcpp::init(argc, argv); // 初始化日志系统 - logger::Logger::Init("pl", "./log"); + logger::Logger::Init("pl", "./nodes_log"); /*创建对应节点的共享指针对象*/ auto node = std::make_shared("pl_node"); /* 运行节点,并检测退出信号*/ diff --git a/src/autonomy/route/src/route_node.cpp b/src/autonomy/route/src/route_node.cpp index 729ac14..3c2e181 100644 --- a/src/autonomy/route/src/route_node.cpp +++ b/src/autonomy/route/src/route_node.cpp @@ -253,7 +253,7 @@ int main(int argc, char** argv) init_main(); rclcpp::init(argc, argv); /*初始化日志系统*/ - logger::Logger::Init("route", "./log"); + logger::Logger::Init("route", "./nodes_log"); /*创建对应节点的共享指针对象*/ auto node = std::make_shared("route_node"); /* 运行节点,并检测退出信号*/ diff --git a/src/base/mc/src/mc.cpp b/src/base/mc/src/mc.cpp index aea5216..b4f97c8 100644 --- a/src/base/mc/src/mc.cpp +++ b/src/base/mc/src/mc.cpp @@ -44,7 +44,7 @@ int main(int argc, char** argv) { rclcpp::init(argc, argv); /*初始化日志系统*/ - logger::Logger::Init("mc", "./log"); + logger::Logger::Init("mc", "./nodes_log"); auto node = rclcpp::Node::make_shared("can_driver_node"); LOG_INFO("Starting mc package..."); diff --git a/src/base/vehicle_params/src/vehicle_params_node.cpp b/src/base/vehicle_params/src/vehicle_params_node.cpp index be715fc..ceda3a3 100644 --- a/src/base/vehicle_params/src/vehicle_params_node.cpp +++ b/src/base/vehicle_params/src/vehicle_params_node.cpp @@ -109,7 +109,7 @@ int main(int argc, char** argv) { rclcpp::init(argc, argv); /*初始化日志系统*/ - logger::Logger::Init("vehicle_params", "./log"); + logger::Logger::Init("vehicle_params", "./nodes_log"); rclcpp::spin(std::make_shared()); rclcpp::shutdown(); diff --git a/src/common/logger/README.md b/src/common/logger/README.md index f1e70d3..1be292e 100644 --- a/src/common/logger/README.md +++ b/src/common/logger/README.md @@ -51,7 +51,7 @@ ament_target_dependencies(your_target ```cpp // 初始化日志系统 -logger::Logger::Init("node_name", "./log"); +logger::Logger::Init("node_name", "./nodes_log"); // 业务逻辑... diff --git a/src/common/logger/src/logger.cpp b/src/common/logger/src/logger.cpp index c3afaa9..3d964b0 100644 --- a/src/common/logger/src/logger.cpp +++ b/src/common/logger/src/logger.cpp @@ -106,8 +106,16 @@ void Logger::InitLogFile() struct tm tm_info; localtime_r(&now_c, &tm_info); + // 按节点名称和日期创建子目录 + char sub_dir[256]; + snprintf(sub_dir, sizeof(sub_dir), "%s/%s/%04d%02d%02d", log_dir_.c_str(), name_.c_str(), + tm_info.tm_year + 1900, tm_info.tm_mon + 1, tm_info.tm_mday); + + // 创建子目录 + std::filesystem::create_directories(sub_dir); + char filename[256]; - snprintf(filename, sizeof(filename), "%s/%s_%04d%02d%02d_%02d%02d%02d.log", log_dir_.c_str(), name_.c_str(), + snprintf(filename, sizeof(filename), "%s/%s_%04d%02d%02d_%02d%02d%02d.log", sub_dir, name_.c_str(), tm_info.tm_year + 1900, tm_info.tm_mon + 1, tm_info.tm_mday, tm_info.tm_hour, tm_info.tm_min, tm_info.tm_sec); diff --git a/src/communication/mqtt_report/src/node/mqtt_vehicle_node.cpp b/src/communication/mqtt_report/src/node/mqtt_vehicle_node.cpp index aac66db..53f455d 100644 --- a/src/communication/mqtt_report/src/node/mqtt_vehicle_node.cpp +++ b/src/communication/mqtt_report/src/node/mqtt_vehicle_node.cpp @@ -170,7 +170,7 @@ int main(int argc, char** argv) rclcpp::init(argc, argv); /*初始化日志系统*/ - logger::Logger::Init("mqtt_report", "./log"); + logger::Logger::Init("mqtt_report", "./nodes_log"); Config config; diff --git a/src/communication/sub/src/main.cpp b/src/communication/sub/src/main.cpp index d207a5b..927e79a 100644 --- a/src/communication/sub/src/main.cpp +++ b/src/communication/sub/src/main.cpp @@ -81,7 +81,7 @@ int main(int argc, char** argv) // 4) ROS2 spin rclcpp::init(argc, argv); /*初始化日志系统*/ - logger::Logger::Init("sub", "./log"); + logger::Logger::Init("sub", "./nodes_log"); LOG_INFO("MQTT address: %s", cfg.address.c_str()); LOG_INFO("MQTT topic template: %s", cfg.remote_topic_template.c_str()); diff --git a/src/communication/task_manager/src/task_manager_main.cpp b/src/communication/task_manager/src/task_manager_main.cpp index e26e776..a5f456f 100644 --- a/src/communication/task_manager/src/task_manager_main.cpp +++ b/src/communication/task_manager/src/task_manager_main.cpp @@ -478,7 +478,7 @@ int main(int argc, char** argv) signal(SIGINT, signalHandler); // 初始化日志系统 - logger::Logger::Init("task_manager", "./log"); + logger::Logger::Init("task_manager", "./nodes_log"); // 默认配置路径 std::string config_path = "config.json"; diff --git a/src/control/ctrl_arbiter/package.xml b/src/control/ctrl_arbiter/package.xml index 725c7a8..c85732b 100644 --- a/src/control/ctrl_arbiter/package.xml +++ b/src/control/ctrl_arbiter/package.xml @@ -11,11 +11,12 @@ sweeper_interfaces rclcpp - + logger + ament_lint_auto ament_lint_common ament_cmake - + \ No newline at end of file diff --git a/src/control/ctrl_arbiter/src/ctrl_arbiter.cpp b/src/control/ctrl_arbiter/src/ctrl_arbiter.cpp index c5ae01c..c97eea1 100644 --- a/src/control/ctrl_arbiter/src/ctrl_arbiter.cpp +++ b/src/control/ctrl_arbiter/src/ctrl_arbiter.cpp @@ -108,7 +108,7 @@ class ArbitrationNode : public rclcpp::Node int main(int argc, char* argv[]) { // 初始化日志系统 - logger::Logger::Init("ctrl_arbiter", "./log"); + logger::Logger::Init("ctrl_arbiter", "./nodes_log"); rclcpp::init(argc, argv); auto node = std::make_shared(); diff --git a/src/control/radio_ctrl/src/radio_ctrl.cpp b/src/control/radio_ctrl/src/radio_ctrl.cpp index 2fab2ad..82a4867 100644 --- a/src/control/radio_ctrl/src/radio_ctrl.cpp +++ b/src/control/radio_ctrl/src/radio_ctrl.cpp @@ -206,7 +206,7 @@ class SBUSNode : public rclcpp::Node int main(int argc, char* argv[]) { // 初始化日志系统 - logger::Logger::Init("radio_ctrl", "./log"); + logger::Logger::Init("radio_ctrl", "./nodes_log"); rclcpp::init(argc, argv); rclcpp::spin(std::make_shared()); // 启动节点 diff --git a/src/control/remote_ctrl/src/remote_ctrl_node.cpp b/src/control/remote_ctrl/src/remote_ctrl_node.cpp index c3c96b7..55a522b 100644 --- a/src/control/remote_ctrl/src/remote_ctrl_node.cpp +++ b/src/control/remote_ctrl/src/remote_ctrl_node.cpp @@ -392,7 +392,7 @@ class RemoteCtrlNode : public rclcpp::Node int main(int argc, char* argv[]) { // 初始化日志系统 - logger::Logger::Init("remote_ctrl", "./log"); + logger::Logger::Init("remote_ctrl", "./nodes_log"); // =============================== // 1. 默认配置路径 + 手动解析 --config diff --git a/src/perception/rtk/src/rtk_node.cpp b/src/perception/rtk/src/rtk_node.cpp index 1956f96..185a4dc 100644 --- a/src/perception/rtk/src/rtk_node.cpp +++ b/src/perception/rtk/src/rtk_node.cpp @@ -195,7 +195,7 @@ class rtk_node : public rclcpp::Node int main(int argc, char** argv) { // 初始化日志系统 - logger::Logger::Init("rtk", "./log"); + logger::Logger::Init("rtk", "./nodes_log"); // unsigned int a = -1; // LOG_DEBUG("%u\n", a);