feat: init sim_smart_car_bridge — ROS2 bridge relay for Origincar HTTP API

Bridge image (GET /image → sensor_msgs/Image), IMU (GET /imu → sensor_msgs/Imu), and cmd_vel (Twist → POST /cmd) between Origincar simulation and ROS2. Co-Authored-By: Claude Opus 4.8 <noreply@anthropic.com>
2026-06-16 22:57:18 +08:00
commit 954ca7f8a7
9 changed files with 557 additions and 0 deletions
--- a/.gitignore
+++ b/.gitignore
@@ -0,0 +1,21 @@
+# Python
+__pycache__/
+*.py[cod]
+*.egg-info/
+
+# Build
+build/
+dist/
+*.egg
+
+# IDE
+.vscode/
+.idea/
+
+# ROS2
+log/
+install/
+
+# OS
+.DS_Store
+Thumbs.db
--- a/README.md
+++ b/README.md
@@ -0,0 +1,95 @@
+# sim_smart_car_bridge
+
+ROS2 桥接中转节点：将 Origincar 仿真的 HTTP API 与 ROS2 话题双向桥接。
+
+## 数据流
+
+```
+仿真机 HTTP API  ←———HTTP———→  origincar_bridge  ←———ROS2———→  你的其他节点
+
+GET  /image  ──→  cv_bridge 解码  ──→  sensor_msgs/Image       /image
+GET  /imu    ──→  填充字段       ──→  sensor_msgs/Imu          /imu
+POST /cmd    ←──  speed/steer   ←──  geometry_msgs/Twist        /cmd_vel
+```
+
+## 依赖
+
+| 依赖 | 用途 |
+|------|------|
+| `rclpy` | ROS2 Python 客户端 |
+| `sensor_msgs` | Image / Imu 消息 |
+| `geometry_msgs` | Twist 消息 (cmd_vel) |
+| `cv_bridge` | OpenCV ↔ ROS Image 转换 |
+| `opencv-python` | 图像解码 |
+| `numpy` | 图像数组处理 |
+| `requests` | HTTP 请求 |
+
+## 安装
+
+```bash
+cd <your_workspace>
+git clone http://106.14.170.56:57575/cyy/sim_smart_car_bridge.git src/origincar_bridge
+colcon build --packages-select origincar_bridge --symlink-install
+source install/setup.bash
+```
+
+## 使用
+
+### launch 启动（推荐）
+
+```bash
+ros2 launch origincar_bridge bridge.launch.py http_host:=192.168.11.159
+```
+
+### 直接运行
+
+```bash
+ros2 run origincar_bridge bridge_node --ros-args -p http_host:=192.168.11.159
+```
+
+### 运行时调参
+
+```bash
+ros2 param set /origincar_bridge image_rate 15.0
+ros2 param set /origincar_bridge timeout 2.0
+```
+
+## 可配置参数
+
+| 参数 | 默认值 | 说明 |
+|------|--------|------|
+| `http_host` | `192.168.11.159` | 仿真机 IP 地址 |
+| `http_port` | `8765` | HTTP API 端口 |
+| `image_topic` | `/image` | 图像发布话题 |
+| `imu_topic` | `/imu` | IMU 发布话题 |
+| `cmd_vel_topic` | `/cmd_vel` | 控制指令订阅话题 |
+| `image_rate` | `30.0` | 图像拉取频率 (Hz) |
+| `imu_rate` | `30.0` | IMU 拉取频率 (Hz) |
+| `timeout` | `1.0` | HTTP 请求超时 (秒) |
+
+## 上游 API
+
+本节点对接 [Origincar Simulation](https://github.com/mxxxdev/motrixsim) 的 HTTP API（端口 8765）：
+
+| 端点 | 方法 | 说明 |
+|------|------|------|
+| `/image` | GET | 前置摄像头 JPEG 图像 |
+| `/imu` | GET | IMU 数据 (MPU6050) JSON |
+| `/state` | GET | 小车状态 JSON |
+| `/cmd` | POST | 发送控制指令 `{"speed": 1.0, "steer": 0.3}` |
+
+## 项目结构
+
+```
+origincar_bridge/
+├── package.xml                    # ROS2 包清单
+├── setup.py                       # ament_python 构建入口
+├── setup.cfg
+├── resource/
+│   └── origincar_bridge           # 包标记
+├── launch/
+│   └── bridge.launch.py           # 启动文件
+└── origincar_bridge/
+    ├── __init__.py
+    └── bridge_node.py             # 桥接节点主程序
+```
--- a/launch/bridge.launch.py
+++ b/launch/bridge.launch.py
@@ -0,0 +1,86 @@
+import os
+
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import Node
+
+
+def generate_launch_description():
+    """启动 origincar_bridge 节点，所有参数可通过命令行覆盖。"""
+
+    # 声明 launch 参数（名称, 默认值, 说明）
+    declare_http_host = DeclareLaunchArgument(
+        "http_host",
+        default_value="192.168.11.159",
+        description="仿真机 IP 地址",
+    )
+    declare_http_port = DeclareLaunchArgument(
+        "http_port",
+        default_value="8765",
+        description="HTTP API 端口",
+    )
+    declare_image_topic = DeclareLaunchArgument(
+        "image_topic",
+        default_value="/image",
+        description="图像发布话题",
+    )
+    declare_imu_topic = DeclareLaunchArgument(
+        "imu_topic",
+        default_value="/imu",
+        description="IMU 发布话题",
+    )
+    declare_cmd_vel_topic = DeclareLaunchArgument(
+        "cmd_vel_topic",
+        default_value="/cmd_vel",
+        description="控制指令订阅话题",
+    )
+    declare_image_rate = DeclareLaunchArgument(
+        "image_rate",
+        default_value="30.0",
+        description="图像拉取频率 (Hz)",
+    )
+    declare_imu_rate = DeclareLaunchArgument(
+        "imu_rate",
+        default_value="30.0",
+        description="IMU 拉取频率 (Hz)",
+    )
+    declare_timeout = DeclareLaunchArgument(
+        "timeout",
+        default_value="1.0",
+        description="HTTP 请求超时 (秒)",
+    )
+
+    # 节点
+    bridge_node = Node(
+        package="origincar_bridge",
+        executable="bridge_node",
+        name="origincar_bridge",
+        output="screen",
+        parameters=[
+            {
+                "http_host": LaunchConfiguration("http_host"),
+                "http_port": LaunchConfiguration("http_port"),
+                "image_topic": LaunchConfiguration("image_topic"),
+                "imu_topic": LaunchConfiguration("imu_topic"),
+                "cmd_vel_topic": LaunchConfiguration("cmd_vel_topic"),
+                "image_rate": LaunchConfiguration("image_rate"),
+                "imu_rate": LaunchConfiguration("imu_rate"),
+                "timeout": LaunchConfiguration("timeout"),
+            }
+        ],
+    )
+
+    return LaunchDescription(
+        [
+            declare_http_host,
+            declare_http_port,
+            declare_image_topic,
+            declare_imu_topic,
+            declare_cmd_vel_topic,
+            declare_image_rate,
+            declare_imu_rate,
+            declare_timeout,
+            bridge_node,
+        ]
+    )
--- a/origincar_bridge/init.py
+++ b/origincar_bridge/init.py
--- a/origincar_bridge/bridge_node.py
+++ b/origincar_bridge/bridge_node.py
@@ -0,0 +1,299 @@
+#!/usr/bin/env python3
+"""
+Origincar ROS2 桥接中转节点
+
+功能：
+  1. 从 HTTP API 拉取图像 (GET /image) → 发布 sensor_msgs/Image 到 ROS2
+  2. 从 HTTP API 拉取 IMU  (GET /imu)   → 发布 sensor_msgs/Imu   到 ROS2
+  3. 订阅 ROS2 /cmd_vel (geometry_msgs/Twist) → HTTP POST /cmd 控制小车
+
+可配置参数：
+  - http_host:  仿真机 IP 地址
+  - http_port:  HTTP 服务端口 (默认 8765)
+  - image_topic: 图像发布话题 (默认 /image)
+  - imu_topic:   IMU 发布话题 (默认 /imu)
+  - cmd_vel_topic: 控制指令订阅话题 (默认 /cmd_vel)
+  - image_rate: 图像拉取频率 Hz (默认 10.0)
+  - imu_rate:   IMU 拉取频率 Hz (默认 50.0)
+  - timeout:    HTTP 请求超时秒数 (默认 1.0)
+
+使用方式:
+  ros2 run origincar_bridge bridge_node --ros-args -p http_host:=192.168.1.100
+  ros2 launch origincar_bridge bridge.launch.py http_host:=192.168.1.100
+"""
+
+import time
+import traceback
+
+import cv2
+import numpy as np
+import requests
+import rclpy
+from rclpy.node import Node
+from rclpy.parameter import Parameter
+
+# ROS2 消息
+from sensor_msgs.msg import Image as RosImage
+from sensor_msgs.msg import Imu
+from geometry_msgs.msg import Twist
+
+from cv_bridge import CvBridge
+
+
+class OrigincarBridge(Node):
+    """将 Origincar 仿真 HTTP API 与 ROS2 话题双向桥接。"""
+
+    def __init__(self):
+        super().__init__("origincar_bridge")
+
+        # ---------- 声明参数 ----------
+        self.declare_parameter("http_host", "192.168.1.100")
+        self.declare_parameter("http_port", 8765)
+        self.declare_parameter("image_topic", "/image")
+        self.declare_parameter("imu_topic", "/imu")
+        self.declare_parameter("cmd_vel_topic", "/cmd_vel")
+        self.declare_parameter("image_rate", 10.0)
+        self.declare_parameter("imu_rate", 50.0)
+        self.declare_parameter("timeout", 1.0)
+
+        # 缓读参数（允许运行时通过 ros2 param set 动态调整）
+        self._read_params()
+
+        # ---------- cv_bridge ----------
+        self.bridge = CvBridge()
+
+        # ---------- 发布者 ----------
+        self.image_pub = self.create_publisher(RosImage, self.image_topic, 10)
+        self.imu_pub = self.create_publisher(Imu, self.imu_topic, 10)
+
+        # ---------- 订阅者 ----------
+        self.cmd_sub = self.create_subscription(
+            Twist, self.cmd_vel_topic, self._cmd_vel_callback, 10
+        )
+
+        # ---------- 定时器 ----------
+        image_period = 1.0 / self.image_rate
+        imu_period = 1.0 / self.imu_rate
+
+        self.image_timer = self.create_timer(image_period, self._fetch_and_publish_image)
+        self.imu_timer = self.create_timer(imu_period, self._fetch_and_publish_imu)
+
+        # 参数变更回调（运行时动态调参）
+        self.add_on_set_parameters_callback(self._on_param_change)
+
+        # 统计
+        self._image_seq = 0
+        self._imu_seq = 0
+        self._cmd_count = 0
+
+        # HTTP Session（连接复用，降低延迟）
+        self._session = requests.Session()
+
+        self.get_logger().info(
+            f"桥接启动: http://{self.http_host}:{self.http_port}\n"
+            f"  图像 [{self.image_topic}] @ {self.image_rate} Hz\n"
+            f"  IMU  [{self.imu_topic}]    @ {self.imu_rate} Hz\n"
+            f"  控制 [{self.cmd_vel_topic}]  → /cmd"
+        )
+
+    # ------------------------------------------------------------------
+    #  参数管理
+    # ------------------------------------------------------------------
+
+    def _read_params(self):
+        self.http_host = self.get_parameter("http_host").value
+        self.http_port = self.get_parameter("http_port").value
+        self.image_topic = self.get_parameter("image_topic").value
+        self.imu_topic = self.get_parameter("imu_topic").value
+        self.cmd_vel_topic = self.get_parameter("cmd_vel_topic").value
+        self.image_rate = self.get_parameter("image_rate").value
+        self.imu_rate = self.get_parameter("imu_rate").value
+        self.timeout = self.get_parameter("timeout").value
+
+    def _on_param_change(self, params):
+        """运行时参数变更回调。允许动态调整 rate / timeout / host 等。"""
+        for p in params:
+            if p.name == "http_host":
+                self.http_host = p.value
+            elif p.name == "http_port":
+                self.http_port = p.value
+            elif p.name == "image_rate":
+                self.image_rate = p.value
+                self.image_timer.timer_period_ns = int(1e9 / self.image_rate)
+            elif p.name == "imu_rate":
+                self.imu_rate = p.value
+                self.imu_timer.timer_period_ns = int(1e9 / self.imu_rate)
+            elif p.name == "timeout":
+                self.timeout = p.value
+            elif p.name == "image_topic":
+                self.image_topic = p.value
+            elif p.name == "imu_topic":
+                self.imu_topic = p.value
+            elif p.name == "cmd_vel_topic":
+                self.cmd_vel_topic = p.value
+        return rclpy.parameter.SetParametersResult(successful=True)
+
+    # ------------------------------------------------------------------
+    #  HTTP 基础方法
+    # ------------------------------------------------------------------
+
+    @property
+    def _base_url(self) -> str:
+        return f"http://{self.http_host}:{self.http_port}"
+
+    def _get(self, endpoint: str):
+        """GET 请求，失败返回 None，打日志。"""
+        try:
+            r = self._session.get(
+                f"{self._base_url}{endpoint}", timeout=self.timeout
+            )
+            r.raise_for_status()
+            return r
+        except requests.ConnectionError:
+            self.get_logger().warn(
+                f"连接仿真机 {self._base_url} 失败，请确认仿真已启动",
+                throttle_duration_sec=5.0,
+            )
+        except requests.Timeout:
+            self.get_logger().warn(
+                f"请求 {endpoint} 超时 ({self.timeout}s)",
+                throttle_duration_sec=5.0,
+            )
+        except requests.RequestException as e:
+            self.get_logger().warn(
+                f"请求 {endpoint} 异常: {e}", throttle_duration_sec=5.0
+            )
+        return None
+
+    def _post(self, endpoint: str, json_data: dict):
+        """POST 请求（不关心返回值）。"""
+        try:
+            self._session.post(
+                f"{self._base_url}{endpoint}",
+                json=json_data,
+                timeout=self.timeout,
+            )
+        except requests.RequestException as e:
+            self.get_logger().warn(
+                f"POST {endpoint} 异常: {e}", throttle_duration_sec=5.0
+            )
+
+    # ------------------------------------------------------------------
+    #  图像 — GET /image → sensor_msgs/Image
+    # ------------------------------------------------------------------
+
+    def _fetch_and_publish_image(self):
+        r = self._get("/image")
+        if r is None or r.status_code != 200:
+            return
+
+        try:
+            arr = np.frombuffer(r.content, np.uint8)
+            cv_img = cv2.imdecode(arr, cv2.IMREAD_COLOR)
+            if cv_img is None:
+                self.get_logger().warn("图像解码失败", throttle_duration_sec=5.0)
+                return
+
+            msg = self.bridge.cv2_to_imgmsg(cv_img, encoding="bgr8")
+            msg.header.stamp = self.get_clock().now().to_msg()
+            msg.header.frame_id = "camera_front"
+            self.image_pub.publish(msg)
+
+        except Exception:
+            self.get_logger().warn(
+                f"图像处理异常:\n{traceback.format_exc()}",
+                throttle_duration_sec=5.0,
+            )
+
+    # ------------------------------------------------------------------
+    #  IMU — GET /imu → sensor_msgs/Imu
+    # ------------------------------------------------------------------
+
+    def _fetch_and_publish_imu(self):
+        r = self._get("/imu")
+        if r is None or r.status_code != 200:
+            return
+
+        try:
+            data = r.json()
+            msg = Imu()
+            msg.header.stamp = self.get_clock().now().to_msg()
+            msg.header.frame_id = "imu_link"
+
+            # 姿态四元数
+            ori = data.get("orientation", {})
+            msg.orientation.w = ori.get("w", 1.0)
+            msg.orientation.x = ori.get("x", 0.0)
+            msg.orientation.y = ori.get("y", 0.0)
+            msg.orientation.z = ori.get("z", 0.0)
+
+            # 角速度 (rad/s)
+            av = data.get("angular_velocity", {})
+            msg.angular_velocity.x = av.get("x", 0.0)
+            msg.angular_velocity.y = av.get("y", 0.0)
+            msg.angular_velocity.z = av.get("z", 0.0)
+
+            # 线加速度 (m/s²)
+            la = data.get("linear_acceleration", {})
+            msg.linear_acceleration.x = la.get("x", 0.0)
+            msg.linear_acceleration.y = la.get("y", 0.0)
+            msg.linear_acceleration.z = la.get("z", 0.0)
+
+            # 协方差置 -1（表示无估计）
+            msg.orientation_covariance[0] = -1.0
+            msg.angular_velocity_covariance[0] = -1.0
+            msg.linear_acceleration_covariance[0] = -1.0
+
+            self.imu_pub.publish(msg)
+
+        except Exception:
+            self.get_logger().warn(
+                f"IMU 处理异常:\n{traceback.format_exc()}",
+                throttle_duration_sec=5.0,
+            )
+
+    # ------------------------------------------------------------------
+    #  控制 — ROS2 /cmd_vel → POST /cmd
+    # ------------------------------------------------------------------
+
+    def _cmd_vel_callback(self, msg: Twist):
+        """将 Twist 消息转换为 HTTP /cmd 控制指令。
+
+        Twist.linear.x  → speed (m/s)
+        Twist.angular.z → steer (rad)
+        """
+        speed = float(msg.linear.x)
+        steer = float(msg.angular.z)
+
+        self._post("/cmd", {"speed": speed, "steer": steer})
+
+        self._cmd_count += 1
+        self.get_logger().debug(
+            f"发送控制: speed={speed:.2f}, steer={steer:.2f}"
+            f"  (总计 {self._cmd_count})"
+        )
+
+    def destroy_node(self):
+        """关闭 HTTP Session 后再销毁节点。"""
+        self._session.close()
+        super().destroy_node()
+
+
+# ------------------------------------------------------------------
+#  entry point
+# ------------------------------------------------------------------
+
+def main():
+    rclpy.init()
+    node = OrigincarBridge()
+    try:
+        rclpy.spin(node)
+    except KeyboardInterrupt:
+        node.get_logger().info("键盘中断，停止桥接")
+    finally:
+        node.destroy_node()
+        rclpy.shutdown()
+
+
+if __name__ == "__main__":
+    main()
--- a/package.xml
+++ b/package.xml
@@ -0,0 +1,27 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>origincar_bridge</name>
+  <version>1.0.0</version>
+  <description>ROS2 bridge relay: HTTP API ⇄ ROS2 topics (image, IMU, /cmd_vel)</description>
+  <maintainer email="user@example.com">cyy</maintainer>
+  <license>Apache-2.0</license>
+
+  <buildtool_depend>ament_python</buildtool_depend>
+
+  <depend>rclpy</depend>
+  <depend>sensor_msgs</depend>
+  <depend>geometry_msgs</depend>
+  <depend>cv_bridge</depend>
+  <depend>python3-numpy</depend>
+  <depend>python3-opencv</depend>
+  <depend>python3-requests</depend>
+
+  <test_depend>ament_copyright</test_depend>
+  <test_depend>ament_flake8</test_depend>
+  <test_depend>ament_pep257</test_depend>
+
+  <export>
+    <build_type>ament_python</build_type>
+  </export>
+</package>
--- a/resource/origincar_bridge
+++ b/resource/origincar_bridge
--- a/setup.cfg
+++ b/setup.cfg
@@ -0,0 +1,4 @@
+[develop]
+script_dir=$base/lib/origincar_bridge
+[install]
+install_scripts=$base/lib/origincar_bridge
--- a/setup.py
+++ b/setup.py
@@ -0,0 +1,25 @@
+from setuptools import find_packages, setup
+
+package_name = "origincar_bridge"
+
+setup(
+    name=package_name,
+    version="1.0.0",
+    packages=find_packages(exclude=["test"]),
+    data_files=[
+        ("share/ament_index/resource_index/packages", ["resource/" + package_name]),
+        ("share/" + package_name, ["package.xml"]),
+        ("share/" + package_name + "/launch", ["launch/bridge.launch.py"]),
+    ],
+    install_requires=["setuptools"],
+    zip_safe=True,
+    maintainer="cyy",
+    maintainer_email="user@example.com",
+    description="ROS2 bridge relay: HTTP API ⇄ ROS2 topics (image, IMU, /cmd_vel)",
+    license="Apache-2.0",
+    entry_points={
+        "console_scripts": [
+            "bridge_node = origincar_bridge.bridge_node:main",
+        ],
+    },
+)