-
Notifications
You must be signed in to change notification settings - Fork 0
/
position_tracker_node.py
executable file
·57 lines (39 loc) · 1.09 KB
/
position_tracker_node.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
#!/usr/bin/env python
import socket
import rospy
from geometry_msgs.msg import Vector3, Quaternion, Wrench, Pose
import os
import struct
HOST = '192.168.1.5'
PORT = 27015
Fdata = Wrench()
Pdata = Pose()
rospy.init_node('position_tracker_node')
Ppub = rospy.Publisher('ep_pose', Pose, queue_size=10)
rate = rospy.Rate(90)
num_params = 7 # 3 position 4 orientation
size_float = 4 # a float is 4 bytes
total_pack_size = num_params*size_float
print 'initializing ROS node...'
s = socket.socket(socket.AF_INET,socket.SOCK_STREAM)
s.connect((HOST,PORT))
print 'connected!'
while True:
# receive from the server
pack = s.recv(total_pack_size,socket.MSG_WAITALL)
# unpack
px,py,pz,q1,q2,q3,q4 = struct.unpack('<fffffff',pack)
Pdata.position.x = float(px)
Pdata.position.y = float(py)
Pdata.position.z = float(pz)
#Pdata.orientation.x = 0
#Pdata.orientation.y = 0
#Pdata.orientation.z = 0
#Pdata.orientation.w = 0
Pdata.orientation.x = float(q1)
Pdata.orientation.y = float(q2)
Pdata.orientation.z = float(q3)
Pdata.orientation.w = float(q4)
Ppub.publish(Pdata)
rate.sleep()
s.close()