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('calibration_launch')
00035 import sys
00036 
00037 import rospy
00038 from calibration_msgs.msg import *
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 
00062         # Specify default max sizes, if they don't exist yet
00063         for cam_id in [x for x in cam_ids if x not in self._cam_sizes]:
00064             self._cam_sizes[cam_id] = 100
00065         for chain_id in [x for x in chain_ids if x not in self._chain_sizes]:
00066             self._chain_sizes[chain_id] = 3000
00067         for laser_id in [x for x in laser_ids if x not in self._laser_sizes]:
00068             self._laser_sizes[laser_id] = 5
00069 
00070     # Store the max sizes for all the caches as dictionaries with (id, max_size) pairs
00071     def set_max_sizes(self, cam_sizes, chain_sizes, laser_sizes):
00072         self._cam_sizes   = cam_sizes
00073         self._laser_sizes = laser_sizes
00074         self._chain_sizes = chain_sizes
00075 
00076     def add_cam_measurement(self, cam_id, m):
00077         cur_cache = self._cam_caches[cam_id]
00078         cur_cache.append( m )
00079         cur_cache.sort(stamped_cmp)
00080         while len(cur_cache) > self._cam_sizes[cam_id]:
00081             cur_cache.pop(0)
00082 
00083     def add_chain_measurement(self, chain_id, m):
00084         cur_cache = self._chain_caches[chain_id]
00085         cur_cache.append( m )
00086         cur_cache.sort(stamped_cmp)
00087         while len(cur_cache) > self._chain_sizes[chain_id]:
00088             cur_cache.pop(0)
00089 
00090     def add_laser_measurement(self, laser_id, m, interval_start, interval_end):
00091         cur_cache = self._laser_caches[laser_id]
00092         cur_cache.append( [m, interval_start, interval_end] )
00093         cur_cache.sort(laser_cmp)
00094         while len(cur_cache) > self._laser_sizes[laser_id]:
00095             cur_cache.pop(0)
00096 
00097     def request_robot_measurement(self, interval_start, interval_end, min_duration = rospy.Duration(2,0)):
00098         # Compute the center of the req interval
00099         req_duration = interval_end   - interval_start
00100         req_center   = interval_start + rospy.Duration(req_duration.to_sec()*.5)
00101 
00102         if req_duration < min_duration:
00103             return None
00104 
00105         # Extract the cam measurements closest to the center of the interval
00106         cam_measurements = dict( [ (x, None) for x in self._cam_caches.keys() ] )
00107         for cam_id in self._cam_caches.keys():
00108             # Get the index elem that is [approx] closest to the center of the interval
00109             cur_cache = self._cam_caches[cam_id]
00110             right_center_index = bisect( [x.header.stamp for x in cur_cache], req_center )
00111 
00112             # Make sure the index makes sense, and is in the interval
00113             #   Note: This is not a 'complete' or 'exact' algorithm. It can definitely be improved... if we care
00114             if right_center_index >= len(cur_cache):
00115                 cam_measurements[cam_id] = None
00116             elif cur_cache[right_center_index].header.stamp > interval_end or cur_cache[right_center_index].header.stamp < interval_start:
00117                 cam_measurements[cam_id] = None
00118             else:
00119                 cam_measurements[cam_id] = cur_cache[right_center_index]
00120 
00121         # Extract the chain measurements closest to the center of the interval
00122         chain_measurements = dict( [ (x, None) for x in self._chain_caches.keys() ] )
00123         for chain_id in self._chain_caches.keys():
00124             # Get the index elem that is [approx] closest to the center of the interval
00125             cur_cache = self._chain_caches[chain_id]
00126             right_center_index = bisect( [x.header.stamp for x in cur_cache], req_center )
00127 
00128             # Make sure the index makes sense, and is in the interval
00129             #   Note: This is not a 'complete' or 'exact' algorithm. It can definitely be improved... if we care
00130             if right_center_index >= len(cur_cache):
00131                 chain_measurements[chain_id] = None
00132             elif cur_cache[right_center_index].header.stamp > interval_end or cur_cache[right_center_index].header.stamp < interval_start:
00133                 chain_measurements[chain_id] = None
00134             else:
00135                 chain_measurements[chain_id] = cur_cache[right_center_index]
00136 
00137         # Use a slightly different strategy for the laser. Get all of them, and then choose the first one
00138         laser_measurements = dict( [ (x, None) for x in self._laser_caches.keys() ] )
00139         for laser_id in self._laser_caches.keys():
00140             cur_cache = self._laser_caches[laser_id]
00141             sub_list = [x for x in cur_cache if x[2] <= interval_end and x[1] >= interval_start]
00142             if len(sub_list) > 0:
00143                 laser_measurements[laser_id] = sub_list[0][0]
00144             else:
00145                 laser_measurements[laser_id] = None
00146 
00147         failed_sensors = []
00148         good_sensors = []
00149 
00150         # See if we got everything that we needed
00151         for cam_id, m in cam_measurements.items():
00152             if m is None:
00153                 failed_sensors.append(cam_id)
00154             else:
00155                 good_sensors.append(cam_id)
00156 
00157         for chain_id, m in chain_measurements.items():
00158             if m is None:
00159                 failed_sensors.append(chain_id)
00160             else:
00161                 good_sensors.append(chain_id)
00162 
00163         for laser_id, m in laser_measurements.items():
00164             if m is None:
00165                 failed_sensors.append(laser_id)
00166             else:
00167                 good_sensors.append(laser_id)
00168 
00169         if len(failed_sensors) > 0:
00170            if len(failed_sensors) > 1:
00171                m= "Failed to capture samples from %s"%(', '.join(failed_sensors))
00172            else:
00173                m= "Failed to capture a sample from %s"%(failed_sensors[0])
00174            return m
00175 
00176         # TODO: eliminate print statement or convert to rospy.log*
00177         print "Received everything!"
00178 
00179         # Push everything into a RobotMeasurement message
00180         m_robot = RobotMeasurement()
00181         m_robot.M_cam   = cam_measurements.values()
00182         m_robot.M_chain = chain_measurements.values()
00183         m_robot.M_laser = laser_measurements.values()
00184 
00185         return m_robot
00186 


calibration_launch
Author(s): Michael Ferguson
autogenerated on Tue Sep 27 2016 04:06:51