ROS2 机器人应用开发工程师：不是"会写节点"，而是能交付可维护的机器人应用，附工程代码

# warehouse_interfaces/action/FetchItem.action
# 定义长任务契约
string item_id
string target_zone
---
bool success
string message
---
float32 progress
string current_phase

# mission_manager.py —— 应用层编排，不碰底层驱动
import rclpy
from rclpy.action import ActionClient
from warehouse_interfaces.action import FetchItem, NavigateToPose

class MissionManager(rclpy.node.Node):
    def __init__(self):
        super().__init__('mission_manager')
        self.nav = ActionClient(self, NavigateToPose, 'navigate_to_pose')
        self.fetch = ActionClient(self, FetchItem, 'fetch_item')
        self.nav.wait_for_server(); self.fetch.wait_for_server()

    def run_mission(self, item_id, zone):
        # 编排：导航→抓取→返航，每步检查结果
        if not self._call(self.nav, NavigateToPose.Goal(zone=zone)):
            return self._abort("导航失败")
        if not self._call(self.fetch, FetchItem.Goal(item_id=item_id)):
            return self._abort("抓取失败")
        return self._call(self.nav, NavigateToPose.Goal(zone="dock"))

    def _call(self, client, goal):
        fut = client.send_goal_async(goal)
        rclpy.spin_until_future_complete(self, fut)
        gh = fut.result()
        if not gh.accepted: return False
        res = gh.get_result_async()
        rclpy.spin_until_future_complete(self, res)
        return res.result().result.success

# bringup.launch.py
from launch import LaunchDescription
from launch_ros.actions import LifecycleNode, Node
from launch.actions import DeclareLaunchArgument, GroupAction
from launch.substitutions import LaunchConfiguration

def generate_launch_description():
    robot_id = LaunchConfiguration('robot_id')
    sim = LaunchConfiguration('sim')
    return LaunchDescription([
        DeclareLaunchArgument('robot_id', default_value='r1'),
        DeclareLaunchArgument('sim', default_value='false'),
        GroupAction([
            LifecycleNode(package='motor_driver', executable='motor_driver',
                          namespace=robot_id, name='motor_driver',
                          parameters=[{'can_interface': 'can0'}]),
            Node(package='nav2_bringup', executable='navigation_launch.py',
                 namespace=robot_id,
                 parameters=[{'use_sim_time': sim}]),
            Node(package='mission', executable='mission_manager',
                 namespace=robot_id),
        ])
    ])

# 1. 检查 tf 树是否连通
ros2 run tf2_tools view_frames
ros2 run tf2_ros tf2_echo map base_link   # 有输出才连通

# 2. 检查关键话题频率和延迟
ros2 topic hz /scan --window 50
ros2 topic delay /tf

# 3. 复现问题：录包带所有话题
ros2 bag record -a -o bug_repro_$(date +%s)

# health_monitor.py —— 上线必备：监控关键话题存活与频率
class HealthMonitor(rclpy.node.Node):
    def __init__(self):
        super().__init__('health_monitor')
        self.last = {}
        for t in ['/scan', '/odom', '/cmd_vel']:
            self.create_subscription(Empty, t, lambda m, t=t: self.last.update({t: self.now()}), 10)
        self.create_timer(1.0, self.check)

    def check(self):
        for t, ts in self.last.items():
            if (self.now() - ts).nanoseconds / 1e9 > 2.0:
                self.get_logger().error(f'{t} 超时未更新，触发降级')

# config/robot_r1.yaml
motor_driver:
  ros__parameters:
    can_interface: can0
    wheel_radius: 0.075
nav2:
  ros__parameters:
    robot_radius: 0.3
    max_vel: 0.8

# 加载时按 robot_id 选配置
self.declare_parameter('config_file', '')
params = yaml.safe_load(open(self.get_parameter('config_file').value))

# regression_test.py
import subprocess, pytest

SCENARIOS = ["pick_deliver", "obstacle_avoid", "low_battery_return"]

@pytest.mark.parametrize("scene", SCENARIOS)
def test_scenario(scene):
    subprocess.run(["ros2","launch","sim","sim.launch.py",
                    f"scenario:={scene}"], check=True, timeout=180)
    out = subprocess.run(["ros2","topic","echo","--once","/mission/result"],
                         capture_output=True, text=True)
    assert "success: true" in out.stdout

# 统一日志落盘 + 远程诊断话题
self.create_subscription(DiagnosticArray, '/diagnostics', self.on_diag, 10)
self.create_service(Trigger, '~/restart_mission', self.restart_cb)