From 954ca7f8a7de57ae0f07cf98efb7ac6014e0a259 Mon Sep 17 00:00:00 2001 From: cyy Date: Tue, 16 Jun 2026 22:57:18 +0800 Subject: [PATCH] =?UTF-8?q?feat:=20init=20sim=5Fsmart=5Fcar=5Fbridge=20?= =?UTF-8?q?=E2=80=94=20ROS2=20bridge=20relay=20for=20Origincar=20HTTP=20AP?= =?UTF-8?q?I?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit 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 --- .gitignore | 21 +++ README.md | 95 ++++++++++ launch/bridge.launch.py | 86 +++++++++ origincar_bridge/__init__.py | 0 origincar_bridge/bridge_node.py | 299 ++++++++++++++++++++++++++++++++ package.xml | 27 +++ resource/origincar_bridge | 0 setup.cfg | 4 + setup.py | 25 +++ 9 files changed, 557 insertions(+) create mode 100644 .gitignore create mode 100644 README.md create mode 100644 launch/bridge.launch.py create mode 100644 origincar_bridge/__init__.py create mode 100644 origincar_bridge/bridge_node.py create mode 100644 package.xml create mode 100644 resource/origincar_bridge create mode 100644 setup.cfg create mode 100644 setup.py diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..3e97236 --- /dev/null +++ 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 diff --git a/README.md b/README.md new file mode 100644 index 0000000..3621063 --- /dev/null +++ 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 +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 # 桥接节点主程序 +``` diff --git a/launch/bridge.launch.py b/launch/bridge.launch.py new file mode 100644 index 0000000..d4a7afa --- /dev/null +++ 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, + ] + ) diff --git a/origincar_bridge/__init__.py b/origincar_bridge/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/origincar_bridge/bridge_node.py b/origincar_bridge/bridge_node.py new file mode 100644 index 0000000..3ef7690 --- /dev/null +++ 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() diff --git a/package.xml b/package.xml new file mode 100644 index 0000000..35e0229 --- /dev/null +++ b/package.xml @@ -0,0 +1,27 @@ + + + + origincar_bridge + 1.0.0 + ROS2 bridge relay: HTTP API ⇄ ROS2 topics (image, IMU, /cmd_vel) + cyy + Apache-2.0 + + ament_python + + rclpy + sensor_msgs + geometry_msgs + cv_bridge + python3-numpy + python3-opencv + python3-requests + + ament_copyright + ament_flake8 + ament_pep257 + + + ament_python + + diff --git a/resource/origincar_bridge b/resource/origincar_bridge new file mode 100644 index 0000000..e69de29 diff --git a/setup.cfg b/setup.cfg new file mode 100644 index 0000000..cb7d7b8 --- /dev/null +++ b/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/origincar_bridge +[install] +install_scripts=$base/lib/origincar_bridge diff --git a/setup.py b/setup.py new file mode 100644 index 0000000..057b06a --- /dev/null +++ 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", + ], + }, +)