954ca7f8a7
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>
300 lines
10 KiB
Python
300 lines
10 KiB
Python
#!/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()
|