-
Notifications
You must be signed in to change notification settings - Fork 2
/
in_tunnel1.py
executable file
·134 lines (84 loc) · 5.03 KB
/
in_tunnel1.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
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
#!/usr/bin/env python
import rospy
import numpy
from sensor_msgs.msg import LaserScan
from tester.msg import raw_sensor_8_10
from std_msgs.msg import Int8
import copy
########################################################################################################################################
#### Publish the process values to '/cmd_vel' node ###################################################################################
########################################################################################################################################
rospy.init_node('sensor', anonymous=True)
pub = rospy.Publisher('/raw_sensor', raw_sensor_8_10, queue_size = 5)
rate = rospy.Rate(10)
########################################################################################################################################
global data2
global copy_data
global stage
stage=0
raw = raw_sensor_8_10()
data2 = [0,0,0,0,0]
copy_data = [0,0,0,0,0]
count = 0
########################################################################################################################################
#### Accept Lidar values and process the values (main) ###############################################################################
########################################################################################################################################
def checking_stage(ss): ### Function that save the stage value from 'main'
global stage
stage=ss.data
stage=3
def callback(data):
if stage>0:
global data2
global copy_data
SensorData = data.ranges # Get the data from Lidar
Front = SensorData[1:18]+SensorData[342:359]
real_Front = SensorData[1:5] + SensorData[354:359]
Front_Left = SensorData[18:54] # Seperate the data depend of direction of turtlebot
Front_Right = SensorData[306:342]
Left_side = SensorData[85:95]
Right_side = SensorData[265:275]
Front_Left_avg = avg(Front_Left)
Front_Right_avg = avg(Front_Right)
# Each data has least 10 values including error values like coming up zero value
# so if error occured, except error values and make the average of their data
Left_side_avg = avg(Left_side)
Right_side_avg = avg(Right_side)
Front_avg = avg(Front)
real_Front_avg = avg(real_Front)
########################################################################################################################################
#### Save the process values in messege form #########################################################################################
########################################################################################################################################
raw.data = [real_Front_avg, Left_side_avg, Right_side_avg]
raw.sharp = [SensorData[40], SensorData[29], SensorData[20], SensorData[9], SensorData[350], SensorData[339], SensorData[329], SensorData[320]]
raw.front = [avg(SensorData[37:40]), avg(SensorData[33:35]), avg(SensorData[26:29]), avg(SensorData[23:25]), avg(SensorData[16:18]), avg(SensorData[13:15]), avg(SensorData[344:346]), avg(SensorData[338:340]), avg(SensorData[332:335]), avg(SensorData[330:334]), avg(SensorData[329:331]), avg(SensorData[321:324]) ]
########################################################################################################################################
data2 = [Front_Left_avg, Front_avg , Front_Right_avg]
copy_data = copy.copy(data2) # data2 values are relocated by 'data2.sort()' So they have to be copied.
data2.sort(reverse=True) # Relocation values to large values to small values.
find_same(data2[0]) # Give the large value to Function. Large value means far distance from turtlebot
rospy.loginfo(raw.front)
pub.publish(raw)
#### main ends ########################################################################################################################
########################################################################################################################################
#### Functions #######################################################################################################################
########################################################################################################################################
def avg(data):
return sum(data)/(len(data)-data.count(0)+0.01) # It makes several value's average
def find_same(data1):
global data2
global copy_data
for i in range(0, len(copy_data)): # Find the same value from copied array 'data2'
if copy_data[i] == data1:
raw.Flag = i+1 # Publish the symbolic number of the Large value before the 'data2' array is relocated
raw.Far = data1 # Symbolic number means the direction
pub.publish(raw)
def scan_sensor():
rospy.Subscriber('/scan', LaserScan, callback)
rospy.Subscriber('/stage',Int8,checking_stage)
rospy.spin()
if __name__ == '__main__':
try:
scan_sensor()
except rospy.ROSInterruptException:
pass