rs2_listener.py
Go to the documentation of this file.
1 import sys
2 import time
3 import rospy
4 from sensor_msgs.msg import Image as msg_Image
5 from sensor_msgs.msg import PointCloud2 as msg_PointCloud2
6 import sensor_msgs.point_cloud2 as pc2
7 from sensor_msgs.msg import Imu as msg_Imu
8 import numpy as np
9 from cv_bridge import CvBridge, CvBridgeError
10 import inspect
11 import ctypes
12 import struct
13 import tf
14 try:
15  from theora_image_transport.msg import Packet as msg_theora
16 except Exception:
17  pass
18 
19 
20 def pc2_to_xyzrgb(point):
21  # Thanks to Panos for his code used in this function.
22  x, y, z = point[:3]
23  rgb = point[3]
24 
25  # cast float32 to int so that bitwise operations are possible
26  s = struct.pack('>f', rgb)
27  i = struct.unpack('>l', s)[0]
28  # you can get back the float value by the inverse operations
29  pack = ctypes.c_uint32(i).value
30  r = (pack & 0x00FF0000) >> 16
31  g = (pack & 0x0000FF00) >> 8
32  b = (pack & 0x000000FF)
33  return x, y, z, r, g, b
34 
35 
37  def __init__(self, params={}):
38  self.result = None
39 
40  self.break_timeout = False
41  self.timeout = params.get('timeout_secs', -1)
42  self.seq = params.get('seq', -1)
43  self.time = params.get('time', None)
44  self.node_name = params.get('node_name', 'rs2_listener')
45  self.bridge = CvBridge()
46  self.listener = None
47  self.prev_msg_time = 0
48  self.fout = None
49 
50 
51  self.themes = {'depthStream': {'topic': '/camera/depth/image_rect_raw', 'callback': self.imageColorCallback, 'msg_type': msg_Image},
52  'colorStream': {'topic': '/camera/color/image_raw', 'callback': self.imageColorCallback, 'msg_type': msg_Image},
53  'pointscloud': {'topic': '/camera/depth/color/points', 'callback': self.pointscloudCallback, 'msg_type': msg_PointCloud2},
54  'alignedDepthInfra1': {'topic': '/camera/aligned_depth_to_infra1/image_raw', 'callback': self.imageColorCallback, 'msg_type': msg_Image},
55  'alignedDepthColor': {'topic': '/camera/aligned_depth_to_color/image_raw', 'callback': self.imageColorCallback, 'msg_type': msg_Image},
56  'static_tf': {'topic': '/camera/color/image_raw', 'callback': self.imageColorCallback, 'msg_type': msg_Image},
57  'accelStream': {'topic': '/camera/accel/sample', 'callback': self.imuCallback, 'msg_type': msg_Imu},
58  }
59 
60  self.func_data = dict()
61 
62  def imuCallback(self, theme_name):
63  def _imuCallback(data):
64  if self.listener is None:
66  self.prev_time = time.time()
67  self.func_data[theme_name].setdefault('value', [])
68  self.func_data[theme_name].setdefault('ros_value', [])
69  try:
70  frame_id = data.header.frame_id
71  value = data.linear_acceleration
72 
73  (trans,rot) = self.listener.lookupTransform('/camera_link', frame_id, rospy.Time(0))
74  quat = tf.transformations.quaternion_matrix(rot)
75  point = np.matrix([value.x, value.y, value.z, 1], dtype='float32')
76  point.resize((4, 1))
77  rotated = quat*point
78  rotated.resize(1,4)
79  rotated = np.array(rotated)[0][:3]
80  except Exception as e:
81  print(e)
82  return
83  self.func_data[theme_name]['value'].append(value)
84  self.func_data[theme_name]['ros_value'].append(rotated)
85  return _imuCallback
86 
87  def imageColorCallback(self, theme_name):
88  def _imageColorCallback(data):
89  self.prev_time = time.time()
90  self.func_data[theme_name].setdefault('avg', [])
91  self.func_data[theme_name].setdefault('ok_percent', [])
92  self.func_data[theme_name].setdefault('num_channels', [])
93  self.func_data[theme_name].setdefault('shape', [])
94  self.func_data[theme_name].setdefault('reported_size', [])
95 
96  try:
97  cv_image = self.bridge.imgmsg_to_cv2(data, data.encoding)
98  except CvBridgeError as e:
99  print(e)
100  return
101  channels = cv_image.shape[2] if len(cv_image.shape) > 2 else 1
102  pyimg = np.asarray(cv_image)
103 
104  ok_number = (pyimg != 0).sum()
105 
106  self.func_data[theme_name]['avg'].append(pyimg.sum() / ok_number)
107  self.func_data[theme_name]['ok_percent'].append(float(ok_number) / (pyimg.shape[0] * pyimg.shape[1]) / channels)
108  self.func_data[theme_name]['num_channels'].append(channels)
109  self.func_data[theme_name]['shape'].append(cv_image.shape)
110  self.func_data[theme_name]['reported_size'].append((data.width, data.height, data.step))
111  return _imageColorCallback
112 
113  def imageDepthCallback(self, data):
114  pass
115 
116  def pointscloudCallback(self, theme_name):
117  def _pointscloudCallback(data):
118  self.prev_time = time.time()
119  print ('Got pointcloud: %d, %d' % (data.width, data.height))
120 
121  self.func_data[theme_name].setdefault('frame_counter', 0)
122  self.func_data[theme_name].setdefault('avg', [])
123  self.func_data[theme_name].setdefault('size', [])
124  self.func_data[theme_name].setdefault('width', [])
125  self.func_data[theme_name].setdefault('height', [])
126  # until parsing pointcloud is done in real time, I'll use only the first frame.
127  self.func_data[theme_name]['frame_counter'] += 1
128 
129  if self.func_data[theme_name]['frame_counter'] == 1:
130  # Known issue - 1st pointcloud published has invalid texture. Skip 1st frame.
131  return
132 
133  try:
134  points = np.array([pc2_to_xyzrgb(pp) for pp in pc2.read_points(data, skip_nans=True, field_names=("x", "y", "z", "rgb")) if pp[0] > 0])
135  except Exception as e:
136  print(e)
137  return
138  self.func_data[theme_name]['avg'].append(points.mean(0))
139  self.func_data[theme_name]['size'].append(len(points))
140  self.func_data[theme_name]['width'].append(data.width)
141  self.func_data[theme_name]['height'].append(data.height)
142  return _pointscloudCallback
143 
144  def wait_for_message(self, params, msg_type=msg_Image):
145  topic = params['topic']
146  print ('connect to ROS with name: %s' % self.node_name)
147  rospy.init_node(self.node_name, anonymous=True)
148 
149  out_filename = params.get('filename', None)
150  if (out_filename):
151  self.fout = open(out_filename, 'w')
152  if msg_type is msg_Imu:
153  col_w = 20
154  print ('Writing to file: %s' % out_filename)
155  columns = ['frame_number', 'frame_time(sec)', 'accel.x', 'accel.y', 'accel.z', 'gyro.x', 'gyro.y', 'gyro.z']
156  line = ('{:<%d}'*len(columns) % (col_w, col_w, col_w, col_w, col_w, col_w, col_w, col_w)).format(*columns) + '\n'
157  sys.stdout.write(line)
158  self.fout.write(line)
159 
160  rospy.loginfo('Subscribing on topic: %s' % topic)
161  self.sub = rospy.Subscriber(topic, msg_type, self.callback)
162 
163  self.prev_time = time.time()
164  break_timeout = False
165  while not any([rospy.core.is_shutdown(), break_timeout, self.result]):
166  rospy.rostime.wallsleep(0.5)
167  if self.timeout > 0 and time.time() - self.prev_time > self.timeout:
168  break_timeout = True
169  self.sub.unregister()
170 
171  return self.result
172 
173  @staticmethod
174  def unregister_all(registers):
175  for test_name in registers:
176  rospy.loginfo('Un-Subscribing test %s' % test_name)
177  registers[test_name]['sub'].unregister()
178 
179  def wait_for_messages(self, themes):
180  # tests_params = {<name>: {'callback', 'topic', 'msg_type', 'internal_params'}}
181  self.func_data = dict([[theme_name, {}] for theme_name in themes])
182 
183  print ('connect to ROS with name: %s' % self.node_name)
184  rospy.init_node(self.node_name, anonymous=True)
185  for theme_name in themes:
186  theme = self.themes[theme_name]
187  rospy.loginfo('Subscribing %s on topic: %s' % (theme_name, theme['topic']))
188  self.func_data[theme_name]['sub'] = rospy.Subscriber(theme['topic'], theme['msg_type'], theme['callback'](theme_name))
189 
190  self.prev_time = time.time()
191  break_timeout = False
192  while not any([rospy.core.is_shutdown(), break_timeout]):
193  rospy.rostime.wallsleep(0.5)
194  if self.timeout > 0 and time.time() - self.prev_time > self.timeout:
195  break_timeout = True
196  self.unregister_all(self.func_data)
197 
198  return self.func_data
199 
200  def callback(self, data):
201  msg_time = data.header.stamp.secs + 1e-9 * data.header.stamp.nsecs
202 
203  if (self.prev_msg_time > msg_time):
204  rospy.loginfo('Out of order: %.9f > %.9f' % (self.prev_msg_time, msg_time))
205  if type(data) == msg_Imu:
206  col_w = 20
207  frame_number = data.header.seq
208  accel = data.linear_acceleration
209  gyro = data.angular_velocity
210  line = ('\n{:<%d}{:<%d.6f}{:<%d.4f}{:<%d.4f}{:<%d.4f}{:<%d.4f}{:<%d.4f}{:<%d.4f}' % (col_w, col_w, col_w, col_w, col_w, col_w, col_w, col_w)).format(frame_number, msg_time, accel.x, accel.y, accel.z, gyro.x, gyro.y, gyro.z)
211  sys.stdout.write(line)
212  if self.fout:
213  self.fout.write(line)
214 
215  self.prev_msg_time = msg_time
216  self.prev_msg_data = data
217 
218  self.prev_time = time.time()
219  if any([self.seq > 0 and data.header.seq >= self.seq,
220  self.time and data.header.stamp.secs == self.time['secs'] and data.header.stamp.nsecs == self.time['nsecs']]):
221  self.result = data
222  self.sub.unregister()
223 
224 
225 
226 def main():
227  if len(sys.argv) < 2 or '--help' in sys.argv or '/?' in sys.argv:
228  print ('USAGE:')
229  print ('------')
230  print ('rs2_listener.py <topic | theme> [Options]')
231  print ('example: rs2_listener.py /camera/color/image_raw --time 1532423022.044515610 --timeout 3')
232  print ('example: rs2_listener.py pointscloud')
233  print ('')
234  print ('Application subscribes on <topic>, wait for the first message matching [Options].')
235  print ('When found, prints the timestamp.')
236  print
237  print ('[Options:]')
238  print ('-s <sequential number>')
239  print ('--time <secs.nsecs>')
240  print ('--timeout <secs>')
241  print ('--filename <filename> : write output to file')
242  exit(-1)
243 
244  # wanted_topic = '/device_0/sensor_0/Depth_0/image/data'
245  # wanted_seq = 58250
246 
247  wanted_topic = sys.argv[1]
248  msg_params = {}
249  if 'points' in wanted_topic:
250  msg_type = msg_PointCloud2
251  elif ('imu' in wanted_topic) or ('gyro' in wanted_topic) or ('accel' in wanted_topic):
252  msg_type = msg_Imu
253  elif 'theora' in wanted_topic:
254  try:
255  msg_type = msg_theora
256  except NameError as e:
257  print ('theora_image_transport is not installed. \nType "sudo apt-get install ros-kinetic-theora-image-transport" to enable registering on messages of type theora.')
258  raise
259  else:
260  msg_type = msg_Image
261 
262  for idx in range(2, len(sys.argv)):
263  if sys.argv[idx] == '-s':
264  msg_params['seq'] = int(sys.argv[idx + 1])
265  if sys.argv[idx] == '--time':
266  msg_params['time'] = dict(zip(['secs', 'nsecs'], [int(part) for part in sys.argv[idx + 1].split('.')]))
267  if sys.argv[idx] == '--timeout':
268  msg_params['timeout_secs'] = int(sys.argv[idx + 1])
269  if sys.argv[idx] == '--filename':
270  msg_params['filename'] = sys.argv[idx+1]
271 
272  msg_retriever = CWaitForMessage(msg_params)
273  if '/' in wanted_topic:
274  msg_params.setdefault('topic', wanted_topic)
275  res = msg_retriever.wait_for_message(msg_params, msg_type)
276  rospy.loginfo('Got message: %s' % res.header)
277  else:
278  themes = [wanted_topic]
279  res = msg_retriever.wait_for_messages(themes)
280  print (res)
281 
282 
283 if __name__ == '__main__':
284  main()
285 
def pc2_to_xyzrgb(point)
Definition: rs2_listener.py:20
def imageColorCallback(self, theme_name)
Definition: rs2_listener.py:87
def __init__(self, params={})
Definition: rs2_listener.py:37
def pointscloudCallback(self, theme_name)
def wait_for_messages(self, themes)
def imuCallback(self, theme_name)
Definition: rs2_listener.py:62
def imageDepthCallback(self, data)
def wait_for_message(self, params, msg_type=msg_Image)


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