提供机器人系统传感器（雷达/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_binocular_camera

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

close_binocular_camera

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

subscribe_lidar_imu

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

subscribe_lidar_point_cloud

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

subscribe_head_rgbd_color_image

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

subscribe_head_rgbd_depth_image

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

subscribe_head_rgbd_camera_info

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

subscribe_binocular_image

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

subscribe_binocular_camera_info

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

数据类型

Imu

IMU 数据结构，包含姿态、角速度、线性加速度等传感器数据。

字段名	类型	描述
timestamp	int64_t	时间戳（纳秒）
temperature	float	温度（摄氏度）
orientation	Quaternion	姿态四元数
angular_velocity	Vector3	角速度（rad/s）
linear_acceleration	Vector3	线性加速度（m/s²）

PointCloud2

点云数据结构，包含三维点云信息。

字段名	类型	描述
header	Header	标准消息头（时间戳+frame_id）
height	int32_t	点云高度（行数）
width	int32_t	点云宽度（列数）
fields	list[PointField]	点字段数组
is_bigendian	bool	字节序
point_step	int32_t	每个点占用的字节数
row_step	int32_t	每行占用的字节数
data	list[uint8_t]	原始点云数据（按字段打包）
is_dense	bool	是否为稠密点云（无无效点）

Image

图像数据结构，支持多种编码格式。

字段名	类型	描述
header	Header	标准消息头（时间戳+frame_id）
height	int32_t	图像高度（像素）
width	int32_t	图像宽度（像素）
encoding	str	图像编码类型，如 "rgb8", "mono8", "bgr8"
is_bigendian	bool	数据是否为大端模式
step	int32_t	每行图像占用的字节数
data	list[uint8_t]	原始图像字节数据

CameraInfo

相机内参与畸变信息，通常与 Image 消息一起发布。

字段名	类型	描述
header	Header	标准消息头（时间戳+frame_id）
height	int32_t	图像高度（行数）
width	int32_t	图像宽度（列数）
distortion_model	str	畸变模型，例如 "plumb_bob"
D	list[double]	畸变参数数组
K	list[double]	相机内参矩阵（9个元素）
R	list[double]	矫正矩阵（9个元素）
P	list[double]	投影矩阵（12个元素）
binning_x	int32_t	水平binning系数
binning_y	int32_t	垂直binning系数
roi_x_offset	int32_t	ROI起始x
roi_y_offset	int32_t	ROI起始y
roi_height	int32_t	ROI高度
roi_width	int32_t	ROI宽度
roi_do_rectify	bool	是否进行矫正

BinocularCameraFrame

双目相机帧数据结构，包含格式和图像帧。

字段名	类型	描述
header	Header	通用消息头（时间戳+frame_id）
format	str	图像格式
data	list[uint8_t]	左目和右目拼接图像数据，左半为左目图像，右半为右目图像