431 lines
15 KiB
Python
431 lines
15 KiB
Python
#!/usr/bin/env python3
|
||
"""
|
||
Origincar ROS2 桥接中转节点
|
||
|
||
将 Origincar 仿真 HTTP API 全面桥接到 ROS2 话题，双向中转。
|
||
|
||
上行（HTTP → ROS2）：
|
||
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, http_port, timeout
|
||
image_topic, imu_topic, scan_topic, odom_topic, power_topic, cmd_vel_topic
|
||
sync_rate — 统一同步频率 Hz（scan/odom/imu/power，默认 30）
|
||
image_rate — 图像拉取频率 Hz（默认 10）
|
||
|
||
使用:
|
||
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.11.159
|
||
"""
|
||
|
||
import traceback
|
||
|
||
import cv2
|
||
import numpy as np
|
||
import requests
|
||
import rclpy
|
||
from rclpy.node import Node
|
||
|
||
# ROS2 消息
|
||
from sensor_msgs.msg import Image as RosImage
|
||
from sensor_msgs.msg import Imu
|
||
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
|
||
|
||
|
||
class OrigincarBridge(Node):
|
||
"""Origincar 仿真 HTTP API ⇄ ROS2 话题 全功能桥接节点。"""
|
||
|
||
def __init__(self):
|
||
super().__init__("origincar_bridge")
|
||
|
||
# ---------- 声明参数 ----------
|
||
self.declare_parameter("http_host", "192.168.11.159")
|
||
self.declare_parameter("http_port", 8765)
|
||
self.declare_parameter("timeout", 1.0)
|
||
|
||
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()
|
||
|
||
# ---------- 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.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(
|
||
Twist, self.cmd_vel_topic, self._cmd_vel_callback, 10
|
||
)
|
||
|
||
# ---------- 定时器 ----------
|
||
# image 独立定时器（JPEG 解码较重，用较低频率）
|
||
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.add_on_set_parameters_callback(self._on_param_change)
|
||
|
||
# ---------- 统计 & Session ----------
|
||
self._cmd_count = 0
|
||
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}] }} sync @ {self.sync_rate} Hz\n"
|
||
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):
|
||
self.http_host = self.get_parameter("http_host").value
|
||
self.http_port = self.get_parameter("http_port").value
|
||
self.timeout = self.get_parameter("timeout").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.sync_rate = self.get_parameter("sync_rate").value
|
||
self.image_rate = self.get_parameter("image_rate").value
|
||
|
||
def _on_param_change(self, params):
|
||
for p in params:
|
||
name = p.name
|
||
if name == "http_host": self.http_host = p.value
|
||
elif name == "http_port": self.http_port = p.value
|
||
elif name == "timeout": self.timeout = p.value
|
||
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_timer.timer_period_ns = int(1e9 / self.image_rate)
|
||
elif name == "image_topic": self.image_topic = p.value
|
||
elif name == "imu_topic": self.imu_topic = p.value
|
||
elif name == "scan_topic": self.scan_topic = p.value
|
||
elif name == "odom_topic": self.odom_topic = p.value
|
||
elif name == "power_topic": self.power_topic = p.value
|
||
elif 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):
|
||
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 _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):
|
||
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,
|
||
)
|
||
|
||
# ------------------------------------------------------------------
|
||
# 同步快速数据 — scan / odom / imu / power (统一定时器)
|
||
# ------------------------------------------------------------------
|
||
|
||
def _sync_fast_data(self):
|
||
"""在同一拍内拉取并发布 scan / odom / imu / power。"""
|
||
now = self.get_clock().now().to_msg()
|
||
|
||
# ---- LaserScan ----
|
||
scan_data = self._get_json("/scan")
|
||
if scan_data:
|
||
try:
|
||
self._publish_scan(scan_data, now)
|
||
except Exception:
|
||
self.get_logger().warn(
|
||
f"LaserScan 异常:\n{traceback.format_exc()}",
|
||
throttle_duration_sec=5.0,
|
||
)
|
||
|
||
# ---- Odometry ----
|
||
odom_data = self._get_json("/odom")
|
||
if odom_data:
|
||
try:
|
||
self._publish_odom(odom_data, now)
|
||
except Exception:
|
||
self.get_logger().warn(
|
||
f"Odometry 异常:\n{traceback.format_exc()}",
|
||
throttle_duration_sec=5.0,
|
||
)
|
||
|
||
# ---- IMU ----
|
||
imu_data = self._get_json("/imu")
|
||
if imu_data:
|
||
try:
|
||
self._publish_imu(imu_data, now)
|
||
except Exception:
|
||
self.get_logger().warn(
|
||
f"IMU 异常:\n{traceback.format_exc()}",
|
||
throttle_duration_sec=5.0,
|
||
)
|
||
|
||
# ---- Power ----
|
||
power_data = self._get_json("/power")
|
||
if power_data:
|
||
try:
|
||
self._publish_power(power_data)
|
||
except Exception:
|
||
self.get_logger().warn(
|
||
f"Power 异常:\n{traceback.format_exc()}",
|
||
throttle_duration_sec=5.0,
|
||
)
|
||
|
||
# ---- 各数据发布方法 ----
|
||
|
||
def _publish_scan(self, data: dict, now):
|
||
"""GET /scan → sensor_msgs/LaserScan
|
||
|
||
HTTP 返回格式:
|
||
angle_min, angle_max, angle_increment, range_min, range_max, ranges[]
|
||
"""
|
||
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 (Twist) → POST /cmd (差速模式 vx/wz)
|
||
# ------------------------------------------------------------------
|
||
|
||
def _cmd_vel_callback(self, msg: Twist):
|
||
"""将 Twist 转为差速控制指令。
|
||
|
||
Twist.linear.x → vx (线速度 m/s)
|
||
Twist.angular.z → wz (角速度 rad/s)
|
||
|
||
HTTP /cmd 只接受差速模式 {"vx": vx, "wz": wz}，
|
||
服务端内部通过 bicycle model 将 wz 转成阿克曼转向角。
|
||
"""
|
||
vx = float(msg.linear.x)
|
||
wz = float(msg.angular.z)
|
||
|
||
self._post("/cmd", {"vx": vx, "wz": wz})
|
||
|
||
self._cmd_count += 1
|
||
self.get_logger().debug(
|
||
f"cmd_vel → vx={vx:.2f}, wz={wz:.2f} (#{self._cmd_count})"
|
||
)
|
||
|
||
# ------------------------------------------------------------------
|
||
# 清理
|
||
# ------------------------------------------------------------------
|
||
|
||
def destroy_node(self):
|
||
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()
|