Solution code for vacuum_cleaner practice of JdeRobot Academy.

MyAlgorithm.py

import numpy as np
import threading
import time
from datetime import datetime
import jderobot
import math
import matplotlib.pyplot as plt
time_cycle = 80   
left = False
class MyAlgorithm(threading.Thread):

    def __init__(self, pose3d, motors, laser, bumper):
        self.pose3d = pose3d
        self.motors = motors
        self.laser = laser
        self.bumper = bumper

        self.stop_event = threading.Event()
        self.kill_event = threading.Event()
        self.lock = threading.Lock()
        threading.Thread.__init__(self, args=self.stop_event)


    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 parse_laser_data(self,laser_data):
        laser = []
        for i in range(laser_data.numLaser):
            dist = laser_data.distanceData[i]/1000.0
            #angle = math.radians(i)
            laser.append(dist)
        return laser

    def spiral(self):
        global spiral_value
        spiral_value =spiral_value + 1
        w= 4*math.exp(-spiral_value*0.01)
        print("w=",w)
        self.motors.sendW(w)
        self.motors.sendV(1)
        
    def backtrack(self):
        self.motors.sendW(0)
        self.motors.sendV(-0.7)
        time.sleep(0.5)
        self.motors.sendV(0)
    def execute(self):
        
        global left
        global spiral_value
        print "Runing"
        obstacle_threshold=0.4
        upper_threshold=0.8
        laser_data = self.laser.getLaserData()
        laser_data=self.parse_laser_data(laser_data)
        laser_data=np.array(laser_data,dtype=float)
        
        #laser_data_gradient=np.gradient(laser_data)
        right_sum=0
        middle_sum=0
        left_sum=0
        for i in range (0,20):
            if laser_data[i]>upper_threshold:
                laser_data[i]=upper_threshold
            right_sum=right_sum+laser_data[i]
        for i in range (75,105):
            if laser_data[i]>upper_threshold:
                laser_data[i]=upper_threshold
            middle_sum=middle_sum+laser_data[i]
        for i in range (160,180):
            if laser_data[i]>upper_threshold:
                laser_data[i]=upper_threshold
            left_sum=left_sum+laser_data[i]
        #if not colliding with wall
        if self.bumper.getBumperData().state == 0:
            
            #print("right=",right_sum/20)
            #print("middle=",middle_sum/30)
            #print("left=",left_sum/20)

            #obstacle all around
            if right_sum/20 < obstacle_threshold+0.16 and middle_sum/30 < obstacle_threshold and left_sum/20 < obstacle_threshold+0.16:
                print("obstacle all around--stop")
                spiral_value = 0
                self.backtrack()
                left=False

            #obstacle in front only
            elif (middle_sum/20) < obstacle_threshold:
                print("obstacle in front --right")
                #turn right
                spiral_value = 0
                self.motors.sendV(0)
                self.motors.sendW(-math.pi)
                time.sleep(0.48)
                self.motors.sendW(0)
                time.sleep(0.2)
                left=False
            #no obstacle in left
            elif (left_sum/20) > obstacle_threshold+0.16:
                print("no obstacle in left --turn to left and move forward")
                if left==False:
                    self.motors.sendV(0)
                    self.motors.sendW(math.pi)
                    time.sleep(0.48)
                    self.motors.sendW(0)
                    left=True
                self.motors.sendW(0)
                self.motors.sendV(0.5)

            else:
                print("else --forward")
                #move forward
                spiral_value = 0
                self.motors.sendW(0)
                self.motors.sendV(0.5)
                left=False
        #fail safe ( in case the robot collides with wall)
        else:
            print("back")
            if (left_sum>right_sum):
                self.motors.sendV(0)
                self.motors.sendW(math.pi)
                time.sleep(0.48)
                self.motors.sendW(0)
            else:
                self.motors.sendV(0)
                self.motors.sendW(-math.pi)
                time.sleep(0.48)
                self.motors.sendW(0)