ROS 2机器人软件系统

1. ROS 2核心概念：Node/Topic/Service/Action

四种通信机制对比

import rclpy
from rclpy.node import Node
from geometry_msgs.msg import Twist

class VelocityController(Node):
    """发布速度命令的ROS 2节点 — 每个节点独立运行"""
    def __init__(self):
        super().__init__('vel_controller')
        self.pub = self.create_publisher(Twist, '/cmd_vel', 10)
        self.timer = self.create_timer(0.1, self.publish_velocity)
        self.get_logger().info('控制器已启动')

    def publish_velocity(self):
        msg = Twist()
        msg.linear.x = 0.2; msg.angular.z = 0.1
        self.pub.publish(msg)

2. URDF/Xacro：机器人的三维装配体文件

<!-- 2轴机械臂的最小URDF — 每个标签对应一个机械设计概念 -->
<robot name="simple_arm">
  <link name="base_link">
    <visual><geometry><cylinder radius="0.1" length="0.2"/></geometry></visual>
    <inertial>
      <mass value="5.0"/>
      <inertia ixx="0.1" iyy="0.1" izz="0.05" ixy="0" ixz="0" iyz="0"/>
    </inertial>
  </link>

  <joint name="joint1" type="revolute">
    <parent link="base_link"/>
    <child link="link1"/>
    <origin xyz="0 0 0.2"/>
    <axis xyz="0 0 1"/>
    <limit lower="-3.14" upper="3.14" effort="50" velocity="2.0"/>
  </joint>
</robot>

3. MoveIt 2运动规划

规划流程

MoveIt 2是ROS 2生态中机械臂运动规划的标准框架：URDF模型→运动学求解器→碰撞检测→轨迹规划→控制器接口

from moveit_configs_utils import MoveItConfigsBuilder
from moveit.planning import MoveItPy

moveit_config = MoveItConfigsBuilder("franka_emika_panda").to_moveit_configs()
panda = MoveItPy(config_dict=moveit_config)
arm = panda.get_planning_component("panda_arm")

from geometry_msgs.msg import Pose
target = Pose()
target.position.x, target.position.y, target.position.z = 0.4, 0.1, 0.5
target.orientation.w = 1.0

arm.set_start_state_to_current_state()
arm.set_pose_target(target, end_effector_link="panda_hand")

plan_result = arm.plan()
if plan_result:
    arm.execute(plan_result.trajectory, controllers=["panda_arm_controller"])
    print("运动完成!")

4. ros2_control 硬件抽象层

为什么需要ros2_control

你设计过PLC控制系统——知道控制器需要统一接口来管理不同类型的电机驱动。ros2_control就是机器人世界的"PLC抽象层"：不管是伺服电机还是步进电机，上层控制算法都用同样的API。

Control Hardware Interface

from ros2_control.hardware import SystemInterface

class MyRobotHardware(SystemInterface):
    def on_init(self, hardware_info):
        self.num_joints = len(hardware_info.joints)
        self.pos = [0.0] * self.num_joints
        self.vel = [0.0] * self.num_joints
        
    def read(self, time, period):
        # 从编码器读取关节位置
        pass
        
    def write(self, time, period):
        # 将命令写入电机驱动器
        pass

5. Navigation 2 自主导航

Nav2架构

Navigation 2是ROS 2的自主导航框架：全局规划器(Global Planner)→局部控制器(Local Controller)→行为树(Behavior Tree)→恢复行为(Recovery)。

关键配置

# nav2_params.yaml 核心参数
controller_server:
  ros__parameters:
    controller_plugins: ["FollowPath"]
    FollowPath:
      plugin: "dwb_core::DWBLocalPlanner"
      
planner_server:
  ros__parameters:
    planner_plugins: ["GridBased"]
    GridBased:
      plugin: "nav2_navfn_planner/NavfnPlanner"

# 代价地图: 膨胀半径 = 机器人半径 + 安全距离
local_costmap:
  ros__parameters:
    inflation_layer:
      inflation_radius: 0.55  # 你设计的机器人最外轮廓半径+裕量

6. ROS 2 Launch系统——中文机械臂的配置文件版本

Launch文件三种格式

Python Launch完整示例：6轴机械臂+相机+MoveIt

from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, GroupAction
from launch.conditions import IfCondition
from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
from launch_ros.actions import Node, PushRosNamespace
from launch_ros.substitutions import FindPackageShare

def generate_launch_description():
    # 声明启动参数
    use_sim = LaunchConfiguration('use_sim', default='true')
    use_rviz = LaunchConfiguration('use_rviz', default='true')
    
    # 1. 机器人状态发布者（关节状态→TF）
    robot_state_pub = Node(
        package='robot_state_publisher',
        executable='robot_state_publisher',
        parameters=[{
            'robot_description': Command([
                'xacro ', PathJoinSubstitution([
                    FindPackageShare('my_arm'), 'urdf', 'arm.urdf.xacro'
                ])
            ]),
            'use_sim_time': use_sim,
        }]
    )
    
    # 2. Gazebo启动（条件：仅仿真模式）
    gazebo = IncludeLaunchDescription(
        PythonLaunchDescriptionSource([
            FindPackageShare('gazebo_ros'), 'launch', 'gazebo.launch.py'
        ]),
        condition=IfCondition(use_sim),
    )
    
    # 3. 生成URDF模型到Gazebo
    spawn_entity = Node(
        package='gazebo_ros',
        executable='spawn_entity.py',
        arguments=['-entity', 'my_arm', '-topic', 'robot_description'],
        condition=IfCondition(use_sim),
    )
    
    # 4. MoveIt 2运动规划
    moveit = IncludeLaunchDescription(
        PythonLaunchDescriptionSource([
            FindPackageShare('my_arm_moveit_config'), 'launch', 'move_group.launch.py'
        ]),
    )
    
    # 5. RViz可视化
    rviz = Node(
        package='rviz2',
        executable='rviz2',
        arguments=['-d', PathJoinSubstitution([
            FindPackageShare('my_arm_moveit_config'), 'rviz', 'moveit.rviz'
        ])],
        condition=IfCondition(use_rviz),
    )
    
    return LaunchDescription([
        DeclareLaunchArgument('use_sim', default_value='true'),
        DeclareLaunchArgument('use_rviz', default_value='true'),
        robot_state_pub, gazebo, spawn_entity, moveit, rviz,
    ])

Launch系统生命周期管理

ROS 2 Launch最大的改进：自动生命周期管理——节点崩溃后自动重启、退出时自动清理资源。这相当于PLC的"看门狗+自动复位"机制。

# 节点崩溃自动重启配置
from launch.actions import RegisterEventHandler
from launch.event_handlers import OnProcessExit

my_node = Node(package='my_package', executable='my_executable')

# 如果节点退出，自动重启
on_exit_restart = RegisterEventHandler(
    OnProcessExit(
        target_action=my_node,
        on_exit=[my_node],  # 重新启动自身
    )
)

7. Gazebo-ROS 2深度桥接

桥接的核心问题

Gazebo和ROS 2是两套独立的消息系统。gz-msgs和ROS 2 msgs需要双向翻译。ros_gz_bridge就是这个"翻译器"。你设计过PLC与上位机的通讯协议——桥接就是机器人世界的协议转换。

双向消息桥接实战

# ros_gz_bridge.yaml — 定义Gazebo↔ROS 2消息映射

# Gazebo → ROS 2 (传感器数据)
- ros_topic_name: "/camera/image_raw"
  gz_topic_name: "/world/default/model/robot/link/camera/sensor/image"
  ros_type_name: "sensor_msgs/msg/Image"
  gz_type_name: "gz.msgs.Image"
  direction: GZ_TO_ROS

- ros_topic_name: "/joint_states"
  gz_topic_name: "/world/default/model/robot/joint_state"
  ros_type_name: "sensor_msgs/msg/JointState"
  gz_type_name: "gz.msgs.Model"
  direction: GZ_TO_ROS

# ROS 2 → Gazebo (控制命令)
- ros_topic_name: "/arm_controller/commands"
  gz_topic_name: "/world/default/model/robot/joint/arm_controller/cmd"
  ros_type_name: "std_msgs/msg/Float64MultiArray"
  gz_type_name: "gz.msgs.Double"
  direction: ROS_TO_GZ

Gazebo传感器仿真——深度相机

<!-- URDF中添加RGB-D相机传感器 -->
<gazebo reference="camera_link">
  <sensor type="rgbd_camera" name="camera">
    <update_rate>30.0</update_rate>
    <camera>
      <horizontal_fov>1.047</horizontal_fov>  <!-- 60度 -->
      <image>
        <width>640</width>
        <height>480</height>
        <format>R8G8B8</format>
      </image>
      <clip>
        <near>0.05</near>
        <far>8.0</far>
      </clip>
    </camera>
    <depth_camera>
      <output>points</output>
    </depth_camera>
  </sensor>
</gazebo>

Service vs Action：什么时候用哪个？

8. rosbag2——机器人世界的"黑匣子"

录制和回放

# 录制指定话题
# ros2 bag record /joint_states /camera/image_raw /arm_controller/commands

# 录制所有话题（含隐藏话题）
# ros2 bag record -a --include-hidden-topics

# 回放（模拟原始数据流）
# ros2 bag play my_recording/

Python API：程序化处理bag数据

import rosbag2_py
from rclpy.serialization import deserialize_message
from sensor_msgs.msg import JointState

def extract_joint_trajectory(bag_path):
    """从rosbag中提取关节轨迹用于训练模仿学习"""
    reader = rosbag2_py.SequentialReader()
    storage_options = rosbag2_py.StorageOptions(uri=bag_path, storage_id='mcap')
    reader.open(storage_options, rosbag2_py.ConverterOptions())
    
    trajectory = []
    while reader.has_next():
        topic, data, t = reader.read_next()
        if topic == '/joint_states':
            msg = deserialize_message(data, JointState)
            trajectory.append((t, msg.position))
    
    return trajectory

# 常用数据格式转换
# MCAP: 新的默认格式（支持零拷贝、压缩、索引）
# SQLite3: 旧格式（仍广泛使用）

rosbag2在机器人学习中的应用

模仿学习的数据采集流程：

1. 遥操作演示 → rosbag2录制所有话题
2. 离线处理 → 提取(图像,关节状态)对
3. 数据增强 → 旋转/裁剪/颜色扰动
4. 训练模型 → 行为克隆/ACT/Diffusion Policy
5. 部署验证 → 重新录制，对比策略输出与人类演示