show_center_depth.py
Go to the documentation of this file.
1 import rospy
2 from sensor_msgs.msg import Image as msg_Image
3 from sensor_msgs.msg import CameraInfo
4 from cv_bridge import CvBridge, CvBridgeError
5 import sys
6 import os
7 import numpy as np
8 import pyrealsense2 as rs2
9 if (not hasattr(rs2, 'intrinsics')):
10  import pyrealsense2.pyrealsense2 as rs2
11 
13  def __init__(self, depth_image_topic, depth_info_topic):
14  self.bridge = CvBridge()
15  self.sub = rospy.Subscriber(depth_image_topic, msg_Image, self.imageDepthCallback)
16  self.sub_info = rospy.Subscriber(depth_info_topic, CameraInfo, self.imageDepthInfoCallback)
17  confidence_topic = depth_image_topic.replace('depth', 'confidence')
18  self.sub_conf = rospy.Subscriber(confidence_topic, msg_Image, self.confidenceCallback)
19  self.intrinsics = None
20  self.pix = None
21  self.pix_grade = None
22 
23  def imageDepthCallback(self, data):
24  try:
25  cv_image = self.bridge.imgmsg_to_cv2(data, data.encoding)
26  # pick one pixel among all the pixels with the closest range:
27  indices = np.array(np.where(cv_image == cv_image[cv_image > 0].min()))[:,0]
28  pix = (indices[1], indices[0])
29  self.pix = pix
30  line = '\rDepth at pixel(%3d, %3d): %7.1f(mm).' % (pix[0], pix[1], cv_image[pix[1], pix[0]])
31 
32  if self.intrinsics:
33  depth = cv_image[pix[1], pix[0]]
34  result = rs2.rs2_deproject_pixel_to_point(self.intrinsics, [pix[0], pix[1]], depth)
35  line += ' Coordinate: %8.2f %8.2f %8.2f.' % (result[0], result[1], result[2])
36  if (not self.pix_grade is None):
37  line += ' Grade: %2d' % self.pix_grade
38  line += '\r'
39  sys.stdout.write(line)
40  sys.stdout.flush()
41 
42  except CvBridgeError as e:
43  print(e)
44  return
45  except ValueError as e:
46  return
47 
48  def confidenceCallback(self, data):
49  try:
50  cv_image = self.bridge.imgmsg_to_cv2(data, data.encoding)
51  grades = np.bitwise_and(cv_image >> 4, 0x0f)
52  if (self.pix):
53  self.pix_grade = grades[self.pix[1], self.pix[0]]
54  except CvBridgeError as e:
55  print(e)
56  return
57 
58 
59 
60  def imageDepthInfoCallback(self, cameraInfo):
61  try:
62  if self.intrinsics:
63  return
64  self.intrinsics = rs2.intrinsics()
65  self.intrinsics.width = cameraInfo.width
66  self.intrinsics.height = cameraInfo.height
67  self.intrinsics.ppx = cameraInfo.K[2]
68  self.intrinsics.ppy = cameraInfo.K[5]
69  self.intrinsics.fx = cameraInfo.K[0]
70  self.intrinsics.fy = cameraInfo.K[4]
71  if cameraInfo.distortion_model == 'plumb_bob':
72  self.intrinsics.model = rs2.distortion.brown_conrady
73  elif cameraInfo.distortion_model == 'equidistant':
74  self.intrinsics.model = rs2.distortion.kannala_brandt4
75  self.intrinsics.coeffs = [i for i in cameraInfo.D]
76  except CvBridgeError as e:
77  print(e)
78  return
79 
80 def main():
81  depth_image_topic = '/camera/depth/image_rect_raw'
82  depth_info_topic = '/camera/depth/camera_info'
83 
84  print ('')
85  print ('show_center_depth.py')
86  print ('--------------------')
87  print ('App to demontrate the usage of the /camera/depth topics.')
88  print ('')
89  print ('Application subscribes to %s and %s topics.' % (depth_image_topic, depth_info_topic))
90  print ('Application then calculates and print the range to the closest object.')
91  print ('If intrinsics data is available, it also prints the 3D location of the object')
92  print ('If a confedence map is also available in the topic %s, it also prints the confidence grade.' % depth_image_topic.replace('depth', 'confidence'))
93  print ('')
94 
95  listener = ImageListener(depth_image_topic, depth_info_topic)
96  rospy.spin()
97 
98 if __name__ == '__main__':
99  node_name = os.path.basename(sys.argv[0]).split('.')[0]
100  rospy.init_node(node_name)
101  main()
bool hasattr(handle obj, handle name)
def imageDepthInfoCallback(self, cameraInfo)
def __init__(self, depth_image_topic, depth_info_topic)


realsense2_camera
Author(s): Sergey Dorodnicov , Doron Hirshberg
autogenerated on Mon Mar 1 2021 03:34:04