#!/usr/bin/env python2

from __future__ import print_function

import numpy as np

import cv2

import requests

import numpy as np

import time, json

import threading

import struct, socket

# Functions for controlling the robot.

#

# A phone connected to the robot has a running webcam,

# and the livestream is processed to determine what

# kind of environment the robot is in. Using computer vision,

# the robot can autonomously follow a line, solve the labyrinth,

# and detect and cross brigdes.

# It can also determine that it's in a forest, but solving the

# forest autonomously is still TODO.

## Commands for robot control via UDP

CMD_MOVE = 1

CMD_ROTATE = 2

CMD_STOP = 3

CMD_JOG = 4

CMD_CRANE = 5

ROBOT_ADDRESS = '192.168.43.123'

ROBOT_PORT = 8123

#

# Communication with robot

#

# create UDP socket

sock = socket.socket( socket.AF_INET, socket.SOCK_DGRAM )

# function to pack and pad a control message for robot

def pack( cmd, flags, speed, value ):

return struct.pack( ">BBhh", cmd, flags, int(speed), int(value) ) + b" "*10

# function to rotate robot

def rotate_robot( angle ):

print( "Posting to robot")

msg = pack( CMD_ROTATE, 0x00, 300, angle )

sock.sendto( msg, (ROBOT_ADDRESS, ROBOT_PORT ) )

# function to move the robot

def move_robot( dist ):

print( "Posting to robot")

msg = pack( CMD_MOVE, 0x00, 450, dist )

sock.sendto( msg, (ROBOT_ADDRESS, ROBOT_PORT ) )

#

# Image processing and robot control

#

# compute image entropy

def compute_entropy( img ):

return entropy

# Generate and test hypotheses whether image contains lines

def generate_line_hypotheses( img ):

return {"no": hypothesis_no_track, "all": hypothesis_all_track, "some": hypothesis_some_track, "centroid": (cx,cy), "side": left_total < right_total}

# find image centroid (center of "mass" )

def find_centroid( img ):

return ret, (cx, cy)

# Thread class for receiving and parsing the MJPEG stream from the phone

class StreamThread( threading.Thread ):

print( "Missing frame..." )

# Negative feedback control for servoing the robot towards target

def servo_towards_centroid( mask, cx ):

time.sleep(0.55 )

def compute_sparsest_area( img ):

return Xflt2 / (Aflt2 + 1)

stream = StreamThread()

stream.start()

ROBOT_STATE = "IDLE"

STATE_HISTORY = []

bridge_cnt = 5

prev_cnt = 0

t_last_servo = time.time()

while True:

# Process image from stream. Rotate it correctly (depends on how

# camera is positioned), and convert it to HSV for easier detection

# of color changes. Based on colors and their distribution in the

# image, detect what type of area we are in and control robot

# accordingly.

try:

if prev_cnt == stream.cnt:

continue

prev_cnt = stream.cnt

t0 = time.time()

img = cv2.imdecode( np.fromstring( stream.output, dtype = np.uint8 ), cv2.IMREAD_COLOR )

bw = cv2.cvtColor( img, cv2.COLOR_BGR2GRAY )

rows, cols = bw.shape

M = cv2.getRotationMatrix2D( (cols/2, rows/2), -90, 1 )

bw = cv2.warpAffine( bw, M, (cols, rows) )

bw = cv2.flip( bw, 1 )

hsv = cv2.cvtColor(img, cv2.COLOR_BGR2HSV)

hsv = cv2.warpAffine( hsv, M, (cols, rows) )

true_color = cv2.warpAffine( img, M, (cols, rows) )

cv2.imshow( "True", true_color )

cv2.imshow( "HSV", hsv )

if ROBOT_STATE != "IDLE":

print( ROBOT_STATE )

hsv = hsv[int(hsv.shape[0]*0.3):, :]

found_lines = False

# Lines on the floor

if True:

limit1 = np.array([0, 0, 220])

limit2 = np.array([180, 15, 255])

mask = cv2.inRange( hsv, limit1, limit2 )

ret, centroid = find_centroid( mask )

if ret:

cx, cy = centroid

cv2.circle(mask, (cy,cx), 10, (255,0,0), 1)

left = mask[:, :int(1*mask.shape[1]/3)].sum()

centre = mask[:, int(1*mask.shape[1]/3):int(2*mask.shape[1]/3)].sum()

right = mask[:, int(2*mask.shape[1]/3):].sum()

q_left = left*1.0/centre

q_right = right*1.0/centre

if False:

if q_left > 0.4:

# turn left

print( "TURN LEFT!" )

pass

elif q_right > 0.4:

#turn right

print( "TURN RIGHT!" )

pass

viivat_lattiassa_mask = mask.copy()

found_lines = True

bridge_cnt = 5

mask = cv2.cvtColor( mask, cv2.COLOR_GRAY2BGR )

cv2.imshow( "Mask, viiva lattiassa", mask )

# Labyrinth button

if True:

limit1 = np.array([25, 220, 220])

limit2 = np.array([35, 255, 255])

mask = cv2.inRange( hsv, limit1, limit2 )

ret, centroid = find_centroid( mask )

mask = cv2.cvtColor( mask, cv2.COLOR_GRAY2BGR )

if ret:

if ROBOT_STATE == "BRIDGE":

ROBOT_STATE = "LABYRINTH_BUTTON"

move_robot(300)

time.sleep(0.25)

cx, cy = centroid

cv2.circle(mask, (cy,cx), 10, (255,0,0), 1)

if ROBOT_STATE == "LABYRINTH_BUTTON":

servo_towards_centroid( mask, cy )

else:

if ROBOT_STATE == "LABYRINTH_BUTTON":

STATE_HISTORY.append( ROBOT_STATE )

time.sleep(1.0)

rotate_robot(-80)

time.sleep(0.55)

move_robot(800)

time.sleep(0.55)

print( "Changing to BRIDGE")

ROBOT_STATE = "BRIDGE"

cv2.imshow( "Mask, labyrinth button", mask )

# Bridge

if True:

limit1 = np.array([0, 50, 150])

limit2 = np.array([35, 255, 255])

mask = cv2.inRange( hsv, limit1, limit2 )

if found_lines:

print( mask.shape, viivat_lattiassa_mask.shape )

mask = cv2.add( mask, viivat_lattiassa_mask )

mask = mask[:mask.shape[0], :]

ret, centroid = find_centroid( mask )

mask = cv2.cvtColor( mask, cv2.COLOR_GRAY2BGR )

if ret:

cx, cy = centroid

cv2.circle(mask, (cy,cx), 10, (255,0,0), 1)

if ROBOT_STATE == "BRIDGE":

servo_towards_centroid( mask, cy )

else:

if ROBOT_STATE == "BRIDGE":

STATE_HISTORY.append( ROBOT_STATE )

bridge_cnt -= 1

if bridge_cnt < 1:

ROBOT_STATE = "IDLE"

cv2.imshow( "Mask, bridge", mask )

# "Forest" of metal pillars

if True:

limit1 = np.array([0, 0, 0])

limit2 = np.array([180, 50, 80])

mask = cv2.inRange( hsv, limit1, limit2 )

ret, centroid = find_centroid( mask )

mask = cv2.cvtColor( mask, cv2.COLOR_GRAY2BGR )

if ret:

cx, cy = centroid

cv2.circle(mask, (cy,cx), 10, (255,0,0), 1)

h,s,v = cv2.split( hsv )

img2 = 255 - v

img2 = cv2.resize( img2, None, fx = 2.0, fy = 2.0 )

cv2.imshow( "Mask, pillars, v2", img2 )

if ROBOT_STATE == "FOREST":

# TODO complete this

sparsest = compute_sparsest_area( img2 )

print("sparsest", sparsest)

#servo_towards_centroid( mask, int(sparsest) )

#time.sleep(0.5)

#move_robot(200)

#time.sleep(0.5)

except IOError:

# If frames are missing (not processed quick enough etc),

# just move to the next one

print( "Missing frame.." )

key = cv2.waitKey(25)

# Exit on Esc

if key & 0xff == 27:

exit(0)

if key & 0xff == ord( 'a' ):

ROBOT_STATE = "BRIDGE"

if key & 0xff == ord(' '):

ROBOT_STATE = "IDLE"