dense_laser_cache.py
Go to the documentation of this file.
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)


dense_laser_assembler
Author(s): Vijay Pradeep
autogenerated on Wed Apr 3 2019 02:59:29