diff --git a/README.md b/README.md index 3621063..4871443 100644 --- a/README.md +++ b/README.md @@ -1,28 +1,45 @@ # 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 /imu ──→ 填充字段 ──→ sensor_msgs/Imu /imu -POST /cmd ←── speed/steer ←── geometry_msgs/Twist /cmd_vel +GET /image ──→ cv_bridge 解码 ──→ sensor_msgs/Image /image +GET /imu ──→ 填充字段 ──→ sensor_msgs/Imu /imu/data_raw +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 客户端 | -| `sensor_msgs` | Image / Imu 消息 | -| `geometry_msgs` | Twist 消息 (cmd_vel) | -| `cv_bridge` | OpenCV ↔ ROS Image 转换 | -| `opencv-python` | 图像解码 | -| `numpy` | 图像数组处理 | -| `requests` | HTTP 请求 | +| `sensor_msgs` | Image / Imu / LaserScan | +| `nav_msgs` | Odometry | +| `geometry_msgs` | Twist / Quaternion | +| `std_msgs` | Float32 (PowerVoltage) | +| `cv_bridge` | OpenCV ↔ ROS Image | +| `opencv-python` | JPEG 解码 | +| `numpy` | 图像数组 | +| `requests` | HTTP 客户端 | ## 安装 @@ -35,15 +52,11 @@ source install/setup.bash ## 使用 -### launch 启动（推荐） - ```bash +# launch 启动（推荐） 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 ``` @@ -51,6 +64,7 @@ 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 sync_rate 20.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 端口 | +| `timeout` | `1.0` | HTTP 请求超时 (s) | +| `sync_rate` | `30.0` | scan/odom/imu/power 统一同步频率 (Hz) | +| `image_rate` | `10.0` | 图像独立拉取频率 (Hz) | | `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` | 控制指令订阅话题 | -| `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}` | -| 端点 | 方法 | 说明 | -|------|------|------| -| `/image` | GET | 前置摄像头 JPEG 图像 | -| `/imu` | GET | IMU 数据 (MPU6050) JSON | -| `/state` | GET | 小车状态 JSON | -| `/cmd` | POST | 发送控制指令 `{"speed": 1.0, "steer": 0.3}` | +### /cmd 控制模式 + +HTTP `/cmd` 使用差速模式 `{"vx": vx, "wz": wz}`（匹配串口协议，与 `Twist` 语义一致），服务端内部通过 bicycle model 将 `wz` 转为阿克曼转向角。 + +```json +{"vx": 1.0, "wz": 0.5} +``` ## 项目结构 ``` origincar_bridge/ +├── README.md +├── .gitignore ├── package.xml # ROS2 包清单 ├── setup.py # ament_python 构建入口 ├── setup.cfg @@ -91,5 +119,5 @@ origincar_bridge/ │ └── bridge.launch.py # 启动文件 └── origincar_bridge/ ├── __init__.py - └── bridge_node.py # 桥接节点主程序 + └── bridge_node.py # ★ 全功能桥接节点 ``` diff --git a/launch/bridge.launch.py b/launch/bridge.launch.py index d4a7afa..6cc72de 100644 --- a/launch/bridge.launch.py +++ b/launch/bridge.launch.py @@ -9,78 +9,77 @@ 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", + "http_host", default_value="192.168.11.159", description="仿真机 IP 地址", ) declare_http_port = DeclareLaunchArgument( - "http_port", - default_value="8765", + "http_port", default_value="8765", 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( - "image_topic", - default_value="/image", + "image_topic", default_value="/image", description="图像发布话题", ) declare_imu_topic = DeclareLaunchArgument( - "imu_topic", - default_value="/imu", + "imu_topic", default_value="/imu/data_raw", 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( - "cmd_vel_topic", - default_value="/cmd_vel", + "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"), - } - ], + parameters=[{ + "http_host": LaunchConfiguration("http_host"), + "http_port": LaunchConfiguration("http_port"), + "timeout": LaunchConfiguration("timeout"), + "sync_rate": LaunchConfiguration("sync_rate"), + "image_rate": LaunchConfiguration("image_rate"), + "image_topic": LaunchConfiguration("image_topic"), + "imu_topic": LaunchConfiguration("imu_topic"), + "scan_topic": LaunchConfiguration("scan_topic"), + "odom_topic": LaunchConfiguration("odom_topic"), + "power_topic": LaunchConfiguration("power_topic"), + "cmd_vel_topic": LaunchConfiguration("cmd_vel_topic"), + }], ) - 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, - ] - ) + return LaunchDescription([ + declare_http_host, declare_http_port, declare_timeout, + declare_sync_rate, declare_image_rate, + declare_image_topic, declare_imu_topic, declare_scan_topic, + declare_odom_topic, declare_power_topic, declare_cmd_vel_topic, + bridge_node, + ]) diff --git a/origincar_bridge/bridge_node.py b/origincar_bridge/bridge_node.py index 3ef7690..b6962d3 100644 --- a/origincar_bridge/bridge_node.py +++ b/origincar_bridge/bridge_node.py @@ -2,27 +2,29 @@ """ 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 控制小车 +将 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: 仿真机 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) + 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.1.100 - ros2 launch origincar_bridge bridge.launch.py 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.11.159 """ -import time import traceback import cv2 @@ -30,33 +32,39 @@ 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 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 话题双向桥接。""" + """Origincar 仿真 HTTP API ⇄ ROS2 话题 全功能桥接节点。""" def __init__(self): 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("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.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 ---------- @@ -64,7 +72,10 @@ class OrigincarBridge(Node): # ---------- 发布者 ---------- 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( @@ -72,69 +83,71 @@ class OrigincarBridge(Node): ) # ---------- 定时器 ---------- - image_period = 1.0 / self.image_rate - imu_period = 1.0 / self.imu_rate + # 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.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 + # ---------- 统计 & Session ---------- 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" + 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.image_topic = self.get_parameter("image_topic").value - self.imu_topic = self.get_parameter("imu_topic").value + 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.image_rate = self.get_parameter("image_rate").value - self.imu_rate = self.get_parameter("imu_rate").value - self.timeout = self.get_parameter("timeout").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): - """运行时参数变更回调。允许动态调整 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": + 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 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 + 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 基础方法 + # HTTP 基础 # ------------------------------------------------------------------ @property @@ -142,7 +155,6 @@ class OrigincarBridge(Node): 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 @@ -151,7 +163,7 @@ class OrigincarBridge(Node): return r except requests.ConnectionError: self.get_logger().warn( - f"连接仿真机 {self._base_url} 失败，请确认仿真已启动", + f"连接失败 {self._base_url}，仿真是否启动？", throttle_duration_sec=5.0, ) except requests.Timeout: @@ -165,8 +177,18 @@ class OrigincarBridge(Node): ) 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): - """POST 请求（不关心返回值）。""" try: self._session.post( 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): @@ -206,75 +228,184 @@ class OrigincarBridge(Node): ) # ------------------------------------------------------------------ - # IMU — GET /imu → sensor_msgs/Imu + # 同步快速数据 — scan / odom / imu / power (统一定时器) # ------------------------------------------------------------------ - def _fetch_and_publish_imu(self): - r = self._get("/imu") - if r is None or r.status_code != 200: - return + def _sync_fast_data(self): + """在同一拍内拉取并发布 scan / odom / imu / power。""" + now = self.get_clock().now().to_msg() - try: - data = r.json() - msg = Imu() - msg.header.stamp = self.get_clock().now().to_msg() - msg.header.frame_id = "imu_link" + # ---- 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, + ) - # 姿态四元数 - 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) + # ---- 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, + ) - # 角速度 (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) + # ---- 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, + ) - # 线加速度 (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) + # ---- 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, + ) - # 协方差置 -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: - self.get_logger().warn( - f"IMU 处理异常:\n{traceback.format_exc()}", - throttle_duration_sec=5.0, - ) + 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 → POST /cmd + # 控制 — ROS2 /cmd_vel (Twist) → POST /cmd (差速模式 vx/wz) # ------------------------------------------------------------------ def _cmd_vel_callback(self, msg: Twist): - """将 Twist 消息转换为 HTTP /cmd 控制指令。 + """将 Twist 转为差速控制指令。 - Twist.linear.x → speed (m/s) - Twist.angular.z → steer (rad) + Twist.linear.x → vx (线速度 m/s) + Twist.angular.z → wz (角速度 rad/s) + + HTTP /cmd 只接受差速模式 {"vx": vx, "wz": wz}， + 服务端内部通过 bicycle model 将 wz 转成阿克曼转向角。 """ - speed = float(msg.linear.x) - steer = float(msg.angular.z) + vx = float(msg.linear.x) + wz = float(msg.angular.z) - self._post("/cmd", {"speed": speed, "steer": steer}) + self._post("/cmd", {"vx": vx, "wz": wz}) self._cmd_count += 1 self.get_logger().debug( - f"发送控制: speed={speed:.2f}, steer={steer:.2f}" - f" (总计 {self._cmd_count})" + f"cmd_vel → vx={vx:.2f}, wz={wz:.2f} (#{self._cmd_count})" ) + # ------------------------------------------------------------------ + # 清理 + # ------------------------------------------------------------------ + def destroy_node(self): - """关闭 HTTP Session 后再销毁节点。""" self._session.close() super().destroy_node() diff --git a/package.xml b/package.xml index 35e0229..284bb88 100644 --- a/package.xml +++ b/package.xml @@ -3,7 +3,10 @@ origincar_bridge 1.0.0 - ROS2 bridge relay: HTTP API ⇄ ROS2 topics (image, IMU, /cmd_vel) + + ROS2 bridge relay for Origincar Simulation HTTP API. + Bridges /image, /imu, /scan, /odom, /power (HTTP→ROS2) and /cmd_vel→/cmd (ROS2→HTTP). + cyy Apache-2.0 @@ -11,7 +14,9 @@ rclpy sensor_msgs + nav_msgs geometry_msgs + std_msgs cv_bridge python3-numpy python3-opencv diff --git a/setup.py b/setup.py index 057b06a..efc2d24 100644 --- a/setup.py +++ b/setup.py @@ -15,7 +15,7 @@ setup( zip_safe=True, maintainer="cyy", 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", entry_points={ "console_scripts": [