robot_measurement_cache.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 # Software License Agreement (BSD License)
00003 #
00004 # Copyright (c) 2008, Willow Garage, Inc.
00005 # All rights reserved.
00006 #
00007 # Redistribution and use in source and binary forms, with or without
00008 # modification, are permitted provided that the following conditions
00009 # are met:
00010 #
00011 #  * Redistributions of source code must retain the above copyright
00012 #    notice, this list of conditions and the following disclaimer.
00013 #  * Redistributions in binary form must reproduce the above
00014 #    copyright notice, this list of conditions and the following
00015 #    disclaimer in the documentation and/or other materials provided
00016 #    with the distribution.
00017 #  * Neither the name of Willow Garage, Inc. nor the names of its
00018 #    contributors may be used to endorse or promote products derived
00019 #    from this software without specific prior written permission.
00020 #
00021 # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022 # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023 # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024 # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025 # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026 # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027 # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028 # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029 # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030 # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031 # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032 # POSSIBILITY OF SUCH DAMAGE.
00033 
00034 import roslib; roslib.load_manifest('pr2_calibration_executive')
00035 import sys
00036 
00037 import rospy
00038 from camera_pose_calibration.msg import RobotMeasurement 
00039 from bisect import bisect
00040 
00041 def stamped_cmp(x,y):
00042     return cmp(x.header.stamp, y.header.stamp)
00043 
00044 def laser_cmp(x,y):
00045     return cmp(x[0].header.stamp, y[0].header.stamp)
00046 
00047 # Store time histories of many camera, chain, and laser measurements. These stored info
00048 # then used when a robot_measurement is requested.
00049 class RobotMeasurementCache:
00050     def __init__(self):
00051         self._cam_sizes   = dict()
00052         self._laser_sizes = dict()
00053         self._chain_sizes = dict()
00054         pass
00055 
00056     # Specify what sensor data we're expecting to receive
00057     def reconfigure(self, cam_ids, chain_ids, laser_ids):
00058         self._cam_caches   = dict( [ (x,[]) for x in cam_ids  ] )
00059         self._laser_caches = dict( [ (x,[]) for x in laser_ids] )
00060         self._chain_caches = dict( [ (x,[]) for x in chain_ids] )
00061         #print "Reconfigured the Cache:"
00062         #print "   cam_ids:   " + ",".join(cam_ids)
00063         #print "   chain_ids: " + ",".join(chain_ids)
00064         #print "   laser_ids: " + ",".join(laser_ids)
00065 
00066         # Specify default max sizes, if they don't exist yet
00067         #print "Specifying default sizes for caches"
00068         for cam_id in [x for x in cam_ids if x not in self._cam_sizes]:
00069             #print "  Adding default size for camera [%s]" % cam_id
00070             self._cam_sizes[cam_id] = 100
00071         for chain_id in [x for x in chain_ids if x not in self._chain_sizes]:
00072             #print "  Adding default size for chain [%s]" % chain_id
00073             self._chain_sizes[chain_id] = 3000
00074         for laser_id in [x for x in laser_ids if x not in self._laser_sizes]:
00075             #print "  Adding default size for laser [%s]" % laser_id
00076             self._laser_sizes[laser_id] = 5
00077 
00078     # Store the max sizes for all the caches as dictionaries with (id, max_size) pairs
00079     def set_max_sizes(self, cam_sizes, chain_sizes, laser_sizes):
00080         self._cam_sizes   = cam_sizes
00081         self._laser_sizes = laser_sizes
00082         self._chain_sizes = chain_sizes
00083 
00084     def add_cam_measurement(self, cam_id, m):
00085         cur_cache = self._cam_caches[cam_id]  # list
00086         cur_cache.append( m )
00087         cur_cache.sort(stamped_cmp)
00088         while len(cur_cache) > self._cam_sizes[cam_id]:
00089             cur_cache.pop(0)
00090 
00091     def add_chain_measurement(self, chain_id, m):
00092         cur_cache = self._chain_caches[chain_id]
00093         cur_cache.append( m )
00094         cur_cache.sort(stamped_cmp)
00095 
00096         #print "Adding elem to cache:"
00097         #print "  Elem stamp:  %u.%09u" % (m.header.stamp.secs, m.header.stamp.nsecs)
00098         #print "  Cache Start: %u.%09u" % (cur_cache[0].header.stamp.secs, cur_cache[0].header.stamp.nsecs)
00099         #print "  Cache End:   %u.%09u" % (cur_cache[-1].header.stamp.secs, cur_cache[-1].header.stamp.nsecs)
00100 
00101         while len(cur_cache) > self._chain_sizes[chain_id]:
00102             cur_cache.pop(0)
00103 
00104     def add_laser_measurement(self, laser_id, m, interval_start, interval_end):
00105         print self._laser_caches.keys()
00106         #print "Trying to add [%s] elem to laser_caches" % laser_id
00107         cur_cache = self._laser_caches[laser_id]
00108         cur_cache.append( [m, interval_start, interval_end] )
00109         cur_cache.sort(laser_cmp)
00110 
00111         while len(cur_cache) > self._laser_sizes[laser_id]:
00112             cur_cache.pop(0)
00113 
00114     def request_robot_measurement(self, interval_start, interval_end, min_duration = rospy.Duration(2,0)):
00115         #print "in request_robot_measurement()"
00116         # Compute the center of the req interval
00117         req_duration = interval_end   - interval_start
00118         req_center   = interval_start + rospy.Duration(req_duration.to_sec()*.5)
00119 
00120         if req_duration < min_duration:
00121             #print "Minimum duration of [%.2fs] not yet reached" % min_duration.to_seconds()
00122             return None
00123 
00124         # Extract the cam measurements closest to the center of the interval
00125         cam_measurements = dict( [ (x, None) for x in self._cam_caches.keys() ] )
00126         for cam_id in self._cam_caches.keys():
00127             #print "On %s" % cam_id
00128             # Get the index elem that is [approx] closest to the center of the interval
00129             cur_cache = self._cam_caches[cam_id]
00130             right_center_index = bisect( [x.header.stamp for x in cur_cache], req_center )
00131             #print "  Total of %u elems" % len(cur_cache)
00132             #print "  Bisect result: %u" % right_center_index
00133             #if len(cur_cache) > 0:
00134             #    print "  Cache Start: %u.%u" % (cur_cache[0].header.stamp.secs, cur_cache[0].header.stamp.nsecs)
00135             #    print "  Cache End:   %u.%u" % (cur_cache[-1].header.stamp.secs, cur_cache[-1].header.stamp.nsecs)
00136 
00137             # Make sure the index makes sense, and is in the interval
00138             #   Note: This is not a 'complete' or 'exact' algorithm. It can definitely be improved... if we care
00139             if right_center_index >= len(cur_cache):
00140                 cam_measurements[cam_id] = None
00141             elif cur_cache[right_center_index].header.stamp > interval_end or cur_cache[right_center_index].header.stamp < interval_start:
00142                 cam_measurements[cam_id] = None
00143             else:
00144                 cam_measurements[cam_id] = cur_cache[right_center_index]
00145 
00146         # Extract the chain measurements closest to the center of the interval
00147         chain_measurements = dict( [ (x, None) for x in self._chain_caches.keys() ] )
00148         for chain_id in self._chain_caches.keys():
00149             #print "On %s" % chain_id
00150             # Get the index elem that is [approx] closest to the center of the interval
00151             cur_cache = self._chain_caches[chain_id]
00152             right_center_index = bisect( [x.header.stamp for x in cur_cache], req_center )
00153 
00154             #print "  Total of %u elems" % len(cur_cache)
00155             #print "  Bisect result: %u" % right_center_index
00156             #if len(cur_cache) > 0:
00157             #    print "  Cache Start: %u.%u" % (cur_cache[0].header.stamp.secs, cur_cache[0].header.stamp.nsecs)
00158             #    print "  Cache End:   %u.%u" % (cur_cache[-1].header.stamp.secs, cur_cache[-1].header.stamp.nsecs)
00159 
00160             # Make sure the index makes sense, and is in the interval
00161             #   Note: This is not a 'complete' or 'exact' algorithm. It can definitely be improved... if we care
00162             if right_center_index >= len(cur_cache):
00163                 chain_measurements[chain_id] = None
00164             elif cur_cache[right_center_index].header.stamp > interval_end or cur_cache[right_center_index].header.stamp < interval_start:
00165                 chain_measurements[chain_id] = None
00166             else:
00167                 chain_measurements[chain_id] = cur_cache[right_center_index]
00168 
00169         # Use a slightly different strategy for the laser. Get all of them, and then choose the first one
00170         laser_measurements = dict( [ (x, None) for x in self._laser_caches.keys() ] )
00171         for laser_id in self._laser_caches.keys():
00172             cur_cache = self._laser_caches[laser_id]
00173             sub_list = [x for x in cur_cache if x[2] <= interval_end and x[1] >= interval_start]
00174             if len(sub_list) > 0:
00175                 laser_measurements[laser_id] = sub_list[0][0]
00176             else:
00177                 laser_measurements[laser_id] = None
00178 
00179         # See if we got everything that we needed
00180         for cam_id, m in cam_measurements.items():
00181             if m is None:
00182                 #print "Didn't get a valid [%s]" % cam_id
00183                 return None
00184 
00185         for chain_id, m in chain_measurements.items():
00186             if m is None:
00187                 #print "Didn't get a [%s]" % chain_id
00188                 return None
00189 
00190         for laser_id, m in laser_measurements.items():
00191             if m is None:
00192                 #print "Didn't get a [%s]" % laser_id
00193                 return None
00194 
00195         print "Received everything!"
00196 
00197         # Push everything into a RobotMeasurement message
00198         m_robot = RobotMeasurement()
00199         m_robot.M_cam   = cam_measurements.values()
00200 
00201         return m_robot
00202 
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Properties Friends


camera_pose_calibration
Author(s): Vijay Pradeep, Wim Meeussen
autogenerated on Thu Aug 15 2013 10:18:08