현자 (HJ)

원본 영상 : youtu.be/AwRrOxVjAWE 

윗 영상의 코드를 직접 따라 해 봤습니다.  아무래도 원본에 에러가 있는 듯 한데 직접 고쳤습니다.

윗 영상은 저처럼 파이션이 낯선 사람들이 공부용으로는 좋은데 로봇 이미지와 코드는 무료가 아닙니다.

이건 제가 직접 만든 로봇 이미지와 입력한 코드입니다. (Zip 화일로 올려놓았습니다)

설명을 들으면서 치다보니 pygame에 익숙해 지는 느낌이라 오히려 좋았습니다.

# File name : difdrive.py
# Link : https://www.youtube.com/watch?v=zHboXMY45YU
# Download Car image & rename to DDR.png : https://imgur.com/SpeVm6L 
# x1 = v*cos(theta)
# y1 = v*sin(theta)
# Theta1 = diff(v) / W

import pygame
import math

class Envir:
   def __init__(self, dimensions):
      # colours
      self.black = (0, 0, 0)
      self.white = (255, 255, 255)
      self.green = (0, 255, 0)
      self.blue  = (0, 0, 255)
      self.red   = (255, 0, 0)
      self.yel   = (70, 70, 70)
      # map dimensions
      self.height = dimensions[0]
      self.width  = dimensions[1]
      # window settings
      pygame.display.set_caption("Differential drive robot")
      self.map = pygame.display.set_mode((self.width, self.height))
      # text variables
      self.font = pygame.font.Font('freesansbold.ttf', 50)
      self.text = self.font.render('default', True, self.white, self.black)
      self.textRect = self.text.get_rect()
      self.textRect.center = (dimensions[1] - 600,
                              dimensions[0] - 100)
      #trail
      self.trail_set=[]                        
                              
   def write_info(self, Vl, Vr, theta):
      txt = f"Vl = {Vl} Vr = {Vr}  theta = {int(math.degrees(theta))}"
      self.text = self.font.render(txt, True, self.white, self.black)
      self.map.blit(self.text, self.textRect)
      
   def trail(self, pos):
      for i in range(0, len(self.trail_set)-1):
         pygame.draw.line(self.map, self.yel, 
            (self.trail_set[i][0], self.trail_set[i][1]),
            (self.trail_set[i+1][0], self.trail_set[i+1][1]))
      if self.trail_set.__sizeof__() > 10000:
         self.trail_set.pop(0)
      self.trail_set.append(pos)   
      
   def robot_frame(self, pos, rotation):
      n = 80
      
      centerx, centery = pos
      x_axis = (centerx + n * math.cos(-rotation), centery + n * math.sin(-rotation))
      y_axis = (centerx + n * math.cos(-rotation + math.pi/2), 
                centery + n * math.sin(-rotation + math.pi/2))
      pygame.draw.line(self.map, self.red, (centerx, centery), x_axis, 3)
      pygame.draw.line(self.map, self.green, (centerx, centery), y_axis, 3)      
                 
class Robot:
   def __init__(self, startpos, robotImg, width):
      self.m2p = 3779.52  # meters to pixels ratio
      # robot dimensions
      self.w = width
      self.x = startpos[0]
      self.y = startpos[1]
      self.theta = 0
      self.vl = 0.01 * self.m2p  # meters / second
      self.vr = 0.01 * self.m2p  # meters / second
      self.maxspeed = 0.02 * self.m2p
      self.minspeed = -0.02 * self.m2p
      # graphics
      self.img = pygame.image.load(robotImg)
      self.rotated = self.img
      self.rect=self.rotated.get_rect(center=(self.x, self.y))
      
   def draw(self, map):
      map.blit(self.rotated, self.rect)   
      
   def move(self, event=None):
      if event is not None:
         if event.type == pygame.KEYDOWN:
            if event.key == pygame.K_KP4:  # Left Arrow in Numpad     
               self.vl += 0.001 * self.m2p
            elif event.key == pygame.K_KP1:  # 1  
               self.vl -= 0.001 * self.m2p    
            elif event.key == pygame.K_KP6:  # Right Arrow in Numpad  
               self.vr += 0.001 * self.m2p
            elif event.key == pygame.K_KP3:  # 3  
               self.vr -= 0.001 * self.m2p
               
      # v=(Vr+Vl)/2                  
      self.x += ((self.vl + self.vr) / 2) * math.cos(self.theta) * dt   
      self.y -= ((self.vl + self.vr) / 2) * math.sin(self.theta) * dt
      self.theta += (self.vr - self.vl) / self.w * dt
      # reset theta
      if self.theta > 2 * math.pi or self.theta < -2 * math.pi :
         self.theta = 0
      # set max speed
      self.vr = min(self.vr, self.maxspeed)
      self.vl = min(self.vl, self.maxspeed)
      # set min speed      
      self.vr = max(self.vr, self.minspeed)
      self.vl = max(self.vl, self.minspeed)
      
      self.rotated = pygame.transform.rotozoom(self.img,
         math.degrees(self.theta), 1)         
      self.rect = self.rotated.get_rect(center=(self.x, self.y))
      
         
      
# initialization
pygame.init()

# start postion
start = (200, 200)

# dimension
dims=(600, 1200)

# running or not
running = True

# the envir
environment = Envir(dims)

# the Robot (begining location, image of robot, width)
robot = Robot(start, "./DDR.png", 0.01 * 3779.52)

# dt (Change in time)
dt = 0
lasttime = pygame.time.get_ticks()

# simulation loop
while running:
   for event in pygame.event.get():
      if event.type == pygame.QUIT:
         running = False
      robot.move(event)
       
   dt = (pygame.time.get_ticks()-lasttime) / 1000     
   lasttime = pygame.time.get_ticks()
   pygame.display.update()
   environment.map.fill(environment.black)
   robot.move()
   environment.write_info(int(robot.vl),
                          int(robot.vr),
                          robot.theta)
   robot.draw(environment.map)          
   environment.trail((robot.x, robot.y))             
   environment.robot_frame((robot.x, robot.y), robot.theta)
                              
   #if pygame.mouse.get_focused():
      #sensorOn = True

(나중에 추가) 아래 코드는 원작자도 에라가 있는 걸 못 고친 듯 합니다.   알고보니 원본 데모 영상에도 똑같은 에러가 있더군요.

별로 가치가 없는 내용이었습니다.

===============================================

알고보틱의 https://www.youtube.com/watch?v=ZxaXfahaP2s 비디오 코드인데 작가의 코드와 똑같지 않은듯 합니다.

약간 결과가 다르게 나오더군요.   

총 5편의 비디오 강의의 결과물인데 강의 영상과는 결과가 다르게 부실하게 나왔습니다.

알고보틱은 예전에는 코드를 판매했는데 살만한 가치까지는 없어보여서 그냥 웹에 뒤지니 대충 나왔습니다.

요즘에는 판매하는 account도 없애버린 것 같더군요.   

env.py

import math
import pygame

class buildEnvironment:
    def __init__(self, MapDimensions):
        pygame.init()
        self.pointCloud = []
        self.externalMap = pygame.image.load('floorplan.png')
        self.maph, self.mapw = MapDimensions
        self.MapWindowName = 'RRT path planning'
        pygame.display.set_caption(self.MapWindowName)
        self.map = pygame.display.set_mode((self.mapw,self.maph))
        self.map.blit(self.externalMap,(0,0))
        #Colours
        self.black = (0, 0, 0)
        self.grey = (70, 70, 70)
        self.Blue = (0, 0, 255)
        self.Green = (0, 255, 0)
        self.Red = (255, 0, 0)
        self.white = (255, 255, 255)


    def AD2pos(self, distance, angle, robotPosition):
        x = distance * math.cos(angle) + robotPosition[0]
        y = -distance * math.sin(angle) + robotPosition[1]
        return (int(x), int(y))

    def dataStorage(self, data):
        print(len(self.pointCloud))
        for element in data:
            point = self.AD2pos(element[0], element[1], element[2])
            if point not in self.pointCloud:
                self.pointCloud.append(point)

    def show_sensorData(self):
        self.infomap = self.map.copy()
        for point in self.pointCloud:
            self.infomap.set_at((int(point[0]), int(point[1])), (255, 0, 0))

sensors.py

import math
import pygame
import numpy as np

def uncertaninty_add(distance, angle, sigma):
    mean = np.array([distance, angle])
    covariance = np.diag(sigma**2)
    distance, angle = np.random.multivariate_normal(mean, covariance)
    distance = max(distance, 0)
    angle = max(angle, 0)
    return [distance, angle]


class LaserSensor:
    def __init__(self, Range, map, uncertainty):
        self.Range = Range
        self.speed = 3 #rounds per second
        self.sigma = np.array([uncertainty[0], uncertainty[1]])
        self.position = (0,0)
        self.map = map
        self.W, self.H = pygame.display.get_surface().get_size()
        self.sensedObstacles = []

    def distance(self, obsatclePosition):
        px = (obsatclePosition[0] - self.position[0])**2
        py = (obsatclePosition[1] - self.position[1])**2
        return math.sqrt(px + py)

    def sense_obstacles(self):
        data = []
        x1, y1 = self.position[0], self.position[1]
        for angle in np.linspace(0, 2*math.pi, 180, False):
            x2, y2 = (x1 + self.Range * math.cos(angle), y1 - self.Range * math.sin(angle))
            for i in range(0,100):
                u = i / 100
                x = int(x2*u + x1*(1-u))
                y = int(y2*u + y1*(1-u))
                if 0 < x < self.W and 0 < y < self.H:
                    color = self.map.get_at((x,y))
                    if (color[0], color[1], color[2]) == (0,0,0):
                        distance = self.distance((x,y))
                        output = uncertaninty_add(distance, angle, self.sigma)
                        output.append(self.position)
                        # store the mesurement
                        data.append(output)
                        break
        if len(data) > 0:
            return data
        else:
            return False

Features.py

# Features.py

import numpy as np
import math
from fractions import Fraction
from scipy.odr import *

# landmarks
Landmarks = []

class featuresDetection:
   def __init__(self):
      # variables
      self.EPSILON = 10
      self.DELTA = 10
      self.SNUM = 6
      self.PMIN = 20
      self.GMAX = 20
      self.SEED_SEGMENTS = []
      self.LINE_SEGMENTS = []
      self.LASERPOINTS = []
      self.LINE_PARAMS = None
      self.NP = len(self.LASERPOINTS) - 1
      self.LMIN = 5 # minimum length of a line segment
      self.LR = 0 # real length of a line segment
      self.PR = 0 # the number of laser points contained
      self.FEATURES = []

   # euclidian distance from point1 to point2
   def dist_point2point(self, point1, point2):
   #def dist_point2point(point1, point2):
      Px = (point1[0] - point2[0]) ** 2
      Py = (point1[1] - point2[1]) ** 2
      return math.sqrt(Px + Py)
      
   # distance point to line written in general form
   def dist_point2line(self, params, point):
      A, B, C = params
      distance = abs(A * point[0] + B * point[1] + C) / math.sqrt(A ** 2 + B ** 2)
      return distance
      
   # extract two points from a line equation under the slope intercepts form
   def line_2points(self, m, b):
      x = 5
      y = m * x + b
      x2 = 2000
      y2 = m * x2 + b
      return [(x, y), (x2, y2)]
      
   # general form to slope-intercept
   def lineForm_G2SI(self, A, B, C):
      m = -A / B
      B = -C / B
      return m, B
      
   # slope-intercept to general form
   def lineForm_Si2G(self, m, B):
      A, B, C = -m, 1, -B
      if A < 0:
         A, B, C = -A, -B, -C
      den_a = Fraction(A).limit_denominator(1000).as_integer_ratio()[1]   
      den_c = Fraction(C).limit_denominator(1000).as_integer_ratio()[1]
   
      gcd = np.gcd(den_a, den_c)
      lcm = den_a * den_c / gcd
      
      A = A * lcm
      B = B * lcm
      C = C * lcm
      return A, B, C
      
   def line_intersect_general(self, params1, params2):
      a1, b1, c1 = params1
      a2, b2, c2 = params2
      if (b1 * a2 - a1 * b2) != 0:
         x = (c1 * b2 - b1 * c2) / (b1 * a2 - a1 * b2)
         y = (a1 * c2 - a2 * c1) / (b1 * a2 - a1 * b2)
      return x, y
      
   def points_2line(self, point1, point2):
      m, b = 0, 0
      if point2[0] == point1[0]:
         pass
      else:
         m = (point2[1] - point1[1]) / (point2[0] - point1[0])
         b = point2[1] - m * point2[0]
      return m, b

   def projection_point2line(self, point, m, b):
      x, y = point
      m2 = -1 / m
      c2 = y - m2 * x
      
      intersection_x = - (b - c2) / (m - m2)
      intersection_y = m2 * intersection_x + c2
      return intersection_x, intersection_y      
      
   def AD2pos(self, distance, angle, robot_position):
      x = distance * math.cos(angle) + robot_position[0]
      y = -distance * math.sin(angle) + robot_position[1]
      return (int(x), int(y))
      
   def laser_points_set(self, data):
      self.LASERPOINTS = []   
      if not data:
         pass
      else:
         for point in data:
            coordinates = self.AD2pos(point[0], point[1], point[2])
            self.LASERPOINTS.append([coordinates, point[1]])
      self.NP = len(self.LASERPOINTS) - 1
   
   # Define a function (quadratic in our case) to fit the data with.
   def linear_func(self, p, x):
      m, b = p
      return m * x + b
      
   def odr_fit(self, laser_points):
      x = np.array([i[0][0] for i in laser_points])
      y = np.array([i[0][1] for i in laser_points])   
      
      # Create a model for fitting.
      linear_model = Model(self.linear_func)
      
      # Create a RealData object using our initiated data from above.
      data = RealData(x, y)
      
      # Set up ODR with the model and data.
      odr_model = ODR(data, linear_model, beta0=[0., 0.])
      
      # Run the regression.
      out = odr_model.run()
      m, b = out.beta
      return m, b
      
   def predictPoint(self, line_params, sensed_point, robotpos):
      m, b = self.points_2line(robotpos, sensed_point)
      params1 = self.lineForm_Si2G(m, b)
      predx, predy = self.line_intersect_general(params1, line_params)
      return predx, predy
      
   def seed_segment_detection(self, robot_position, break_point_ind):
      flag = True
      self.NP = max(0, self.NP)
      self.SEED_SEGMENTS = []
      for i in range(break_point_ind, (self.NP - self.PMIN)):
         predicted_points_to_draw = []
         j = i + self.SNUM
         m, c = self.odr_fit(self.LASERPOINTS[i:j])
         
         params = self.lineForm_Si2G(m, c)
         
         for k in range(i, j):
            predicted_point = self.predictPoint(params, self.LASERPOINTS[k][0], robot_position)
            predicted_points_to_draw.append(predicted_point)
            d1 = self.dist_point2point(predicted_point, self.LASERPOINTS[k][0])
            
            if d1 > self.DELTA:
               flag = False
               break
            d2 = self.dist_point2line(params, predicted_point)
            
            if d2 > self.EPSILON:
               flag = False
               break
         if flag:
            self.LINE_PARAMS = params
            return [self.LASERPOINTS[i:j], predicted_points_to_draw, (i, j)]
            
      return False
   
   def seed_segment_growing(self, indices, break_point):
      line_eq = self.LINE_PARAMS
      i, j = indices
      # Beginning and Final points in the line segment
      PB, PF = max(break_point, i - 1), min(j + 1, len(self.LASERPOINTS) - 1)
      
      while self.dist_point2line(line_eq, self.LASERPOINTS[PF][0]) < self.EPSILON: 
         if PF > self.NP - 1:
            break
         else:
            m, b = self.odr_fit(self.LASERPOINTS[PB:PF])
            line_eq = self.lineForm_Si2G(m, b)
            
            POINT = self.LASERPOINTS[PF][0]
          
         PF = PF + 1
         NEXTPOINT = self.LASERPOINTS[PF][0]
         if self.dist_point2point(POINT, NEXTPOINT) > self.GMAX:
            break
            
      PF = PF - 1
      
      while self.dist_point2line(line_eq, self.LASERPOINTS[PB][0]) < self.EPSILON:
         if PB < break_point:
            break
         else:
            m, b = self.odr_fit(self.LASERPOINTS[PB:PF])
            line_eq = self.lineForm_Si2G(m, b)
            POINT = self.LASERPOINTS[PB][0]
            
         PB = PB - 1
         NEXTPOINT = self.LASERPOINTS[PB][0]
         if self.dist_point2point(POINT, NEXTPOINT) > self.GMAX:
            break
      PB = PB + 1
      
      LR = self.dist_point2point(self.LASERPOINTS[PB][0], self.LASERPOINTS[PF][0])
      PR = len(self.LASERPOINTS[PB:PF])
      
      if (LR >= self.LMIN) and (PR >= self.PMIN):
         self.LINE_PARAMS = line_eq
         m, b = self.lineForm_G2SI(line_eq[0], line_eq[1], line_eq[2])
         self.two_points = self.line_2points(m, b)
         self.LINE_SEGMENTS.append((self.LASERPOINTS[PB + 1][0], self.LASERPOINTS[PF - 1][0]))
         return [self.LASERPOINTS[PB:PF], self.two_points,
         (self.LASERPOINTS[PB + 1][0], self.LASERPOINTS[PF - 1][0]), PF, line_eq, (m, b)]
      else:
         return False
         
   def lineFeats2point(self):
      new_rep = [] # the new representation of the features
      
      for feature in self.FEATURES:
         projection = self.projection_point2line((0,0), feature[0][0], feature[0][1])
         new_rep.append([feature[0], feature[1], projection])
         
      return new_rep
         
def landmark_association(landmarks):
   thresh = 10   # 10 was default
   for l in landmarks:
   
      flag = False
      for i, Landmark in enumerate(Landmarks):
         dist = featuresDetection.dist_point2point(l[2], Landmark[2])
         if dist < thresh:
            if not is_overlap(l[1], Landmark[1]):
               continue
            else:
               Landmarks.pop(i)   
               Landmarks.insert(i, l)
               flag = True      
               
               break
         if not flag:
            Landmarks.append(l)
            
def is_overlap(seg1, seg2):
   length1 = featuresDetection.dist_point2point(seg1[0], seg1[1])
   length2 = featuresDetection.dist_point2point(seg2[0], seg2[1])               
   center1 = ((seg1[0][0] + seg1[1][0]) / 2, (seg1[0][1] + seg1[1][1]) / 2)
   center2 = ((seg2[0][0] + seg2[1][0]) / 2, (seg2[0][1] + seg2[1][1]) / 2)
   dist = featuresDetection.dist_point2point(center1, center2)
   if dist > (length1 + length2) / 2:
      return False
   else:
      return True

main.py

# Main for Feature Extraction from 2D LIDAR
#import SLAM_YTB import env, sensors, Features
import env, sensors, Features
import random
import pygame

def random_color():
   levels = range(32,256,32)
   return tuple(random.choice(levels) for _ in range(3))
FeatureMAP=Features.featuresDetection()   
environment = env.buildEnvironment((600,1200))
originalMap = environment.map.copy()
laser = sensors.LaserSensor(200, originalMap, uncertainty=(0.5, 0.01))
environment.map.fill((255,255,255))
environment.infomap = environment.map.copy()
originalMap = environment.map.copy()
running = True
FEATURE_DETECTION=True
BREAK_POINT_IND = 0

while running :
   #environment.infomap = originalMap.copy()
   FEATURE_DETECTION = True
   BREAK_POINT_IND = 0
   ENDPOINTS=[0,0]
   sensorOn = False
   PREDICTED_POINTS_TODRAW=[]
      
   for envent in pygame.event.get():
      if envent.type == pygame.QUIT:
         running = False
   if pygame.mouse.get_focused():
      sensorOn = True
   elif not pygame.mouse.get_focused():
      sensorOn = False        
   if sensorOn: 
      position = pygame.mouse.get_pos()
      laser.position = position
      sensor_data = laser.sense_obstacles()
      FeatureMAP.laser_points_set(sensor_data)
      while BREAK_POINT_IND < (FeatureMAP.NP - FeatureMAP.PMIN):
         seedSeg = FeatureMAP.seed_segment_detection(laser.position,BREAK_POINT_IND)
         if seedSeg == False:
            break
         else:
            seedSegment = seedSeg[0]
            PREDICTED_POINTS_TODRAW = seedSeg[1]
            INDICES = seedSeg[2] 
            results = FeatureMAP.seed_segment_growing(INDICES, BREAK_POINT_IND)
            if results == False:
               BREAK_POINT_IND = INDICES[1]
               continue
            else:
               line_eq = results[1]
               m, c = results[5]
               line_seg = results[0]
               OUTERMOST = results[2]
               BREAK_POINT_IND = results[3]
               
               ENDPOINTS[0] = FeatureMAP.projection_point2line(OUTERMOST[0], m, c)
               ENDPOINTS[1] = FeatureMAP.projection_point2line(OUTERMOST[1], m, c)
               FeatureMAP.FEATURES.append([[m, c], ENDPOINTS])
               
               COLOR = random_color()
               for point in line_seg:
                  #environment.infomap.set_at((int(point[0][0]), int(point[0][1])), (0, 255, 0))
                  pygame.draw.circle(environment.infomap, COLOR, (int(point[0][0]), int(point[0][1])), 2, 0)
               #pygame.draw.line(environment.infomap, (255, 0, 0), ENDPOINTS[0], ENDPOINTS[1], 2)

               pygame.draw.line(environment.infomap, (0, 255, 100), ENDPOINTS[0], ENDPOINTS[1], 1)
               environment.dataStorage(sensor_data)
               
               FeatureMAP.FEATURES=FeatureMAP.lineFeats2point()
               Features.landmark_association(FeatureMAP.FEATURES)
      for landmark in Features.Landmarks:
         pygame.draw.line(environment.infomap, (100, 100, 155), landmark[1][0], landmark[1][1], 4)
               
   environment.map.blit(environment.infomap, (0, 0))
   pygame.display.update()

원본을 다운받은 곳은 초기 영상분이었습니다.   이 곳에 Map 화일들이 있습니다.  https://github.com/kabbel/SLAM