中级 · 3-4周

项目四：ROS 2 移动机器人最小系统

从零构建ROS 2移动机器人系统：节点通信、SLAM建图、Nav2导航与Gazebo仿真

中级难度预计3-4周 11个章节

1. 项目概述

从零构建一个ROS 2移动机器人系统，覆盖：

ROS 2节点、话题、服务、Action

里程计（轮式里程计 + IMU融合）

激光雷达SLAM (slam_toolbox)

导航 (Nav2)

RViz2可视化

Gazebo仿真

2. 难度：★★★☆☆ (3/5)

3. 预估时间：3-4周

---

4. 1. 系统架构

                    ┌─────────────┐
                    │   /cmd_vel  │  ← 键盘遥控/Nav2
                    └──────┬──────┘
                           │
              ┌────────────▼────────────┐
              │    diff_drive_controller │
              │    (话题 → 轮速)         │
              └────────┬─────┬──────────┘
                       │     │
           ┌───────────▼┐ ┌─▼──────────┐
           │  left_wheel │ │ right_wheel│
           │  /joint_states              │
           └──────┬──────┘ └──────┬─────┘
                  │               │
          ┌───────▼───────────────▼──────┐
          │     robot_state_publisher    │
          │     (joint_states → TF)      │
          └──────────────┬───────────────┘
                         │
          ┌──────────────▼───────────────┐
          │        /odom (里程计)        │
          │        /scan (激光雷达)      │
          │        /imu  (IMU数据)      │
          └──────────────┬───────────────┘
                         │
          ┌──────────────▼───────────────┐
          │       slam_toolbox           │
          │       (建图)                  │
          └──────────────┬───────────────┘
                         │
          ┌──────────────▼───────────────┐
          │         Nav2                  │
          │    (全局规划 + 局部控制)      │
          └──────────────────────────────┘

---

5. 2. 环境配置

# 安装ROS 2 Humble (Ubuntu 22.04)
sudo apt update
sudo apt install ros-humble-desktop
sudo apt install ros-humble-gazebo-ros2-control
sudo apt install ros-humble-slam-toolbox
sudo apt install ros-humble-navigation2
sudo apt install ros-humble-nav2-bringup
sudo apt install ros-humble-turtlebot3*

# 配置环境
echo "source /opt/ros/humble/setup.bash" >> ~/.bashrc
echo "export TURTLEBOT3_MODEL=waffle" >> ~/.bashrc
source ~/.bashrc

# 创建工作空间
mkdir -p ~/ros2_ws/src
cd ~/ros2_ws
colcon build --symlink-install

---

6. 3. 自定义机器人节点

3.1 里程计发布节点

#!/usr/bin/env python3
"""里程计发布节点：从轮速编码器计算里程计"""
"kw">class="kw">import rclpy
"kw">from rclpy.node "kw">class="kw">import Node
"kw">from nav_msgs.msg "kw">class="kw">import Odometry
"kw">from sensor_msgs.msg "kw">class="kw">import JointState
"kw">from geometry_msgs.msg "kw">class="kw">import TransformStamped
"kw">class="kw">import tf2_ros
"kw">class="kw">import math
"kw">class="kw">import numpy "kw">as np

"kw">class OdomPublisher(Node):
    "kw">def __init__("kw">self):
        super().__init__('odom_publisher')
        "kw">self.tf_broadcaster = tf2_ros.TransformBroadcaster("kw">self)
        "kw">self.odom_pub = "kw">self.create_publisher(Odometry, '/odom', 10)
        "kw">self.joint_sub = "kw">self.create_subscription(
            JointState, '/joint_states', "kw">self.joint_state_callback, 10
        )

        # 机器人参数 (TurtleBot3 waffle)
        "kw">self.wheel_radius = 0.033      # 轮半径 [m]
        "kw">self.wheel_separation = 0.287  # 轮距 [m]

        # 位姿状态
        "kw">self.x = 0.0; "kw">self.y = 0.0; "kw">self.theta = 0.0
        "kw">self.last_left_pos = 0.0; "kw">self.last_right_pos = 0.0
        "kw">self.last_time = "kw">self.get_clock().now()

    "kw">def joint_state_callback("kw">self, msg):
        # 找到左右轮的关节位置
        "kw">try:
            left_idx = msg.name.index('wheel_left_joint')
            right_idx = msg.name.index('wheel_right_joint')
        "kw">class="kw">except ValueError:
            "kw">class="kw">return

        left_pos = msg.position[left_idx]
        right_pos = msg.position[right_idx]
        current_time = "kw">self.get_clock().now()

        # 计算轮子转动量
        d_left = left_pos - "kw">self.last_left_pos
        d_right = right_pos - "kw">self.last_right_pos

        # 差速运动学
        d_linear = "kw">self.wheel_radius * (d_left + d_right) / 2.0
        d_angular = "kw">self.wheel_radius * (d_right - d_left) / "kw">self.wheel_separation

        dt = (current_time - "kw">self.last_time).nanoseconds / 1e9

        # 更新位姿
        "kw">if abs(d_angular) < 1e-6:
            "kw">self.x += d_linear * math.cos("kw">self.theta)
            "kw">self.y += d_linear * math.sin("kw">self.theta)
        "kw">else:
            "kw">self.x += (d_linear / d_angular) * \
                      (math.sin("kw">self.theta + d_angular) - math.sin("kw">self.theta))
            "kw">self.y += (d_linear / d_angular) * \
                      (math.cos("kw">self.theta) - math.cos("kw">self.theta + d_angular))
        "kw">self.theta += d_angular

        # 发布里程计
        odom = Odometry()
        odom.header.stamp = current_time.to_msg()
        odom.header.frame_id = 'odom'
        odom.child_frame_id = 'base_footprint'

        odom.pose.pose.position.x = "kw">self.x
        odom.pose.pose.position.y = "kw">self.y
        # 四元数
        odom.pose.pose.orientation.z = math.sin("kw">self.theta / 2)
        odom.pose.pose.orientation.w = math.cos("kw">self.theta / 2)

        odom.twist.twist.linear.x = d_linear / dt "kw">if dt > 0 "kw">else 0.0
        odom.twist.twist.angular.z = d_angular / dt "kw">if dt > 0 "kw">else 0.0

        "kw">self.odom_pub.publish(odom)

        # 发布TF (odom → base_footprint)
        tf = TransformStamped()
        tf.header = odom.header
        tf.child_frame_id = 'base_footprint'
        tf.transform.translation.x = "kw">self.x
        tf.transform.translation.y = "kw">self.y
        tf.transform.rotation = odom.pose.pose.orientation
        "kw">self.tf_broadcaster.sendTransform(tf)

        # 保存状态
        "kw">self.last_left_pos = left_pos
        "kw">self.last_right_pos = right_pos
        "kw">self.last_time = current_time

"kw">def main():
    rclpy.init()
    node = OdomPublisher()
    rclpy.spin(node)
    node.destroy_node()
    rclpy.shutdown()

"kw">if __name__ == '__main__':
    main()

3.2 键盘遥控节点

#!/usr/bin/env python3
"""键盘遥控：wasd控制机器人"""
"kw">class="kw">import rclpy
"kw">from rclpy.node "kw">class="kw">import Node
"kw">from geometry_msgs.msg "kw">class="kw">import Twist
"kw">class="kw">import sys
"kw">class="kw">import termios
"kw">class="kw">import tty

"kw">class KeyboardTeleop(Node):
    "kw">def __init__("kw">self):
        super().__init__('keyboard_teleop')
        "kw">self.pub = "kw">self.create_publisher(Twist, '/cmd_vel', 10)
        "kw">self.get_logger().info("键盘遥控已启动: w/s=前进/后退, a/d=左转/右转, q=退出")

    "kw">def get_key("kw">self):
        fd = sys.stdin.fileno()
        old = termios.tcgetattr(fd)
        "kw">try:
            tty.setraw(fd)
            ch = sys.stdin.read(1)
        finally:
            termios.tcsetattr(fd, termios.TCSADRAIN, old)
        "kw">class="kw">return ch

    "kw">def run("kw">self):
        linear = 0.0
        angular = 0.0

        "kw">while rclpy.ok():
            key = "kw">self.get_key()
            "kw">if key == 'w': linear = 0.2
            "kw">elif key == 's': linear = -0.2
            "kw">elif key == 'a': angular = 0.5
            "kw">elif key == 'd': angular = -0.5
            "kw">elif key == 'x': linear = 0.0; angular = 0.0
            "kw">elif key == 'q': "kw">break

            twist = Twist()
            twist.linear.x = linear
            twist.angular.z = angular
            "kw">self.pub.publish(twist)

"kw">def main():
    rclpy.init()
    node = KeyboardTeleop()
    "kw">try:
        node.run()
    "kw">class="kw">except KeyboardInterrupt:
        "kw">pass
    node.destroy_node()
    rclpy.shutdown()

3.3 launch文件

# launch/sim_robot.launch.py
"kw">from launch "kw">class="kw">import LaunchDescription
"kw">from launch_ros.actions "kw">class="kw">import Node
"kw">from launch.actions "kw">class="kw">import IncludeLaunchDescription
"kw">from launch.launch_description_sources "kw">class="kw">import PythonLaunchDescriptionSource
"kw">from launch.substitutions "kw">class="kw">import PathJoinSubstitution
"kw">from launch_ros.substitutions "kw">class="kw">import FindPackageShare

"kw">def generate_launch_description():
    "kw">class="kw">return LaunchDescription([
        # Gazebo
        IncludeLaunchDescription(
            PythonLaunchDescriptionSource([
                PathJoinSubstitution([
                    FindPackageShare('gazebo_ros'),
                    'launch', 'gazebo.launch.py'
                ])
            ])
        ),

        # 里程计
        Node(
            package='my_robot',
            executable='odom_publisher.py',
            name='odom_publisher',
        ),

        # SLAM
        Node(
            package='slam_toolbox',
            executable='async_slam_toolbox_node',
            name='slam_toolbox',
            parameters=[{'use_sim_time': "kw">True}],
        ),

        # Rviz2
        Node(
            package='rviz2',
            executable='rviz2',
            name='rviz2',
        ),
    ])

---

7. 4. Nav2 导航配置

# config/nav2_params.yaml
controller_server:
  ros__parameters:
    controller_frequency: 20.0
    FollowPath:
      plugin: "dwb_core::DWBLocalPlanner"
      max_vel_x: 0.26
      min_vel_x: -0.26
      max_vel_theta: 1.0
      min_vel_theta: -1.0

planner_server:
  ros__parameters:
    planner_plugins: ["GridBased"]
    GridBased:
      plugin: "nav2_navfn_planner/NavfnPlanner"

behavior_server:
  ros__parameters:
    costmap_topic: /global_costmap/costmap_raw
    footprint_topic: /global_costmap/published_footprint
    cycle_frequency: 10.0
    behavior_plugins: ["spin", "backup", "drive_on_heading", "wait"]
    spin:
      plugin: "nav2_behaviors/Spin"
    backup:
      plugin: "nav2_behaviors/BackUp"

---

8. 5. 最小导航示例

#!/usr/bin/env python3
"""使用Nav2 Action进行导航"""
"kw">class="kw">import rclpy
"kw">from rclpy.node "kw">class="kw">import Node
"kw">from rclpy.action "kw">class="kw">import ActionClient
"kw">from nav2_msgs.action "kw">class="kw">import NavigateToPose
"kw">from geometry_msgs.msg "kw">class="kw">import PoseStamped

"kw">class SimpleNavigator(Node):
    "kw">def __init__("kw">self):
        super().__init__('simple_navigator')
        "kw">self.action_client = ActionClient("kw">self, NavigateToPose, 'navigate_to_pose')

    "kw">def send_goal("kw">self, x, y, yaw):
        goal_msg = NavigateToPose.Goal()
        goal_msg.pose = PoseStamped()
        goal_msg.pose.header.frame_id = 'map'

        goal_msg.pose.pose.position.x = x
        goal_msg.pose.pose.position.y = y

        "kw">class="kw">import math
        goal_msg.pose.pose.orientation.z = math.sin(yaw / 2)
        goal_msg.pose.pose.orientation.w = math.cos(yaw / 2)

        "kw">self.action_client.wait_for_server()
        future = "kw">self.action_client.send_goal_async(goal_msg)
        future.add_done_callback("kw">self.goal_response_callback)

    "kw">def goal_response_callback("kw">self, future):
        goal_handle = future.result()
        "kw">if "kw">not goal_handle.accepted:
            "kw">self.get_logger().info('目标被拒绝')
            "kw">class="kw">return
        "kw">self.get_logger().info('目标已接受，执行中...')

        result_future = goal_handle.get_result_async()
        result_future.add_done_callback("kw">self.result_callback)

    "kw">def result_callback("kw">self, future):
        result = future.result().result
        "kw">self.get_logger().info(f'导航完成: {result}')

"kw">def main():
    rclpy.init()
    nav = SimpleNavigator()

    # 导航到几个目标点
    nav.send_goal(1.0, 0.0, 0.0)   # (x=1, y=0, heading=0)
    nav.send_goal(1.0, 1.0, 1.57)  # (x=1, y=1, heading=90°)
    nav.send_goal(0.0, 0.0, 0.0)   # 回原点

    rclpy.spin(nav)
    nav.destroy_node()
    rclpy.shutdown()