capture_exec.py
Go to the documentation of this file.
1 #!/usr/bin/env python
2 # Software License Agreement (BSD License)
3 #
4 # Copyright (c) 2008, Willow Garage, Inc.
5 # All rights reserved.
6 #
7 # Redistribution and use in source and binary forms, with or without
8 # modification, are permitted provided that the following conditions
9 # are met:
10 #
11 # * Redistributions of source code must retain the above copyright
12 # notice, this list of conditions and the following disclaimer.
13 # * Redistributions in binary form must reproduce the above
14 # copyright notice, this list of conditions and the following
15 # disclaimer in the documentation and/or other materials provided
16 # with the distribution.
17 # * Neither the name of Willow Garage, Inc. nor the names of its
18 # contributors may be used to endorse or promote products derived
19 # from this software without specific prior written permission.
20 #
21 # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22 # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23 # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24 # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25 # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26 # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27 # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28 # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29 # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30 # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31 # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32 # POSSIBILITY OF SUCH DAMAGE.
33 
34 import rospy
35 import yaml
36 import sys
37 import actionlib
38 import joint_states_settler.msg
39 import time
40 import threading
41 import os
42 import string
43 
44 import capture_executive
45 from capture_executive.config_manager import ConfigManager
47 from capture_executive.robot_measurement_cache import RobotMeasurementCache
48 
49 from calibration_msgs.msg import RobotMeasurement
50 
51 from urdf_parser_py.urdf import URDF
52 
54  def __init__(self, config_dir, system, robot_desc, output_debug=False):
55  # Load configs
56  self.cam_config = yaml.load(open(config_dir + "/cam_config.yaml"))
57  self.chain_config = yaml.load(open(config_dir + "/chain_config.yaml"))
58  self.laser_config = yaml.load(open(config_dir + "/laser_config.yaml"))
59  self.controller_config = yaml.load(open(config_dir + "/controller_config.yaml"))
60  # Not all robots have lasers.... :(
61  if self.laser_config == None:
62  self.laser_config = dict()
63  # Debug mode makes bag files huge...
64  self.output_debug = output_debug
65 
66  # Error message from sample capture
67  self.message = None
68 
69  # Status message from interval computation
70  self.interval_status = None
71 
72  # parse urdf and get list of links
73  links = URDF().parse(robot_desc).link_map.keys()
74 
75  # load system config
76  system = yaml.load(open(system))
77 
78  # remove cams that are not on urdf
79  for cam in self.cam_config.keys():
80  try:
81  link = system['sensors']['rectified_cams'][cam]['frame_id']
82  if not link in links:
83  print 'URDF does not contain link [%s]. Removing camera [%s]' % (link, cam)
84  del self.cam_config[cam]
85  except:
86  print 'System description does not contain camera [%s]' % cam
87  del self.cam_config[cam]
88 
89  # remove arms that are not on urdf
90  for chain in self.chain_config.keys():
91  try:
92  link = system['sensors']['chains'][chain]['tip']
93  if not link in links:
94  print 'URDF does not contain link [%s]. Removing chain [%s]' % (link, chain)
95  del self.chain_config[chain]
96  except:
97  print 'System description does not contain chain [%s]' % chain
98  del self.chain_config[chain]
99 
101  self.lock = threading.Lock()
102 
103  # Specifies if we're currently waiting for a sample
104  self.active = False
105 
106  # Construct Configuration Manager (manages configuration of nodes in the pipeline)
108  self.chain_config,
109  self.laser_config,
110  self.controller_config)
111 
112  # Construct a manager for each sensor stream (Don't enable any of them)
113  self.cam_managers = [ (cam_id, CamManager( cam_id, self.add_cam_measurement) ) for cam_id in self.cam_config.keys() ]
114  self.chain_managers = [ (chain_id, ChainManager(chain_id, self.add_chain_measurement) ) for chain_id in self.chain_config.keys() ]
115  self.laser_managers = [ (laser_id, LaserManager(laser_id, self.add_laser_measurement) ) for laser_id in self.laser_config.keys() ]
116 
117  # Subscribe to topic containing stable intervals
118  self.request_interval_sub = rospy.Subscriber("intersected_interval", calibration_msgs.msg.Interval, self.request_callback)
119 
120  # Subscribe to topic containing stable intervals
121  self.interval_status_sub = rospy.Subscriber(
122  "intersected_interval_status",
123  calibration_msgs.msg.IntervalStatus, self.status_callback)
124 
125  # Hardcoded cache sizes. Not sure where these params should live
126  # ...
127 
128  def capture(self, next_configuration, timeout):
129  done = False
130  self.m_robot = None
131  self.message = None
132  self.interval_status = None
133 
134  timeout_time = rospy.Time().now() + timeout
135 
136  self.lock.acquire()
137  if self.active:
138  rospy.logerr("Can't capture since we're already active")
139  done = True
140  self.lock.release()
141 
142  camera_measurements = next_configuration["camera_measurements"]
143  next_configuration["camera_measurements"] = []
144 
145  for (i, cam) in enumerate(camera_measurements):
146  if self.cam_config.has_key(cam["cam_id"]):
147  next_configuration["camera_measurements"].append(cam)
148  else:
149  rospy.logdebug("Not capturing measurement for camera: %s"%(cam["cam_id"]))
150 
151  # Set up the pipeline
152  self.config_manager.reconfigure(next_configuration)
153 
154  time.sleep(2.0)
155 
156  # Set up the cache with only the sensors we care about
157  cam_ids = [x["cam_id"] for x in next_configuration["camera_measurements"]]
158  chain_ids = [x["chain_id"] for x in next_configuration["joint_measurements"]]
159  try:
160  laser_ids = [x["laser_id"] for x in next_configuration["laser_measurements"]]
161  except:
162  laser_ids = list()
163  self.cache.reconfigure(cam_ids, chain_ids, laser_ids)
164 
165  rospy.logdebug("Setting up sensor managers")
166  enable_list = []
167  disable_list = []
168  # Set up the sensor managers
169  for cam_id, cam_manager in self.cam_managers:
170  if cam_id in [x["cam_id"] for x in next_configuration["camera_measurements"]]:
171  enable_list.append(cam_id)
172  cam_manager.enable(self.output_debug)
173  else:
174  disable_list.append(cam_id)
175  cam_manager.disable()
176 
177  for chain_id, chain_manager in self.chain_managers:
178  if chain_id in [x["chain_id"] for x in next_configuration["joint_measurements"]]:
179  enable_list.append(chain_id)
180  chain_manager.enable()
181  else:
182  disable_list.append(chain_id)
183  chain_manager.disable()
184 
185  for laser_id, laser_manager in self.laser_managers:
186  enabled_lasers = [x["laser_id"] for x in next_configuration["laser_measurements"]]
187  if laser_id in enabled_lasers:
188  enable_list.append(laser_id)
189  laser_manager.enable(self.output_debug)
190  else:
191  disable_list.append(laser_id)
192  laser_manager.disable()
193 
194  print "Enabling:"
195  for cur_enabled in enable_list:
196  print " + %s" % cur_enabled
197  print "Disabling:"
198  for cur_disabled in disable_list:
199  print " - %s" % cur_disabled
200 
201  self.lock.acquire()
202  self.active = True
203  self.lock.release()
204 
205  print "\nCollecting sensor data.."
206  # Keep waiting until the request_callback function populates the m_robot msg
207  while (not rospy.is_shutdown()) and (not done) and (rospy.Time().now() < timeout_time):
208  time.sleep(.1)
209  self.lock.acquire()
210  if self.m_robot is not None:
211  done = True
212  self.lock.release()
213 
214  print "Stopping sensor streams..\n"
215  # Stop listening to all the sensor streams
216  for cam_id, cam_manager in self.cam_managers:
217  cam_manager.disable()
218  for chain_id, chain_manager in self.chain_managers:
219  chain_manager.disable()
220  for laser_id, laser_manager in self.laser_managers:
221  laser_manager.disable()
222 
223  # Turn off the entire pipeline
224  self.config_manager.stop()
225 
226  # Fill in meta information about the sample
227  if self.m_robot is not None:
228  self.m_robot.sample_id = next_configuration["sample_id"]
229  self.m_robot.target_id = next_configuration["target"]["target_id"]
230  self.m_robot.chain_id = next_configuration["target"]["chain_id"]
231  elif self.interval_status is not None:
232  total = 0
233  bad_sensors = []
234  l = len(self.interval_status.names)
235  if l != len(self.interval_status.yeild_rates):
236  rospy.logerr("Invalid interval status message; names and yeild rates have different lengths")
237  l = min(l, len(self.interval_status.yeild_rates))
238  # analyze status message
239  for i in range(l):
240  if self.interval_status.yeild_rates[i] == 0.0:
241  bad_sensors.append(self.interval_status.names[i])
242  total += self.interval_status.yeild_rates[i]
243 
244  # if we didn't get any samples from some sensors, complain
245  if len(bad_sensors) > 0:
246  print "Didn't get good data from %s"%(', '.join(bad_sensors))
247  else:
248  # if we got data from all of our sensors, complain about the
249  # ones that were below the mean (heuristic)
250  avg = total / l
251  for i in range(l):
252  if self.interval_status.yeild_rates[i] <= avg:
253  bad_sensors.append(self.interval_status.names[i])
254  print "%s may be performing poorly"%(", ".join(bad_sensors))
255  elif self.message is not None:
256  print self.message
257 
258  self.lock.acquire()
259  self.active = False
260  self.lock.release()
261 
262  return self.m_robot
263 
264  def request_callback(self, msg):
265  # See if the interval is big enough to care
266  if (msg.end - msg.start) < rospy.Duration(1,0):
267  return
268 
269  self.lock.acquire()
270  if self.active:
271  # Do some query into the cache, and possibly stop stuff from running
272  m = self.cache.request_robot_measurement(msg.start, msg.end)
273  if isinstance(m, basestring):
274  self.message = m
275  else:
276  self.m_robot = m
277  # We found a sample, so we can deactive (kind of a race condition, since 'active' triggers capture() to exit... I don't care)
278  if self.m_robot is not None:
279  self.active = False
280  self.lock.release()
281 
282  def status_callback(self, msg):
283  self.lock.acquire()
284  if self.active:
285  self.interval_status = msg
286  self.lock.release()
287 
288  def add_cam_measurement(self, cam_id, msg):
289  if len(msg.image_points) > 0:
290  self.lock.acquire()
291  self.cache.add_cam_measurement(cam_id, msg)
292  self.lock.release()
293 
294  def add_chain_measurement(self, chain_id, msg):
295  self.lock.acquire()
296  self.cache.add_chain_measurement(chain_id, msg)
297  self.lock.release()
298 
299  def add_laser_measurement(self, laser_id, msg, interval_start, interval_end):
300  self.lock.acquire()
301  self.cache.add_laser_measurement(laser_id, msg, interval_start, interval_end)
302  self.lock.release()
303 
304 if __name__=='__main__':
305  rospy.init_node("capture_executive_node")
306 
307  rospy.logdebug("Starting executive...")
308  rospy.sleep(5.0)
309 
310  # TODO: convert to parameters?
311  samples_dir = rospy.myargv()[1]
312  config_dir = rospy.myargv()[2]
313  system = rospy.myargv()[3]
314 
315  try:
316  robot_description = rospy.get_param('robot_description')
317  except:
318  rospy.logfatal('robot_description not set, exiting')
319  sys.exit(-1)
320 
321  executive = CaptureExecutive(config_dir, system, robot_description)
322  time.sleep(1.0)
323 
324  # setup our samples
325  sample_steps = list()
326  sample_names = dict()
327  sample_options = dict()
328  sample_success = dict()
329  sample_failure = dict()
330  for directory in os.listdir(samples_dir):
331  try:
332  sample_options[directory] = yaml.load(open(samples_dir + '/' + directory + '/config.yaml'))
333  sample_steps.append(directory)
334  except IOError:
335  continue
336  sample_names[directory] = [x for x in os.listdir(samples_dir + '/' + directory + '/') if '.yaml' in x and x != 'config.yaml']
337  sample_names[directory].sort()
338  sample_success[directory] = 0
339  sample_failure[directory] = 0
340  sample_steps.sort()
341 
342  for step in sample_steps:
343  rospy.logdebug("%s Samples: \n - %s" % (sample_options[step]['group'], "\n - ".join(sample_names[step])))
344 
345  pub = rospy.Publisher("robot_measurement", RobotMeasurement)
346 
347  try:
348  for step in sample_steps:
349  keep_collecting = True
350  full_paths = [samples_dir + '/' + step + '/' + x for x in sample_names[step] ]
351  if len(full_paths) == 0:
352  rospy.logfatal(samples_dir + '/' + step + '/' + ' is not found' )
353  rospy.logfatal('please run make_samples.py' )
354  sys.exit(-1)
355  cur_config = yaml.load(open(full_paths[0]))
356  m_robot = executive.capture(cur_config, rospy.Duration(0.01))
357  while not rospy.is_shutdown() and keep_collecting:
358  print
359  print sample_options[step]["prompt"]
360  print "Press <enter> to continue, type N to exit this step"
361  resp = raw_input(">>>")
362  if string.upper(resp) == "N":
363  print sample_options[step]["finish"]
364  keep_collecting = False
365  else:
366  for cur_sample_path in full_paths:
367  print "On %s sample [%s]" % (sample_options[step]["group"], cur_sample_path)
368  cur_config = yaml.load(open(cur_sample_path))
369  m_robot = executive.capture(cur_config, rospy.Duration(40))
370  if m_robot is None:
371  print "--------------- Failed To Capture a %s Sample -----------------" % sample_options[step]["group"]
372  if not sample_options[step]["repeat"]:
373  sample_failure[step] += 1
374  else:
375  print "++++++++++++++ Successfully Captured a %s Sample ++++++++++++++" % sample_options[step]["group"]
376  sample_success[step] += 1
377  pub.publish(m_robot)
378  print "Succeeded on %u %s samples" % (sample_success[step], sample_options[step]["group"])
379  if rospy.is_shutdown():
380  break
381  keep_collecting = sample_options[step]["repeat"]
382  except EOFError:
383  print "Exiting"
384 
385  time.sleep(1)
386 
387  print "Calibration data collection has completed!"
388  for step in sample_steps:
389  if sample_options[step]["repeat"]:
390  print "%s Samples: %u" % (sample_options[step]["group"], sample_success[step])
391  else:
392  print "%s Samples: %u/%u" % (sample_options[step]["group"], sample_success[step], sample_success[step] + sample_failure[step])
393  print ""
394  print "You can now kill this node, along with any other calibration nodes that are running."
395 
396 
def capture(self, next_configuration, timeout)
def add_laser_measurement(self, laser_id, msg, interval_start, interval_end)
def add_chain_measurement(self, chain_id, msg)
def add_cam_measurement(self, cam_id, msg)
def __init__(self, config_dir, system, robot_desc, output_debug=False)
Definition: capture_exec.py:54


calibration_launch
Author(s): Michael Ferguson
autogenerated on Thu Jun 6 2019 19:17:33