ALOHA多相机Realsense配置以及数据采集

简介

在诸多模仿学习的工作中，均使用到多个Realsense相机作为数据输入端。本文探讨多个Realsense的ros节点启动。

环境配置

librealsense
realsense_ros

#python
h5py
opencv-python

多相机启动

ALOHA工程仅需要彩色图像进行输入。因此需要只保留彩色图像，当要求帧率为50Hz时，需要将相机的频率调整至60Hz，然后每6帧丢弃一帧。在${realsense_ws}/realsense_camera/launch/includes文件夹下，创建一个专用的配置文件d405_30fps.xml

  <!--将无关传感器关闭以节省带宽-->
  <arg name="enable_fisheye"      default="false"/>
  <arg name="enable_fisheye1"     default="false"/>
  <arg name="enable_fisheye2"     default="false"/>
  <arg name="enable_depth"        default="false"/>
  <arg name="enable_confidence"   default="false"/>
  <arg name="enable_infra"        default="false"/>
  <arg name="enable_infra1"       default="false"/>
  <arg name="enable_infra2"       default="false"/>
  <arg name="infra_rgb"           default="false"/>
  <arg name="enable_gyro"         default="false"/>
  <arg name="enable_accel"        default="false"/>
  <arg name="enable_pose"         default="false"/>
  
  <arg name="color_width"         default="640"/>
  <arg name="color_height"        default="480"/>
  <arg name="enable_color"        default="true"/>
  <arg name="color_fps"           default="30"/>

然后创建多相机启动launch文件。创建launch/aloha_30.launch。本文采用和ALOHA同样的3个D405硬件配置。建议绑定相机序列号，这样可以防止相机顺序接反。序列号在相机机身以及realsense-viewer中均可查到。

<launch>
  <arg name="serial_no_left"    			default="130xxxxxxxxx"/> 			<!-- Note: Replace with actual serial number -->
  <arg name="serial_no_right"    			default="230xxxxxxxxx"/> 			<!-- Note: Replace with actual serial number -->
  <arg name="serial_no_main"    			default="130xxxxxxxxx"/> 			<!-- Note: Replace with actual serial number -->
  <arg name="left"              			default="left"/>		<!-- Note: Replace with camera name -->
  <arg name="right"              			default="right"/>		<!-- Note: Replace with camera name -->
  <arg name="main"              			default="main"/>		<!-- Note: Replace with camera name -->
  <arg name="tf_prefix_left"         default="$(arg left)"/>
  <arg name="tf_prefix_right"        default="$(arg right)"/>
  <arg name="tf_prefix_main"         default="$(arg main)"/>
  <arg name="initial_reset"             default="false"/>

  <group ns="$(arg left)">
    <include file="$(find realsense2_camera)/launch/includes/d405_30fps.xml">
      <arg name="serial_no"             value="$(arg serial_no_left)"/>
      <arg name="tf_prefix"         		value="$(arg tf_prefix_left)"/>
      <arg name="initial_reset"         value="$(arg initial_reset)"/>
    </include>
  </group>

  <group ns="$(arg right)">
    <include file="$(find realsense2_camera)/launch/includes/d405_30fps.xml">
      <arg name="serial_no"             value="$(arg serial_no_right)"/>
      <arg name="tf_prefix"		          value="$(arg tf_prefix_right)"/>
      <arg name="initial_reset"         value="$(arg initial_reset)"/>
    </include>
  </group>
  
  <group ns="$(arg main)" if="$(eval serial_no_main != '')">
    <include file="$(find realsense2_camera)/launch/includes/d405_30fps.xml">
      <arg name="serial_no"             value="$(arg serial_no_main)"/>
      <arg name="tf_prefix"		          value="$(arg tf_prefix_main)"/>
      <arg name="initial_reset"         value="$(arg initial_reset)"/>
    </include>
  </group>

</launch>

此时运行ros节点，保证3个相机供电、带宽正常的情况下，可以在rviz中看到3个相机的画面。

数据采集

ALOHA需要将图像抓取后，存入hdf5文件。实现上，采用ROS同步多个topic，图像抓取后，存入hdf5。代码如下

#!/usr/bin/env python
import message_filters
import rospy
from sensor_msgs.msg import Image
from cv_bridge import CvBridge
import cv2
from message_filters import ApproximateTimeSynchronizer
import h5py
import signal
import numpy as np

import argparse
from datetime import datetime


out_name = datetime.now().strftime('%Y-%m-%d-%H_%M_%S')+".hdf5"
print("Saving file " + out_name)

parser = argparse.ArgumentParser(description='h5saver')
parser.add_argument('--save', type=str, default=out_name, help='')
args = parser.parse_args()


bridge = CvBridge()
cv_image1 = None

hf = h5py.File(args.save, "w")

topic_1 = "/left/color/image_raw"
topic_2 = "/right/color/image_raw"
topic_3 = "/top/color/image_raw"

topic_left  = "observations/images/left_wrist"
topic_right = "observations/images/right_wrist"
topic_top  = "observations/images/top"

img_arrayl = []
img_arrayr = []
img_arrayt = []
 
def signal_handler(signal, frame):
    # 处理Ctrl+C信号的逻辑
    print("捕获到Ctrl+C信号！")
    dt = h5py.string_dtype(encoding='ascii')
    print(np.array(img_arrayl).shape)
    # hf.create_dataset(topic_left , data=np.array(img_arrayl), dtype=dt)
    # hf.create_dataset(topic_right, data=np.array(img_arrayr), dtype=dt)
    # hf.create_dataset(topic_top , data=np.array(img_arrayt), dtype=dt)
    hf[topic_left ] = np.array(img_arrayl)
    hf[topic_right] = np.array(img_arrayr)
    hf[topic_top ] = np.array(img_arrayt)
    print("OK")
    exit(0)

def callback(img1, img2, img3):
    global cv_image1, bridge, hf

    cv_image1 = bridge.imgmsg_to_cv2(img1, "bgr8")
    cv_image2 = bridge.imgmsg_to_cv2(img2, "bgr8")
    cv_image3 = bridge.imgmsg_to_cv2(img3, "bgr8")

    if cv_image1 is not None and cv_image2 is not None and cv_image3 is not None :
        img_arrayl.append(cv_image1)
        img_arrayr.append(cv_image2)
        img_arrayt.append(cv_image3)


    cv2.imshow("Image 1", cv_image1)
    cv2.imshow("Image 2", cv_image2)
    cv2.imshow("Image 3", cv_image3)
    cv2.waitKey(1)


 
if __name__ == '__main__':
    rospy.init_node('sync_images')
 
    topics = [topic_1, topic_2, topic_3]
    topics_sub1 = message_filters.Subscriber(topic_1, Image) 
    topics_sub2 = message_filters.Subscriber(topic_2, Image)
    topics_sub3 = message_filters.Subscriber(topic_3, Image)
    topics_subs = [topics_sub1, topics_sub2, topics_sub3]
 
    sync = message_filters.ApproximateTimeSynchronizer(\
            topics_subs, \
            2, \
            0.9)
    sync.registerCallback(callback)
 
    rate = rospy.Rate(60)
    while not rospy.is_shutdown():
        signal.signal(signal.SIGINT, signal_handler)
        rate.sleep()