sim_sensor.py
Go to the documentation of this file.
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()


fingertip_pressure
Author(s): Blaise Gassend/blaise@willowgarage.com
autogenerated on Thu Jan 2 2014 11:39:26