How to convert Python script to ROS script? (Measuring distance between two points using RealSense depth camera)

Question

I have written a Python script which uses a RealSense depth camera to measure the distance between two points in the input image. The distance returned is very accurate. I have tried converting this Python script to a rospy script, but I do not get the same result. I'm not sure if I'm not subscribing to the correct ROS topics or not handling the data correctly. The working Python script is written as follows - how can I convert this to a ROS script, please?

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

I suggest you take a look at the official realsense2 ROS wrapper repository. Maybe they already have some of the functionalities that you want.

Answer 2

To convert your python script to a "ROS script" you have to :

1. Import the ROS libraries (rospy + msg type you want to use)
2. Init a node and a publisher of the type you want (I chose Float64 but you can change it to whatever you prefer according to the data you want to publish)
3. Publish the data on the topic at the rate of 10Hz (can be modified too) in the while loop

I am not able to test your code as I don't have the camera/hardwares but this is what a normal 'ROS code' should look like.

Hope it helps !

    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