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 00032 """ 00033 Grab data from topic <given> of generic <type>. 00034 """ 00035 import roslib; roslib.load_manifest('pr2_clutter_helper') 00036 #depnds on sensor_msgs, opencv2, cv_bridge 00037 import rospy 00038 from sensor_msgs.msg import PointCloud 00039 import cv #version 2.1 expected 00040 from cv_bridge import CvBridge 00041 #may need image_transport? 00042 00043 import time #sleep 00044 from random import * #randint 00045 00046 TOPIC_NAME = "tilt_laser_cloud" 00047 global_ros_data = None 00048 NODE_NAME = 'my_listener' 00049 ''' 00050 Comments: 00051 Traps exactly one message from the topic specified. This is 00052 To run 'main', this assumes the launch of "pr2_laser_snapshotter" with mapping to "tilt_laser_cloud" 00053 Basic use is to import and call "subscribe_once" method. 00054 ''' 00055 00056 def init(node_name = NODE_NAME): 00057 rospy.init_node(node_name, anonymous=True) 00058 00059 def myCallback(data): 00060 print 'callback called' 00061 global global_ros_data 00062 global_ros_data = data 00063 00064 ##def listener(): 00065 ## rospy.init_node('my_listener', anonymous=True) 00066 ## rospy.Subscriber("tilt_laser_cloud", PointCloud, cloudCallback) 00067 ## rospy.spin() 00068 00069 def subscribe_once(topic_name=TOPIC_NAME, msg_type=PointCloud): 00070 init() 00071 #if not something, init. 00072 s = rospy.Subscriber(topic_name, msg_type, myCallback) 00073 #spin until 1 message is received. 00074 print 'sleeping now: waitin for topic', topic_name 00075 while (not global_ros_data): 00076 rospy.sleep(.1) 00077 if rospy.is_shutdown(): #catch a CTRL-C 00078 print '\n Forcing exit' 00079 return False; #check for 'false' on output to catch error results. 00080 print 'waking and exiting now' 00081 s.unregister() 00082 global global_ros_data 00083 return global_ros_data #value will be returned as 'global_ros_data' 00084 00085 if __name__ == 'main': 00086 data = subscribe_once(TOPIC_NAME, PointCloud) 00087 00088 #assume global_ros_data is defined. 00089 #do something with the pointcloud 00090 for i in range(10): 00091 pt = data.points[i] 00092 print "(%f, %f, %f)" %(pt.x, pt.y, pt.z) 00093 00094 00095 00096 00097 00098