def execute(self, userdata):
global global_inx
for i in range(3):
turn_turtlebot(-math.pi / 2)
drive_for_dist(-0.3, 0, 0.2)
wait(1)
asus_copy = np.copy(asus_hsv)
asus_copy[:asus_copy.shape[0] / 2, :] = 0
shapes = shape_detect.detect(asus_copy, "red", 4000)
try:
if g_green_shape[0] in shapes:
print('FOUND')
path = os.path.dirname(os.path.realpath(__file__))
sound_client.playWave(path + '/sounds/' +
'checkpoint-hit.wav')
# g_green_shape = []
except:
pass
drive_for_dist(0.3, 0, 0.2)
turn_turtlebot(math.pi / 2 + 0.1)
if i != 2:
drive_for_dist(0.3, 0, 0.4)
print("LOCATION3 SHAPES", shapes)
global_inx += 2
turn_turtlebot(-0.3)
return 'done'
def execute(self, userdata):
global target, global_inx, g_green_shape
turn_turtlebot(-math.pi / 10)
stop_time = rospy.get_time() + 2
while (rospy.get_time() < stop_time):
pass
green_shapes = shape_detect.detect(asus_hsv, "green", 1000)
g_green_shape = green_shapes
red_shapes = shape_detect.detect(asus_hsv, "red", 1000)
num = min(3, len(green_shapes) + len(red_shapes))
show_count(num)
print("green", green_shapes)
print("red", red_shapes)
print("NUMBER OF OBJECTS DETECTED AT LOCATION2: ", num)
global_inx -= 1
target = math.pi #spin around at location2
return 'done'
def execute(self, userdata):
global target, global_inx, g_green_shape
turn_turtlebot(-math.pi / 10) # correct angle a bit
# wait for turtlebot to stop
stop_time = rospy.get_time() + 0.5
while (rospy.get_time() < stop_time):
pass
# detect shapes
green_shapes = shape_detect.detect(hsv, "green", 1000)
g_green_shape = green_shapes
red_shapes = shape_detect.detect(hsv, "red", 1000)
num = min(3, len(green_shapes) + len(red_shapes)) # HACK
show_count(num) # display on lights and sound
print("green", green_shapes)
print("red", red_shapes)
print("NUMBER OF OBJECTS DETECTED AT LOCATION2: ", num)
global_inx -= 1
target = math.pi #spin around at location2
return 'done'
def execute(self, userdata):
cv2.imshow("window", image)
global target, global_inx
drive_turtlebot(0, 0)
target = math.pi / 2 # turn right away from location1
# wait for turtlebot to stop
stop_time = rospy.get_time() + 1
while (rospy.get_time() < stop_time):
pass
rospy.wait_for_message('/scan', LaserScan)
# Scan objects with shape detector
num_of_objects = len(shape_detect.detect(hsv, "red", 1000))
if num_of_objects == 4: # HACK
num_of_objects = 3
#num_of_objects = len(get_objects(obj_ranges))
show_count(num_of_objects) # display on lights and sound
print("NUMBER OF OBJECTS DETECTED AT LOCATION1: ", num_of_objects)
global_inx += 1
return 'done'
def detect_single_shape():
drive_for_dist(0.3, 0, 0.4)
asus_copy = np.copy(asus_hsv)
asus_copy[asus_copy.shape[0] / 2:, :] = 0
shape = shape_detect.detect(asus_copy, "red", 1000)
print('here')
print(shape)
if shape:
print(shape[0], g_green_shape[0])
if shape[0] == g_green_shape[0]:
drive_for_dist(0.3, 0, 0.16)
path = os.path.dirname(os.path.realpath(__file__))
sound_client.playWave(path + '/sounds/' + 'checkpoint-hit.wav')
led1_pub.publish(Led.BLACK)
led2_pub.publish(Led.BLACK)
led1_pub.publish(Led.ORANGE)
led2_pub.publish(Led.GREEN)
wait(2)
drive_for_dist(-0.3, 0, 0.16)
print('done')
drive_for_dist(-0.3, 0, 0.4)
def execute(self, userdata):
global target, global_inx, g_green_shape
target = math.pi / 2 # rotations back from location 3
turn_turtlebot(-math.pi / 10 + 0.3)
# back up a bit so we don't miss the next line
stop_time = rospy.get_time() + 0.8
while (rospy.get_time() < stop_time):
drive_turtlebot(-0.2, 0)
stop_time = rospy.get_time() + 1.5
while (rospy.get_time() < stop_time):
pass
shapes = shape_detect.detect(asus_hsv, "red", 4000)
try:
if g_green_shape[0] in shapes:
path = os.path.dirname(os.path.realpath(__file__))
#g_mpv = subprocess.Popen(['aplay', path+'/'+'checkpoint-hit.wav'])
sound_client.playWave(path + '/sounds/' + 'checkpoint-hit.wav')
g_green_shape = []
except:
pass
print("LOCATION3 SHAPES", shapes)
stop_time = rospy.get_time() + 0.8
while (rospy.get_time() < stop_time):
drive_turtlebot(0.2, 0)
turn_turtlebot(math.pi / 2)
stop_time = rospy.get_time() + 0.5
while (rospy.get_time() < stop_time):
drive_turtlebot(-0.2, 0)
# stop_time = rospy.get_time() + 0.7
# while(rospy.get_time() < stop_time):
# drive_turtlebot(-turtlebot_speed,0)
global_inx += 2
return 'done'
def execute(self, userdata):
global target, global_inx
target = math.pi / 2 # rotations back from location 3
turn_turtlebot(-math.pi / 10) # fix angle
# wait for turtlebot to stop
stop_time = rospy.get_time() + 0.5
while (rospy.get_time() < stop_time):
pass
# detect shape
shapes = shape_detect.detect(hsv, "red", 7000)
print("LOCATION3 SHAPES", shapes)
if g_green_shape[0] in shapes: # match with green shape from location2
path = os.path.dirname(os.path.realpath(__file__))
g_mpv = subprocess.Popen(
['aplay', path + '/' + 'checkpoint-hit.wav'])
# back up a bit so we don't miss the next line
turn_turtlebot(math.pi / 2)
stop_time = rospy.get_time() + 0.7
while (rospy.get_time() < stop_time):
drive_turtlebot(-0.5, 0)
global_inx += 2
return 'done'
def scan_front(idx):
led1_pub.publish(Led.BLACK)
led2_pub.publish(Led.BLACK)
# back up
stop_time = rospy.get_time() + 0.9
while (rospy.get_time() < stop_time):
drive_turtlebot(-0.2, 0)
# wait
stop_time = rospy.get_time() + 2
while (rospy.get_time() < stop_time):
pass
# respresents what signal to send
result = 0
# scan
red_shapes = shape_detect.detect(asus_hsv, "red", 1000)
if markers:
print("MARKER DETECTED: " + str(markers[0]))
result = 3
elif red_shapes:
print("RED SHAPE DETECTED: " + str(red_shapes[0]))
result = 1
try:
if red_shapes[0] == g_green_shape[0]:
result = 2
except:
pass
elif idx == [7, 6, 5, 4, 3, 9, 8, 2][5]:
result = 4
else:
print("EMPTY PARKING SPOT")
# move forward
stop_time = rospy.get_time() + 0.9
while (rospy.get_time() < stop_time):
drive_turtlebot(0.2, 0)
# send out result message with led or soundplay
path = os.path.dirname(os.path.realpath(__file__))
if result == 4:
# empty
led1_pub.publish(Led.RED)
led2_pub.publish(Led.RED)
sound_client.playWave(path + '/sounds/' + 'cow1.wav')
elif result == 1:
# shape
led1_pub.publish(Led.ORANGE)
led2_pub.publish(Led.ORANGE)
sound_client.playWave(path + '/sounds/' + 'cat_meow_x.wav')
elif result == 2:
# matched with green shape
led1_pub.publish(Led.ORANGE)
led2_pub.publish(Led.GREEN)
sound_client.playWave(path + '/sounds/' + 'checkpoint-hit.wav')
elif result == 3:
# ar tag
led1_pub.publish(Led.GREEN)
led2_pub.publish(Led.GREEN)
sound_client.playWave(path + '/sounds/' + 'chicken.wav')
# wait
stop_time = rospy.get_time() + 3
while (rospy.get_time() < stop_time):
pass
stop_time = rospy.get_time() + 1.2
while (rospy.get_time() < stop_time):
drive_turtlebot(-0.2, 0)