set_cams_transforms.py
Go to the documentation of this file.
1 import rospy
2 import sys
3 import tf
4 import tf2_ros
5 import geometry_msgs.msg
6 
7 import termios
8 import tty
9 import os
10 import time
11 import math
12 import json
13 
14 
15 def getch():
16  fd = sys.stdin.fileno()
17  old_settings = termios.tcgetattr(fd)
18  try:
19  tty.setraw(sys.stdin.fileno())
20  ch = sys.stdin.read(1)
21 
22  finally:
23  termios.tcsetattr(fd, termios.TCSADRAIN, old_settings)
24  return ch
25 
26 
27 def main():
28  return
29 
30 
31 def print_status(status):
32  sys.stdout.write('%-8s%-8s%-8s%-40s\r' % (status['mode'], status[status['mode']]['value'], status[status['mode']]['step'], status['message']))
33 
34 
35 def publish_status(broadcaster, status):
36  static_transformStamped = geometry_msgs.msg.TransformStamped()
37  static_transformStamped.header.stamp = rospy.Time.now()
38  static_transformStamped.header.frame_id = from_cam
39 
40  static_transformStamped.child_frame_id = to_cam
41  static_transformStamped.transform.translation.x = status['x']['value']
42  static_transformStamped.transform.translation.y = status['y']['value']
43  static_transformStamped.transform.translation.z = status['z']['value']
44 
45  quat = tf.transformations.quaternion_from_euler(math.radians(status['roll']['value']),
46  math.radians(status['pitch']['value']),
47  math.radians(status['azimuth']['value']))
48  static_transformStamped.transform.rotation.x = quat[0]
49  static_transformStamped.transform.rotation.y = quat[1]
50  static_transformStamped.transform.rotation.z = quat[2]
51  static_transformStamped.transform.rotation.w = quat[3]
52  broadcaster.sendTransform(static_transformStamped)
53 
54 
55 if __name__ == '__main__':
56  if len(sys.argv) < 3:
57  print ('USAGE:')
58  print ('set_cams_transforms.py from_cam to_cam x y z azimuth pitch roll')
59  print ('x, y, z: in meters')
60  print ('azimuth, pitch, roll: in degrees')
61  print
62  print ('If parameters are not given read last used parameters.')
63  print
64  print ('[OPTIONS]')
65  print ('--file <file name> : if given, default values are loaded from file')
66  sys.exit(-1)
67 
68  from_cam, to_cam = sys.argv[1:3]
69  try:
70  filename = sys.argv[sys.argv.index('--file')+1]
71  print ('Using file %s' % os.path.abspath(filename))
72  except:
73  filename = os.path.join(os.path.dirname(__file__), '_set_cams_info_file.txt')
74  print ('Using default file %s' % os.path.abspath(filename))
75 
76  if len(sys.argv) >= 9:
77  x, y, z, yaw, pitch, roll = [float(arg) for arg in sys.argv[3:10]]
78  status = {'mode': 'pitch',
79  'x': {'value': x, 'step': 0.1},
80  'y': {'value': y, 'step': 0.1},
81  'z': {'value': z, 'step': 0.1},
82  'azimuth': {'value': yaw, 'step': 1},
83  'pitch': {'value': pitch, 'step': 1},
84  'roll': {'value': roll, 'step': 1},
85  'message': ''}
86  print ('Use given initial values.')
87  else:
88  try:
89  status = json.load(open(filename, 'r'))
90  print ('Read initial values from file.')
91  except IOError as e:
92  print ('Failed reading initial parameters from file %s' % filename)
93  print ('Initial parameters must be given for initial run or if an un-initialized file has been given.')
94  sys.exit(-1)
95 
96  rospy.init_node('my_static_tf2_broadcaster')
98 
99  print
100  print ('Press the following keys to change mode: x, y, z, (a)zimuth, (p)itch, (r)oll')
101  print ('For each mode, press 6 to increase by step and 4 to decrease')
102  print ('Press + to multiply step by 2 or - to divide')
103  print
104  print ('Press Q to quit')
105  print
106 
107  status_keys = [key[0] for key in status.keys()]
108  print ('%-8s%-8s%-8s%s' % ('Mode', 'value', 'step', 'message'))
109  print_status(status)
110  publish_status(broadcaster, status)
111  while True:
112  kk = getch()
113  status['message'] = ''
114  try:
115  key_idx = status_keys.index(kk)
116  status['mode'] = status.keys()[key_idx]
117  except ValueError as e:
118  if kk.upper() == 'Q':
119  sys.stdout.write('\n')
120  exit(0)
121  elif kk == '4':
122  status[status['mode']]['value'] -= status[status['mode']]['step']
123  elif kk == '6':
124  status[status['mode']]['value'] += status[status['mode']]['step']
125  elif kk == '-':
126  status[status['mode']]['step'] /= 2.0
127  elif kk == '+':
128  status[status['mode']]['step'] *= 2.0
129  else:
130  status['message'] = 'Invalid key:' + kk
131 
132  print_status(status)
133  publish_status(broadcaster, status)
134  json.dump(status, open(filename, 'w'), indent=4)
135 
136  #rospy.spin()
def publish_status(broadcaster, status)


realsense2_camera
Author(s): Sergey Dorodnicov , Doron Hirshberg
autogenerated on Fri Feb 19 2021 03:32:17