00001 #!/usr/bin/python 00002 # 00003 # Copyright (c) 2010, Georgia Tech Research Corporation 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 are met: 00008 # * Redistributions of source code must retain the above copyright 00009 # notice, this list of conditions and the following disclaimer. 00010 # * Redistributions in binary form must reproduce the above copyright 00011 # notice, this list of conditions and the following disclaimer in the 00012 # documentation and/or other materials provided with the distribution. 00013 # * Neither the name of the Georgia Tech Research Corporation nor the 00014 # names of its contributors may be used to endorse or promote products 00015 # derived from this software without specific prior written permission. 00016 # 00017 # THIS SOFTWARE IS PROVIDED BY GEORGIA TECH RESEARCH CORPORATION ''AS IS'' AND 00018 # ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 00019 # WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 00020 # DISCLAIMED. IN NO EVENT SHALL GEORGIA TECH BE LIABLE FOR ANY DIRECT, INDIRECT, 00021 # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT 00022 # LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, 00023 # OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 00024 # LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE 00025 # OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF 00026 # ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 00027 # 00028 00029 # \author Jason Okerman (Healthcare Robotics Lab, Georgia Tech.) 00030 00031 import roslib; roslib.load_manifest('pr2_clutter_helper') 00032 #depnds on sensor_msgs, opencv2, cv_bridge 00033 import rospy 00034 from sensor_msgs.msg import PointCloud 00035 import cv #version 2.1 expected 00036 from cv_bridge import CvBridge 00037 #may need image_transport? 00038 00039 import time #sleep 00040 from random import * #randint 00041 00042 TOPIC_NAME = "tilt_laser_cloud" 00043 global_ros_data = None 00044 ''' 00045 Comments: 00046 Traps exactly one message from the topic specified. This is 00047 To run 'main', this assumes the launch of "pr2_laser_snapshotter" with mapping to "tilt_laser_cloud" 00048 Basic use is to import and call "subscribe_once" method. 00049 ''' 00050 00051 def init(): 00052 rospy.init_node('my_listener', anonymous=True) 00053 00054 def myCallback(data): 00055 print 'callback called' 00056 global global_ros_data 00057 global_ros_data = data 00058 00059 ##def listener(): 00060 ## rospy.init_node('my_listener', anonymous=True) 00061 ## rospy.Subscriber("tilt_laser_cloud", PointCloud, cloudCallback) 00062 ## rospy.spin() 00063 00064 def subscribe_once(topic_name=TOPIC_NAME, msg_type=PointCloud): 00065 init() 00066 #if not something, init. 00067 s = rospy.Subscriber(topic_name, msg_type, myCallback) 00068 #spin until 1 message is received. 00069 print 'sleeping now: waitin for topic', topic_name 00070 while (not global_ros_image): 00071 time.sleep(.1) 00072 print 'waking and exiting now' 00073 s.unregister() 00074 global global_ros_data 00075 return global_ros_data #value will be returned as 'global_ros_data' 00076 00077 if __name__ == 'main': 00078 data = subscribe_once(TOPIC_NAME, PointCloud) 00079 00080 #assume global_ros_data is defined. 00081 #do something with the pointcloud 00082 for i in range(10): 00083 pt = data.points[i] 00084 print "(%f, %f, %f)" %(pt.x, pt.y, pt.z) 00085 00086 00087 00088 00089 00090