3.4. RGBD图像采集

功能介绍

为实现环境感知能力，机器人产品中通常会搭载摄像头、ToF等类型的传感器。为降低用户传感器适配和使用成本，TogetherROS会对多种常用传感器进行封装，并抽象成Sensor Hub模块，支持ROS标准图像消息以及自定义图像消息输出。用户只需要修改配置参数，即可实现传感器节点的创建和使用，目前已支持的 RGBD 传感器类型如下所示：

类型 型号 规格
摄像头 CP3AM 200W

代码仓库：https://c-gitlab.horizon.ai/HHP/box/hobot_sensors

准备工作

  1. 确认摄像头正确接入旭日X3派，例如 RGBD 摄像头的接入方式如下图：

    hobot_rgbd

  2. 旭日X3派已烧录好地平线提供的Ubuntu20.0.4或Linux系统镜像。

  3. 旭日X3派已成功安装TogetherROS。

  4. PC 端需安装 ros2 foxy 版本和rviz2

  sudo apt install ros-foxy-rviz-common ros-foxy-rviz-default-plugins ros-foxy-rviz2

注意：该功能目前在旭日X3 SDB旭日X3派上可直接体验，旭日X3派旭日X3派需要外接转接板

使用方式

下面以 CP3AM 为例，介绍摄像头数据获取和预览的方法：

  1. 通过SSH登录旭日X3派，并通过下述命令启动Sensor Hub节点

    Ubuntu

    # 配置 TogetherROS 环境：
source /opt/tros/local_setup.bash
cp /opt/tros/lib/libgc2053_linear.so /lib/sensorlib/
cp -r /opt/tros/lib/rgbd_sensor/parameter .
# lanuch 方式启动
ros2 launch rgbd_sensor rgbd_sensor.launch.py

    Linux

    # 配置 TogetherROS 环境：
export LD_LIBRARY_PATH=/opt/tros/lib:${LD_LIBRARY_PATH}
cp /opt/tros/lib/libgc2053_linear.so /lib/sensorlib/
cp -r /opt/tros/lib/rgbd_sensor/parameter .
# 参数设置：
/opt/tros/lib/rgbd_sensor/rgbd_sensor --ros-args -p io_method:=ros

  2. 如程序输出如下信息，说明节点已成功启动

    [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!

...

  3. PC机上查询当前话题，查询命令及返回结果如下：

    source /opt/ros/foxy/local_setup.bash
ros2 topic list

    输出： /rgbd_CP3AM/depth/image_rect_raw

    /rgbd_CP3AM/depth/color/points

    /rgbd_CP3AM/aligned_depth_to_color/color/points

    /rgbd_CP3AM/infra/image_rect_raw

    /rgbd_CP3AM/color/image_rect_raw

    /parameter_events

    /rosout

  4. 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 即可。

    hobot_rgbd_sensor

注意事项

如遇到Sensor Hub节点启动异常，可通过下述步骤进行问题排查：

  1. 检查硬件连接

  2. 是否设置TogetherROS环境

  3. 参数是否正确，具体参考Hobot_Sensors README.md