Organize host and robot streaming releases
This commit is contained in:
Mike Mi
23
robot/ros2/OmniSocketGo_robot_ros/ros2/README.md
Normal file
23
robot/ros2/OmniSocketGo_robot_ros/ros2/README.md
Normal file
@@ -0,0 +1,23 @@
|
||||
# omnisocket_camera_bridge
|
||||
|
||||
这是 OmniSocketGo robot 端的 ROS 2 RGB 桥接包。它订阅头部和腰部的 `sensor_msgs/msg/Image`，把最新帧写入固定大小的共享内存文件，供 C 视频管线读取。
|
||||
|
||||
```bash
|
||||
cd ~/OmniSocketGo_robot_ros/ros2
|
||||
source /opt/ros/jazzy/setup.bash
|
||||
colcon build
|
||||
source install/setup.bash
|
||||
ros2 run omnisocket_camera_bridge omnisocket_ros_camera_bridge
|
||||
```
|
||||
|
||||
参数：
|
||||
|
||||
```text
|
||||
head_topic /ob_camera_head/color/image_raw
|
||||
waist_topic /ob_camera_waist/color/image_raw
|
||||
head_shm /dev/shm/omnisocket-rgb-head
|
||||
waist_shm /dev/shm/omnisocket-rgb-waist
|
||||
max_frame_bytes 8294400
|
||||
```
|
||||
|
||||
支持 `rgb8`、`bgr8`、`rgba8`、`bgra8` 和 `mono8`。带行填充的 ROS 图像会被压缩为连续行后再写入共享内存；C 端据消息编码转换为 FFmpeg 像素格式。该包不访问 V4L2 设备。
|
||||
@@ -0,0 +1 @@
|
||||
"""ROS2 RGB bridge used by OmniSocketGo_robot_ros."""
|
||||
@@ -0,0 +1,217 @@
|
||||
#!/usr/bin/env python3
|
||||
"""Bridge ROS2 RGB images into the shared-memory input used by b_side_omnid.
|
||||
|
||||
The Orbbec ROS2 driver remains the only process that opens the physical camera.
|
||||
This node only subscribes to sensor_msgs/Image and publishes the latest frame for
|
||||
the C transport daemon. A bounded latest-frame slot is intentional: old video
|
||||
frames are discarded instead of increasing end-to-end latency.
|
||||
"""
|
||||
|
||||
import mmap
|
||||
import os
|
||||
import struct
|
||||
import threading
|
||||
from typing import Optional
|
||||
|
||||
import rclpy
|
||||
from rclpy.node import Node
|
||||
from rclpy.qos import QoSProfile, ReliabilityPolicy, HistoryPolicy, DurabilityPolicy
|
||||
from sensor_msgs.msg import Image
|
||||
|
||||
|
||||
MAGIC = 0x52494D47 # RIMG
|
||||
VERSION = 1
|
||||
HEADER_BYTES = 64
|
||||
HEADER_FORMAT = "<Q8I3Q"
|
||||
HEADER_SIZE = struct.calcsize(HEADER_FORMAT)
|
||||
DEFAULT_MAX_FRAME_BYTES = 1920 * 1080 * 4
|
||||
|
||||
ENCODING_RGB8 = 1
|
||||
ENCODING_BGR8 = 2
|
||||
ENCODING_RGBA8 = 3
|
||||
ENCODING_BGRA8 = 4
|
||||
ENCODING_MONO8 = 5
|
||||
|
||||
|
||||
def _encoding_info(encoding: str):
|
||||
normalized = encoding.lower()
|
||||
values = {
|
||||
"rgb8": (ENCODING_RGB8, 3),
|
||||
"bgr8": (ENCODING_BGR8, 3),
|
||||
"rgba8": (ENCODING_RGBA8, 4),
|
||||
"bgra8": (ENCODING_BGRA8, 4),
|
||||
"mono8": (ENCODING_MONO8, 1),
|
||||
}
|
||||
return values.get(normalized)
|
||||
|
||||
|
||||
class SharedImageSlot:
|
||||
"""One seqlock-protected latest-frame file."""
|
||||
|
||||
def __init__(self, path: str, max_frame_bytes: int):
|
||||
self.path = path
|
||||
self.max_frame_bytes = max_frame_bytes
|
||||
self.sequence = 0
|
||||
self.lock = threading.Lock()
|
||||
|
||||
parent = os.path.dirname(path)
|
||||
if parent:
|
||||
os.makedirs(parent, exist_ok=True)
|
||||
self.fd = os.open(path, os.O_CREAT | os.O_RDWR, 0o660)
|
||||
self.mapping_bytes = HEADER_BYTES + max_frame_bytes
|
||||
os.ftruncate(self.fd, self.mapping_bytes)
|
||||
self.mapping = mmap.mmap(self.fd, self.mapping_bytes, access=mmap.ACCESS_WRITE)
|
||||
struct.pack_into("<Q", self.mapping, 0, 0)
|
||||
|
||||
def close(self):
|
||||
self.mapping.flush()
|
||||
self.mapping.close()
|
||||
os.close(self.fd)
|
||||
|
||||
def write(self, msg: Image) -> bool:
|
||||
info = _encoding_info(msg.encoding)
|
||||
if info is None:
|
||||
return False
|
||||
encoding, bytes_per_pixel = info
|
||||
|
||||
width = int(msg.width)
|
||||
height = int(msg.height)
|
||||
step = int(msg.step)
|
||||
row_bytes = width * bytes_per_pixel
|
||||
if width <= 0 or height <= 0 or step < row_bytes:
|
||||
return False
|
||||
|
||||
data = bytes(msg.data)
|
||||
required = step * (height - 1) + row_bytes
|
||||
if len(data) < required:
|
||||
return False
|
||||
|
||||
# ROS Image permits row padding. The C side consumes packed rows.
|
||||
if step == row_bytes:
|
||||
payload = data[: row_bytes * height]
|
||||
else:
|
||||
payload = b"".join(
|
||||
data[row * step : row * step + row_bytes] for row in range(height)
|
||||
)
|
||||
if len(payload) > self.max_frame_bytes:
|
||||
return False
|
||||
|
||||
timestamp_ns = int(msg.header.stamp.sec) * 1_000_000_000 + int(
|
||||
msg.header.stamp.nanosec
|
||||
)
|
||||
with self.lock:
|
||||
odd_sequence = self.sequence + 1
|
||||
if odd_sequence % 2 == 0:
|
||||
odd_sequence += 1
|
||||
# Odd sequence means the slot is being written.
|
||||
struct.pack_into(
|
||||
HEADER_FORMAT,
|
||||
self.mapping,
|
||||
0,
|
||||
odd_sequence,
|
||||
MAGIC,
|
||||
VERSION,
|
||||
width,
|
||||
height,
|
||||
row_bytes,
|
||||
encoding,
|
||||
len(payload),
|
||||
0,
|
||||
timestamp_ns,
|
||||
0,
|
||||
0,
|
||||
)
|
||||
self.mapping[HEADER_BYTES : HEADER_BYTES + len(payload)] = payload
|
||||
even_sequence = odd_sequence + 1
|
||||
struct.pack_into("<Q", self.mapping, 0, even_sequence)
|
||||
self.sequence = even_sequence
|
||||
return True
|
||||
|
||||
|
||||
class RosCameraBridge(Node):
|
||||
def __init__(self):
|
||||
super().__init__("omnisocket_ros_camera_bridge")
|
||||
self.declare_parameter(
|
||||
"head_topic", "/ob_camera_head/color/image_raw"
|
||||
)
|
||||
self.declare_parameter(
|
||||
"waist_topic", "/ob_camera_waist/color/image_raw"
|
||||
)
|
||||
self.declare_parameter(
|
||||
"head_shm", "/dev/shm/omnisocket-rgb-head"
|
||||
)
|
||||
self.declare_parameter(
|
||||
"waist_shm", "/dev/shm/omnisocket-rgb-waist"
|
||||
)
|
||||
self.declare_parameter("max_frame_bytes", DEFAULT_MAX_FRAME_BYTES)
|
||||
|
||||
max_frame_bytes = int(self.get_parameter("max_frame_bytes").value)
|
||||
if max_frame_bytes <= 0:
|
||||
raise ValueError("max_frame_bytes must be positive")
|
||||
|
||||
head_shm = str(self.get_parameter("head_shm").value)
|
||||
waist_shm = str(self.get_parameter("waist_shm").value)
|
||||
self.head_slot = SharedImageSlot(head_shm, max_frame_bytes)
|
||||
self.waist_slot = SharedImageSlot(waist_shm, max_frame_bytes)
|
||||
self._unsupported = set()
|
||||
|
||||
qos = QoSProfile(
|
||||
history=HistoryPolicy.KEEP_LAST,
|
||||
depth=2,
|
||||
reliability=ReliabilityPolicy.BEST_EFFORT,
|
||||
durability=DurabilityPolicy.VOLATILE,
|
||||
)
|
||||
head_topic = str(self.get_parameter("head_topic").value)
|
||||
waist_topic = str(self.get_parameter("waist_topic").value)
|
||||
self.head_subscription = self.create_subscription(
|
||||
Image, head_topic, self._head_callback, qos
|
||||
)
|
||||
self.waist_subscription = self.create_subscription(
|
||||
Image, waist_topic, self._waist_callback, qos
|
||||
)
|
||||
self.get_logger().info(
|
||||
f"RGB bridge ready: head={head_topic} -> {head_shm}, "
|
||||
f"waist={waist_topic} -> {waist_shm}, max={max_frame_bytes} bytes"
|
||||
)
|
||||
|
||||
def _write(self, slot: SharedImageSlot, msg: Image, label: str):
|
||||
if _encoding_info(msg.encoding) is None:
|
||||
if msg.encoding not in self._unsupported:
|
||||
self._unsupported.add(msg.encoding)
|
||||
self.get_logger().error(
|
||||
f"unsupported {label} image encoding: {msg.encoding}"
|
||||
)
|
||||
return
|
||||
if not slot.write(msg):
|
||||
self.get_logger().warning(
|
||||
f"discarded invalid or oversized {label} image frame"
|
||||
)
|
||||
|
||||
def _head_callback(self, msg: Image):
|
||||
self._write(self.head_slot, msg, "head")
|
||||
|
||||
def _waist_callback(self, msg: Image):
|
||||
self._write(self.waist_slot, msg, "waist")
|
||||
|
||||
def destroy_node(self):
|
||||
self.head_slot.close()
|
||||
self.waist_slot.close()
|
||||
super().destroy_node()
|
||||
|
||||
|
||||
def main(args: Optional[list] = None):
|
||||
rclpy.init(args=args)
|
||||
node = None
|
||||
try:
|
||||
node = RosCameraBridge()
|
||||
rclpy.spin(node)
|
||||
except KeyboardInterrupt:
|
||||
pass
|
||||
finally:
|
||||
if node is not None:
|
||||
node.destroy_node()
|
||||
rclpy.shutdown()
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
@@ -0,0 +1,16 @@
|
||||
<?xml version="1.0"?>
|
||||
<package format="3">
|
||||
<name>omnisocket_camera_bridge</name>
|
||||
<version>0.1.0</version>
|
||||
<description>ROS2 RGB image bridge for OmniSocketGo_robot_ros.</description>
|
||||
<maintainer email="robot@example.invalid">OmniSocketGo maintainers</maintainer>
|
||||
<license>Proprietary</license>
|
||||
|
||||
<buildtool_depend>ament_python</buildtool_depend>
|
||||
<depend>rclpy</depend>
|
||||
<depend>sensor_msgs</depend>
|
||||
|
||||
<export>
|
||||
<build_type>ament_python</build_type>
|
||||
</export>
|
||||
</package>
|
||||
@@ -0,0 +1,4 @@
|
||||
[develop]
|
||||
script_dir=$base/lib/omnisocket_camera_bridge
|
||||
[install]
|
||||
install_scripts=$base/lib/omnisocket_camera_bridge
|
||||
@@ -0,0 +1,20 @@
|
||||
from setuptools import find_packages, setup
|
||||
|
||||
package_name = "omnisocket_camera_bridge"
|
||||
|
||||
setup(
|
||||
name=package_name,
|
||||
version="0.1.0",
|
||||
packages=find_packages(exclude=["test"]),
|
||||
data_files=[
|
||||
("share/ament_index/resource_index/packages", ["resource/" + package_name]),
|
||||
("share/" + package_name, ["package.xml"]),
|
||||
],
|
||||
install_requires=["setuptools"],
|
||||
zip_safe=True,
|
||||
entry_points={
|
||||
"console_scripts": [
|
||||
"omnisocket_ros_camera_bridge = omnisocket_camera_bridge.ros_camera_bridge:main",
|
||||
],
|
||||
},
|
||||
)
|
||||
Reference in New Issue
Block a user