def execute(self, userdata):
global shutdown_requested
self.client.send_goal(self.target)
self.client.wait_for_result()
print("Goal reached")
try:
shape = v2.shapeDetection('red', 1)
print("red shape detected:" + shape)
except Exception as e:
print(e)
traceback.print_exc()
shape = "triangle"
if shape == event_two.previous_shape:
self.led1_pub.publish(Led.GREEN)
self.sound_pub.publish(1)
time.sleep(1)
self.led1_pub.publish(0)
time.sleep(1)
self.led1_pub.publish(Led.GREEN)
self.led2_pub.publish(Led.ORANGE)
self.sound_pub.publish(1)
time.sleep(1)
self.led1_pub.publish(0)
self.led2_pub.publish(0)
if shutdown_requested:
return 'done4'
else:
return 'find_markers'
def execute(self, userdata):
global shutdown_requested
global checked
global previous_shape
while not shutdown_requested:
if self.callbacks.line_white_mask is not None and self.callbacks.red_mask is not None:
symbol_red_mask = self.callbacks.symbol_red_mask.copy()
symbol_green_mask = self.callbacks.symbol_green_mask.copy()
h = self.callbacks.main_h
w = self.callbacks.main_w
symbol_red_mask[0:h / 4, 0:w] = 0
symbol_red_mask[3 * h / 4:h, 0:w] = 0
symbol_green_mask[0:h / 4, 0:w] = 0
symbol_green_mask[3 * h / 4:h, 0:w] = 0
count = v2.count_objects(symbol_green_mask)
count += v2.count_objects(symbol_red_mask)
for i in range(int(count)):
self.sound_pub.publish(1)
time.sleep(1)
checked = True
#previous_shape = detect_shape.detect_shape(symbol_green_mask, h, w)
try:
previous_shape = v2.shapeDetection('green', 1)
print("green shape detected:" + previous_shape)
except Exception as e:
print(e)
traceback.print_exc()
return 'rotate_180'
return 'done1'
def execute(self, userdata):
global shutdown_requested
global number_of_checks
while not shutdown_requested:
if self.callbacks.line_white_mask is not None and self.callbacks.red_mask is not None:
number_of_checks += 1
h = self.callbacks.main_h
w = self.callbacks.main_w
symbol_red_mask = self.callbacks.symbol_red_mask.copy()
symbol_red_mask[0:h / 2, 0:w] = 0
try:
shape = v2.shapeDetection('red', 2)
print("red shape detected:" + shape)
except Exception as e:
print(e)
traceback.print_exc()
if shape is None:
time.sleep(1)
return "rotate_right"
if shape == event_two.previous_shape:
self.sound_pub.publish(1)
time.sleep(1)
return "rotate_right"
return 'done3'