laser_image_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 from time import sleep
00041 from laser_scan.msg import *
00042 from std_msgs.msg import *
00043 from dense_laser_assembler.msg import *
00044 from pr2_mechanism_controllers.msg import *
00045 from sensor_msgs.msg import Image
00046 
00047 
00048 # Takes the dense laser scan data and converts it into images to can be viewed in
00049 # image_view. Generally used as a debugging tool.
00050 
00051 
00052 def rescale(x, max_val) :
00053     lower_lim = max_val*0.0
00054     upper_lim = max_val*1.0
00055     #lower_lim = 2000
00056     #upper_lim = 3000
00057     a = x - lower_lim
00058     if a <= 0 :
00059         return chr(0)
00060     elif a > upper_lim -lower_lim :
00061         return chr(255)
00062     
00063     return chr(int((2**8-1)*a/(upper_lim-lower_lim)))
00064 
00065 def intensity_callback(msg) :
00066     image = sensor_msgs.msg.Image()
00067     image.header = msg.header
00068     image.label  = 'intensity'
00069     image.encoding = 'mono'
00070     image.depth = 'uint8'
00071     max_val = max(msg.data.data)
00072     #image.uint8_data.data   = "".join([chr(int((2**8-1)*x/max_val)) for x in msg.data.data])
00073     image.uint8_data.data   = "".join([rescale(x,max_val) for x in msg.data.data])
00074     image.uint8_data.layout = msg.data.layout
00075     intensity_pub.publish(image)
00076 
00077 def range_callback(msg) :
00078     image = sensor_msgs.msg.Image()
00079     image.header = msg.header
00080     image.label  = 'range'
00081     image.encoding = 'mono'
00082     image.depth = 'uint8'
00083     max_val = max(msg.data.data)
00084     image.uint8_data.data   = "".join([chr(int((2**8-1)*x/max_val)) for x in msg.data.data])
00085     image.uint8_data.layout = msg.data.layout
00086     range_pub.publish(image)
00087 
00088 if __name__ == '__main__':
00089 
00090     rospy.init_node('laser_imager', anonymous=False)
00091 
00092     intensity_pub = rospy.Publisher("dense_tilt_scan/intensity_image", Image)
00093     range_pub     = rospy.Publisher("dense_tilt_scan/range_image",     Image)
00094     
00095     intensity_sub = rospy.Subscriber("dense_tilt_scan/intensity", Float32MultiArrayStamped, intensity_callback)
00096     range_sub     = rospy.Subscriber("dense_tilt_scan/range",     Float32MultiArrayStamped, range_callback)
00097 
00098     rospy.spin()


dense_laser_assembler
Author(s): Vijay Pradeep
autogenerated on Fri Aug 28 2015 12:06:34