ROS同步订阅realsense的彩色图和深度图(python实现) |
您所在的位置：网站首页 › ros订阅topic › ROS同步订阅realsense的彩色图和深度图(python实现) |
|
首先要保证realsense-ros功能包的正常使用，具体安装使用如下：Jetson Nano ros melodic+realsense+aruco_杰杰！的博客-CSDN博客 1.创建订阅者 #! /usr/bin/env python import rospy from std_msgs.msg import String from sensor_msgs.msg import Image, CameraInfo import message_filters import cv2 from cv_bridge import CvBridge, CvBridgeError def callback(data1,data2): bridge = CvBridge() color_image = bridge.imgmsg_to_cv2(data1, 'bgr8') depth_image = bridge.imgmsg_to_cv2(data2, '16UC1') cv2.imshow('color_image',color_image) cv2.waitKey(1000) c_x = 320 c_y = 240 real_z = depth_image[c_y, c_x] * 0.001 real_x = (c_x- ppx) / fx * real_z real_y = (c_y - ppy) / fy * real_z rospy.loginfo("potion:x=%f,y=%f,z=%f",real_x,real_y,real_z) #输出图像中心点在相机坐标系下的x,y,z if __name__ == '__main__': global fx, fy, ppx, ppy #相机内参 fx = 609.134765 fy = 608.647949 ppx = 312.763214 ppy = 240.882049 rospy.init_node('get_image', anonymous=True) color = message_filters.Subscriber("/camera/color/image_raw", Image) depth = message_filters.Subscriber("/camera/aligned_depth_to_color/image_raw", Image) # color_depth = message_filters.ApproximateTimeSynchronizer([color, depth], 10, 1, allow_headerless=True) # 接近时间同步 color_depth = message_filters.TimeSynchronizer([color, depth], 1) # 绝对时间同步 color_depth.registerCallback(callback) #同时订阅/camera/color/image_raw和/camera/aligned_depth_to_color/image_raw话题，并利用message_filters实现话题同步，共同调用callback rospy.spin()2.运行launch文件 realsense-ros中自带很多示例的launch文件，在realsense2_camera/launch目录下，launch文件中主要涉及到一些realsense所发布话题的参数设置，包括帧率、图像分辨率，以及是否发布彩色图、深度图、点云等。 我们这里运行rs_aligned_depth.launch,终端输入如下命令,这个launch文件可以同时发布/camera/color/image_raw和/camera/aligned_depth_to_color/image_raw等多个话题，运行后，使用命令rostopic list查看该launch文件所发布的话题。 roslaunch realsense2_camera rs_aligned_depth.launch 3.为第一步中的python文件添加可执行权限，编译、souce devel/detup.bash,执行即可。 |
今日新闻 |
推荐新闻 |
| CopyRight 2018-2019 办公设备维修网 版权所有 豫ICP备15022753号-3 |