传感器控制服务

约 4510 字大约 15 分钟

2025-01-27

提供机器人系统传感器（雷达/rgbd相机/三目相机）服务，通过SensorController可以通过RPC和话题方式实现对机器人的传感器进行指令控制和状态获取。

接口定义

SensorController 是封装机器人各类传感器的管理类，支持Lidar、RGBD 相机与三目相机的初始化、控制与数据订阅。

⚠️ Notice: 当前RGBD传感器接口暂未完全开放。

SensorController

项目	内容
类名	SensorController
构造函数	`controller = SensorController()`
功能概述	构造函数，初始化传感器控制器对象。
备注	构造内部状态。

initialize

项目	内容
函数名	initialize
函数声明	`bool initialize()`
功能概述	初始化控制器，包括资源申请和驱动加载。
返回值	`True` 表示成功，`False` 表示失败。
备注	调用前需先构造对象。

shutdown

项目	内容
函数名	shutdown
函数声明	`void shutdown()`
功能概述	关闭所有传感器连接并释放资源。
备注	配合 initialize 使用。

open_lidar

项目	内容
函数名	open_lidar
函数声明	`Status open_lidar()`
功能概述	打开雷达。
返回值	`Status` 对象，`Status.code == ErrorCode.OK` 表示成功。
备注	阻塞接口。

close_lidar

项目	内容
函数名	close_lidar
函数声明	`Status close_lidar()`
功能概述	关闭雷达。
返回值	`Status` 对象，`Status.code == ErrorCode.OK` 表示成功。
备注	阻塞接口，配合打开函数使用。

open_head_rgbd_camera

项目	内容
函数名	open_head_rgbd_camera
函数声明	`Status open_head_rgbd_camera()`
功能概述	打开头部 RGBD 相机（头部与腰部）。
返回值	`Status` 对象，`Status.code == ErrorCode.OK` 表示成功。
备注	阻塞接口。

close_head_rgbd_camera

项目	内容
函数名	close_head_rgbd_camera
函数声明	`Status close_head_rgbd_camera()`
功能概述	关闭头部 RGBD 相机。
返回值	`Status` 对象，`Status.code == ErrorCode.OK` 表示成功。
备注	阻塞接口，配合打开函数使用。

open_waist_rgbd_camera

项目	内容
函数名	open_waist_rgbd_camera
函数声明	`Status open_waist_rgbd_camera()`
功能概述	打开腰部 RGBD 相机（头部与腰部）。
返回值	`Status` 对象，`Status.code == ErrorCode.OK` 表示成功。
备注	阻塞接口。

close_waist_rgbd_camera

项目	内容
函数名	close_waist_rgbd_camera
函数声明	`Status close_waist_rgbd_camera()`
功能概述	关闭腰部 RGBD 相机。
返回值	`Status` 对象，`Status.code == ErrorCode.OK` 表示成功。
备注	阻塞接口，配合打开函数使用。

open_trinocular_camera

项目	内容
函数名	open_trinocular_camera
函数声明	`Status open_trinocular_camera()`
功能概述	打开三目相机。
返回值	`Status` 对象，`Status.code == ErrorCode.OK` 表示成功。
备注	阻塞接口。

close_trinocular_camera

项目	内容
函数名	close_trinocular_camera
函数声明	`Status close_trinocular_camera()`
功能概述	关闭三目相机。
返回值	`Status` 对象，`Status.code == ErrorCode.OK` 表示成功。
备注	阻塞接口。

项目	内容
函数名	subscribe_lidar_imu
函数声明	`void subscribe_lidar_imu(callback)`
功能概述	订阅雷达 IMU 数据
参数说明	callback：接收到数据后的处理函数, 签名为 callback(data: Imu) -> None
备注	非阻塞接口，回调函数会在数据更新时被调用。

项目	内容
函数名	subscribe_lidar_point_cloud
函数声明	`void subscribe_lidar_point_cloud(callback)`
功能概述	订阅雷达点云数据
参数说明	callback：接收到点云后的处理函数, 签名为 callback(data: PointCloud2) -> None
备注	非阻塞接口，回调函数会在数据更新时被调用。

项目	内容
函数名	subscribe_head_rgbd_color_image
函数声明	`void subscribe_head_rgbd_color_image(callback)`
功能概述	订阅头部 RGBD 彩色图像数据
参数说明	callback：接收到图像后的处理函数, 签名为 callback(data: Image) -> None
备注	非阻塞接口，回调函数会在数据更新时被调用。

项目	内容
函数名	subscribe_head_rgbd_color_camera_info
函数声明	`void subscribe_head_rgbd_color_camera_info(callback)`
功能概述	订阅头部 RGBD 彩色相机参数
参数说明	callback：接收到参数后的处理函数, 签名为 callback(data: CameraInfo) -> None
备注	非阻塞接口，回调函数会在数据更新时被调用。

项目	内容
函数名	subscribe_head_rgbd_depth_image
函数声明	`void subscribe_head_rgbd_depth_image(callback)`
功能概述	订阅头部 RGBD 深度图像数据
参数说明	callback：接收到图像后的处理函数, 签名为 callback(data: Image) -> None
备注	非阻塞接口，回调函数会在数据更新时被调用。

项目	内容
函数名	subscribe_head_rgbd_depth_camera_info
函数声明	`void subscribe_head_rgbd_depth_camera_info(callback)`
功能概述	订阅头部 RGBD 深度相机参数
参数说明	callback：接收到参数后的处理函数, 签名为 callback(data: CameraInfo) -> None
备注	非阻塞接口，回调函数会在数据更新时被调用。

项目	内容
函数名	subscribe_waist_rgbd_color_image
函数声明	`void subscribe_waist_rgbd_color_image(callback)`
功能概述	订阅腰部 RGBD 彩色图像数据
参数说明	callback：接收到图像后的处理函数, 签名为 callback(data: Image) -> None
备注	非阻塞接口，回调函数会在数据更新时被调用。

项目	内容
函数名	subscribe_waist_rgbd_color_camera_info
函数声明	`void subscribe_waist_rgbd_color_camera_info(callback)`
功能概述	订阅腰部 RGBD 彩色相机参数
参数说明	callback：接收到参数后的处理函数, 签名为 callback(data: CameraInfo) -> None
备注	非阻塞接口，回调函数会在数据更新时被调用。

项目	内容
函数名	subscribe_waist_rgbd_depth_image
函数声明	`void subscribe_waist_rgbd_depth_image(callback)`
功能概述	订阅腰部 RGBD 深度图像数据
参数说明	callback：接收到图像后的处理函数, 签名为 callback(data: Image) -> None
备注	非阻塞接口，回调函数会在数据更新时被调用。

项目	内容
函数名	subscribe_waist_rgbd_depth_camera_info
函数声明	`void subscribe_waist_rgbd_depth_camera_info(callback)`
功能概述	订阅腰部 RGBD 深度相机参数
参数说明	callback：接收到参数后的处理函数, 签名为 callback(data: CameraInfo) -> None
备注	非阻塞接口，回调函数会在数据更新时被调用。

项目	内容
函数名	subscribe_trinocular_image
函数声明	`void subscribe_trinocular_image(callback)`
功能概述	订阅三目相机图像帧
参数说明	callback：接收到图像帧后的处理函数, 签名为 callback(data: TrinocularCameraFrame) -> None
备注	非阻塞接口，回调函数会在数据更新时被调用。

类型定义

`Imu` — IMU 数据结构体

字段名	类型	描述
`timestamp`	`int64`	时间戳（单位：纳秒）
`orientation`	`list[float]`	姿态四元数（w, x, y, z）
`angular_velocity`	`list[float]`	角速度（单位：rad/s）
`linear_acceleration`	`list[float]`	线加速度（单位：m/s²）
`temperature`	`float`	温度（单位：摄氏度或其他，应明确单位）

字段名	类型	描述
`stamp`	`int64`	时间戳（单位：纳秒）
`frame_id`	`str`	坐标系名称

`PointField` — 点字段描述

字段名	类型	描述
`name`	`str`	字段名（如 x、y、z、intensity）
`offset`	`int32`	起始字节偏移
`datatype`	`int8`	数据类型（对应常量）
`count`	`int32`	每点此字段包含元素数量

`PointCloud2` — 点云数据结构体

字段名	类型	描述
`header`	`Header`	消息头
`height`	`int32`	行数
`width`	`int32`	列数
`fields`	`list[PointField]`	点字段数组
`is_bigendian`	`bool`	字节序
`point_step`	`int32`	每个点的字节数
`row_step`	`int32`	每行的字节数
`data`	`bytes`	原始点云字节数据
`is_dense`	`bool`	是否稠密点云

`Image` — 图像数据结构体

字段名	类型	描述
`header`	`Header`	消息头
`height`	`int32`	图像高度（像素）
`width`	`int32`	图像宽度（像素）
`encoding`	`str`	编码类型（如 rgb8, mono8）
`is_bigendian`	`bool`	是否为大端模式
`step`	`int32`	每行图像占用字节数
`data`	`bytes`	原始图像字节数据

`CameraInfo` — 相机内参与畸变信息

字段名	类型	描述
`header`	`Header`	消息头
`height`	`int32`	图像高度
`width`	`int32`	图像宽度
`distortion_model`	`str`	畸变模型（如 plumb_bob）
`D`	`list[float]`	畸变参数数组
`K`	`list[float]`	相机内参矩阵（3x3）
`R`	`list[float]`	矫正矩阵（3x3）
`P`	`list[float]`	投影矩阵（3x4）
`binning_x`	`int32`	水平 binning 系数
`binning_y`	`int32`	垂直 binning 系数
`roi_x_offset`	`int32`	ROI 起始 x 坐标
`roi_y_offset`	`int32`	ROI 起始 y 坐标
`roi_height`	`int32`	ROI 高度
`roi_width`	`int32`	ROI 宽度
`roi_do_rectify`	`bool`	是否进行矫正

`TrinocularCameraFrame` — 三目相机帧数据结构

字段名	类型	描述
`header`	`Header`	通用消息头
`vin_time`	`int64`	图像采集时间（纳秒）
`decode_time`	`int64`	图像解码时间（纳秒）
`imgfl_array`	`bytes`	左目图像数据
`imgf_array`	`bytes`	中目图像数据
`imgfr_array`	`bytes`	右目图像数据