RGBD图像采集
功能介绍
为实现环境感知能力，机器人产品中通常会搭载摄像头、ToF等类型的传感器。为降低用户传感器适配和使用成本，TogetheROS.Bot会对多种常用传感器进行封装，并抽象成hobot_sensor模块，支持ROS标准图像消息，自定义图像消息输出以及相机标定数据发布。目前已支持的 RGBD 传感器类型如下所示：
| 类型 | 型号 | 规格 |
|---|---|---|
| 摄像头 | CP3AM | 200W |
支持平台
| 平台 | 运行方式 | 示例功能 |
|---|---|---|
| 旭日X3派 | Ubuntu 20.04 | 启动RGBD摄像头，并在PC端通过rviz2预览RGB图和深度图 |
准备工作
X3平台
确认摄像头正确接入旭日X3派，RGBD模组的接入方式如下图：
注意：RGBD模组需要额外转接板才能接到旭日X3派上。
旭日X3派已烧录好地平线提供的Ubuntu 20.04系统镜像。
旭日X3派已成功安装tros.b
确认PC机能够通过网络访问旭日X3派
PC端需安装 ros2 foxy 版本和 rviz2，安装命令如下：
sudo apt install ros-foxy-rviz-common ros-foxy-rviz-default-plugins ros-foxy-rviz2
使用方式
旭日X3派
下面以 CP3AM 为例，介绍摄像头数据获取和预览的方法：
通过SSH登录旭日X3派，并通过下述命令启动hobot_sensor节点
# 配置 tros.b 环境： source /opt/tros/local_setup.bash cp -r /opt/tros/lib/rgbd_sensor/parameter . # lanuch 方式启动 ros2 launch rgbd_sensor rgbd_sensor.launch.py
如程序输出如下信息，说明节点已成功启动
[WARN] [1654573498.706920307] [example]: [wuwl]->This is rgbd! sh: 1: echo: echo: I/O error pipeId[1], mipiIdx[1], vin_vps_mode[3] [ERROR]["LOG"][irs2381c_utility.c:192] 2381 enter sensor_init_setting [ERROR]["LOG"][irs2381c_utility.c:200] start write 2381c reg camera read reg: 0xa001 val:0x7 ... [ERROR]["LOG"][irs2381c_utility.c:207] end write 2381c reg HB_MIPI_InitSensor end HB_MIPI_SetDevAttr end pstHbVideoDev->vin_fd = 29 sensorID: 634-2362-2676-68d0 find local calib_file find local calib_file SDK Version: V4.4.35 build 20220525 09:27:53. read file(./calib-0634-2362-2676-68d0.bin), ok, file_len=132096, read_len=132096....... module config file(user custom) is: ./parameter/T00P11A-17.ini. parse calib data, data len:132096... sunny_degzip2 decode_len=155575. calib data with crc. parse calib data, ok. max roi (firstly): (0, 224, 0, 128). cur roi (firstly): (0, 224, 0, 128). HB_MIPI_InitSensor end HB_MIPI_SetDevAttr end pstHbVideoDev->vin_fd = 55 vencChnAttr.stRcAttr.enRcMode=11 mmzAlloc paddr = 0x1a6e6000, vaddr = 0x917e1000 camera read reg: 0x9400 val:0x1 ... [wuwl-StartCamera]->camT=3, ret=0. camera read reg: 0x3e val:0x40 [ERROR]["vio_devop"][utils/dev_ioctl.c:121] [499334.399304]dev_node_dqbuf_ispoll[121]: failed to ioctl: dq (14 - Bad address) [ERROR]["vio_devop"][utils/dev_ioctl.c:189] [499334.399355]entity_node_dqbuf_ispoll[189]: dev type(9) dq failed [ERROR]["vio_core"][commom_grp/binding_main.c:1034] [499334.399371]comm_dq_no_data[1034]: G1 MIPI_SIF_MODULE module chn0 dq failed! maybe framedrop error_detail -14 [wuwl-StartCamera]->camT=1, ret=0. [INFO] [1654573500.226606117] [rclcpp]: [childStart]-> ret=0 ! [INFO] [1654573500.226831567] [rclcpp]: [StartStream]->pthread create sucess [INFO] [1654573500.226963854] [rclcpp]: <========>[doCapStreamLoop]->Start. [WARN] [1654573500.226998103] [rgbd_node]: [RgbdNode]->mipinode init sucess. [WARN] [1654573500.227352507] [example]: [wuwl]->rgbd init! [WARN] [1654573500.228502174] [example]: [wuwl]->rgbd add_node! [INFO] [1662723985.860666547] [rgbd_node]: publish camera info. [INFO] [1662723985.866077156] [rgbd_node]: [pub_ori_pcl]->pub pcl w:h=24192:1,nIdx-24192:sz=24192. [INFO] [1662723985.876428980] [rgbd_node]: [timer_ros_pub]->pub dep w:h=224:129,sz=982464, infra w:h=224:108, sz=24192. [INFO] [1662723985.946767230] [rgbd_node]: publish camera info. [INFO] [1662723985.951415418] [rgbd_node]: [pub_ori_pcl]->pub pcl w:h=24192:1,nIdx-24192:sz=24192. [INFO] [1662723985.960161280] [rgbd_node]: [timer_ros_pub]->pub dep w:h=224:129,sz=982464, infra w:h=224:108, sz=24192. ...
PC机上查询当前话题，查询命令及返回结果如下：
source /opt/ros/foxy/local_setup.bash ros2 topic list输出：
/rgbd_CP3AM/depth/image_rect_raw /rgbd_CP3AM/depth/color/points /rgbd_CP3AM/color/camera_info /rgbd_CP3AM/aligned_depth_to_color/color/points /rgbd_CP3AM/infra/image_rect_raw /rgbd_CP3AM/color/image_rect_raw /parameter_events /rosout
PC机上订阅话题，并预览摄像头数据
source /opt/ros/foxy/local_setup.bash ros2 run rviz2 rviz2在 rviz2 界面上点击 add 按钮，添加rgbd_sensor 所发布 topic （参见目录3所标示的 rgbd_CP3AM 相关 topic），订阅点云需要把rviz2 配置的Global Options 里面的选项“Fixed Frame”修改为 “depth”，就可以观看实时点云信息。在 point 话题配置中，里面point type 选择points 即可。
在PC机上查询相机内参
source /opt/ros/foxy/local_setup.bash ros2 topic echo /rgbd_CP3AM/color/camera_info
输出结果如下:
header: stamp: sec: 119811 nanosec: 831645108 frame_id: color height: 1080 width: 1920 distortion_model: plumb_bob d: - -0.32267 - 0.083221 - 0.000164 - -0.002134 - 0.0 k: - 1066.158339 - 0.0 - 981.393777 - 0.0 - 1068.659998 - 545.569587 - 0.0 - 0.0 - 1.0 r: - 1.0 - 0.0 - 0.0 - 0.0 - 1.0 - 0.0 - 0.0 - 0.0 - 1.0 p: - 741.315308 - 0.0 - 968.865379 - 0.0 - 0.0 - 969.43042 - 546.524343 - 0.0 - 0.0 - 0.0 - 1.0 - 0.0 binning_x: 0 binning_y: 0 roi: x_offset: 0 y_offset: 0 height: 0 width: 0 do_rectify: false
注意事项
如遇到hobot_sensor节点启动异常，可通过下述步骤进行问题排查：
检查硬件连接
是否设置tros.b环境
参数是否正确，具体参考Hobot_Sensors README.md