hrl_pr2_door_opening: emulate_ati

Go to the documentation of this file.
00001 #!/usr/bin/python
00002 
00003 import numpy as np, math
00004 import copy
00005 from threading import RLock
00006 
00007 import roslib; roslib.load_manifest('hrl_pr2_door_opening')
00008 import rospy
00009 
00010 from hrl_msgs.msg import FloatArray
00011 from geometry_msgs.msg import Twist
00012 
00013 def ft_cb(data):
00014     lock.acquire()
00015     ft_val[0] = data.linear.x
00016     ft_val[1] = data.linear.y
00017     ft_val[2] = data.linear.z
00018     ft_val[3] = data.angular.x
00019     ft_val[4] = data.angular.y
00020     ft_val[5] = data.angular.z
00021     lock.release()
00022 
00023 if __name__ == '__main__':
00024     lock = RLock()
00025     ft_val = [0.] * 6
00026     pub = rospy.Subscriber('/r_cart/state/wrench', Twist, ft_cb)
00027     pub = rospy.Publisher('/force_torque_ft2_estimate', FloatArray)
00028     rospy.init_node('ati_ft_emulator')
00029     rospy.loginfo('Started the ATI FT emulator.')
00030     rt = rospy.Rate(100)
00031     while not rospy.is_shutdown():
00032         lock.acquire()
00033         send_ft_val = copy.copy(ft_val)
00034         lock.release()
00035         fa = FloatArray(rospy.Header(stamp=rospy.Time.now()), send_ft_val)
00036         pub.publish(fa)
00037         rt.sleep()
00038 
00039