两轮机器人激光雷达避障教程

本教程在上一节内容的基础上（课程链接：创建一个ROS2小车，掌握ROS2工程以及消息订阅机制），为两轮小车增加激光雷达传感器，并实现简单的避障功能。基于 ROS 2 Jazzy + Gazebo Harmonic 环境。

---

目录

1. 新增文件一览
2. 添加激光雷达传感器
3. 配置 ROS-Gazebo 桥接
4. 修复轮子旋转轴向
5. 在 RViz2 中可视化雷达数据
6. 编写避障脚本
7. 运行避障实验
8. 避坑总结

---

1. 新增文件一览

本教程涉及以下新增和修改的文件：

my_robot_description/
├── config/
│   └── bridge_config.yaml      # ROS-Gazebo 桥接配置（新增）
├── launch/
│   └── sim.launch.py           # 修改：增加桥接器节点
├── scripts/
│   └── obstacle_avoidance.py   # 避障脚本（新增）
└── urdf/
    └── robot.urdf.xacro        # 修改：增加激光雷达link和sensor

---

2. 添加激光雷达传感器

2.1 添加激光雷达 link

在 robot.urdf.xacro 中，在 </robot> 前添加激光雷达的 link 和 joint：

    <!-- 激光雷达传感器 link：固定在底盘上 -->
    <joint name="laser_joint" type="fixed">
        <parent link="chassis" />
        <child link="laser_frame" />
        <origin xyz="0.1 0 0.175" rpy="0 0 0" />
    </joint>

    <link name="laser_frame">
        <visual>
            <geometry>
                <cylinder radius="0.05" length="0.04" />
            </geometry>
            <material name="black" />
        </visual>
    </link>

提示：laser_frame 是一个"虚拟"link，仅用于挂载传感器。激光雷达不需要 collision 和 inertial。

2.2 添加 Gazebo 传感器配置

关键：<gazebo> 标签必须写在 <link> 外面，与 <link> 同级。错误地将 <gazebo> 写在 <link> 内部会导致传感器无法正常工作。

    <gazebo reference="laser_frame">
        <sensor name="laser" type="gpu_lidar">
            <gz_frame_id>laser_frame</gz_frame_id>
            <pose>0 0 0 0 0 0</pose>
            <topic>scan</topic>
            <update_rate>10</update_rate>
            <ray>
                <scan>
                    <horizontal>
                        <samples>640</samples>
                        <min_angle>-3.14</min_angle>
                        <max_angle>3.14</max_angle>
                    </horizontal>
                </scan>
                <range>
                    <min>0.1</min>
                    <max>12.0</max>
                </range>
            </ray>
            <always_on>1</always_on>
            <visualize>true</visualize>
        </sensor>
    </gazebo>

2.3 添加 Gazebo 传感器插件

在差速驱动插件的 <gazebo> 标签内添加：

        <!-- 激光雷达传感器的gazebo插件声明-->
        <plugin
            filename="gz-sim-sensors-system"
            name="gz::sim::systems::Sensors">
            <render_engine>ogre2</render_engine>
        </plugin>

关键：<filename>gz-sim-sensors-system</filename> 是 Gazebo Harmonic 的正确写法，Gazebo Sim（Ignition）时期使用的是 libgz-sim-sensors-system.so。如果使用旧写法，雷达可能无法正常工作。

---

3. 配置 ROS-Gazebo 桥接

3.1 创建桥接配置文件

在 config/ 目录下创建 bridge_config.yaml：

# 激光雷达扫描
- ros_topic_name: "/scan"
  gz_topic_name: "/scan"
  ros_type_name: "sensor_msgs/msg/LaserScan"
  gz_type_name: "gz.msgs.LaserScan"
  direction: GZ_TO_ROS

# 运动控制指令
- ros_topic_name: "/cmd_vel"
  gz_topic_name: "/cmd_vel"
  ros_type_name: "geometry_msgs/msg/Twist"
  gz_type_name: "gz.msgs.Twist"
  direction: ROS_TO_GZ

# 关节状态
- ros_topic_name: "/joint_states"
  gz_topic_name: "/world/empty/model/my_cool_robot/joint_state"
  ros_type_name: "sensor_msgs/msg/JointState"
  gz_type_name: "gz.msgs.Model"
  direction: GZ_TO_ROS

# 里程计数据
- ros_topic_name: "/odom"
  gz_topic_name: "/odom"
  ros_type_name: "nav_msgs/msg/Odometry"
  gz_type_name: "gz.msgs.Odometry"
  direction: GZ_TO_ROS

# 坐标变换 (TF)
- ros_topic_name: "/tf"
  gz_topic_name: "/model/my_cool_robot/tf"
  ros_type_name: "tf2_msgs/msg/TFMessage"
  gz_type_name: "gz.msgs.Pose_V"
  direction: GZ_TO_ROS

注意：gz_topic_name 中的路径可能随世界名称改变。如果你的 Gazebo 世界不是 empty，需要相应修改 /world/empty/model/my_cool_robot/joint_state 中的 empty。

3.2 修改 sim.launch.py

修改 sim.launch.py，增加桥接器节点：

import os
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.actions import IncludeLaunchDescription, DeclareLaunchArgument
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch_ros.actions import Node
from launch.substitutions import LaunchConfiguration

def generate_launch_description():
    pkg_name = 'my_robot_description'
    pkg_share = get_package_share_directory(pkg_name)

    use_sim_time = LaunchConfiguration('use_sim_time', default='true')

    # 1. 模型发布节点 (RSP)
    rsp = IncludeLaunchDescription(
        PythonLaunchDescriptionSource([os.path.join(
            pkg_share, 'launch', 'rsp.launch.py'
        )]), launch_arguments={'use_sim_time': use_sim_time}.items()
    )

    # 2. 启动 Gazebo Harmonic
    gazebo = IncludeLaunchDescription(
        PythonLaunchDescriptionSource([os.path.join(
            get_package_share_directory('ros_gz_sim'), 'launch', 'gz_sim.launch.py'
        )]), launch_arguments={
            'gz_args': '-r empty.sdf',
            'use_sim_time': use_sim_time
        }.items()
    )

    # 3. 桥接器配置 (Bridge)
    bridge_config = os.path.join(pkg_share, 'config', 'bridge_config.yaml')

    ros_gz_bridge = Node(
        package='ros_gz_bridge',
        executable='parameter_bridge',
        parameters=[{
            'config_file': bridge_config,
            'use_sim_time': True
        }],
        output='screen'
    )

    # 4. 在 Gazebo 中生成机器人
    spawn_entity = Node(
        package='ros_gz_sim',
        executable='create',
        arguments=[
            '-topic', 'robot_description',
            '-name', 'my_cool_robot',
            '-z', '0.1'
        ],
        parameters=[{'use_sim_time': True}],
        output='screen'
    )

    return LaunchDescription([
        DeclareLaunchArgument('use_sim_time', default_value='true'),
        rsp,
        gazebo,
        ros_gz_bridge,
        spawn_entity
    ])

关键：use_sim_time: True 是解决 TF_OLD_DATA 警告的关键配置，务必添加。

3.3 更新 CMakeLists.txt

确保 CMakeLists.txt 中包含 config 目录的安装：

install(DIRECTORY
  launch
  urdf
  worlds
  config
  scripts
  DESTINATION share/${PROJECT_NAME}
)

编译项目：

cd ~/ros2_ws
colcon build --symlink-install
source install/setup.bash

---

4. 修复轮子旋转轴向

这是一个极其隐蔽但致命的 Bug。如果你发现发送直线指令时机器人却原地转圈，检查左右轮的 rpy 值。

问题现象

发送直线指令：

ros2 topic pub /cmd_vel geometry_msgs/msg/Twist "{linear: {x: 0.2}}"

机器人不走直线，而是疯狂转圈。

原因

圆柱体默认沿 Z 轴延伸。为让轮子着地，需要绕 X 轴旋转 ±90°。但如果左右轮旋转方向不一致：

• 左轮：rpy="-${pi/2} 0 0"
• 右轮：rpy="${pi/2} 0 0"

会导致左右轮的局部坐标系 Z 轴方向相反，差速驱动插件计算出的左右轮转速一个向前、一个向后。

解决方法

修改右轮关节，使左右轮 rpy 一致：

    <!-- 右轮关节：连续旋转关节，允许右轮无限旋转 -->
    <joint name="right_wheel_joint" type="continuous">
        <parent link="base_link" />
        <child link="right_wheel" />
        <origin xyz="0 -0.175 0" rpy="-${pi/2} 0 0" />  <!-- 改为和左轮一致 -->
        <axis xyz="0 0 1" />
    </joint>

---

5. 在 RViz2 中可视化雷达数据

运行仿真后，新开终端启动 RViz2：

rviz2

配置步骤：

1. Fixed Frame：选择 base_link
2. Add → 选择 LaserScan，Topic 选择 /scan

只要选对话题，将任意障碍物放到小车面前，小车可以扫描到其点云。

如果 RViz 中看不到红色点云，检查：

1. Fixed Frame 是否正确：如果选择 map 或 odom 而非 base_link，点云无法显示
2. TF 是否正常：ros2 run tf2_tools view_frames 查看 TF 树
3. 雷达话题是否有数据：ros2 topic echo /scan 查看是否有数据

---

6. 编写避障脚本

在 scripts/ 目录下创建 obstacle_avoidance.py：

#!/usr/bin/env python3
import rclpy
from rclpy.node import Node
from geometry_msgs.msg import Twist
from sensor_msgs.msg import LaserScan
import numpy as np

class RobustAvoider(Node):
    def __init__(self):
        super().__init__('robust_avoider')
        self.publisher_ = self.create_publisher(Twist, '/cmd_vel', 10)
        self.subscription = self.create_subscription(LaserScan, '/scan', self.listener_callback, 10)
        self.safe_distance = 0.8  # 0.8米内避障
        self.get_logger().info("避障脚本已启动，监听 /scan 中...")

    def listener_callback(self, msg):
        ranges = np.array(msg.ranges)
        
        # --- 关键过滤步骤 ---
        # 剔除 0 (通常是无效数据) 和 inf (无穷远)
        ranges = np.where((ranges < 0.05) | np.isinf(ranges), 10.0, ranges)
        
        # 确定正前方区域 (取中间 10% 的数据量)
        num_samples = len(ranges)
        margin = num_samples // 20  # 取左右各 5%
        center = num_samples // 2
        front_sector = ranges[center - margin : center + margin]
        
        avg_dist = np.mean(front_sector)
        min_dist = np.min(front_sector)

        move_cmd = Twist()
        if min_dist < self.safe_distance:
            self.get_logger().warn(f'⚠️ 障碍物过近! 最小距离: {min_dist:.2f}m')
            move_cmd.linear.x = 0.0
            move_cmd.angular.z = 0.5
        else:
            self.get_logger().info(f'✅ 路径安全。前方平均距离: {avg_dist:.2f}m')
            move_cmd.linear.x = 0.3
            move_cmd.angular.z = 0.0

        self.publisher_.publish(move_cmd)

def main(args=None):
    rclpy.init(args=args)
    rclpy.spin(RobustAvoider())
    rclpy.shutdown()

if __name__ == '__main__':
    main()

赋予执行权限：

chmod +x scripts/obstacle_avoidance.py

---

7. 运行避障实验

7.1 启动仿真

cd ~/ros2_ws/
source install/setup.bash
ros2 launch my_robot_description sim.launch.py

7.2 运行避障脚本

新开终端：

# 该命令必须运行install/setup.bash后才有用
ros2 run my_robot_description obstacle_avoidance.py

7.3 验证

1. 在 Gazebo 中观察机器人是否向前移动
2. 将障碍物（如方块）放置在机器人前方，观察机器人是否原地旋转避障
3. 移除障碍物，观察机器人是否继续前进

---

8. 避坑总结

错误现象	原因	解决方案
/scan 的 frame_id 是 base_link/laser 而非 laser_frame/laser	<gazebo> 写在 <link> 内部	将 <gazebo> 移到 <link> 外部，与 <link> 同级，新版本要用<gz_frame_id>
RViz2 中看不到激光雷达点云	Fixed Frame 设置错误	改为 base_link 或 my_cool_robot/base_link
发送直线指令机器人却原地转圈	左右轮 rpy 值不一致	左右轮都使用 rpy="-${pi/2} 0 0"
机器人"提前"转圈	雷达扫到了自己的身体	雷达 min_range 设置为 0.1，过滤近距离噪点
雷达数据全是 10m（inf）	桥接配置未生效	检查 bridge_config.yaml 中 /scan 的映射，确认 gz_topic_name 路径正确
TF_OLD_DATA 警告	桥接器未使用仿真时间	在桥接器 Node 参数中添加 use_sim_time: True
雷达可视化有误或报错	传感器插件 filename 错误	Gazebo Harmonic 使用 gz-sim-sensors-system，不是 libgz-sim-sensors-system.so

---

恭喜！

你已完成激光雷达的集成和避障功能的实现。掌握的新知识：

• ROS 2 与 Gazebo 之间的桥接通信
• Gazebo GPU LiDAR 传感器的配置
• 基于激光雷达的简单避障逻辑
• 左右轮坐标系一致性的重要性

---

原理总结：激光雷达数据流

1. Gazebo 传感器：GPU LiDAR 每 10Hz 扫描周围环境，生成 gz.msgs.LaserScan
2. 桥接器：ros_gz_bridge 将 gz.msgs.LaserScan 转换为 sensor_msgs/msg/LaserScan，发布到 /scan 话题
3. RViz2：订阅 /scan，根据 frame_id（laser_frame）将点云正确叠加到 TF 树中
4. 避障脚本：订阅 /scan，分析正前方扇区的最小距离，决定是前进还是转向

本次开发源码全都上传到了github：turtle3双轮小车SLAM入门
我在另一个脚本写了个用pid控制小车画五角星