# 项目四：ROS 2 移动机器人最小系统 ## 项目概述 从零构建一个ROS 2移动机器人系统，覆盖： - ROS 2节点、话题、服务、Action - 里程计（轮式里程计 + IMU融合） - 激光雷达SLAM (slam_toolbox) - 导航 (Nav2) - RViz2可视化 - Gazebo仿真 ## 难度：★★★☆☆ (3/5) ## 预估时间：3-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 │ │ (全局规划 + 局部控制) │ └──────────────────────────────┘ ``` --- ## 2. 环境配置 ```bash # 安装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 ``` --- ## 3. 自定义机器人节点 ### 3.1 里程计发布节点 ```python #!/usr/bin/env python3 """里程计发布节点：从轮速编码器计算里程计""" import rclpy from rclpy.node import Node from nav_msgs.msg import Odometry from sensor_msgs.msg import JointState from geometry_msgs.msg import TransformStamped import tf2_ros import math import numpy as np class OdomPublisher(Node): def __init__(self): super().__init__('odom_publisher') self.tf_broadcaster = tf2_ros.TransformBroadcaster(self) self.odom_pub = self.create_publisher(Odometry, '/odom', 10) self.joint_sub = self.create_subscription( JointState, '/joint_states', self.joint_state_callback, 10 ) # 机器人参数 (TurtleBot3 waffle) self.wheel_radius = 0.033 # 轮半径 [m] self.wheel_separation = 0.287 # 轮距 [m] # 位姿状态 self.x = 0.0; self.y = 0.0; self.theta = 0.0 self.last_left_pos = 0.0; self.last_right_pos = 0.0 self.last_time = self.get_clock().now() def joint_state_callback(self, msg): # 找到左右轮的关节位置 try: left_idx = msg.name.index('wheel_left_joint') right_idx = msg.name.index('wheel_right_joint') except ValueError: return left_pos = msg.position[left_idx] right_pos = msg.position[right_idx] current_time = self.get_clock().now() # 计算轮子转动量 d_left = left_pos - self.last_left_pos d_right = right_pos - self.last_right_pos # 差速运动学 d_linear = self.wheel_radius * (d_left + d_right) / 2.0 d_angular = self.wheel_radius * (d_right - d_left) / self.wheel_separation dt = (current_time - self.last_time).nanoseconds / 1e9 # 更新位姿 if abs(d_angular) < 1e-6: self.x += d_linear * math.cos(self.theta) self.y += d_linear * math.sin(self.theta) else: self.x += (d_linear / d_angular) * \ (math.sin(self.theta + d_angular) - math.sin(self.theta)) self.y += (d_linear / d_angular) * \ (math.cos(self.theta) - math.cos(self.theta + d_angular)) 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 = self.x odom.pose.pose.position.y = self.y # 四元数 odom.pose.pose.orientation.z = math.sin(self.theta / 2) odom.pose.pose.orientation.w = math.cos(self.theta / 2) odom.twist.twist.linear.x = d_linear / dt if dt > 0 else 0.0 odom.twist.twist.angular.z = d_angular / dt if dt > 0 else 0.0 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 = self.x tf.transform.translation.y = self.y tf.transform.rotation = odom.pose.pose.orientation self.tf_broadcaster.sendTransform(tf) # 保存状态 self.last_left_pos = left_pos self.last_right_pos = right_pos self.last_time = current_time def main(): rclpy.init() node = OdomPublisher() rclpy.spin(node) node.destroy_node() rclpy.shutdown() if __name__ == '__main__': main() ``` ### 3.2 键盘遥控节点 ```python #!/usr/bin/env python3 """键盘遥控：wasd控制机器人""" import rclpy from rclpy.node import Node from geometry_msgs.msg import Twist import sys import termios import tty class KeyboardTeleop(Node): def __init__(self): super().__init__('keyboard_teleop') self.pub = self.create_publisher(Twist, '/cmd_vel', 10) self.get_logger().info("键盘遥控已启动: w/s=前进/后退, a/d=左转/右转, q=退出") def get_key(self): fd = sys.stdin.fileno() old = termios.tcgetattr(fd) try: tty.setraw(fd) ch = sys.stdin.read(1) finally: termios.tcsetattr(fd, termios.TCSADRAIN, old) return ch def run(self): linear = 0.0 angular = 0.0 while rclpy.ok(): key = self.get_key() if key == 'w': linear = 0.2 elif key == 's': linear = -0.2 elif key == 'a': angular = 0.5 elif key == 'd': angular = -0.5 elif key == 'x': linear = 0.0; angular = 0.0 elif key == 'q': break twist = Twist() twist.linear.x = linear twist.angular.z = angular self.pub.publish(twist) def main(): rclpy.init() node = KeyboardTeleop() try: node.run() except KeyboardInterrupt: pass node.destroy_node() rclpy.shutdown() ``` ### 3.3 launch文件 ```python # launch/sim_robot.launch.py from launch import LaunchDescription from launch_ros.actions import Node from launch.actions import IncludeLaunchDescription from launch.launch_description_sources import PythonLaunchDescriptionSource from launch.substitutions import PathJoinSubstitution from launch_ros.substitutions import FindPackageShare def generate_launch_description(): 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': True}], ), # Rviz2 Node( package='rviz2', executable='rviz2', name='rviz2', ), ]) ``` --- ## 4. Nav2 导航配置 ```yaml # 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" ``` --- ## 5. 最小导航示例 ```python #!/usr/bin/env python3 """使用Nav2 Action进行导航""" import rclpy from rclpy.node import Node from rclpy.action import ActionClient from nav2_msgs.action import NavigateToPose from geometry_msgs.msg import PoseStamped class SimpleNavigator(Node): def __init__(self): super().__init__('simple_navigator') self.action_client = ActionClient(self, NavigateToPose, 'navigate_to_pose') def send_goal(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 import math goal_msg.pose.pose.orientation.z = math.sin(yaw / 2) goal_msg.pose.pose.orientation.w = math.cos(yaw / 2) self.action_client.wait_for_server() future = self.action_client.send_goal_async(goal_msg) future.add_done_callback(self.goal_response_callback) def goal_response_callback(self, future): goal_handle = future.result() if not goal_handle.accepted: self.get_logger().info('目标被拒绝') return self.get_logger().info('目标已接受，执行中...') result_future = goal_handle.get_result_async() result_future.add_done_callback(self.result_callback) def result_callback(self, future): result = future.result().result self.get_logger().info(f'导航完成: {result}') 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() ``` --- ## 6. 验收标准 1. **Gazebo仿真**：在Gazebo中启动TurtleBot3，能通过键盘遥控 2. **里程计**：里程计漂移 < 5% (10m直线移动后) 3. **SLAM建图**：用slam_toolbox建图，地图尺寸误差 < 10cm 4. **自主导航**：在已知地图中，Nav2导航成功率 > 90% (目标误差 < 10cm) 5. **代码质量**：所有节点通过ros2 launch启动，无hardcode路径 --- ## 7. 常见坑 - **TF树不完整**：检查 `ros2 run tf2_tools view_frames` - **时间同步**：仿真中所有节点必须设置 `use_sim_time:=true` - **costmap参数**：`robot_radius` 或 `footprint` 必须正确 - **Gazebo模型路径**：export `GAZEBO_MODEL_PATH` ## 参考资源 - [ROS 2 Tutorials](https://docs.ros.org/en/humble/Tutorials.html) - [Nav2 Documentation](https://navigation.ros.org/) - [Gazebo + ROS 2](https://gazebosim.org/docs/latest/ros2_integration/)