cyy/sim_smart_car_bridge
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

更新接口

Browse Source
This commit is contained in:
cyy
2026-06-20 21:54:04 +08:00
parent 954ca7f8a7
commit a58cbbf481
5 changed files with 366 additions and 203 deletions
Download Patch File Download Diff File Expand all files Collapse all files

90
README.md
View File

@@ -1,28 +1,45 @@
# sim_smart_car_bridge # sim_smart_car_bridge
ROS2 桥接中转节点：将 Origincar 仿真的 HTTP API 与 ROS2 话题双向桥接。 ROS2 桥接中转节点：将 [Origincar 仿真](https://github.com/mxxxdev/motrixsim) 的 HTTP API（端口 8765）全面桥接到 ROS2 话题。
## 数据流 ## 数据流
``` ```
仿真机 HTTP API ←———HTTP———→ origincar_bridge ←———ROS2———→ 你的其他节点 origincar_bridge
仿真机 HTTP API ←──── HTTP ────→ ←── ROS2 ──→ 你的其他节点
GET /image ──→ cv_bridge 解码 ──→ sensor_msgs/Image /image GET /image ──→ cv_bridge 解码 ──→ sensor_msgs/Image /image
GET /imu ──→ 填充字段 ──→ sensor_msgs/Imu /imu GET /imu ──→ 填充字段 ──→ sensor_msgs/Imu /imu/data_raw
POST /cmd ←── speed/steer ←── geometry_msgs/Twist /cmd_vel GET /scan ──→ 填充字段 ──→ sensor_msgs/LaserScan /scan
GET /odom ──→ 填充字段 ──→ nav_msgs/Odometry /odom
GET /power ──→ Float32 ──→ std_msgs/Float32 /PowerVoltage
POST /cmd ←── vx / wz ←── geometry_msgs/Twist /cmd_vel
``` ```
| 方向 | 话题 | 消息类型 | HTTP 端点 | 频率 |
|------|------|----------|-----------|------|
| 上行 | `/image` | `sensor_msgs/Image` | `GET /image` | 10 Hz（可配） |
| 上行 | `/imu/data_raw` | `sensor_msgs/Imu` | `GET /imu` | 30 Hz sync |
| 上行 | `/scan` | `sensor_msgs/LaserScan` | `GET /scan` | 30 Hz sync |
| 上行 | `/odom` | `nav_msgs/Odometry` | `GET /odom` | 30 Hz sync |
| 上行 | `/PowerVoltage` | `std_msgs/Float32` | `GET /power` | 30 Hz sync |
| 下行 | `/cmd_vel` | `geometry_msgs/Twist` | `POST /cmd` | 事件驱动 |
> **帧 ID**: `camera_front`（图像）、`gyro_link`（IMU）、`laser`（雷达）、`odom`→`base_link`（里程计）
## 依赖 ## 依赖
| 依赖 | 用途 | | 依赖 | 用途 |
|------|------| |------|------|
| `rclpy` | ROS2 Python 客户端 | | `rclpy` | ROS2 Python 客户端 |
| `sensor_msgs` | Image / Imu 消息 | | `sensor_msgs` | Image / Imu / LaserScan |
| `geometry_msgs` | Twist 消息 (cmd_vel) | | `nav_msgs` | Odometry |
| `cv_bridge` | OpenCV ↔ ROS Image 转换 | | `geometry_msgs` | Twist / Quaternion |
| `opencv-python` | 图像解码 | | `std_msgs` | Float32 (PowerVoltage) |
| `numpy` | 图像数组处理 | | `cv_bridge` | OpenCV ↔ ROS Image |
| `requests` | HTTP 请求 | | `opencv-python` | JPEG 解码 |
| `numpy` | 图像数组 |
| `requests` | HTTP 客户端 |
## 安装 ## 安装
@@ -35,15 +52,11 @@ source install/setup.bash
## 使用 ## 使用
### launch 启动（推荐）
```bash ```bash
# launch 启动（推荐）
ros2 launch origincar_bridge bridge.launch.py http_host:=192.168.11.159 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 ros2 run origincar_bridge bridge_node --ros-args -p http_host:=192.168.11.159
``` ```
@@ -51,6 +64,7 @@ ros2 run origincar_bridge bridge_node --ros-args -p http_host:=192.168.11.159
```bash ```bash
ros2 param set /origincar_bridge image_rate 15.0 ros2 param set /origincar_bridge image_rate 15.0
ros2 param set /origincar_bridge sync_rate 20.0
ros2 param set /origincar_bridge timeout 2.0 ros2 param set /origincar_bridge timeout 2.0
``` ```
@@ -58,30 +72,44 @@ ros2 param set /origincar_bridge timeout 2.0
| 参数 | 默认值 | 说明 | | 参数 | 默认值 | 说明 |
|------|--------|------| |------|--------|------|
| `http_host` | `192.168.11.159` | 仿真机 IP 地址 | | `http_host` | `192.168.11.159` | 仿真机 IP |
| `http_port` | `8765` | HTTP API 端口 | | `http_port` | `8765` | HTTP API 端口 |
| `timeout` | `1.0` | HTTP 请求超时 (s) |
| `sync_rate` | `30.0` | scan/odom/imu/power 统一同步频率 (Hz) |
| `image_rate` | `10.0` | 图像独立拉取频率 (Hz) |
| `image_topic` | `/image` | 图像发布话题 | | `image_topic` | `/image` | 图像发布话题 |
| `imu_topic` | `/imu` | IMU 发布话题 | | `imu_topic` | `/imu/data_raw` | IMU 发布话题 |
| `scan_topic` | `/scan` | 激光雷达发布话题 |
| `odom_topic` | `/odom` | 里程计发布话题 |
| `power_topic` | `/PowerVoltage` | 电池电压发布话题 |
| `cmd_vel_topic` | `/cmd_vel` | 控制指令订阅话题 | | `cmd_vel_topic` | `/cmd_vel` | 控制指令订阅话题 |
| `image_rate` | `30.0` | 图像拉取频率 (Hz) |
| `imu_rate` | `30.0` | IMU 拉取频率 (Hz) |
| `timeout` | `1.0` | HTTP 请求超时 (秒) |
## 上游 API ## 上游 HTTP API
本节点对接 [Origincar Simulation](https://github.com/mxxxdev/motrixsim) 的 HTTP API（端口 8765）： | 端点 | 方法 | 返回 | 说明 |
|------|------|------|------|
| `/image` | GET | JPEG bytes | 前置摄像头图像 |
| `/imu` | GET | JSON | IMU 数据（MPU6050 噪声） |
| `/scan` | GET | JSON | LSLIDAR N10 2D 激光雷达（360°, 450 点） |
| `/odom` | GET | JSON | 里程计（位置/姿态/速度） |
| `/power` | GET | JSON | 电池电压 `{"voltage": 12.0}` |
| `/state` | GET | JSON | 小车完整状态 |
| `/cmd` | POST | — | 控制指令 `{"vx": vx, "wz": wz}` |
| 端点 | 方法 | 说明 | ### /cmd 控制模式
|------|------|------|
| `/image` | GET | 前置摄像头 JPEG 图像 | HTTP `/cmd` 使用差速模式 `{"vx": vx, "wz": wz}`（匹配串口协议，与 `Twist` 语义一致），服务端内部通过 bicycle model 将 `wz` 转为阿克曼转向角。
| `/imu` | GET | IMU 数据 (MPU6050) JSON |
| `/state` | GET | 小车状态 JSON | ```json
| `/cmd` | POST | 发送控制指令 `{"speed": 1.0, "steer": 0.3}` | {"vx": 1.0, "wz": 0.5}
```
## 项目结构 ## 项目结构
``` ```
origincar_bridge/ origincar_bridge/
├── README.md
├── .gitignore
├── package.xml # ROS2 包清单 ├── package.xml # ROS2 包清单
├── setup.py # ament_python 构建入口 ├── setup.py # ament_python 构建入口
├── setup.cfg ├── setup.cfg
@@ -91,5 +119,5 @@ origincar_bridge/
│ └── bridge.launch.py # 启动文件 │ └── bridge.launch.py # 启动文件
└── origincar_bridge/ └── origincar_bridge/
├── __init__.py ├── __init__.py
└── bridge_node.py # 桥接节点主程序 └── bridge_node.py # ★ 全功能桥接节点
``` ```

103
launch/bridge.launch.py
View File

@@ -9,78 +9,77 @@ from launch_ros.actions import Node
def generate_launch_description(): def generate_launch_description():
"""启动 origincar_bridge 节点，所有参数可通过命令行覆盖。""" """启动 origincar_bridge 节点，所有参数可通过命令行覆盖。"""
# 声明 launch 参数（名称, 默认值, 说明）
declare_http_host = DeclareLaunchArgument( declare_http_host = DeclareLaunchArgument(
"http_host", "http_host", default_value="192.168.11.159",
default_value="192.168.11.159",
description="仿真机 IP 地址", description="仿真机 IP 地址",
) )
declare_http_port = DeclareLaunchArgument( declare_http_port = DeclareLaunchArgument(
"http_port", "http_port", default_value="8765",
default_value="8765",
description="HTTP API 端口", description="HTTP API 端口",
) )
declare_timeout = DeclareLaunchArgument(
"timeout", default_value="1.0",
description="HTTP 请求超时 (秒)",
)
declare_sync_rate = DeclareLaunchArgument(
"sync_rate", default_value="30.0",
description="scan/odom/imu/power 同步频率 (Hz)",
)
declare_image_rate = DeclareLaunchArgument(
"image_rate", default_value="10.0",
description="图像拉取频率 (Hz)",
)
# 话题名称
declare_image_topic = DeclareLaunchArgument( declare_image_topic = DeclareLaunchArgument(
"image_topic", "image_topic", default_value="/image",
default_value="/image",
description="图像发布话题", description="图像发布话题",
) )
declare_imu_topic = DeclareLaunchArgument( declare_imu_topic = DeclareLaunchArgument(
"imu_topic", "imu_topic", default_value="/imu/data_raw",
default_value="/imu",
description="IMU 发布话题", description="IMU 发布话题",
) )
declare_scan_topic = DeclareLaunchArgument(
"scan_topic", default_value="/scan",
description="激光雷达发布话题",
)
declare_odom_topic = DeclareLaunchArgument(
"odom_topic", default_value="/odom",
description="里程计发布话题",
)
declare_power_topic = DeclareLaunchArgument(
"power_topic", default_value="/PowerVoltage",
description="电池电压发布话题",
)
declare_cmd_vel_topic = DeclareLaunchArgument( declare_cmd_vel_topic = DeclareLaunchArgument(
"cmd_vel_topic", "cmd_vel_topic", default_value="/cmd_vel",
default_value="/cmd_vel",
description="控制指令订阅话题", 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( bridge_node = Node(
package="origincar_bridge", package="origincar_bridge",
executable="bridge_node", executable="bridge_node",
name="origincar_bridge", name="origincar_bridge",
output="screen", output="screen",
parameters=[ parameters=[{
{ "http_host": LaunchConfiguration("http_host"),
"http_host": LaunchConfiguration("http_host"), "http_port": LaunchConfiguration("http_port"),
"http_port": LaunchConfiguration("http_port"), "timeout": LaunchConfiguration("timeout"),
"image_topic": LaunchConfiguration("image_topic"), "sync_rate": LaunchConfiguration("sync_rate"),
"imu_topic": LaunchConfiguration("imu_topic"), "image_rate": LaunchConfiguration("image_rate"),
"cmd_vel_topic": LaunchConfiguration("cmd_vel_topic"), "image_topic": LaunchConfiguration("image_topic"),
"image_rate": LaunchConfiguration("image_rate"), "imu_topic": LaunchConfiguration("imu_topic"),
"imu_rate": LaunchConfiguration("imu_rate"), "scan_topic": LaunchConfiguration("scan_topic"),
"timeout": LaunchConfiguration("timeout"), "odom_topic": LaunchConfiguration("odom_topic"),
} "power_topic": LaunchConfiguration("power_topic"),
], "cmd_vel_topic": LaunchConfiguration("cmd_vel_topic"),
}],
) )
return LaunchDescription( return LaunchDescription([
[ declare_http_host, declare_http_port, declare_timeout,
declare_http_host, declare_sync_rate, declare_image_rate,
declare_http_port, declare_image_topic, declare_imu_topic, declare_scan_topic,
declare_image_topic, declare_odom_topic, declare_power_topic, declare_cmd_vel_topic,
declare_imu_topic, bridge_node,
declare_cmd_vel_topic, ])
declare_image_rate,
declare_imu_rate,
declare_timeout,
bridge_node,
]
)

367
origincar_bridge/bridge_node.py
View File

@@ -2,27 +2,29 @@
""" """
Origincar ROS2 桥接中转节点 Origincar ROS2 桥接中转节点
功能： 将 Origincar 仿真 HTTP API 全面桥接到 ROS2 话题，双向中转。
1. 从 HTTP API 拉取图像 (GET /image) → 发布 sensor_msgs/Image 到 ROS2
2. 从 HTTP API 拉取 IMU (GET /imu) → 发布 sensor_msgs/Imu 到 ROS2 上行（HTTP → ROS2）：
3. 订阅 ROS2 /cmd_vel (geometry_msgs/Twist) → HTTP POST /cmd 控制小车 GET /image → sensor_msgs/Image (独立定时器，可配置频率)
GET /imu → sensor_msgs/Imu (统一同步定时器)
GET /scan → sensor_msgs/LaserScan (统一同步定时器)
GET /odom → nav_msgs/Odometry (统一同步定时器)
GET /power → std_msgs/Float32 (统一同步定时器)
下行（ROS2 → HTTP）：
geometry_msgs/Twist → POST /cmd (差速模式: vx/wz)
可配置参数： 可配置参数：
- http_host: 仿真机 IP 地址 http_host, http_port, timeout
- http_port: HTTP 服务端口 (默认 8765) image_topic, imu_topic, scan_topic, odom_topic, power_topic, cmd_vel_topic
- image_topic: 图像发布话题 (默认 /image) sync_rate — 统一同步频率 Hz（scan/odom/imu/power，默认 30）
- imu_topic: IMU 发布话题 (默认 /imu) image_rate — 图像拉取频率 Hz（默认 10）
- 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 run origincar_bridge bridge_node --ros-args -p http_host:=192.168.11.159
ros2 launch origincar_bridge bridge.launch.py http_host:=192.168.1.100 ros2 launch origincar_bridge bridge.launch.py http_host:=192.168.11.159
""" """
import time
import traceback import traceback
import cv2 import cv2
@@ -30,33 +32,39 @@ import numpy as np
import requests import requests
import rclpy import rclpy
from rclpy.node import Node from rclpy.node import Node
from rclpy.parameter import Parameter
# ROS2 消息 # ROS2 消息
from sensor_msgs.msg import Image as RosImage from sensor_msgs.msg import Image as RosImage
from sensor_msgs.msg import Imu from sensor_msgs.msg import Imu
from geometry_msgs.msg import Twist from sensor_msgs.msg import LaserScan
from nav_msgs.msg import Odometry
from geometry_msgs.msg import Twist, Quaternion
from std_msgs.msg import Float32
from cv_bridge import CvBridge from cv_bridge import CvBridge
class OrigincarBridge(Node): class OrigincarBridge(Node):
"""将 Origincar 仿真 HTTP API 与 ROS2 话题双向桥接。""" """Origincar 仿真 HTTP API ⇄ ROS2 话题 全功能桥接节点。"""
def __init__(self): def __init__(self):
super().__init__("origincar_bridge") super().__init__("origincar_bridge")
# ---------- 声明参数 ---------- # ---------- 声明参数 ----------
self.declare_parameter("http_host", "192.168.1.100") self.declare_parameter("http_host", "192.168.11.159")
self.declare_parameter("http_port", 8765) 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) self.declare_parameter("timeout", 1.0)
# 缓读参数（允许运行时通过 ros2 param set 动态调整） self.declare_parameter("image_topic", "/image")
self.declare_parameter("imu_topic", "/imu/data_raw")
self.declare_parameter("scan_topic", "/scan")
self.declare_parameter("odom_topic", "/odom")
self.declare_parameter("power_topic", "/PowerVoltage")
self.declare_parameter("cmd_vel_topic", "/cmd_vel")
self.declare_parameter("sync_rate", 30.0) # scan/odom/imu/power 频率
self.declare_parameter("image_rate", 10.0) # 图像独立频率
self._read_params() self._read_params()
# ---------- cv_bridge ---------- # ---------- cv_bridge ----------
@@ -64,7 +72,10 @@ class OrigincarBridge(Node):
# ---------- 发布者 ---------- # ---------- 发布者 ----------
self.image_pub = self.create_publisher(RosImage, self.image_topic, 10) self.image_pub = self.create_publisher(RosImage, self.image_topic, 10)
self.imu_pub = self.create_publisher(Imu, self.imu_topic, 10) self.imu_pub = self.create_publisher(Imu, self.imu_topic, 10)
self.scan_pub = self.create_publisher(LaserScan, self.scan_topic, 10)
self.odom_pub = self.create_publisher(Odometry, self.odom_topic, 10)
self.power_pub = self.create_publisher(Float32, self.power_topic, 10)
# ---------- 订阅者 ---------- # ---------- 订阅者 ----------
self.cmd_sub = self.create_subscription( self.cmd_sub = self.create_subscription(
@@ -72,69 +83,71 @@ class OrigincarBridge(Node):
) )
# ---------- 定时器 ---------- # ---------- 定时器 ----------
image_period = 1.0 / self.image_rate # image 独立定时器（JPEG 解码较重，用较低频率）
imu_period = 1.0 / self.imu_rate self.image_timer = self.create_timer(
1.0 / self.image_rate, self._fetch_and_publish_image
)
# 统一同步定时器（scan / odom / imu / power 在同一拍内完成）
self.sync_timer = self.create_timer(
1.0 / self.sync_rate, self._sync_fast_data
)
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.add_on_set_parameters_callback(self._on_param_change)
# 统计 # ---------- 统计 & Session ----------
self._image_seq = 0
self._imu_seq = 0
self._cmd_count = 0 self._cmd_count = 0
# HTTP Session（连接复用，降低延迟）
self._session = requests.Session() self._session = requests.Session()
self.get_logger().info( self.get_logger().info(
f"桥接启动: http://{self.http_host}:{self.http_port}\n" f"桥接启动 → http://{self.http_host}:{self.http_port}\n"
f" 图像 [{self.image_topic}] @ {self.image_rate} Hz\n" f" 上行: 图像 [{self.image_topic}] @ {self.image_rate} Hz\n"
f" IMU [{self.imu_topic}] @ {self.imu_rate} Hz\n" f" IMU [{self.imu_topic}] }} sync @ {self.sync_rate} Hz\n"
f" 控制 [{self.cmd_vel_topic}] → /cmd" f" Scan [{self.scan_topic}] }}\n"
f" Odom [{self.odom_topic}] }}\n"
f" Power[{self.power_topic}] }}\n"
f" 下行: 控制 [{self.cmd_vel_topic}] → POST /cmd (vx/wz)"
) )
# ------------------------------------------------------------------ # ------------------------------------------------------------------
# 参数管理 # 参数
# ------------------------------------------------------------------ # ------------------------------------------------------------------
def _read_params(self): def _read_params(self):
self.http_host = self.get_parameter("http_host").value self.http_host = self.get_parameter("http_host").value
self.http_port = self.get_parameter("http_port").value self.http_port = self.get_parameter("http_port").value
self.image_topic = self.get_parameter("image_topic").value self.timeout = self.get_parameter("timeout").value
self.imu_topic = self.get_parameter("imu_topic").value self.image_topic = self.get_parameter("image_topic").value
self.imu_topic = self.get_parameter("imu_topic").value
self.scan_topic = self.get_parameter("scan_topic").value
self.odom_topic = self.get_parameter("odom_topic").value
self.power_topic = self.get_parameter("power_topic").value
self.cmd_vel_topic = self.get_parameter("cmd_vel_topic").value self.cmd_vel_topic = self.get_parameter("cmd_vel_topic").value
self.image_rate = self.get_parameter("image_rate").value self.sync_rate = self.get_parameter("sync_rate").value
self.imu_rate = self.get_parameter("imu_rate").value self.image_rate = self.get_parameter("image_rate").value
self.timeout = self.get_parameter("timeout").value
def _on_param_change(self, params): def _on_param_change(self, params):
"""运行时参数变更回调。允许动态调整 rate / timeout / host 等。"""
for p in params: for p in params:
if p.name == "http_host": name = p.name
self.http_host = p.value if name == "http_host": self.http_host = p.value
elif p.name == "http_port": elif name == "http_port": self.http_port = p.value
self.http_port = p.value elif name == "timeout": self.timeout = p.value
elif p.name == "image_rate": elif name == "sync_rate":
self.sync_rate = p.value
self.sync_timer.timer_period_ns = int(1e9 / self.sync_rate)
elif name == "image_rate":
self.image_rate = p.value self.image_rate = p.value
self.image_timer.timer_period_ns = int(1e9 / self.image_rate) self.image_timer.timer_period_ns = int(1e9 / self.image_rate)
elif p.name == "imu_rate": elif name == "image_topic": self.image_topic = p.value
self.imu_rate = p.value elif name == "imu_topic": self.imu_topic = p.value
self.imu_timer.timer_period_ns = int(1e9 / self.imu_rate) elif name == "scan_topic": self.scan_topic = p.value
elif p.name == "timeout": elif name == "odom_topic": self.odom_topic = p.value
self.timeout = p.value elif name == "power_topic": self.power_topic = p.value
elif p.name == "image_topic": elif name == "cmd_vel_topic": self.cmd_vel_topic = p.value
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) return rclpy.parameter.SetParametersResult(successful=True)
# ------------------------------------------------------------------ # ------------------------------------------------------------------
# HTTP 基础方法 # HTTP 基础
# ------------------------------------------------------------------ # ------------------------------------------------------------------
@property @property
@@ -142,7 +155,6 @@ class OrigincarBridge(Node):
return f"http://{self.http_host}:{self.http_port}" return f"http://{self.http_host}:{self.http_port}"
def _get(self, endpoint: str): def _get(self, endpoint: str):
"""GET 请求，失败返回 None，打日志。"""
try: try:
r = self._session.get( r = self._session.get(
f"{self._base_url}{endpoint}", timeout=self.timeout f"{self._base_url}{endpoint}", timeout=self.timeout
@@ -151,7 +163,7 @@ class OrigincarBridge(Node):
return r return r
except requests.ConnectionError: except requests.ConnectionError:
self.get_logger().warn( self.get_logger().warn(
f"连接仿真机 {self._base_url} 失败，请确认仿真已启动", f"连接失败 {self._base_url}，仿真是否启动？",
throttle_duration_sec=5.0, throttle_duration_sec=5.0,
) )
except requests.Timeout: except requests.Timeout:
@@ -165,8 +177,18 @@ class OrigincarBridge(Node):
) )
return None return None
def _get_json(self, endpoint: str) -> dict | None:
r = self._get(endpoint)
if r is not None:
try:
return r.json()
except Exception:
self.get_logger().warn(
f"解析 {endpoint} JSON 失败", throttle_duration_sec=5.0
)
return None
def _post(self, endpoint: str, json_data: dict): def _post(self, endpoint: str, json_data: dict):
"""POST 请求（不关心返回值）。"""
try: try:
self._session.post( self._session.post(
f"{self._base_url}{endpoint}", f"{self._base_url}{endpoint}",
@@ -179,7 +201,7 @@ class OrigincarBridge(Node):
) )
# ------------------------------------------------------------------ # ------------------------------------------------------------------
# 图像 — GET /image → sensor_msgs/Image # 图像 — GET /image → sensor_msgs/Image (独立定时器)
# ------------------------------------------------------------------ # ------------------------------------------------------------------
def _fetch_and_publish_image(self): def _fetch_and_publish_image(self):
@@ -206,75 +228,184 @@ class OrigincarBridge(Node):
) )
# ------------------------------------------------------------------ # ------------------------------------------------------------------
# IMU — GET /imu → sensor_msgs/Imu # 同步快速数据 — scan / odom / imu / power (统一定时器)
# ------------------------------------------------------------------ # ------------------------------------------------------------------
def _fetch_and_publish_imu(self): def _sync_fast_data(self):
r = self._get("/imu") """在同一拍内拉取并发布 scan / odom / imu / power。"""
if r is None or r.status_code != 200: now = self.get_clock().now().to_msg()
return
try: # ---- LaserScan ----
data = r.json() scan_data = self._get_json("/scan")
msg = Imu() if scan_data:
msg.header.stamp = self.get_clock().now().to_msg() try:
msg.header.frame_id = "imu_link" self._publish_scan(scan_data, now)
except Exception:
self.get_logger().warn(
f"LaserScan 异常:\n{traceback.format_exc()}",
throttle_duration_sec=5.0,
)
# 姿态四元数 # ---- Odometry ----
ori = data.get("orientation", {}) odom_data = self._get_json("/odom")
msg.orientation.w = ori.get("w", 1.0) if odom_data:
msg.orientation.x = ori.get("x", 0.0) try:
msg.orientation.y = ori.get("y", 0.0) self._publish_odom(odom_data, now)
msg.orientation.z = ori.get("z", 0.0) except Exception:
self.get_logger().warn(
f"Odometry 异常:\n{traceback.format_exc()}",
throttle_duration_sec=5.0,
)
# 角速度 (rad/s) # ---- IMU ----
av = data.get("angular_velocity", {}) imu_data = self._get_json("/imu")
msg.angular_velocity.x = av.get("x", 0.0) if imu_data:
msg.angular_velocity.y = av.get("y", 0.0) try:
msg.angular_velocity.z = av.get("z", 0.0) self._publish_imu(imu_data, now)
except Exception:
self.get_logger().warn(
f"IMU 异常:\n{traceback.format_exc()}",
throttle_duration_sec=5.0,
)
# 线加速度 (m/s²) # ---- Power ----
la = data.get("linear_acceleration", {}) power_data = self._get_json("/power")
msg.linear_acceleration.x = la.get("x", 0.0) if power_data:
msg.linear_acceleration.y = la.get("y", 0.0) try:
msg.linear_acceleration.z = la.get("z", 0.0) self._publish_power(power_data)
except Exception:
self.get_logger().warn(
f"Power 异常:\n{traceback.format_exc()}",
throttle_duration_sec=5.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) def _publish_scan(self, data: dict, now):
"""GET /scan → sensor_msgs/LaserScan
except Exception: HTTP 返回格式:
self.get_logger().warn( angle_min, angle_max, angle_increment, range_min, range_max, ranges[]
f"IMU 处理异常:\n{traceback.format_exc()}", """
throttle_duration_sec=5.0, msg = LaserScan()
) msg.header.stamp = now
msg.header.frame_id = "laser"
msg.angle_min = float(data["angle_min"])
msg.angle_max = float(data["angle_max"])
msg.angle_increment = float(data["angle_increment"])
msg.range_min = float(data.get("range_min", 0.15))
msg.range_max = float(data.get("range_max", 3.0))
msg.ranges = [float(r) for r in data["ranges"]]
# intensities 置空
msg.intensities = []
self.scan_pub.publish(msg)
def _publish_odom(self, data: dict, now):
"""GET /odom → nav_msgs/Odometry
HTTP 返回格式:
x, y, yaw, vx, vy, wz, orientation{w,x,y,z}
"""
msg = Odometry()
msg.header.stamp = now
msg.header.frame_id = "odom"
msg.child_frame_id = "base_link"
msg.pose.pose.position.x = float(data["x"])
msg.pose.pose.position.y = float(data["y"])
msg.pose.pose.position.z = 0.0
ori = data.get("orientation", {})
msg.pose.pose.orientation = Quaternion(
w=float(ori.get("w", 1.0)),
x=float(ori.get("x", 0.0)),
y=float(ori.get("y", 0.0)),
z=float(ori.get("z", 0.0)),
)
msg.twist.twist.linear.x = float(data.get("vx", 0.0))
msg.twist.twist.linear.y = float(data.get("vy", 0.0))
msg.twist.twist.angular.z = float(data.get("wz", 0.0))
# 协方差置 -1（无估计）
msg.pose.covariance[0] = -1.0
msg.twist.covariance[0] = -1.0
self.odom_pub.publish(msg)
def _publish_imu(self, data: dict, now):
"""GET /imu → sensor_msgs/Imu
HTTP 返回格式:
orientation{w,x,y,z}, angular_velocity{x,y,z}, linear_acceleration{x,y,z}
"""
msg = Imu()
msg.header.stamp = now
msg.header.frame_id = "gyro_link"
ori = data.get("orientation", {})
msg.orientation = Quaternion(
w=float(ori.get("w", 1.0)),
x=float(ori.get("x", 0.0)),
y=float(ori.get("y", 0.0)),
z=float(ori.get("z", 0.0)),
)
av = data.get("angular_velocity", {})
msg.angular_velocity.x = float(av.get("x", 0.0))
msg.angular_velocity.y = float(av.get("y", 0.0))
msg.angular_velocity.z = float(av.get("z", 0.0))
la = data.get("linear_acceleration", {})
msg.linear_acceleration.x = float(la.get("x", 0.0))
msg.linear_acceleration.y = float(la.get("y", 0.0))
msg.linear_acceleration.z = float(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)
def _publish_power(self, data: dict):
"""GET /power → std_msgs/Float32
HTTP 返回格式:
{"voltage": 12.0}
"""
msg = Float32()
msg.data = float(data.get("voltage", 0.0))
self.power_pub.publish(msg)
# ------------------------------------------------------------------ # ------------------------------------------------------------------
# 控制 — ROS2 /cmd_vel → POST /cmd # 控制 — ROS2 /cmd_vel (Twist) → POST /cmd (差速模式 vx/wz)
# ------------------------------------------------------------------ # ------------------------------------------------------------------
def _cmd_vel_callback(self, msg: Twist): def _cmd_vel_callback(self, msg: Twist):
"""将 Twist 消息转换为 HTTP /cmd 控制指令。 """将 Twist 转为差速控制指令。
Twist.linear.x → speed (m/s) Twist.linear.x → vx (线速度 m/s)
Twist.angular.z → steer (rad) Twist.angular.z → wz (角速度 rad/s)
HTTP /cmd 只接受差速模式 {"vx": vx, "wz": wz}，
服务端内部通过 bicycle model 将 wz 转成阿克曼转向角。
""" """
speed = float(msg.linear.x) vx = float(msg.linear.x)
steer = float(msg.angular.z) wz = float(msg.angular.z)
self._post("/cmd", {"speed": speed, "steer": steer}) self._post("/cmd", {"vx": vx, "wz": wz})
self._cmd_count += 1 self._cmd_count += 1
self.get_logger().debug( self.get_logger().debug(
f"发送控制: speed={speed:.2f}, steer={steer:.2f}" f"cmd_vel → vx={vx:.2f}, wz={wz:.2f} (#{self._cmd_count})"
f" (总计 {self._cmd_count})"
) )
# ------------------------------------------------------------------
# 清理
# ------------------------------------------------------------------
def destroy_node(self): def destroy_node(self):
"""关闭 HTTP Session 后再销毁节点。"""
self._session.close() self._session.close()
super().destroy_node() super().destroy_node()

7
package.xml
View File

@@ -3,7 +3,10 @@
<package format="3"> <package format="3">
<name>origincar_bridge</name> <name>origincar_bridge</name>
<version>1.0.0</version> <version>1.0.0</version>
<description>ROS2 bridge relay: HTTP API ⇄ ROS2 topics (image, IMU, /cmd_vel)</description> <description>
ROS2 bridge relay for Origincar Simulation HTTP API.
Bridges /image, /imu, /scan, /odom, /power (HTTP→ROS2) and /cmd_vel→/cmd (ROS2→HTTP).
</description>
<maintainer email="user@example.com">cyy</maintainer> <maintainer email="user@example.com">cyy</maintainer>
<license>Apache-2.0</license> <license>Apache-2.0</license>
@@ -11,7 +14,9 @@
<depend>rclpy</depend> <depend>rclpy</depend>
<depend>sensor_msgs</depend> <depend>sensor_msgs</depend>
<depend>nav_msgs</depend>
<depend>geometry_msgs</depend> <depend>geometry_msgs</depend>
<depend>std_msgs</depend>
<depend>cv_bridge</depend> <depend>cv_bridge</depend>
<depend>python3-numpy</depend> <depend>python3-numpy</depend>
<depend>python3-opencv</depend> <depend>python3-opencv</depend>

2
setup.py
View File

@@ -15,7 +15,7 @@ setup(
zip_safe=True, zip_safe=True,
maintainer="cyy", maintainer="cyy",
maintainer_email="user@example.com", maintainer_email="user@example.com",
description="ROS2 bridge relay: HTTP API ⇄ ROS2 topics (image, IMU, /cmd_vel)", description="ROS2 bridge relay: HTTP API ⇄ ROS2 topics (image, IMU, scan, odom, power, /cmd_vel)",
license="Apache-2.0", license="Apache-2.0",
entry_points={ entry_points={
"console_scripts": [ "console_scripts": [