修复了雷达扫描范围的bug，修复了里程计数据的发布者冲突的问题。目前存在问题：odom坐标系在运动过程中还是会飘

src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/src/lslidar_driver.cc

@@ -1134,7 +1134,7 @@ namespace lslidar_driver
 				{
 					auto scan = sensor_msgs::msg::LaserScan::UniquePtr(new sensor_msgs::msg::LaserScan());
 					//int scan_num = ceil((angle_able_max - angle_able_min) / 360 * count_num) + 1;
-					int scan_num = fixed_array_length;//cyy_addcyy_add
+					int scan_num = ceil((angle_able_max - angle_able_min) / 360 * count_num) + 1;
 					
 					std::vector<ScanPoint> points;
 					rclcpp::Time start_time;
@@ -1160,7 +1160,7 @@ namespace lslidar_driver
 						scan->angle_min = 2 * M_PI * angle_able_min / 360;
 						scan->angle_max = 2 * M_PI * angle_able_max / 360;
 					}
-					scan->angle_increment = 2 * M_PI / (double)(fixed_array_length - 1);
+					scan->angle_increment = 2 * M_PI / (double)(scan_num - 1);
 
 					scan->range_min = min_range;
 					scan->range_max = max_range;
@@ -1169,7 +1169,7 @@ namespace lslidar_driver
 					scan->intensities.reserve(scan_num);
 					scan->intensities.assign(scan_num, std::numeric_limits<float>::infinity());
 					scan->scan_time = 0.1;
-					scan->time_increment = 0.1 / (double)(fixed_array_length - 1);
+					scan->time_increment = 0.1 / (double)(scan_num - 1);
 
 					int start_num = floor(angle_able_min * count_num / 360);
 					int end_num = floor(angle_able_max * count_num / 360);

src/gc_navigation2_slamtoolbox/launch/gc_nav2_with_slam_online_real.launch.py

@@ -1,6 +1,7 @@
 # ============================================================================
 # gc_nav2_with_slam_online_real.launch.py
 # 功能：真机实时建图 — slam_toolbox 从零建图 + Nav2 导航
+# 依赖：需先启动 origincar_bringup（底盘驱动 + EKF）
 # ============================================================================
 
 import os
@@ -29,7 +30,7 @@ def generate_launch_description():
         pkg_dir, 'params', 'gc_navigation_slam.yaml'))
     slam_params_file = os.path.join(pkg_dir, 'config', 'slam_toolbox_mapping.yaml')
 
-    # === 3. slam_toolbox：实时建图 → 直接发布到 /map ===
+    # === 3. slam_toolbox：实时建图 → 发布 map→odom ===
     slam_toolbox_node = Node(
         package='slam_toolbox',
         executable='async_slam_toolbox_node',
@@ -49,7 +50,7 @@ def generate_launch_description():
         }.items(),
     )
 
-    # === 5. 机器人模型 + TF ===
+    # === 5. 机器人模型（URDF 已定义 base_footprint→base_link，无需 static TF）===
     model = DeclareLaunchArgument(
         name='model',
         default_value=os.path.join(origincar_urdf_dir, 'urdf', 'origincar.urdf'))
@@ -63,17 +64,9 @@ def generate_launch_description():
                      'use_sim_time': use_sim_time, 'publish_frequency': 30.0}],
     )
 
-    base_footprint_tf = Node(
-        package='tf2_ros',
-        executable='static_transform_publisher',
-        name='base_footprint_to_base_link',
-        arguments=['0', '0', '0', '0', '0', '0', 'base_footprint', 'base_link'],
-    )
-
     # === 6. 组装 ===
     ld.add_action(model)
     ld.add_action(robot_state_publisher)
-    ld.add_action(base_footprint_tf)
     ld.add_action(slam_toolbox_node)
     ld.add_action(navigation_launch)
 

src/origincar_base/config/ekf.yaml

@@ -13,7 +13,7 @@ ekf_filter_node:
 
         map_frame: map
         odom_frame: odom
-        base_link_frame: base_link
+        base_link_frame: base_footprint
         world_frame: odom
 
         odom0: odom

src/origincar_base/include/origincar_base/origincar_base.h

@@ -212,6 +212,7 @@ private:
 	rclcpp::Subscription<std_msgs::msg::Int32>::SharedPtr Sign_Switch_Sub;
 
 	string usart_port_name, robot_frame_id, gyro_frame_id, odom_frame_id, akm_cmd_vel, test;
+	bool publish_tf_;
 	std::string cmd_vel;
 	int serial_baud_rate;
 	RECEIVE_DATA Receive_Data;

src/origincar_base/launch/origincar_bringup.launch.py

@@ -37,12 +37,6 @@ def generate_launch_description():
     )
 
     
-    base_to_link = launch_ros.actions.Node(
-            package='tf2_ros', 
-            executable='static_transform_publisher', 
-            name='base_to_link',
-            arguments=['0', '0', '0','0', '0','0','base_footprint','base_link'],#arguments=['0.41', '0.12', '0','0', '0','0','base_footprint','base_link'],
-    )
     
 
     base_to_gyro = launch_ros.actions.Node(
@@ -73,10 +67,21 @@ def generate_launch_description():
             remappings=[("odometry/filtered", "odom_combined")]
             )
                               
+    # 从 URDF 生成 robot_description，供 joint_state_publisher 和 robot_state_publisher 共用
+    from launch_ros.parameter_descriptions import ParameterValue
+    from launch.substitutions import Command
+
+    robot_description = ParameterValue(
+        Command(['xacro ', os.path.join(
+            get_package_share_directory('origincar_description'),
+            'urdf', 'origincar.urdf')]),
+        value_type=str)
+
     joint_state_publisher_node = launch_ros.actions.Node(
             package='joint_state_publisher', 
             executable='joint_state_publisher', 
             name='joint_state_publisher',
+            parameters=[{'robot_description': robot_description}],
     )
 
     ld = LaunchDescription()
@@ -84,7 +89,6 @@ def generate_launch_description():
     ld.add_action(carto_slam_dec)
     ld.add_action(akmcar_dec)
     ld.add_action(origincar_base)
-    ld.add_action(base_to_link)
     ld.add_action(base_to_gyro)
     ld.add_action(joint_state_publisher_node)
     ld.add_action(choose_car)

src/origincar_base/launch/robot_mode_description.launch.py

@@ -1,24 +1,27 @@
 import os
-from pathlib import Path
-import launch_ros.actions
-import launch
 from ament_index_python.packages import get_package_share_directory
 from launch import LaunchDescription
-from launch.actions import (DeclareLaunchArgument, GroupAction,LogInfo,
-                            IncludeLaunchDescription, SetEnvironmentVariable)
-from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import Node
+from launch_ros.parameter_descriptions import ParameterValue
+from launch.substitutions import Command
+
 
 def generate_launch_description():
-    origincar_description = GroupAction([
-        launch_ros.actions.Node(
-            package='robot_state_publisher', 
-            executable='robot_state_publisher', 
-            name='robot_state_publisher',
-            arguments=[os.path.join(get_package_share_directory('origincar_description'),'urdf','origincar.urdf')]
-        )
-    ])
- 
-    ld = LaunchDescription()
+    urdf_path = os.path.join(
+        get_package_share_directory('origincar_description'),
+        'urdf', 'origincar.urdf')
+    
+    robot_description = ParameterValue(
+        Command(['xacro ', urdf_path]), value_type=str)
 
-    ld.add_action(origincar_description)
-    return ld
+    robot_state_publisher = Node(
+        package='robot_state_publisher',
+        executable='robot_state_publisher',
+        name='robot_state_publisher',
+        parameters=[{'robot_description': robot_description,
+                     'publish_frequency': 30.0}],
+    )
+
+    ld = LaunchDescription()
+    ld.add_action(robot_state_publisher)
+    return ld

src/origincar_base/src/origincar_base.cpp

@@ -225,7 +225,9 @@ void origincar_base::Publish_Odom()
     t.transform.translation.z = 0.0;
 
     t.transform.rotation = odom_quat;
-    tf_broadcaster_->sendTransform(t);
+    if (publish_tf_) {
+      tf_broadcaster_->sendTransform(t);
+    }
     odom_publisher->publish(odom);
     robotpose_publisher->publish(robotpose);
     robotvel_publisher->publish(robotvel); 
@@ -264,7 +266,11 @@ bool origincar_base::Get_Sensor_Data()
 {
     short transition_16 = 0, j = 0, Header_Pos = 0, Tail_Pos = 0;
     uint8_t Receive_Data_Pr[RECEIVE_DATA_SIZE] = {0}; 
-    Stm32_Serial.read(Receive_Data_Pr,sizeof (Receive_Data_Pr)); 
+    try {
+      Stm32_Serial.read(Receive_Data_Pr,sizeof (Receive_Data_Pr));
+    } catch (const serial::SerialException& e) {
+      return false;
+    }
     for (j = 0; j < 24; j++) {
       if (Receive_Data_Pr[j] == FRAME_HEADER)
       Header_Pos=j;
@@ -375,6 +381,7 @@ origincar_base::origincar_base()
   this->declare_parameter<std::string>("odom_frame_id", "odom");
   this->declare_parameter<std::string>("robot_frame_id", "base_link");
   this->declare_parameter<std::string>("gyro_frame_id", "gyro_link");
+  this->declare_parameter<bool>("publish_tf", false);
 
   this->get_parameter("serial_baud_rate", serial_baud_rate);
   this->get_parameter("usart_port_name", usart_port_name);
@@ -383,6 +390,7 @@ origincar_base::origincar_base()
   this->get_parameter("odom_frame_id", odom_frame_id);
   this->get_parameter("robot_frame_id", robot_frame_id);
   this->get_parameter("gyro_frame_id", gyro_frame_id);
+  this->get_parameter("publish_tf", publish_tf_);
 
   odom_publisher = create_publisher<nav_msgs::msg::Odometry>("odom", 10);
 

src/origincar_description/urdf/origincar.urdf

@@ -4,12 +4,6 @@
   <link name="base_footprint">
     <origin xyz="0 0 0" rpy="0 0 0" />
   </link>
-<<<<<<< HEAD
-  <!-- 基础底盘 -->
-
-
-=======
->>>>>>> mo_new
   <link name="base_link">
   </link>
   <joint name="base_joint" type="fixed">
@@ -89,11 +83,7 @@
     </inertial>
   </link>
   <joint name="fr_left_steer_joint" type="revolute">
-<<<<<<< HEAD
-    <origin xyz="0.0841 0.0945 -0.03" rpy="0 0 0" />
-=======
     <origin xyz="0.0715 0.0945 -0.03" rpy="0 0 0" />
->>>>>>> mo_new
     <parent link="chassis_link" />
     <child link="fr_left_steer_link" />
     <axis xyz="0 0 1" />
@@ -146,11 +136,7 @@
     </inertial>
   </link>
   <joint name="fr_right_steer_joint" type="revolute">
-<<<<<<< HEAD
-    <origin xyz="0.0841 -0.0945 -0.03" rpy="0 0 0" />
-=======
     <origin xyz="0.0715 -0.0945 -0.03" rpy="0 0 0" />
->>>>>>> mo_new
     <parent link="chassis_link" />
     <child link="fr_right_steer_link" />
     <axis xyz="0 0 1" />
@@ -219,11 +205,7 @@
     </collision>
   </link>
   <joint name="re_left_wheel_joint" type="continuous">
-<<<<<<< HEAD
-    <origin xyz="-0.0841 0.0945 -0.03" rpy="1.5708 0 0" />
-=======
     <origin xyz="-0.0715 0.0945 -0.03" rpy="1.5708 0 0" />
->>>>>>> mo_new
     <parent link="chassis_link" />
     <child link="re_left_wheel_link" />
     <axis xyz="0 0 -1" />
@@ -257,11 +239,7 @@
     </collision>
   </link>
   <joint name="re_right_wheel_joint" type="continuous">
-<<<<<<< HEAD
-    <origin xyz="-0.0841 -0.0945 -0.03" rpy="-1.5708 0 0" />
-=======
     <origin xyz="-0.0715 -0.0945 -0.03" rpy="-1.5708 0 0" />
->>>>>>> mo_new
     <parent link="chassis_link" />
     <child link="re_right_wheel_link" />
     <axis xyz="0 0 1" />
@@ -287,11 +265,7 @@
     </inertial>
   </link>
   <joint name="virtual_steering_wheel_joint" type="revolute">
-<<<<<<< HEAD
-    <origin xyz="0.0841 0.0 0.0" rpy="0.0 0.0 0.0" />
-=======
     <origin xyz="0.0715 0.0 0.0" rpy="0.0 0.0 0.0" />
->>>>>>> mo_new
     <parent link="chassis_link" />
     <child link="virtual_steer_link" />
     <axis xyz="-1 0 0" />
@@ -336,10 +310,6 @@
     <origin rpy="0 0 0" xyz="0 0 0.045" />
   </joint>
 
-<<<<<<< HEAD
-
-=======
->>>>>>> mo_new
   <!-- ackerman -->
 
   <gazebo>

调试记录.log

@@ -16,4 +16,154 @@ Bus 001 Device 001: ID 1d6b:0002 Linux Foundation 2.0 root hub
 
 
 16:48
-雷达串口问题解决，原因是type-c转接线的芯片问题，换了一条转接线后雷达串口设备正常显示为ttyACM0和ttyACM1了。
+雷达串口问题解决，原因是type-c转接线的芯片问题，换了一条转接线后雷达串口设备正常显示为ttyACM0和ttyACM1了。
+============================================================
+2026/6/11 — TF树修复 & base驱动稳定性修复
+============================================================
+
+## 问题1: SLAM Toolbox Message Filter 丢帧
+**现象**: `Message Filter dropping message: frame laser_link ... reason discarding message because the queue is full`
+**原因**: `slam_toolbox_mapping.yaml` 中 `scan_queue_size` 偏小，激光雷达 10Hz、minimum_time_interval 0.5s，队列缓冲不足
+**修复**: 将 `scan_queue_size` 适当调大即可（属配置调优项，未做硬编码修改）
+
+## 问题2: SLAM Toolbox Message Filter 时间戳过期
+**现象**: `Message Filter dropping message: frame laser_link ... reason the timestamp on the message is earlier than all the data in the transform cache`
+**原因**: 激光帧时间戳早于 TF 缓存最早数据，通常刚启动时 TF 尚未就绪或处理积压
+**修复**: 随问题3 TF树修复后缓解
+
+## 问题3: TF 树断连 — bt_navigator 找不到 map→base_footprint (核心问题)
+**现象**: `Could not find a connection between map and base_footprint because they are not part of the same tree. Tf has two or more unconnected trees.`
+**根因**: 两个节点同时发布 `odom→base_link` TF:
+  - base 驱动 (origincar_base.cpp:228) — 原始轮式里程计
+  - EKF (ekf.yaml: base_link_frame=base_link, publish_tf=true) — 融合 odom+IMU
+  两节点争夺同一条 TF，导致树撕裂
+**修复** (4处改动):
+  1. ekf.yaml: base_link_frame 从 base_link 改为 base_footprint
+     → EKF 发布 odom→base_footprint，不再与 base 驱动争抢
+  2. origincar_base.cpp: 新增 publish_tf 参数，默认 false
+     → base 驱动默认不再广播 TF
+  3. gc_nav2_with_slam_online_real.launch.py: 移除重复的 base_footprint→base_link static TF
+     → URDF 已定义该关节 (z=-0.09)，static TF 提供 identity (z=0) 造成冲突
+  4. origincar_bringup.launch.py: 移除 base_to_link static TF（同上）
+**修复后 TF 树**:
+  map → odom → base_footprint → base_link → chassis_link → borad_link → laser_link
+   ↑       ↑           ↑                ↑
+  SLAM    EKF融合     URDF             URDF
+
+## 问题4: cmd_vel_to_ackermann_drive.py 找不到
+**现象**: `executable cmd_vel_to_ackermann_drive.py not found on the libexec directory`
+**原因**: 源文件缺执行权限 (644)，--symlink-install 时安装路径继承源文件权限
+**修复**: chmod +x scripts/cmd_vel_to_ackermann_drive.py
+
+## 问题5: base 驱动串口异常崩溃
+**现象**: `terminate called after throwing serial::SerialException: device reports readiness to read but returned no data`
+**原因**: Get_Sensor_Data() 中 Stm32_Serial.read() 抛异常未捕获
+**修复**: origincar_base.cpp 中 Stm32_Serial.read() 包裹 try-catch，捕获 SerialException 后 return false
+
+## 问题6: URDF 文件 git 冲突未解决
+**现象**: xacro 解析失败 — XML parsing error: not well-formed (invalid token)
+**原因**: origincar.urdf 存在 8 处 git merge 冲突标记 (HEAD vs mo_new)，涉及轮距参数 (0.0841 vs 0.0715)
+**修复**: 清除所有冲突标记，采用 mo_new 分支轮距值
+
+## 问题7: joint_state_publisher 找不到 robot_description
+**现象**: `Waiting for robot_description to be published on the robot_description topic...`
+**原因**: robot_mode_description.launch.py 使用旧式命令行参数传 URDF，未发布到 topic
+**修复**: 改为参数方式传递 robot_description，同时传给 joint_state_publisher
+
+## 编译记录
+  - 命令: colcon build --packages-select origincar_base --symlink-install
+  - 结果: 通过
+
+================================================================================
+日期: 2026-06-13
+问题: LSLiDAR /scan 可视化中出现空扇形面
+================================================================================
+
+【现象】
+在 rviz/foxglove 中查看 /scan 话题时，显示中凭空出现一个约60°的扇形区域
+没有任何点（inf），物体轮廓在该边界处像是被"切断"一般。但实际雷达能够检测
+到360°所有方向的物体。
+
+【根因分析】
+驱动代码 lslidar_driver.cc 中，N10 雷达走的是 else 分支（Path 2）。
+第1137行被硬编码为:
+    int scan_num = fixed_array_length;  // 450
+
+而原始正确公式（被注释掉）为:
+    //int scan_num = ceil((angle_able_max - angle_able_min) / 360 * count_num) + 1;
+
+问题机制:
+  - LiDAR 硬件每圈输出约374个数据点 (分辨率 ~0.96°/点)，完整覆盖360°
+  - 但 fixed_array_length=450 强制分配了450个bin (分辨率 0.80°/点)
+  - 索引映射 point_idx = round((360-degree) * count_num / 360) 最大只能到373
+  - 所以 indices 374-449 (78个bin, ~60°) 永远是 inf
+  - 所有真实数据被"压缩"到 0-299° 范围内显示
+
+同时 angle_increment 和 time_increment 也使用了 fixed_array_length
+导致与实际数据不匹配。
+
+【修复内容】
+文件: src/lslidar_driver/src/lslidar_driver.cc
+
+修改1 (line 1137):
+  - 修复前: int scan_num = fixed_array_length;//cyy_addcyy_add
+  + 修复后: int scan_num = ceil((angle_able_max - angle_able_min) / 360 * count_num) + 1;
+
+修改2 (line 1163):
+  - 修复前: scan->angle_increment = 2 * M_PI / (double)(fixed_array_length - 1);
+  + 修复后: scan->angle_increment = 2 * M_PI / (double)(scan_num - 1);
+
+修改3 (line 1172):
+  - 修复前: scan->time_increment = 0.1 / (double)(fixed_array_length - 1);
+  + 修复后: scan->time_increment = 0.1 / (double)(scan_num - 1);
+
+【验证】
+编译通过。需要重启 lslidar_driver_node 使修复生效:
+  ros2 lifecycle set lslidar_driver_node shutdown
+  然后重新启动 launch 文件。
+
+================================================================================
+日期: 2026-06-13
+问题: LSLiDAR /scan 可视化中出现空扇形面
+================================================================================
+
+【现象】
+在 rviz/foxglove 中查看 /scan 话题时，显示中凭空出现一个约60°的扇形区域
+没有任何点（inf），物体轮廓在该边界处像是被切断一般。但实际雷达能够检测
+到360°所有方向的物体。
+
+【根因分析】
+驱动代码 lslidar_driver.cc 中，N10 雷达走的是 else 分支（Path 2）。
+第1137行被硬编码为:
+    int scan_num = fixed_array_length;  // 450
+
+而原始正确公式（被注释掉）为:
+    //int scan_num = ceil((angle_able_max - angle_able_min) / 360 * count_num) + 1;
+
+问题机制:
+  - LiDAR 硬件每圈输出约374个数据点 (分辨率 ~0.96°/点)，完整覆盖360°
+  - 但 fixed_array_length=450 强制分配了450个bin (分辨率 0.80°/点)
+  - 索引映射 point_idx = round((360-degree) * count_num / 360) 最大只能到373
+  - 所以 indices 374-449 (78个bin, ~60°) 永远是 inf
+  - 所有真实数据被压缩到 0-299° 范围内显示
+
+同时 angle_increment 和 time_increment 也使用了 fixed_array_length
+导致与实际数据不匹配。
+
+【修复内容】
+文件: src/lslidar_driver/src/lslidar_driver.cc
+
+修改1 (line 1137):
+  - 修复前: int scan_num = fixed_array_length;
+  + 修复后: int scan_num = ceil((angle_able_max - angle_able_min) / 360 * count_num) + 1;
+
+修改2 (line 1163):
+  - 修复前: scan->angle_increment = 2 * M_PI / (double)(fixed_array_length - 1);
+  + 修复后: scan->angle_increment = 2 * M_PI / (double)(scan_num - 1);
+
+修改3 (line 1172):
+  - 修复前: scan->time_increment = 0.1 / (double)(fixed_array_length - 1);
+  + 修复后: scan->time_increment = 0.1 / (double)(scan_num - 1);
+
+【验证】
+编译通过。需要重启 lslidar_driver_node 使修复生效。