How to convert Python script to ROS script? (Measuring distance between two points using RealSense depth camera)
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()