简介
在诸多模仿学习的工作中,均使用到多个Realsense相机作为数据输入端。本文探讨多个Realsense的ros节点启动。
环境配置
bash
librealsense
realsense_ros
#python
h5py
opencv-python
多相机启动
ALOHA工程仅需要彩色图像进行输入。因此需要只保留彩色图像,当要求帧率为50Hz时,需要将相机的频率调整至60Hz,然后每6帧丢弃一帧。在${realsense_ws}/realsense_camera/launch/includes
文件夹下,创建一个专用的配置文件d405_30fps.xml
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
中均可查到。
xml
<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。代码如下
python
#!/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()