forked from Netrux/Indoor_Position_Control
-
Notifications
You must be signed in to change notification settings - Fork 0
/
subscriber_to_nparray.py
95 lines (84 loc) · 2.06 KB
/
subscriber_to_nparray.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
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
import rospy
import numpy as np
from sensor_msgs.msg import LaserScan
from geometry_msgs.msg import PoseStamped
import math as m
global vertex
#vertex=0
incremet = 0.00435422640294
rospy.init_node("scanner")
rospy.Rate(10)
f = open("slam1.ply","w")
ply_header = '''ply
format ascii 1.0
element vertex
property float x
property float y
property float z
end_header
'''
f.write(ply_header)
comp = np.ones([1,1440])*30
def callback_imu(msg):
global x,y,z
#print("hello",msg.pose.position)
x = msg.pose.position.x
y = msg.pose.position.y
z = msg.pose.position.z
qx = msg.pose.orientation.x
qy = msg.pose.orientation.y
qz = msg.pose.orientation.z
qw = msg.pose.orientation.w
#print("callback",x,y,z,qx)
def callback_scan(msg):
#print("callback")
k=np.array(msg.ranges)
print("scan",k.shape)
#print(type(k[27]))
# if k[27]>25:
# print("infinity")
#np.savetxt("a.txt",k[27:29])
# for i in range(0,1440,1):
# r = k[i]
# #print(i,r)
# ang = i*incremet
# if r<30:
# #print(1,ang,m.sin(ang),r)
# print(i,r)
# fx = r*m.sin(ang)# + x
# fy = r*m.cos(ang)# + y
# #z = z
# f.write(str(fx)+" " + str(y)+" "+str(z)+"\n")
for i in range(0,1440,1):
r = k[i]
ang = i*incremet
if r <30:
#print(i,ang,m.sin(ang),r)
#vertex+=1
f.write(str(r*m.sin(ang)+x)+" " + str(r*m.cos(ang)+y )+" "+str(z)+"\n")
else:
continue
def callback_vect(msg):
#print("callback")
print("scan")
k=np.array(msg.ranges)
k = k*(k<comp)*1
k = k.reshape(1440,1)
print(k.shape)
ang = np.ones(k.shape)
print(ang.shape)
for i in range(0,1440,1):
#r = k[i]
ang[i,0] = i*incremet
fx = k*(np.sin(ang))
fy = k*(np.cos(ang))
fz = np.ones(k.shape)*z
final = np.array([fx,fy,fz])
f.write(str(final))
rospy.Subscriber("/mavros/local_position/pose",PoseStamped,callback_imu,queue_size=10)
rospy.Subscriber("/scan",LaserScan,callback_scan,queue_size=10)
print("hello")
while (not rospy.is_shutdown()) and KeyboardInterrupt:
rospy.spin()
f.close()
print("closed")#,vertex)