assembler_node.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 
00003 # Software License Agreement (BSD License)
00004 #
00005 # Copyright (c) 2008, Willow Garage, Inc.
00006 # All rights reserved.
00007 #
00008 # Redistribution and use in source and binary forms, with or without
00009 # modification, are permitted provided that the following conditions
00010 # are met:
00011 #
00012 #  * Redistributions of source code must retain the above copyright
00013 #    notice, this list of conditions and the following disclaimer.
00014 #  * Redistributions in binary form must reproduce the above
00015 #    copyright notice, this list of conditions and the following
00016 #    disclaimer in the documentation and/or other materials provided
00017 #    with the distribution.
00018 #  * Neither the name of Willow Garage, Inc. nor the names of its
00019 #    contributors may be used to endorse or promote products derived
00020 #    from this software without specific prior written permission.
00021 #
00022 # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00023 # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00024 # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00025 # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00026 # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00027 # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00028 # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00029 # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00030 # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00031 # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00032 # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00033 # POSSIBILITY OF SUCH DAMAGE.
00034 #
00035 
00036 
00037 import roslib; roslib.load_manifest('dense_laser_assembler')
00038 import sys
00039 import rospy
00040 import threading
00041 from time import sleep
00042 from sensor_msgs.msg import LaserScan
00043 from pr2_msgs import LaserScannerSignal
00044 from mechanism_msgs.msg import MechanismState
00045 from std_msgs.msg import *
00046 from dense_laser_assembler.msg import *
00047 from pr2_mechanism_controllers.msg import *
00048 from dense_laser_assembler.dense_laser_cache import DenseLaserCache
00049 from sensor_msgs.msg import *
00050 
00051 # Globals
00052 laser_cache = [ ]
00053 first_signal = True
00054 prev_signal_time = [ ]
00055 image_pub = [ ]
00056 test_pub = [ ]
00057 
00058 
00059 def scan_callback(msg) :
00060     req_lock.acquire()
00061     laser_cache.add_scan(msg)
00062     laser_cache.process_scans()
00063     laser_cache.process_interval_reqs()
00064     req_lock.release()
00065 
00066 def mech_callback(msg) :
00067     req_lock.acquire()
00068     laser_cache.add_mech_state(msg)
00069     laser_cache.process_scans()
00070     laser_cache.process_interval_reqs()
00071     req_lock.release()
00072 
00073 def signal_callback(msg) :
00074     req_lock.acquire()
00075     global first_signal
00076     global prev_signal
00077     laser_cache.process_scans()
00078     if first_signal :
00079         first_signal = False
00080     else :
00081         laser_cache.add_interval_req(prev_signal.header.stamp, msg.header.stamp)
00082         laser_cache.process_interval_reqs()
00083     prev_signal = msg
00084     print 'Got signal message!'
00085     print '  Signal=%i' % msg.signal
00086     req_lock.release()
00087 
00088 def interval_req_callback(scans) :
00089 
00090     # Check data consistency
00091     if (len(scans) == 0) :
00092         print 'Processed interval with no scans. Not publishing'
00093         return
00094 
00095     rows = len(scans)
00096     cols = len(scans[0].scan.ranges)
00097     if any( [len(x.scan.ranges) != cols for x in scans] ) or any( [len(x.scan.intensities) != cols for x in scans]) :
00098         print "# Readings aren't consistent across interval"
00099         return
00100 
00101 
00102     print 'About to process cloud with %u scans' % len(scans)
00103 
00104     # Sort by tilting joint angle
00105     sorted_scans = sorted(scans, lambda x,y:cmp(x.pos[0],y.pos[0]))
00106 
00107 
00108     msg_header = rospy.Header()
00109     msg_header.stamp = scans[-1].scan.header.stamp
00110     msg_header.frame_id = scans[-1].scan.header.frame_id
00111 
00112     layout = MultiArrayLayout([ MultiArrayDimension('rows', rows, rows*cols),
00113                                 MultiArrayDimension('cols', cols, cols),
00114                                 MultiArrayDimension('elem', 1, 1) ], 0 )
00115     # Build the messages in a reasonable ROS format
00116     intensity_msg = Float32MultiArrayStamped()
00117     range_msg = Float32MultiArrayStamped()
00118     info_msg  = scans[-1].scan
00119     joint_msg = Float32MultiArrayStamped()
00120 
00121     intensity_msg.header = msg_header
00122     range_msg.header     = msg_header
00123     joint_msg.header     = msg_header
00124 
00125     intensity_msg.data.layout = layout
00126     range_msg.data.layout     = layout
00127     joint_msg.data.layout = MultiArrayLayout([ MultiArrayDimension('rows', rows, rows*2),
00128                                                MultiArrayDimension('cols', 2, 2),
00129                                                MultiArrayDimension('elem', 1, 1) ], 0 )
00130     #joint_msg.data.layout = layout
00131 
00132     # Clear out data from the info message. (Keep everything except intensity and range data)
00133     #info_msg.ranges = [ ]
00134     #info_msg.intensities = [ ]
00135 
00136     # Populate intensities & ranges
00137     intensity_msg.data.data = []
00138     range_msg.data.data = []
00139     for x in sorted_scans :
00140         intensity_msg.data.data.extend(x.scan.intensities)
00141         range_msg.data.data.extend(x.scan.ranges)
00142         joint_msg.data.data.extend(x.pos)
00143 
00144     intensity_pub.publish(intensity_msg)
00145     range_pub.publish(range_msg)
00146     info_pub.publish(info_msg)
00147     joint_pos_pub.publish(joint_msg)
00148 
00149     #image = sensor_msgs.msg.Image()
00150 
00151     #image.header = msg_header
00152     #image.label  = 'intensity'
00153     #image.encoding = 'mono'
00154     #image.depth = 'uint8'
00155     #max_val = max(joint_msg.data.data)
00156     #min_val = min(joint_msg.data.data)
00157     #image.uint8_data.data   = "".join([chr(int(255*x/max_val)) for x in intensity_msg.data.data])
00158     #image.uint8_data.data   = "".join([chr(int(255*(x-min_val)/(max_val-min_val))) for x in joint_msg.data.data])
00159     #image.uint8_data.data = ''
00160     #image.uint8_data.layout = intensity_msg.data.layout
00161     #image_pub.publish(image)
00162 
00163 
00164     print '  Processed cloud with %u rays' % len(intensity_msg.data.data)
00165 
00166 if __name__ == '__main__':
00167 
00168     req_lock = threading.Lock()
00169 
00170     print 'Initializing DenseLaserCache'
00171     laser_cache = DenseLaserCache(interval_req_callback, 200, 1000, 100) ;
00172     print 'Done initializing'
00173 
00174     rospy.init_node('dense_assembler', anonymous=False)
00175     signal_sub = rospy.Subscriber("laser_tilt_controller/laser_scanner_signal",
00176                                   LaserScannerSignal, signal_callback)
00177 
00178     intensity_pub = rospy.Publisher("dense_tilt_scan/intensity", Float32MultiArrayStamped)
00179     range_pub     = rospy.Publisher("dense_tilt_scan/range",     Float32MultiArrayStamped)
00180     info_pub      = rospy.Publisher("dense_tilt_scan/scan_info", LaserScan)
00181     joint_pos_pub = rospy.Publisher("dense_tilt_scan/joint_info",Float32MultiArrayStamped)
00182     #image_pub = rospy.Publisher("test_image", Image)
00183 
00184     scan_sub = rospy.Subscriber("tilt_scan", LaserScan, scan_callback)
00185 
00186     mech_sub = rospy.Subscriber("mechanism_state", MechanismState, mech_callback)
00187 
00188     rospy.spin()
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Properties Friends Defines


dense_laser_assembler
Author(s): Vijay Pradeep
autogenerated on Mon Aug 19 2013 10:50:20