去除终端带颜色显示输出

src/autonomy/fu/src/fu_node.cpp

@@ -669,6 +669,7 @@ class fu_node : public rclcpp::Node
         is_start = msg->is_start;
 
         LOG_INFO("[规划层消息回调] 目标点位置： x -- %.2f , y -- %.2f", pl_x_, pl_y_);
+        LOG_INFO("[规划层消息回调] pl_speed_： %d ", pl_speed_);
     }
 
     // ======== 绕障路点规划 ========
@@ -763,7 +764,7 @@ class fu_node : public rclcpp::Node
         auto now = system_clock::now();
         duration<double> time_since_last = now - last_rtk_time;
 
-        if (g_rtk.reliability != 0 && time_since_last <= rtk_timeout)
+        if (g_rtk.reliability == 1 && time_since_last <= rtk_timeout)
         {
             // RTK信号良好：使用绝对坐标判断
             double distance = ntzx_GPS_length(g_rtk.lon, g_rtk.lat, target.lon, target.lat);
@@ -934,7 +935,8 @@ class fu_node : public rclcpp::Node
         // 检查RTK信号
         auto rtk_now = system_clock::now();
         duration<double> time_since_last_rtk = rtk_now - last_rtk_time;
-        bool rtk_signal_good = (g_rtk.reliability != 0 && time_since_last_rtk <= rtk_timeout);
+        bool rtk_signal_good = (g_rtk.reliability == 1 && time_since_last_rtk <= rtk_timeout);
+        // LOG_INFO("rtk_signal_good ? %d",rtk_signal_good);
 
         // 状态3: 雷达数据超时，停车
         if (grid_age > 3.0)
@@ -1034,7 +1036,7 @@ class fu_node : public rclcpp::Node
             // 若之前处于WAITING状态，且现在没有障碍物且RTK信号良好，恢复到NORMAL状态
             auto now = system_clock::now();
             duration<double> time_since_last_rtk = now - last_rtk_time;
-            bool rtk_signal_good = (g_rtk.reliability != 0 && time_since_last_rtk <= rtk_timeout);
+            bool rtk_signal_good = (g_rtk.reliability == 1 && time_since_last_rtk <= rtk_timeout);
 
             if (state_machine_->getCurrentState() == StateMachine::WAITING &&
                 !state_machine_->obstacle_analysis.has_front_critical &&

src/common/launch_system/launch/start_all.launch.py

@@ -1,6 +1,6 @@
 from launch import LaunchDescription
 from launch_ros.actions import Node
-from launch.actions import IncludeLaunchDescription
+from launch.actions import IncludeLaunchDescription, TimerAction
 from launch.launch_description_sources import PythonLaunchDescriptionSource
 from ament_index_python.packages import get_package_share_directory
 import os
@@ -11,116 +11,193 @@ def generate_launch_description():
     rslidar_sdk_path = get_package_share_directory("rslidar_sdk")
     fu_path = get_package_share_directory("fu")
 
+    # ===================== 按顺序延时启动所有节点 =====================
+    # 1. 车辆参数节点 (立即启动)
+    vehicle_params_node = Node(
+        package="vehicle_params",
+        executable="vehicle_params_node",
+        name="vehicle_params_node",
+        output="screen",
+        respawn=True,
+        respawn_delay=2,
+    )
+
+    # 2. CAN遥控节点 (延时0.2s)
+    can_radio_ctrl_node = TimerAction(
+        period=0.2,
+        actions=[Node(
+            package="can_radio_ctrl",
+            executable="can_radio_ctrl_node",
+            name="can_radio_ctrl_node",
+            output="screen",
+            respawn=True,
+            respawn_delay=2,
+        )]
+    )
+    # Node(
+    #     package="radio_ctrl",
+    #     executable="radio_ctrl_node",
+    #     name="radio_ctrl_node",
+    #     output="screen",
+    #     respawn=True,
+    #     respawn_delay=2,
+    # ),
+
+    # 3. 远程控制节点 (延时0.4s)
+    remote_ctrl_node = TimerAction(
+        period=0.4,
+        actions=[Node(
+            package="remote_ctrl",
+            executable="remote_ctrl_node",
+            name="remote_ctrl_node",
+            output="screen",
+            respawn=True,
+            respawn_delay=2,
+        )]
+    )
+
+    # 4. MC电机控制节点 (延时0.6s)
+    mc_node = TimerAction(
+        period=0.6,
+        actions=[Node(
+            package="mc",
+            executable="mc_node",
+            name="mc_node",
+            output="screen",
+            respawn=True,
+            respawn_delay=2,
+        )]
+    )
+
+    # 5. 控制仲裁节点 (延时0.8s)
+    ctrl_arbiter_node = TimerAction(
+        period=0.8,
+        actions=[Node(
+            package="ctrl_arbiter",
+            executable="ctrl_arbiter_node",
+            name="ctrl_arbiter_node",
+            output="screen",
+            respawn=True,
+            respawn_delay=2,
+        )]
+    )
+
+    # 6. MQTT上报节点 (延时1.0s) —— 【关键：必须最后启动】
+    mqtt_report_node = TimerAction(
+        period=1.0,
+        actions=[Node(
+            package="mqtt_report",
+            executable="mqtt_report_node",
+            name="mqtt_report_node",
+            output="screen",
+            respawn=True,
+            respawn_delay=2,
+        )]
+    )
+
+    # 7. 雷达启动 (延时1.5s)
+    lidar_launch = TimerAction(
+        period=1.5,
+        actions=[IncludeLaunchDescription(
+            PythonLaunchDescriptionSource(
+                os.path.join(rslidar_sdk_path, "launch", "start_double.launch.py")
+            )
+        )]
+    )
+
+    # 8. RTK定位节点 (延时2.0s)
+    rtk_node = TimerAction(
+        period=2.0,
+        actions=[Node(
+            package="rtk",
+            executable="rtk_node",
+            name="rtk_node",
+            parameters=[
+                os.path.join(
+                    get_package_share_directory("rtk"), "config", "rtk_params.yaml"
+                )
+            ],
+            output="screen",
+        )]
+    )
+
+    # 9. 路径节点 (延时2.5s)
+    route_node = TimerAction(
+        period=2.5,
+        actions=[Node(
+            package="route",
+            executable="route_node",
+            name="route_node",
+            output="screen",
+            respawn=True,
+            respawn_delay=2,
+        )]
+    )
+
+    # 10. 订阅节点 (延时3.0s)
+    sub_node = TimerAction(
+        period=3.0,
+        actions=[Node(
+            package="sub",
+            executable="sub_node",
+            name="sub_node",
+            output="screen",
+            respawn=True,
+            respawn_delay=2,
+        )]
+    )
+
+    # 11. 任务管理节点 (延时3.5s)
+    task_manager_node = TimerAction(
+        period=3.5,
+        actions=[Node(
+            package="task_manager",
+            executable="task_manager_node",
+            name="task_manager_node",
+            output="screen",
+            respawn=True,
+            respawn_delay=2,
+        )]
+    )
+
+    # 12. 规划控制节点 (延时4.0s)
+    pl_node = TimerAction(
+        period=4.0,
+        actions=[Node(
+            package="pl",
+            executable="pl_node",
+            name="pl_node",
+            output="screen",
+            respawn=True,
+            respawn_delay=2,
+        )]
+    )
+
+    # 13. FU功能节点 (延时4.5s)
+    fu_launch = TimerAction(
+        period=4.5,
+        actions=[IncludeLaunchDescription(
+            PythonLaunchDescriptionSource(
+                os.path.join(fu_path, "launch", "fu.launch.py")
+            )
+        )]
+    )
+
+    # ===================== 组装启动列表 =====================
     return LaunchDescription(
         [
-            Node(
+            vehicle_params_node,
-                package="vehicle_params",
+            can_radio_ctrl_node,
-                executable="vehicle_params_node",
+            remote_ctrl_node,
-                name="vehicle_params_node",
+            mc_node,
-                output="screen",
+            ctrl_arbiter_node,
-                respawn=True,
+            mqtt_report_node,
-                respawn_delay=2,
+            lidar_launch,
-            ),
+            rtk_node,
-            # Node(
+            route_node,
-            #     package="radio_ctrl",
+            sub_node,
-            #     executable="radio_ctrl_node",
+            task_manager_node,
-            #     name="radio_ctrl_node",
+            pl_node,
-            #     output="screen",
+            fu_launch,
-            #     respawn=True,
-            #     respawn_delay=2,
-            # ),
-            Node(
-                package="can_radio_ctrl",
-                executable="can_radio_ctrl_node",
-                name="can_radio_ctrl_node",
-                output="screen",
-                respawn=True,
-                respawn_delay=2,
-            ),
-            Node(
-                package="remote_ctrl",
-                executable="remote_ctrl_node",
-                name="remote_ctrl_node",
-                output="screen",
-                respawn=True,
-                respawn_delay=2,
-            ),
-            Node(
-                package="mc",
-                executable="mc_node",
-                name="mc_node",
-                output="screen",
-                respawn=True,
-                respawn_delay=2,
-            ),
-            Node(
-                package="ctrl_arbiter",
-                executable="ctrl_arbiter_node",
-                name="ctrl_arbiter_node",
-                output="screen",
-                respawn=True,
-                respawn_delay=2,
-            ),
-            Node(
-                package="mqtt_report",
-                executable="mqtt_report_node",
-                name="mqtt_report_node",
-                output="screen",
-                respawn=True,
-                respawn_delay=2,
-            ),
-            IncludeLaunchDescription(
-                PythonLaunchDescriptionSource(
-                    os.path.join(rslidar_sdk_path, "launch", "start_double.launch.py")
-                )
-            ),
-            Node(
-                package="rtk",
-                executable="rtk_node",
-                name="rtk_node",
-                parameters=[
-                    os.path.join(
-                        get_package_share_directory("rtk"), "config", "rtk_params.yaml"
-                    )
-                ],  # 从YAML文件加载参数
-                output="screen",
-            ),
-            Node(
-                package="route",
-                executable="route_node",
-                name="route_node",
-                output="screen",
-                respawn=True,
-                respawn_delay=2,
-            ),
-            Node(
-                package="sub",
-                executable="sub_node",
-                name="sub_node",
-                output="screen",
-                respawn=True,
-                respawn_delay=2,
-            ),
-            Node(
-                package="task_manager",
-                executable="task_manager_node",
-                name="task_manager_node",
-                output="screen",
-                respawn=True,
-                respawn_delay=2,
-            ),
-            Node(
-                package="pl",
-                executable="pl_node",
-                name="pl_node",
-                output="screen",
-                respawn=True,
-                respawn_delay=2,
-            ),
-            IncludeLaunchDescription(
-                PythonLaunchDescriptionSource(
-                    os.path.join(fu_path, "launch", "fu.launch.py")
-                )
-            ),
         ]
     )

src/common/logger/src/logger.cpp

@@ -231,30 +231,7 @@ void Logger::WriteLogThread()
                 }
             }
 
-            // 同时输出到控制台（带颜色，INFO级别无颜色）
+            std::cerr << formatted_msg;
-            std::string colored_msg;
-            switch (msg.level)
-            {
-                case LogLevel::DEBUG:
-                    colored_msg = "\033[34m" + formatted_msg + "\033[0m";  // 蓝色
-                    break;
-                case LogLevel::INFO:
-                    colored_msg = formatted_msg;  // INFO级别无颜色
-                    break;
-                case LogLevel::WARN:
-                    colored_msg = "\033[33m" + formatted_msg + "\033[0m";  // 黄色
-                    break;
-                case LogLevel::ERROR:
-                    colored_msg = "\033[31m" + formatted_msg + "\033[0m";  // 红色
-                    break;
-                case LogLevel::FATAL:
-                    colored_msg = "\033[35m" + formatted_msg + "\033[0m";  // 紫色
-                    break;
-                default:
-                    colored_msg = formatted_msg;
-                    break;
-            }
-            std::cerr << colored_msg;
         }
     }