00001 #! /usr/bin/python 00002 00003 #*********************************************************** 00004 #* Software License Agreement (BSD License) 00005 #* 00006 #* Copyright (c) 2009, Willow Garage, Inc. 00007 #* All rights reserved. 00008 #* 00009 #* Redistribution and use in source and binary forms, with or without 00010 #* modification, are permitted provided that the following conditions 00011 #* are met: 00012 #* 00013 #* * Redistributions of source code must retain the above copyright 00014 #* notice, this list of conditions and the following disclaimer. 00015 #* * Redistributions in binary form must reproduce the above 00016 #* copyright notice, this list of conditions and the following 00017 #* disclaimer in the documentation and/or other materials provided 00018 #* with the distribution. 00019 #* * Neither the name of the Willow Garage nor the names of its 00020 #* contributors may be used to endorse or promote products derived 00021 #* from this software without specific prior written permission. 00022 #* 00023 #* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 00024 #* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 00025 #* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 00026 #* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 00027 #* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 00028 #* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 00029 #* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 00030 #* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 00031 #* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 00032 #* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 00033 #* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00034 #* POSSIBILITY OF SUCH DAMAGE. 00035 #*********************************************************** 00036 00037 # Author: Blaise Gassend 00038 00039 # Simulates the presence of a fingertip sensor 00040 00041 import roslib 00042 roslib.load_manifest('fingertip_pressure') 00043 import rospy 00044 from math import sin, cos 00045 import threading 00046 00047 from pr2_msgs.msg import PressureState 00048 00049 class pressureSimulator: 00050 def callback(self, pressurestate): 00051 #print "callback" 00052 self.l_finger_tip = pressurestate.l_finger_tip 00053 self.r_finger_tip = pressurestate.r_finger_tip 00054 self.datatimestamp = pressurestate.header.stamp 00055 self.dataready = True 00056 00057 def publish(self): 00058 ps = PressureState() 00059 ps.header.stamp = rospy.get_rostime(); 00060 ps.l_finger_tip = [] 00061 ps.r_finger_tip = [] 00062 t = rospy.get_time() 00063 for i in range(0,22): 00064 ph = .1 * t * (i / 22. + 1) 00065 ps.l_finger_tip.append(4000*(1+sin(ph))) 00066 ps.r_finger_tip.append(4000*(1+cos(ph))) 00067 self.pub.publish(ps) 00068 00069 def __init__(self, dest): 00070 rospy.init_node('sim_sensor', anonymous=True) 00071 rospy.sleep(.2) 00072 00073 self.pub = rospy.Publisher(dest, PressureState) 00074 00075 00076 if __name__ == '__main__': 00077 #@todo it would be nice to read an xml configuration file to get these parameters. 00078 s1 = pressureSimulator('pressure/r_gripper_motor') 00079 s2 = pressureSimulator('pressure/l_gripper_motor') 00080 00081 while not rospy.is_shutdown(): 00082 rospy.sleep(0.1) 00083 s1.publish() 00084 s2.publish()