🤖 第16课：ROS2集成

📌 ROS2集成概述

ROS2是服务机器人事实上的中间件标准，提供节点通信、参数管理、生命周期等基础能力：

📌 ROS2节点通信模拟

class ROS2NodeSimulator:
    """ROS2节点通信模拟"""
    def __init__(self):
        self.nodes = {}
        self.topics = {}
        self.services = {}
        self.log = []

    def create_node(self, name):
        node = {"name": name, "publishers": [], "subscribers": [], "services": []}
        self.nodes[name] = node
        self.log.append(f"[创建节点] {name}")
        return node

    def create_publisher(self, node_name, topic, msg_type):
        self.topics.setdefault(topic, {"pubs": [], "subs": [], "msg_type": msg_type})
        self.topics[topic]["pubs"].append(node_name)
        self.nodes[node_name]["publishers"].append(topic)
        self.log.append(f"[发布者] {node_name} → {topic} ({msg_type})")

    def create_subscriber(self, node_name, topic, msg_type, callback=None):
        self.topics.setdefault(topic, {"pubs": [], "subs": [], "msg_type": msg_type})
        self.topics[topic]["subs"].append(node_name)
        self.nodes[node_name]["subscribers"].append(topic)
        self.log.append(f"[订阅者] {node_name} ← {topic} ({msg_type})")

    def create_service(self, node_name, service_name, srv_type):
        self.services[service_name] = {"server": node_name, "srv_type": srv_type}
        self.nodes[node_name]["services"].append(service_name)
        self.log.append(f"[服务] {node_name} 提供 {service_name} ({srv_type})")

    def publish(self, topic, msg):
        if topic in self.topics:
            subs = self.topics[topic]["subs"]
            self.log.append(f"[消息] {topic}: {msg} → {len(subs)}个订阅者")
            return len(subs)
        return 0

    def call_service(self, service_name, request):
        if service_name in self.services:
            self.log.append(f"[服务调用] {service_name}: {request}")
            return {"success": True, "result": f"{service_name}处理完成"}
        return {"success": False, "result": "服务不存在"}

    def get_graph(self):
        return {"nodes": len(self.nodes), "topics": len(self.topics), "services": len(self.services)}

ros = ROS2NodeSimulator()

# 创建导航系统节点
ros.create_node("nav_server")
ros.create_node("cmd_vel_mux")
ros.create_node("laser_filter")
ros.create_node("costmap_2d")
ros.create_node("robot_controller")

ros.create_publisher("laser_filter", "/scan_filtered", "LaserScan")
ros.create_subscriber("costmap_2d", "/scan_filtered", "LaserScan")
ros.create_publisher("costmap_2d", "/costmap", "OccupancyGrid")
ros.create_subscriber("nav_server", "/costmap", "OccupancyGrid")
ros.create_publisher("nav_server", "/plan", "Path")
ros.create_subscriber("robot_controller", "/plan", "Path")
ros.create_publisher("robot_controller", "/cmd_vel", "Twist")
ros.create_subscriber("cmd_vel_mux", "/cmd_vel", "Twist")

ros.create_service("nav_server", "/navigate_to_pose", "NavigateToPose")
ros.create_service("costmap_2d", "/clear_costmap", "ClearCostmap")

print("ROS2导航系统集成模拟")
print("=" * 55)
for entry in ros.log:
    print(f"  {entry}")

print(f"\n计算图: {ros.get_graph()}")
result = ros.call_service("/navigate_to_pose", {"x": 5.0, "y": 3.0, "theta": 0})
print(f"服务调用: {result}")
ros.publish("/cmd_vel", "linear: {x: 0.5}, angular: {z: 0.0}")
print("✅ ROS2集成验证通过")

📌 Launch与部署配置

class ROS2LaunchConfig:
    """ROS2 Launch配置生成"""
    def __init__(self):
        self.nodes = []
        self.params = {}
        self.remappings = {}

    def add_node(self, pkg, executable, name, output="screen", params=None):
        self.nodes.append({
            "pkg": pkg, "exec": executable, "name": name,
            "output": output, "params": params or {}
        })

    def generate_launch_file(self):
        """生成launch.py内容"""
        lines = [
            "from launch import LaunchDescription",
            "from launch_ros.actions import Node",
            "",
            "def generate_launch_description():",
            "    return LaunchDescription(["
        ]
        for node in self.nodes:
            params_str = ""
            if node["params"]:
                params_str = f", parameters={node['params']}"
            lines.append(f'        Node(')
            lines.append(f'            package="{node["pkg"]}",')
            lines.append(f'            executable="{node["exec"]}",')
            lines.append(f'            name="{node["name"]}",')
            lines.append(f'            output="{node["output"]}"{params_str}')
            lines.append(f'        ),')
        lines.append("    ])")
        return "\n".join(lines)

    def generate_docker_compose(self):
        """生成docker-compose.yml"""
        services = []
        for node in self.nodes:
            svc = f"""  {node['name']}:
    image: service-robot:latest
    command: ros2 run {node['pkg']} {node['exec']}
    network_mode: host
    environment:
      - ROS_DOMAIN_ID=30"""
            services.append(svc)
        return "version: '3'\nservices:\n" + "\n".join(services)

config = ROS2LaunchConfig()
config.add_node("nav2_controller", "controller_server", "controller_server",
                params={"controller_frequency": 10.0, "FollowPath": {"plugin": "dwb_core::DWBLocalPlanner"}})
config.add_node("nav2_planner", "planner_server", "planner_server",
                params={"planner_plugins": ["GridBased"], "GridBased": {"plugin": "nav2_navfn_planner/NavfnPlanner"}})
config.add_node("nav2_bt_navigator", "bt_navigator", "bt_navigator",
                params={"default_nav_to_pose_bt_xml": "navigate_to_pose_w_replanning_and_recovery.xml"})
config.add_node("robot_base", "diff_drive_controller", "base_controller",
                params={"wheel_separation": 0.4, "wheel_radius": 0.08})
config.add_node("sensors", "laser_scan_filter", "scan_filter")

print("ROS2 Launch配置生成")
print("=" * 55)
print("\n📋 launch.py:")
print(config.generate_launch_file()[:600] + "\n...")
print("\n📋 docker-compose.yml:")
print(config.generate_docker_compose()[:500] + "\n...")
print("\n✅ Launch配置验证通过")

📌 QoS策略配置

class QoSPolicy:
    """ROS2 QoS策略配置"""
    def __init__(self):
        self.policies = {
            "sensor_data": {
                "reliability": "BEST_EFFORT",
                "durability": "VOLATILE",
                "history": "KEEP_LAST", "depth": 5,
                "use_case": "激光雷达、IMU等高频传感器数据"
            },
            "reliable": {
                "reliability": "RELIABLE",
                "durability": "TRANSIENT_LOCAL",
                "history": "KEEP_LAST", "depth": 10,
                "use_case": "地图、参数等关键数据"
            },
            "default": {
                "reliability": "RELIABLE",
                "durability": "VOLATILE",
                "history": "KEEP_LAST", "depth": 10,
                "use_case": "一般话题如cmd_vel"
            },
            "service_call": {
                "reliability": "RELIABLE",
                "durability": "VOLATILE",
                "history": "KEEP_LAST", "depth": 1,
                "use_case": "服务调用和动作目标"
            },
        }
        self.topic_qos = {
            "/scan": "sensor_data",
            "/scan_filtered": "sensor_data",
            "/cmd_vel": "default",
            "/map": "reliable",
            "/plan": "default",
            "/costmap": "sensor_data",
            "/navigate_to_pose": "service_call",
        }

    def get_qos(self, topic):
        policy_name = self.topic_qos.get(topic, "default")
        return self.policies[policy_name]

    def check_compatibility(self, pub_qos, sub_qos):
        """检查发布者和订阅者QoS兼容性"""
        if pub_qos["reliability"] == "RELIABLE" and sub_qos["reliability"] == "BEST_EFFORT":
            return {"compatible": True, "note": "RELIABLE→BEST_EFFORT 兼容，但丢失可靠性保证"}
        if pub_qos["reliability"] == "BEST_EFFORT" and sub_qos["reliability"] == "RELIABLE":
            return {"compatible": False, "note": "BEST_EFFORT→RELIABLE 不兼容！"}
        if pub_qos["durability"] == "TRANSIENT_LOCAL" and sub_qos["durability"] == "VOLATILE":
            return {"compatible": True, "note": "晚加入订阅者可能错过消息"}
        return {"compatible": True, "note": "完全兼容"}

qos = QoSPolicy()
print("ROS2 QoS策略配置")
print("=" * 55)

print("\n📋 话题QoS配置:")
for topic, policy_name in qos.topic_qos.items():
    policy = qos.get_qos(topic)
    print(f"  {topic:25s} → {policy_name}: {policy['reliability']}/{policy['durability']}")
    print(f"    用途: {policy['use_case']}")

print("\n📋 QoS兼容性检查:")
checks = [
    ("/scan", "sensor_data", "default"),
    ("/map", "reliable", "sensor_data"),
    ("/cmd_vel", "default", "reliable"),
]
for topic, pub_policy, sub_policy in checks:
    result = qos.check_compatibility(qos.policies[pub_policy], qos.policies[sub_policy])
    status = "✅" if result["compatible"] else "❌"
    print(f"  {status} {topic}: {pub_policy}→{sub_policy}: {result['note']}")

print("\n✅ QoS策略验证通过")

📌 ROS2 vs ROS1

📌 练习

📌 成就