Sensor Control Service

About 3805 wordsAbout 13 min

2025-01-27

Provides robot system sensor (LiDAR/RGBD camera/trinocular camera) service. Through SensorController, you can control robot sensors and obtain status via RPC and topic methods.

Interface Definition

SensorController is a management class that encapsulates various robot sensors, supporting initialization, control, and data subscription of LiDAR, RGBD cameras, and trinocular cameras.

⚠️ Notice: Current trinocular and RGBD sensor interfaces are not fully open yet.

SensorController

Item	Content
Class Name	SensorController
Constructor	`controller = SensorController()`
Function Overview	Constructor, initializes sensor controller object.
Notes	Constructs internal state.

initialize

Item	Content
Function Name	initialize
Function Declaration	`bool initialize()`
Function Overview	Initialize controller, including resource allocation and driver loading.
Return Value	`True` indicates success, `False` indicates failure.
Notes	Object must be constructed before calling.

shutdown

Item	Content
Function Name	shutdown
Function Declaration	`void shutdown()`
Function Overview	Close all sensor connections and release resources.
Notes	Used in conjunction with initialize.

open_lidar

Item	Content
Function Name	open_lidar
Function Declaration	`Status open_lidar()`
Function Overview	Open LiDAR.
Return Value	`Status` object, `Status.code == ErrorCode.OK` indicates success.
Notes	Blocking interface.

close_lidar

Item	Content
Function Name	close_lidar
Function Declaration	`Status close_lidar()`
Function Overview	Close LiDAR.
Return Value	`Status` object, `Status.code == ErrorCode.OK` indicates success.
Notes	Blocking interface, used in conjunction with open function.

open_head_rgbd_camera

Item	Content
Function Name	open_head_rgbd_camera
Function Declaration	`Status open_head_rgbd_camera()`
Function Overview	Open head RGBD camera (head and waist).
Return Value	`Status` object, `Status.code == ErrorCode.OK` indicates success.
Notes	Blocking interface.

close_head_rgbd_camera

Item	Content
Function Name	close_head_rgbd_camera
Function Declaration	`Status close_head_rgbd_camera()`
Function Overview	Close head RGBD camera.
Return Value	`Status` object, `Status.code == ErrorCode.OK` indicates success.
Notes	Blocking interface, used in conjunction with open function.

open_waist_rgbd_camera

Item	Content
Function Name	open_waist_rgbd_camera
Function Declaration	`Status open_waist_rgbd_camera()`
Function Overview	Open waist RGBD camera (head and waist).
Return Value	`Status` object, `Status.code == ErrorCode.OK` indicates success.
Notes	Blocking interface.

close_waist_rgbd_camera

Item	Content
Function Name	close_waist_rgbd_camera
Function Declaration	`Status close_waist_rgbd_camera()`
Function Overview	Close waist RGBD camera.
Return Value	`Status` object, `Status.code == ErrorCode.OK` indicates success.
Notes	Blocking interface, used in conjunction with open function.

open_trinocular_camera

Item	Content
Function Name	open_trinocular_camera
Function Declaration	`Status open_trinocular_camera()`
Function Overview	Open trinocular camera.
Return Value	`Status` object, `Status.code == ErrorCode.OK` indicates success.
Notes	Blocking interface.

close_trinocular_camera

Item	Content
Function Name	close_trinocular_camera
Function Declaration	`Status close_trinocular_camera()`
Function Overview	Close trinocular camera.
Return Value	`Status` object, `Status.code == ErrorCode.OK` indicates success.
Notes	Blocking interface.

Item	Content
Function Name	subscribe_lidar_imu
Function Declaration	`void subscribe_lidar_imu(callback)`
Function Overview	Subscribe to LiDAR IMU data
Parameter Description	callback: Data processing function after receiving data, signature is callback(data: Imu) -> None
Notes	Non-blocking interface, callback function will be called when data is updated.

Item	Content
Function Name	subscribe_lidar_point_cloud
Function Declaration	`void subscribe_lidar_point_cloud(callback)`
Function Overview	Subscribe to LiDAR point cloud data
Parameter Description	callback: Point cloud processing function after receiving data, signature is callback(data: PointCloud2) -> None
Notes	Non-blocking interface, callback function will be called when data is updated.

Item	Content
Function Name	subscribe_head_rgbd_color_image
Function Declaration	`void subscribe_head_rgbd_color_image(callback)`
Function Overview	Subscribe to head RGBD color image data
Parameter Description	callback: Image processing function after receiving data, signature is callback(data: Image) -> None
Notes	Non-blocking interface, callback function will be called when data is updated.

Item	Content
Function Name	subscribe_head_rgbd_color_camera_info
Function Declaration	`void subscribe_head_rgbd_color_camera_info(callback)`
Function Overview	Subscribe to head RGBD color camera parameters
Parameter Description	callback: Parameter processing function after receiving data, signature is callback(data: CameraInfo) -> None
Notes	Non-blocking interface, callback function will be called when data is updated.

Item	Content
Function Name	subscribe_head_rgbd_depth_image
Function Declaration	`void subscribe_head_rgbd_depth_image(callback)`
Function Overview	Subscribe to head RGBD depth image data
Parameter Description	callback: Image processing function after receiving data, signature is callback(data: Image) -> None
Notes	Non-blocking interface, callback function will be called when data is updated.

Item	Content
Function Name	subscribe_head_rgbd_depth_camera_info
Function Declaration	`void subscribe_head_rgbd_depth_camera_info(callback)`
Function Overview	Subscribe to head RGBD depth camera parameters
Parameter Description	callback: Parameter processing function after receiving data, signature is callback(data: CameraInfo) -> None
Notes	Non-blocking interface, callback function will be called when data is updated.

Item	Content
Function Name	subscribe_waist_rgbd_color_image
Function Declaration	`void subscribe_waist_rgbd_color_image(callback)`
Function Overview	Subscribe to waist RGBD color image data
Parameter Description	callback: Image processing function after receiving data, signature is callback(data: Image) -> None
Notes	Non-blocking interface, callback function will be called when data is updated.

Item	Content
Function Name	subscribe_waist_rgbd_color_camera_info
Function Declaration	`void subscribe_waist_rgbd_color_camera_info(callback)`
Function Overview	Subscribe to waist RGBD color camera parameters
Parameter Description	callback: Parameter processing function after receiving data, signature is callback(data: CameraInfo) -> None
Notes	Non-blocking interface, callback function will be called when data is updated.

Item	Content
Function Name	subscribe_waist_rgbd_depth_image
Function Declaration	`void subscribe_waist_rgbd_depth_image(callback)`
Function Overview	Subscribe to waist RGBD depth image data
Parameter Description	callback: Image processing function after receiving data, signature is callback(data: Image) -> None
Notes	Non-blocking interface, callback function will be called when data is updated.

Item	Content
Function Name	subscribe_waist_rgbd_depth_camera_info
Function Declaration	`void subscribe_waist_rgbd_depth_camera_info(callback)`
Function Overview	Subscribe to waist RGBD depth camera parameters
Parameter Description	callback: Parameter processing function after receiving data, signature is callback(data: CameraInfo) -> None
Notes	Non-blocking interface, callback function will be called when data is updated.

Item	Content
Function Name	subscribe_trinocular_image
Function Declaration	`void subscribe_trinocular_image(callback)`
Function Overview	Subscribe to trinocular camera image frames
Parameter Description	callback: Image frame processing function after receiving data, signature is callback(data: TrinocularCameraFrame) -> None
Notes	Non-blocking interface, callback function will be called when data is updated.

Type Definitions

`Imu` — IMU Data Structure

Field Name	Type	Description
`timestamp`	`int64`	Timestamp (unit: nanoseconds)
`orientation`	`list[float]`	Attitude quaternion (w, x, y, z)
`angular_velocity`	`list[float]`	Angular velocity (unit: rad/s)
`linear_acceleration`	`list[float]`	Linear acceleration (unit: m/s²)
`temperature`	`float`	Temperature (unit: Celsius or other, unit should be specified)

`Header` — Common Message Header Structure

Field Name	Type	Description
`stamp`	`int64`	Timestamp (unit: nanoseconds)
`frame_id`	`str`	Coordinate frame name

`PointField` — Point Field Description

Field Name	Type	Description
`name`	`str`	Field name (such as x, y, z, intensity)
`offset`	`int`	Starting byte offset
`datatype`	`int8`	Data type (corresponding constant)
`count`	`int`	Number of elements this field contains per point

`PointCloud2` — Point Cloud Data Structure

Field Name	Type	Description
`header`	`Header`	Message header
`height`	`int`	Number of rows
`width`	`int`	Number of columns
`fields`	`list[PointField]`	Point field array
`is_bigendian`	`bool`	Byte order
`point_step`	`int`	Number of bytes per point
`row_step`	`int`	Number of bytes per row
`data`	`bytes`	Raw point cloud byte data
`is_dense`	`bool`	Whether dense point cloud

`Image` — Image Data Structure

Field Name	Type	Description
`header`	`Header`	Message header
`height`	`int`	Image height (pixels)
`width`	`int`	Image width (pixels)
`encoding`	`str`	Encoding type (such as rgb8, mono8)
`is_bigendian`	`bool`	Whether big-endian mode
`step`	`int`	Number of bytes per image row
`data`	`bytes`	Raw image byte data

`CameraInfo` — Camera Intrinsic Parameters and Distortion Information

Field Name	Type	Description
`header`	`Header`	Message header
`height`	`int`	Image height
`width`	`int`	Image width
`distortion_model`	`str`	Distortion model (such as plumb_bob)
`D`	`list[float]`	Distortion parameter array
`K`	`list[float]`	Camera intrinsic parameter matrix (3x3)
`R`	`list[float]`	Rectification matrix (3x3)
`P`	`list[float]`	Projection matrix (3x4)
`binning_x`	`int`	Horizontal binning coefficient
`binning_y`	`int`	Vertical binning coefficient
`roi_x_offset`	`int`	ROI starting x coordinate
`roi_y_offset`	`int`	ROI starting y coordinate
`roi_height`	`int`	ROI height
`roi_width`	`int`	ROI width
`roi_do_rectify`	`bool`	Whether to perform rectification

`TrinocularCameraFrame` — Trinocular Camera Frame Data Structure

Field Name	Type	Description
`header`	`Header`	Common message header
`vin_time`	`int64`	Image acquisition time (nanoseconds)
`decode_time`	`int64`	Image decoding time (nanoseconds)
`imgfl_array`	`bytes`	Left eye image data
`imgf_array`	`bytes`	Center eye image data
`imgfr_array`	`bytes`	Right eye image data