Solution code for Follow Line exercise.

MyAlgorithm.py

#!/usr/bin/python
#-*- coding: utf-8 -*-

import numpy as np
import threading
import time
import cv2
from datetime import datetime
error=0
integral = 0
time_cycle = 80
class MyAlgorithm(threading.Thread):

    def __init__(self, camera, motors):
        self.camera = camera
        self.motors = motors
        self.image=None
        self.stop_event = threading.Event()
        self.kill_event = threading.Event()
        self.lock = threading.Lock()
        threading.Thread.__init__(self, args=self.stop_event)

    def setImageFiltered(self, image):
        self.lock.acquire()
        self.image=image
        self.lock.release()

    def getImageFiltered(self):
        self.lock.acquire()
        tempImage=self.image
        self.lock.release()
        return tempImage

    def run (self):

        while (not self.kill_event.is_set()):

            start_time = datetime.now()

            if not self.stop_event.is_set():
                self.execute()

            finish_Time = datetime.now()

            dt = finish_Time - start_time
            ms = (dt.days * 24 * 60 * 60 + dt.seconds) * 1000 + dt.microseconds / 1000.0
            #print (ms)
            if (ms < time_cycle):
                time.sleep((time_cycle - ms) / 1000.0)

    def stop (self):
        self.motors.sendV(0)
        self.motors.sendW(0)
        self.stop_event.set()

    def play (self):
        if self.is_alive():
            self.stop_event.clear()
        else:
            self.start()

    def kill (self):
        self.kill_event.set()


    def execute(self):
        global error
        global integral
        red_upper=(6,255,138)
        red_lower=(0,56,78)
        kernel = np.ones((8,8), np.uint8)
        image = self.camera.getImage().data
        image_cropped=image[250:,:,:]
        image_blur = cv2.GaussianBlur(image_cropped, (27,27), 0)
        image_hsv = cv2.cvtColor(image_blur, cv2.COLOR_RGB2HSV)
        image_mask = cv2.inRange(image_hsv, red_lower,red_upper)
        image_mask = cv2.bitwise_and(image_hsv,image_hsv , mask=image_mask)
        image_eroded = cv2.erode(image_mask, kernel, iterations = 3)

        image_gray = cv2.cvtColor(cv2.cvtColor(image_mask , cv2.COLOR_HSV2BGR) , cv2.COLOR_BGR2GRAY)
        v = np.median(image)
        sigma=0.33
        lower = int(max(0, (1.0 - sigma) * v))
        upper = int(min(255, (1.0 + sigma) * v))
        image_edges = cv2.Canny(image_gray, lower , upper)
        lines = cv2.HoughLines(image_edges, 1, np.pi / 180*2, 60)

        threshold_angle=75
        edge_lines=[]
        try:
            for (rho, theta) in lines[0]:
                if (rho > 0 and theta < np.pi/180*threshold_angle) or ( rho < 0 and theta > np.pi/180*(180 - threshold_angle)):
                    a = np.cos(theta)
                    b = np.sin(theta)
                    x0 = a * rho
                    y0 = b * rho
                    x1 = int(x0 + 500 * (-b))
                    y1 = int(y0 + 500 * (a))
                    x2 = int(x0 - 500 * (-b))
                    y2 = int(y0 - 500 * (a))
                    edge_lines.append(tuple([rho,theta]))
                    cv2.line(image_mask, (x1, y1), (x2, y2), (0, 0, 255), 2)
            different_theta_list=[]
            for (rho, theta) in edge_lines:
                if not different_theta_list :
                    different_theta_list.append(tuple([rho , theta]))
                else:
                    count=0
                    for (rho_check , theta_check) in different_theta_list :
                        if abs (theta - theta_check) < ((np.pi/180) *5):
                            break
                        else:
                            count= count+1
                    if count>=len(different_theta_list):
                        different_theta_list.append(tuple([rho , theta]))
            
            for i in range(0, len(different_theta_list)):
                (rho ,theta) =different_theta_list[i]
                degree = ((180 * theta) /np.pi)
                different_theta_list[i]= (rho , degree)
            #print("different_theta_list",different_theta_list)
            #print("--------------------------------")
            if len(different_theta_list) > 1:
                for i in range (0, len(different_theta_list)):
                    if different_theta_list[i][0] > 0 :
                        (rho , theta) = different_theta_list[i]
                        different_theta_list[i]= tuple([rho , 90 - theta])
                    elif different_theta_list[i][0] < 0:
                        (rho , theta) = different_theta_list[i]
                        different_theta_list[i]= tuple([rho , 270 - theta])
                mid= (different_theta_list[0][1] + different_theta_list[1][1])/2
                kp =0.01
                kd = 0.04
                ki = 0
                new_error = 90-mid 
                proportional = kp * error
                #print (error)
                rate_of_change = new_error -error
                error = new_error
                derivative = kd * rate_of_change

                integral = integral + error
                integral = ki * integral

                output = proportional + derivative + integral
                print("w", output)
                self.motors.sendW(output)
                self.motors.sendV(3.5)
                
        except TypeError:
            pass 
        #print(image_mask.shape)
        # Add your code here 

        print "Runing"
        #EXAMPLE OF HOW TO SEND INFORMATION TO THE ROBOT ACTUATORS
        #self.motors.sendV(10)
        #self.motors.sendW(5)

        #SHOW THE FILTERED IMAGE ON THE GUI
        output_image=cv2.cvtColor(image_mask, cv2.COLOR_HSV2RGB)
        self.setImageFiltered(output_image)