00001
00002
00003 """
00004 connect to Oculus Prime Server Application
00005 poll server for odometry data
00006 broadcast tranform between base_link and odom frames
00007 """
00008
00009 from math import radians, sin, cos
00010 import rospy, tf
00011 from nav_msgs.msg import Odometry
00012 import oculusprimesocket
00013
00014
00015
00016
00017 pos = [0.0, 0.0, 0.0]
00018 before = 0
00019 now = 0
00020 default_lag = 0.035
00021
00022
00023 def broadcast(s):
00024 global before, pos, now
00025 now = rospy.Time.now() - rospy.Duration(rospy.get_param('~odom_lag', default_lag))
00026
00027 dt = (now-before).to_sec()
00028 before = now
00029
00030 distance = float(s[2])/1000
00031 delta_x = distance * cos(pos[2])
00032 delta_y = distance * sin(pos[2])
00033 delta_th = radians(float(s[3]))
00034 pos[0] += delta_x
00035 pos[1] += delta_y
00036 pos[2] += delta_th
00037
00038
00039 odom_quat = tf.transformations.quaternion_from_euler(0, 0, pos[2])
00040 br.sendTransform((pos[0], pos[1], 0), odom_quat, now, "base_link","odom")
00041
00042
00043
00044
00045
00046
00047 odom = Odometry()
00048 odom.header.stamp = now
00049 odom.header.frame_id = "odom"
00050
00051
00052 odom.pose.pose.position.x = pos[0]
00053 odom.pose.pose.position.y = pos[1]
00054 odom.pose.pose.position.z = 0.0
00055 odom.pose.pose.orientation.x = odom_quat[0]
00056 odom.pose.pose.orientation.y = odom_quat[1]
00057 odom.pose.pose.orientation.z = odom_quat[2]
00058 odom.pose.pose.orientation.w = odom_quat[3]
00059 odom.pose.covariance= [0.000001, 0, 0, 0, 0, 0,
00060 0, 0.000001, 0, 0, 0, 0,
00061 0, 0, 0.000001, 0, 0, 0,
00062 0, 0, 0, 0.000001, 0, 0,
00063 0, 0, 0, 0, 0.000001, 0,
00064 0, 0, 0, 0, 0, 0.000001]
00065
00066
00067 odom.child_frame_id = "base_link"
00068 odom.twist.twist.linear.x = distance / dt
00069 odom.twist.twist.linear.y = 0
00070 odom.twist.twist.linear.z = 0
00071 odom.twist.twist.angular.x = 0
00072 odom.twist.twist.angular.y = 0
00073 odom.twist.twist.angular.z = delta_th / dt
00074
00075
00076 odom_pub.publish(odom)
00077
00078 def cleanup():
00079 oculusprimesocket.sendString("state delete odometrybroadcast")
00080 oculusprimesocket.sendString("odometrystop")
00081
00082 oculusprimesocket.sendString("log odom_tf.py disconnecting")
00083
00084
00085
00086
00087
00088 rospy.init_node('odom_tf', anonymous=False)
00089 before = rospy.Time.now()
00090 br = tf.TransformBroadcaster()
00091 odom_pub = rospy.Publisher('odom', Odometry, queue_size=10)
00092 rospy.on_shutdown(cleanup)
00093 oculusprimesocket.connect()
00094 oculusprimesocket.sendString("log odom_tf.py connected")
00095 oculusprimesocket.sendString("state odometrybroadcast 250")
00096 oculusprimesocket.sendString("odometrystart")
00097 broadcast("* * 0 0".split())
00098
00099 while not rospy.is_shutdown():
00100
00101
00102
00103
00104
00105 s = oculusprimesocket.replyBufferSearch("<state> distanceangle ")
00106 if not s=="":
00107 broadcast(s.split())
00108
00109
00110 rospy.sleep(0.005)