-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy pathmain_driver.py
162 lines (128 loc) · 4.73 KB
/
main_driver.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
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
#!/usr/bin/python
import rospy
import cv2
import numpy as np
from std_msgs.msg import String
from enum import Enum
from sensor_msgs.msg import LaserScan
from ar_track_alvar_msgs.msg import AlvarMarkers
from cv_bridge import CvBridge, CvBridgeError
class MainDriver:
def __init__(self):
self.states = States(3)
self.state_pub = rospy.Publisher("/state", String, queue_size=2)
self.left_pub = rospy.Publisher("/left_scan", LaserScan, queue_size=2)
self.right_pub = rospy.Publisher("/right_scan", LaserScan, queue_size=2)
rospy.Subscriber("/scan", LaserScan, self.laser_callback)
rospy.Subscriber("/ar_pose_marker", AlvarMarkers, self.ar_callback)
self.ar_data = None
rospy.loginfo("Starting with " + str(self.states))
def laser_callback(self, msg):
self.laser_data = msg
self.process_data()
def ar_callback(self, msg):
self.ar_data = msg
self.process_data()
def process_data(self):
if self.ar_data != None:# and len(self.ar_data.markers) > 0:
if self.states == States.ROLLING_WEAVE: # check if we are on hairpin
if self.loop_ar(0, self.ar_data.markers) and self.ar_valid_distance(0, self.ar_data.markers):
self.states = States(2)
rospy.loginfo("Switching to Hairpin")
elif self.states == States.HAIRPIN: # check if we are on overpass
if self.loop_ar(1, self.ar_data.markers) and self.ar_valid_distance(1, self.ar_data.markers):
self.states = States(3)
rospy.loginfo("Switching to Overpass")
elif self.states == States.OVERPASS: # check if we are on yellow brick
if self.loop_ar(1, self.ar_data.markers) and self.ar_valid_distance(1, self.ar_data.markers):
self.states = States(4)
rospy.loginfo("Switching to Yellow Brick")
elif self.states == States.YELLOW_BRICK: # check if we are underpass
if self.loop_ar(3, self.ar_data.markers) and self.ar_valid_distance(3, self.ar_data.markers):
self.states = States(5)
rospy.loginfo("Switching to Underpass")
elif self.states == States.UNDERPASS: # check if we are on boa basin
if self.loop_ar(2, self.ar_data.markers) and self.ar_valid_distance(2, self.ar_data.markers):
self.states = States(6)
rospy.loginfo("Switching to Boa Basin")
elif self.states == States.BOA_BASIN: # check if we are on mesh wall
if self.loop_ar(20, self.ar_data.markers) and self.ar_valid_distance(20, self.ar_data.markers):
self.states = States(7)
rospy.loginfo("Switching to First Mesh")
elif self.states == States.MESH_WALL_FIRST: # check if on second mesh part
# get indices for 90 degrees right and 90 degrees left
right_index, left_index = self.get_index(180, self.laser_data)
middle = int((right_index + left_index) / 2)
# get the ranges for the left and right side
scan_left = self.laser_data.ranges[middle : left_index]
scan_right = self.laser_data.ranges[right_index : middle]
#print right_index, middle, left_index
# find the average of those ranges
left_avg = np.mean(self.laser_data.ranges[780:908])
right_avg = np.mean(self.laser_data.ranges[172:300])
#print "left_avg: ", left_avg
#print "right_avg: ", right_avg
if right_avg > left_avg: # greater than
self.states = States(8)
print right_avg
print left_avg
rospy.loginfo("Switching to Second Mesh")
self.send_state()
def send_state(self):
state_string = String()
if self.states == States(1):
state_string.data = "WHS"
elif self.states == States(2):
state_string.data = "HPS"
elif self.states == States(3):
state_string.data = "OPS"
elif self.states == States(4):
state_string.data = "YBWS"
elif self.states == States(5):
state_string.data = "UPS"
elif self.states == States(6):
state_string.data = "BBS"
elif self.states == States(7):
state_string.data = "MWS"
elif self.states == States(8):
state_string.data = "MWS2"
self.state_pub.publish(state_string)
def loop_ar(self, id, markers):
for tag in markers:
if tag.id == id:
return True
return False
def ar_valid_distance(self, id, markers):
for tag in markers:
if tag.id == id:
if float(tag.pose.pose.position.z) < 1.8:
return True
return False
def get_index(self, angle, scan):
angle_radian = angle * (np.pi/180)
start = scan.angle_min
end = scan.angle_max
radian_per_index = scan.angle_increment
offsets = angle_radian / 2
index_offset = int(offsets / radian_per_index)
return 540 - index_offset, 540 + index_offset
'''
def get_avg(self, ranges):
sum = 0
for x in ranges:
sum += x
return int(sum / len(ranges))
'''
class States(Enum):
ROLLING_WEAVE = 1
HAIRPIN = 2
OVERPASS = 3
UNDERPASS = 4
YELLOW_BRICK = 5
BOA_BASIN = 6
MESH_WALL_FIRST = 7
MESH_WALL_SECOND = 8
if __name__ == "__main__":
rospy.init_node("state_machine")
node = MainDriver()
rospy.spin()