如何将Python脚本转换为ROS脚本？ （使用 RealSense 深度相机测量两点之间的距离）

Question

我写了一个 Python 脚本，它使用 RealSense 深度相机来测量输入图像中两点之间的距离。返回的距离非常准确。我曾尝试将此 Python 脚本转换为 rospy 脚本，但没有得到相同的结果。我不确定我是否没有订阅正确的 ROS 主题或没有正确处理数据。有效的 Python 脚本编写如下 - 请问如何将其转换为 ROS 脚本？

import pyrealsense2 as rs
import math

class distance_measure:
    def __init__(self):
        self.pipeline = rs.pipeline()
        self.pipeline.start(rs.config())
        print("Class initialized")

    def depth_cam(self):
        align_to = rs.stream.color
        align = rs.align(align_to)

        for i in range(5):
            self.pipeline.wait_for_frames()
            print("Received frame", i)

        while True:
            frames = self.pipeline.wait_for_frames()
            aligned_frames = align.process(frames)
            color_frame = aligned_frames.get_color_frame()
            self.depth_frame = aligned_frames.get_depth_frame()

            self.color_intrin = color_frame.profile.as_video_stream_profile().intrinsics

            self.x1 = 480
            self.y1 = 550
            self.x2 = 810
            self.y2 = self.y1
            ans = self.calculate_distance()
            print('distance:',ans)

            break

    def calculate_distance(self):
        udist = self.depth_frame.get_distance(self.x1, self.y1)
        vdist = self.depth_frame.get_distance(self.x2, self.y2)
        print(udist,vdist)

        point1 = rs.rs2_deproject_pixel_to_point(self.color_intrin, [self.x1, self.y1], udist)
        point2 = rs.rs2_deproject_pixel_to_point(self.color_intrin, [self.x2, self.y2], vdist)
        print(str(point1)+str(point2))

        dist = math.sqrt(math.pow(point1[0] - point2[0], 2) + math.pow(point1[1] - point2[1], 2) + math.pow(point1[2] - point2[2], 2))
        return dist


if __name__ == '__main__':
    distance_measure().depth_cam()

Answer 1

要将您的 python 脚本转换为“ROS 脚本”，您必须：

1. 导入ROS库（rospy + msg type you want to use）
2. 初始化一个节点和一个你想要的类型的发布者（我选择的是Float64，但是你可以根据你要发布的数据改成你喜欢的）
3. 在while循环中以10Hz的频率发布主题数据（也可以修改）

我无法测试您的代码，因为我没有相机/硬件，但这是正常的“ROS 代码”应该是什么样子。

希望对您有所帮助！

    import pyrealsense2 as rs
    import math
    #Import ROS libraries
    import rospy
    from std_msgs.msg import Float64

    class distance_measure:
    def __init__(self):
        self.pipeline = rs.pipeline()
        self.pipeline.start(rs.config())
        #Init ROS node, rate, publisher
        rospy.init_node('your_node_name', anonymous=False)
        rate = rospy.Rate(10) # 10hz
        pub = rospy.Publisher('your_topic_name', Float64, queue_size=10)
        print("Class initialized")

    def depth_cam(self):
        align_to = rs.stream.color
        align = rs.align(align_to)

        for i in range(5):
            self.pipeline.wait_for_frames()
            print("Received frame", i)

        #while roscore is running/working
        while not rospy.is_shutdown():
            frames = self.pipeline.wait_for_frames()
            aligned_frames = align.process(frames)
            color_frame = aligned_frames.get_color_frame()
            self.depth_frame = aligned_frames.get_depth_frame()
            self.color_intrin = color_frame.profile.as_video_stream_profile().intrinsics

            self.x1 = 480
            self.y1 = 550
            self.x2 = 810
            self.y2 = self.y1
            ans = self.calculate_distance()
            print('distance:',ans)
            #Publish data and sleep to achieve the publishing rate
            pub.publish(ans)
            rate.sleep()

    def calculate_distance(self):
        udist = self.depth_frame.get_distance(self.x1, self.y1)
        vdist = self.depth_frame.get_distance(self.x2, self.y2)
        print(udist,vdist)

        point1 = rs.rs2_deproject_pixel_to_point(self.color_intrin, [self.x1, self.y1], udist)
        point2 = rs.rs2_deproject_pixel_to_point(self.color_intrin, [self.x2, self.y2], vdist)
        print(str(point1)+str(point2))

        dist = math.sqrt(math.pow(point1[0] - point2[0], 2) + math.pow(point1[1] - point2[1], 2) + math.pow(point1[2] - point2[2], 2))
        return dist


    if __name__ == '__main__':
        try:
            distance_measure().depth_cam()
        #What to do if there is a ROS interrupt exception
        except rospy.ROSInterruptException:
            pass

Answer 2

我建议你看看官方realsense2 ROS wrapper repository。也许他们已经拥有您想要的一些功能。