$search
00001 # Software License Agreement (BSD License) 00002 # 00003 # Copyright (c) 2008, Willow Garage, Inc. 00004 # All rights reserved. 00005 # 00006 # Redistribution and use in source and binary forms, with or without 00007 # modification, are permitted provided that the following conditions 00008 # are met: 00009 # 00010 # * Redistributions of source code must retain the above copyright 00011 # notice, this list of conditions and the following disclaimer. 00012 # * Redistributions in binary form must reproduce the above 00013 # copyright notice, this list of conditions and the following 00014 # disclaimer in the documentation and/or other materials provided 00015 # with the distribution. 00016 # * Neither the name of Willow Garage, Inc. nor the names of its 00017 # contributors may be used to endorse or promote products derived 00018 # from this software without specific prior written permission. 00019 # 00020 # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 00021 # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 00022 # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 00023 # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 00024 # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 00025 # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 00026 # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 00027 # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 00028 # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 00029 # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 00030 # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00031 # POSSIBILITY OF SUCH DAMAGE. 00032 # 00033 00034 import roslib 00035 roslib.load_manifest('dense_laser_assembler') 00036 00037 import time 00038 import rospy 00039 import sys 00040 import threading 00041 from laser_scan.msg import * 00042 from roslib import rostime 00043 00044 # Stores a single laser scan plus some meta information 00045 class LaserCacheElem() : 00046 pass 00047 00048 00049 class DenseLaserCache() : 00050 def __init__(self, req_callback, max_mech_len, max_laser_done_len, max_laser_wait_len) : 00051 00052 # Define callback for interval cloud req 00053 self._req_callback = req_callback 00054 00055 # Specify all queue lengths 00056 self._max_mech_len = max_mech_len 00057 self._max_laser_done_len = max_laser_done_len 00058 self._max_laser_wait_len = max_laser_wait_len 00059 00060 # Init mutexes 00061 self._mech_lock = threading.Lock() 00062 self._laser_done_lock = threading.Lock() 00063 self._laser_wait_lock = threading.Lock() 00064 self._interval_req_lock = threading.Lock() 00065 00066 # Define queues 00067 self._mech_q = [ ] # Mechanism State Data 00068 self._laser_done_q = [] # Laser scans that have already been processed 00069 self._laser_wait_q = [] # Laser scans that we've received but haven't/can't processed yet 00070 self._interval_req_q = [] # Stores list of intervals that we need to process 00071 00072 def add_mech_state(self, msg) : 00073 self._mech_lock.acquire() 00074 self._mech_q.append(msg) 00075 while len(self._mech_q) > self._max_mech_len : 00076 self._mech_q.pop(0) 00077 self._mech_lock.release() 00078 00079 def add_scan(self, msg) : 00080 self._laser_wait_lock.acquire() 00081 self._laser_wait_q.append(msg) 00082 while len(self._laser_wait_q) > self._max_laser_wait_len : 00083 self._laser_wait_q.pop(0) 00084 self._laser_wait_lock.release() 00085 00086 # Iterate through the list of scans we're waiting to process, and process each one 00087 # if we're ready. A scan is ready to be processed if there is at least one 00088 # MechanismState message available before and after the scan. The processed scan is 00089 # then added to the laser_done queue 00090 def process_scans(self) : 00091 self._mech_lock.acquire() 00092 00093 if len(self._mech_q) > 0 : 00094 oldest_mech_time = self._mech_q[0].header.stamp 00095 newest_mech_time = self._mech_q[-1].header.stamp 00096 else : 00097 self._mech_lock.release() 00098 return 00099 00100 self._laser_wait_lock.acquire() 00101 00102 scan_interval = [ (x.header.stamp, 00103 # x.header.stamp + rostime.Duration().from_seconds(0.0) ) 00104 x.header.stamp+rostime.Duration().from_seconds(len(x.ranges)*x.time_increment)) 00105 for x in self._laser_wait_q ] 00106 00107 laser_pending_q = [x for i,x in enumerate(self._laser_wait_q) 00108 if scan_interval[i][1] < newest_mech_time and 00109 scan_interval[i][0] > oldest_mech_time] 00110 # Purge entire queue, except scans in the future 00111 self._laser_wait_q = [x for i,x in enumerate(self._laser_wait_q) 00112 if scan_interval[i][1] >= newest_mech_time] 00113 00114 self._laser_wait_lock.release() 00115 00116 # Now process the pending queue 00117 processed = [self._process_pending(x,self._mech_q) for x in laser_pending_q] 00118 00119 #if len(processed) > 0 : 00120 # print 'processed %i scans' % len(processed) 00121 00122 self._laser_done_lock.acquire() 00123 self._laser_done_q = self._laser_done_q + processed 00124 while len(self._laser_done_q) > self._max_laser_done_len : 00125 self._laser_done_q.pop(0) 00126 self._laser_done_lock.release() 00127 self._mech_lock.release() 00128 # Process a scan, assuming that we have MechansimState data available before and after the scan 00129 # \param scan_msg The message that we want to process 00130 # \param the time-order queue of mechanism_state data 00131 # \param The processed scan element 00132 def _process_pending(self, scan_msg, mech_q): 00133 scan_end = scan_msg.header.stamp + rostime.Duration().from_seconds(len(scan_msg.ranges)*scan_msg.time_increment) 00134 mech_before = [x for x in mech_q if x.header.stamp < scan_msg.header.stamp][-1] 00135 mech_after = [x for x in mech_q if x.header.stamp > scan_end][0] 00136 00137 # Get the joint position and time associated with the MechanismState before the scan 00138 ind = [x.name for x in mech_before.joint_states].index('laser_tilt_mount_joint') 00139 pos_before = mech_before.joint_states[ind].position 00140 time_before= mech_before.header.stamp 00141 00142 # Get the joint position and time associated with the MechanismState before the scan 00143 ind = [x.name for x in mech_after.joint_states].index('laser_tilt_mount_joint') 00144 pos_after = mech_after.joint_states[ind].position 00145 time_after= mech_after.header.stamp 00146 00147 elapsed = (time_after - time_before).to_seconds() 00148 00149 # Linear interp 00150 #pos_during = elapsed_after/elapsed_total*pos_before + elapsed_before/elapsed_total*pos_after 00151 00152 #key_rays = range(0, len(scan_msg.ranges)) 00153 key_rays = [0, len(scan_msg.ranges)-1] 00154 time_during = [ scan_msg.header.stamp 00155 + rostime.Duration().from_seconds(scan_msg.time_increment*x) 00156 for x in key_rays] 00157 time_normalized = [ (x - time_before).to_seconds()/elapsed for x in time_during] 00158 if any( [x > 1 for x in time_normalized]) or any( [x < 0 for x in time_normalized] ) : 00159 print 'Error computing normalized time' 00160 pos_during = [ pos_before * (1-x) + pos_after * x for x in time_normalized ] 00161 00162 elem = LaserCacheElem() 00163 elem.pos = pos_during 00164 elem.scan = scan_msg 00165 return elem 00166 00167 # Returns a list of processed scan elems between the start and end time 00168 def get_dense_cloud(self, start, end) : 00169 self._laser_done_lock.acquire() 00170 trimmed_done = [x for x in self._laser_done_q 00171 if x.scan.header.stamp <= end and 00172 x.scan.header.stamp >= start] 00173 self._laser_done_lock.release() 00174 return trimmed_done 00175 00176 # Returns the time associated with the latest scan 00177 def get_latest_done_scan_time(self) : 00178 self._laser_done_lock.acquire() 00179 if len(self._laser_done_q) > 0 : 00180 latest_time = self._laser_done_q[-1].scan.header.stamp 00181 else : 00182 latest_time = rostime.Time().from_seconds(0.0) 00183 self._laser_done_lock.release() 00184 return latest_time 00185 00186 # Adds a request for extracting data during an interval. This request will be attempted to 00187 # processed when process_interval_reqs is called. 00188 def add_interval_req(self, start, end) : 00189 self._interval_req_lock.acquire() 00190 self._interval_req_q.append([start, end]) 00191 self._interval_req_lock.release() 00192 self.process_interval_reqs() 00193 00194 # Attempts to service all the interval requests added by calling add_interval_req. A req can 00195 # be serviced if there exists a processed scan elem with a timestamp greater than the end time 00196 # of the interval request 00197 def process_interval_reqs(self) : 00198 00199 latest_time = self.get_latest_done_scan_time() 00200 00201 # Split list into request we're ready to process, on one's we're not ready for 00202 self._interval_req_lock.acquire() 00203 pending_reqs = [x for x in self._interval_req_q if x[1] < latest_time] 00204 idle_reqs = [x for x in self._interval_req_q if x[1] >= latest_time] 00205 self._interval_req_q = idle_reqs 00206 self._interval_req_lock.release() 00207 00208 # Compute clouds for the requests we're ready to process 00209 clouds = [self.get_dense_cloud(x[0], x[1]) for x in pending_reqs] 00210 00211 for x in clouds : 00212 self._req_callback(x)