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 npimport argparse
from datetime import datetimeout_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 = Nonehf = 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, hfcv_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()