#!/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()