00001 # Copyright (c) 2009, Georgia Tech Research Corporation 00002 # All rights reserved. 00003 # 00004 # Redistribution and use in source and binary forms, with or without 00005 # modification, are permitted provided that the following conditions are met: 00006 # * Redistributions of source code must retain the above copyright 00007 # notice, this list of conditions and the following disclaimer. 00008 # * Redistributions in binary form must reproduce the above copyright 00009 # notice, this list of conditions and the following disclaimer in the 00010 # documentation and/or other materials provided with the distribution. 00011 # * Neither the name of the Georgia Tech Research Corporation nor the 00012 # names of its contributors may be used to endorse or promote products 00013 # derived from this software without specific prior written permission. 00014 # 00015 # THIS SOFTWARE IS PROVIDED BY GEORGIA TECH RESEARCH CORPORATION ''AS IS'' AND 00016 # ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 00017 # WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 00018 # DISCLAIMED. IN NO EVENT SHALL GEORGIA TECH BE LIABLE FOR ANY DIRECT, INDIRECT, 00019 # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT 00020 # LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, 00021 # OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 00022 # LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE 00023 # OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF 00024 # ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 00025 # 00026 ## author Advait Jain (Healthcare Robotics Lab, Georgia Tech.) 00027 00028 import roslib; roslib.load_manifest('zenither') 00029 import rospy 00030 from hrl_msgs.msg import FloatArray 00031 from threading import RLock 00032 00033 class ZenitherClient(): 00034 def __init__(self, init_ros_node = False, 00035 zenither_pose_topic = 'zenither_pose'): 00036 if init_ros_node: 00037 rospy.init_node('ZenitherClient') 00038 self.h = None 00039 self.lock = RLock() 00040 rospy.Subscriber(zenither_pose_topic, FloatArray, self.pose_cb) 00041 00042 def pose_cb(self, fa): 00043 self.lock.acquire() 00044 self.h = fa.data[0] 00045 self.lock.release() 00046 00047 def height(self): 00048 self.lock.acquire() 00049 h = self.h 00050 self.lock.release() 00051 return h 00052 00053 if __name__ == '__main__': 00054 zc = ZenitherClient(init_ros_node = True) 00055 while not rospy.is_shutdown(): 00056 print 'h:', zc.height() 00057 rospy.sleep(0.1) 00058 00059