robot_measurement_cache.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 roslib; roslib.load_manifest('calibration_launch')
35 import sys
36 
37 import rospy
38 from calibration_msgs.msg import *
39 from bisect import bisect
40 
41 def stamped_cmp(x,y):
42  return cmp(x.header.stamp, y.header.stamp)
43 
44 def laser_cmp(x,y):
45  return cmp(x[0].header.stamp, y[0].header.stamp)
46 
47 # Store time histories of many camera, chain, and laser measurements. These stored info
48 # then used when a robot_measurement is requested.
50  def __init__(self):
51  self._cam_sizes = dict()
52  self._laser_sizes = dict()
53  self._chain_sizes = dict()
54  pass
55 
56  # Specify what sensor data we're expecting to receive
57  def reconfigure(self, cam_ids, chain_ids, laser_ids):
58  self._cam_caches = dict( [ (x,[]) for x in cam_ids ] )
59  self._laser_caches = dict( [ (x,[]) for x in laser_ids] )
60  self._chain_caches = dict( [ (x,[]) for x in chain_ids] )
61 
62  # Specify default max sizes, if they don't exist yet
63  for cam_id in [x for x in cam_ids if x not in self._cam_sizes]:
64  self._cam_sizes[cam_id] = 100
65  for chain_id in [x for x in chain_ids if x not in self._chain_sizes]:
66  self._chain_sizes[chain_id] = 3000
67  for laser_id in [x for x in laser_ids if x not in self._laser_sizes]:
68  self._laser_sizes[laser_id] = 5
69 
70  # Store the max sizes for all the caches as dictionaries with (id, max_size) pairs
71  def set_max_sizes(self, cam_sizes, chain_sizes, laser_sizes):
72  self._cam_sizes = cam_sizes
73  self._laser_sizes = laser_sizes
74  self._chain_sizes = chain_sizes
75 
76  def add_cam_measurement(self, cam_id, m):
77  cur_cache = self._cam_caches[cam_id]
78  cur_cache.append( m )
79  cur_cache.sort(stamped_cmp)
80  while len(cur_cache) > self._cam_sizes[cam_id]:
81  cur_cache.pop(0)
82 
83  def add_chain_measurement(self, chain_id, m):
84  cur_cache = self._chain_caches[chain_id]
85  cur_cache.append( m )
86  cur_cache.sort(stamped_cmp)
87  while len(cur_cache) > self._chain_sizes[chain_id]:
88  cur_cache.pop(0)
89 
90  def add_laser_measurement(self, laser_id, m, interval_start, interval_end):
91  cur_cache = self._laser_caches[laser_id]
92  cur_cache.append( [m, interval_start, interval_end] )
93  cur_cache.sort(laser_cmp)
94  while len(cur_cache) > self._laser_sizes[laser_id]:
95  cur_cache.pop(0)
96 
97  def request_robot_measurement(self, interval_start, interval_end, min_duration = rospy.Duration(2,0)):
98  # Compute the center of the req interval
99  req_duration = interval_end - interval_start
100  req_center = interval_start + rospy.Duration(req_duration.to_sec()*.5)
101 
102  if req_duration < min_duration:
103  return None
104 
105  # Extract the cam measurements closest to the center of the interval
106  cam_measurements = dict( [ (x, None) for x in self._cam_caches.keys() ] )
107  for cam_id in self._cam_caches.keys():
108  # Get the index elem that is [approx] closest to the center of the interval
109  cur_cache = self._cam_caches[cam_id]
110  right_center_index = bisect( [x.header.stamp for x in cur_cache], req_center )
111 
112  # Make sure the index makes sense, and is in the interval
113  # Note: This is not a 'complete' or 'exact' algorithm. It can definitely be improved... if we care
114  if right_center_index >= len(cur_cache):
115  cam_measurements[cam_id] = None
116  elif cur_cache[right_center_index].header.stamp > interval_end or cur_cache[right_center_index].header.stamp < interval_start:
117  cam_measurements[cam_id] = None
118  else:
119  cam_measurements[cam_id] = cur_cache[right_center_index]
120 
121  # Extract the chain measurements closest to the center of the interval
122  chain_measurements = dict( [ (x, None) for x in self._chain_caches.keys() ] )
123  for chain_id in self._chain_caches.keys():
124  # Get the index elem that is [approx] closest to the center of the interval
125  cur_cache = self._chain_caches[chain_id]
126  right_center_index = bisect( [x.header.stamp for x in cur_cache], req_center )
127 
128  # Make sure the index makes sense, and is in the interval
129  # Note: This is not a 'complete' or 'exact' algorithm. It can definitely be improved... if we care
130  if right_center_index >= len(cur_cache):
131  chain_measurements[chain_id] = None
132  elif cur_cache[right_center_index].header.stamp > interval_end or cur_cache[right_center_index].header.stamp < interval_start:
133  chain_measurements[chain_id] = None
134  else:
135  chain_measurements[chain_id] = cur_cache[right_center_index]
136 
137  # Use a slightly different strategy for the laser. Get all of them, and then choose the first one
138  laser_measurements = dict( [ (x, None) for x in self._laser_caches.keys() ] )
139  for laser_id in self._laser_caches.keys():
140  cur_cache = self._laser_caches[laser_id]
141  sub_list = [x for x in cur_cache if x[2] <= interval_end and x[1] >= interval_start]
142  if len(sub_list) > 0:
143  laser_measurements[laser_id] = sub_list[0][0]
144  else:
145  laser_measurements[laser_id] = None
146 
147  failed_sensors = []
148  good_sensors = []
149 
150  # See if we got everything that we needed
151  for cam_id, m in cam_measurements.items():
152  if m is None:
153  failed_sensors.append(cam_id)
154  else:
155  good_sensors.append(cam_id)
156 
157  for chain_id, m in chain_measurements.items():
158  if m is None:
159  failed_sensors.append(chain_id)
160  else:
161  good_sensors.append(chain_id)
162 
163  for laser_id, m in laser_measurements.items():
164  if m is None:
165  failed_sensors.append(laser_id)
166  else:
167  good_sensors.append(laser_id)
168 
169  if len(failed_sensors) > 0:
170  if len(failed_sensors) > 1:
171  m= "Failed to capture samples from %s"%(', '.join(failed_sensors))
172  else:
173  m= "Failed to capture a sample from %s"%(failed_sensors[0])
174  return m
175 
176  # TODO: eliminate print statement or convert to rospy.log*
177  print "Received everything!"
178 
179  # Push everything into a RobotMeasurement message
180  m_robot = RobotMeasurement()
181  m_robot.M_cam = cam_measurements.values()
182  m_robot.M_chain = chain_measurements.values()
183  m_robot.M_laser = laser_measurements.values()
184 
185  return m_robot
186 
def add_laser_measurement(self, laser_id, m, interval_start, interval_end)
def set_max_sizes(self, cam_sizes, chain_sizes, laser_sizes)
def request_robot_measurement(self, interval_start, interval_end, min_duration=rospy.Duration(2, 0))


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