ROS2系列教程：完整实体机器人项目实战（软硬协同）

1
robot_ws/
2
├── src/
3
│   ├── robot_bringup/          # 总启动包：launch + 参数 + RViz 配置
4
│   │   ├── launch/
5
│   │   │   ├── robot.launch.py     # 一键启动全部
6
│   │   │   ├── mapping.launch.py   # 建图模式
7
│   │   │   └── navigation.launch.py# 导航模式
8
│   │   ├── config/
9
│   │   │   ├── robot.yaml          # 机器人描述参数（轮距/轮径/编码器）
10
│   │   │   └── nav2_params.yaml    # Nav2 全套参数
11
│   │   ├── urdf/
12
│   │   │   └── robot.urdf.xacro    # 统一机器人模型
13
│   │   ├── rviz/
14
│   │   │   ├── mapping.rviz
15
│   │   │   └── navigation.rviz
16
│   │   ├── package.xml
17
│   │   └── CMakeLists.txt
18
│   ├── robot_description/       # 模型描述（URDF/Xacro/meshes）
19
│   ├── robot_driver/            # 下位机驱动（串口桥 + 传感器聚合）
20
│   │   └── robot_driver/
21
│   │       ├── serial_bridge.py      # 串口收发（第 32 篇）
22
│   │       ├── sensor_aggregator.py  # 传感器汇聚（第 32 篇）
23
│   │       ├── imu_node.py           # IMU 发布（第 32 篇）
24
│   │       └── battery_monitor.py    # 电量监控
25
│   └── robot_tasks/             # 任务层（导航任务链等）
26
│       └── robot_tasks/
27
│           ├── task_navigation.py    # 多点导航（第 33 篇）
28
│           └── task_mission.py       # 综合任务调度

1
┌─────────────────────────────────────────────────────────┐
2
│                       上位机 (ROS2)                      │
3
│                                                         │
4
│  /cmd_vel ──► serial_bridge ──► 串口(115200) ──► STM32   │
5
│                ▲   │                                   │
6
│                │   ▼                                   │
7
│  /odom ◄───────┘   └──► /imu/data ──► robot_localization│
8
│  /scan (激光雷达)  ──► slam_toolbox / AMCL              │
9
│  /battery_voltage ──► battery_monitor ──► 急停话题       │
10
│                                                         │
11
│  Nav2: map_server + AMCL + planner + controller + BT    │
12
│  tasks: 多点导航 / 充电任务 / 巡航任务                   │
13
└─────────────────────────────────────────────────────────┘

1
# robot.launch.py：一键总启动（带模式参数）
2
from launch import LaunchDescription
3
from launch.actions import DeclareLaunchArgument, ExecuteProcess
4
from launch.substitutions import LaunchConfiguration
5
from launch_ros.actions import Node
6
import os
7

8
def generate_launch_description():
9
    mode = LaunchConfiguration('mode', default='navigation')
10
    return LaunchDescription([
11
        DeclareLaunchArgument('mode', default_value='navigation',
12
                              description='navigation | mapping | manual'),
13
        # ===== 基础层：模型 + 驱动（所有模式都需要） =====
14
        Node(package='robot_state_publisher', executable='robot_state_publisher',
15
             parameters=[{'robot_description': open(
16
                 os.path.expanduser('~/robot_ws/src/robot_bringup/urdf/robot.urdf.xacro')).read()}],
17
             output='screen'),
18
        Node(package='robot_driver', executable='serial_bridge',
19
             parameters=[{'port': '/dev/ttyUSB1', 'baudrate': 115200}],
20
             output='screen'),
21
        Node(package='robot_driver', executable='sensor_aggregator',
22
             parameters=[{'low_battery_threshold': 10.8}],
23
             output='screen'),
24
        # ===== 模式分支 =====
25
        # 模式由 Exec 动态选择，这里以 navigation 为例：
26
        Node(package='nav2_map_server', executable='map_server',
27
             parameters=[{'yaml_filename': os.path.expanduser('~/maps/room1_clean.yaml')}],
28
             output='screen'),
29
        Node(package='nav2_amcl', executable='amcl',
30
             parameters=['~/robot_ws/src/robot_bringup/config/nav2_params.yaml'],
31
             output='screen'),
32
        Node(package='nav2_lifecycle_manager', executable='lifecycle_manager',
33
             name='lifecycle_manager_localization',
34
             parameters=[{'autostart': True,
35
                          'node_names': ['map_server', 'amcl']}],
36
             output='screen'),
37
        Node(package='nav2_planner', executable='planner_server',
38
             parameters=['~/robot_ws/src/robot_bringup/config/nav2_params.yaml'],
39
             output='screen'),
40
        Node(package='nav2_controller', executable='controller_server',
41
             parameters=['~/robot_ws/src/robot_bringup/config/nav2_params.yaml'],
42
             output='screen'),
43
        Node(package='nav2_bt_navigator', executable='bt_navigator',
44
             parameters=['~/robot_ws/src/robot_bringup/config/nav2_params.yaml'],
45
             output='screen'),
46
        Node(package='nav2_lifecycle_manager', executable='lifecycle_manager',
47
             name='lifecycle_manager_navigation',
48
             parameters=[{'autostart': True,
49
                          'node_names': ['planner_server', 'controller_server',
50
                                         'bt_navigator']}],
51
             output='screen'),
52
    ])

1
# watchdog.py：上位机看门狗（在 serial_bridge 内集成）
2
import rclpy
3
from rclpy.node import Node
4
from std_msgs.msg import Float32
5

6
class Watchdog(Node):
7
    def __init__(self):
8
        super().__init__('watchdog')
9
        self.last_heartbeat = self.get_clock().now()
10
        self.timeout = 0.5  # 500ms 无心跳视为掉线
11
        self.timer = self.create_timer(0.1, self.check)
12
        self.pub_stop = self.create_publisher(Float32, 'robot_status/emergency_stop', 10)
13

14
    def heartbeat(self):  # 由 serial_bridge 在收到帧时调用
15
        self.last_heartbeat = self.get_clock().now()
16

17
    def check(self):
18
        if (self.get_clock().now() - self.last_heartbeat).nanoseconds > self.timeout * 1e9:
19
            self.get_logger().error('下位机掉线，发布急停！')
20
            msg = Float32(); msg.data = 1.0
21
            self.pub_stop.publish(msg)

1
# safety_filter.py：订阅 /scan，距离过近直接覆盖 /cmd_vel 为 0
2
import rclpy
3
from rclpy.node import Node
4
from sensor_msgs.msg import LaserScan
5
from geometry_msgs.msg import Twist
6

7
class SafetyFilter(Node):
8
    def __init__(self):
9
        super().__init__('safety_filter')
10
        self.min_distance = 0.25  # 25cm 内必须停车
11
        self.sub = self.create_subscription(LaserScan, '/scan', self.on_scan, 10)
12
        self.sub_cmd = self.create_subscription(Twist, '/cmd_vel_raw', self.on_cmd, 10)
13
        self.pub = self.create_publisher(Twist, '/cmd_vel', 10)  # 最终输出
14
        self.current_cmd = Twist()
15

16
    def on_cmd(self, msg):
17
        self.current_cmd = msg
18

19
    def on_scan(self, scan):
20
        # 只检查前方 ±60° 的障碍（避开侧面正常贴近）
21
        danger = False
22
        n = len(scan.ranges)
23
        for i in range(n):
24
            angle = scan.angle_min + i * scan.angle_increment
25
            if abs(angle) < 1.047:  # 60° 内
26
                if 0.05 < scan.ranges[i] < self.min_distance:
27
                    danger = True
28
                    break
29
        if danger:
30
            twist = Twist()  # 全零 = 急停
31
            self.pub.publish(twist)
32
        else:
33
            self.pub.publish(self.current_cmd)
34

35
def main():
36
    rclpy.init()
37
    node = SafetyFilter()
38
    rclpy.spin(node)

1
# task_mission.py：综合任务调度（巡航 + 回充 + 待机）
2
import rclpy
3
from rclpy.node import Node
4
from nav2_simple_commander.robot_navigator import BasicNavigator, TaskResult
5
from geometry_msgs.msg import PoseStamped
6
import math
7

8
class Mission(Node):
9
    def __init__(self):
10
        super().__init__('mission')
11
        self.nav = BasicNavigator()
12
        # 巡逻点（真实地图坐标）
13
        self.patrol_points = [
14
            self._pose(1.0, 1.0, 0.0),
15
            self._pose(2.0, 2.5, 1.57),
16
            self._pose(1.5, 3.5, 3.14),
17
        ]
18
        self.charging_station = self._pose(0.0, 0.0, 0.0)
19
        self.battery = 100.0
20
        self.timer = self.create_timer(0.5, self.loop)
21

22
    def _pose(self, x, y, yaw):
23
        p = PoseStamped()
24
        p.header.frame_id = 'map'
25
        p.header.stamp = self.nav.get_clock().now().to_msg()
26
        p.pose.position.x = x; p.pose.position.y = y
27
        p.pose.orientation.z = math.sin(yaw/2); p.pose.orientation.w = math.cos(yaw/2)
28
        return p
29

30
    def loop(self):
31
        if self.battery < 20.0:
32
            self.get_logger().info('电量低，去充电')
33
            self._go(self.charging_station)
34
        else:
35
            for pt in self.patrol_points:
36
                self._go(pt)
37

38
    def _go(self, pose):
39
        self.nav.goToPose(pose)
40
        while not self.nav.isTaskComplete():
41
            rclpy.spin_once(self, timeout_sec=0.1)
42
        if self.nav.getResult() != TaskResult.SUCCEEDED:
43
            self.get_logger().warn('导航失败，重试一次')
44
            self.nav.goToPose(pose)
45
            while not self.nav.isTaskComplete():
46
                rclpy.spin_once(self, timeout_sec=0.1)
47

48
def main():
49
    rclpy.init()
50
    node = Mission()
51
    rclpy.spin(node)
52
    rclpy.shutdown()
53

54
if __name__ == '__main__':
55
    main()

1
# robot_diagnose.py：一键体检
2
import subprocess
3

4
checks = [
5
    ('/odom 频率', 'ros2 topic hz /odom --window 10'),
6
    ('/scan 频率', 'ros2 topic hz /scan --window 10'),
7
    ('TF odom→base_link', 'ros2 run tf2_ros tf2_echo odom base_link'),
8
    ('TF base_link→laser', 'ros2 run tf2_ros tf2_echo base_link laser_link'),
9
    ('节点列表', 'ros2 node list'),
10
    ('串口设备', 'ls -l /dev/ttyUSB*'),
11
]
12

13
for name, cmd in checks:
14
    print(f'===== {name} =====')
15
    try:
16
        out = subprocess.run(cmd.split(), capture_output=True, text=True, timeout=15)
17
        print(out.stdout[-500:] or out.stderr[-500:])
18
    except Exception as e:
19
        print(f'[FAIL] {e}')