현자 (HJ)

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

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

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

알고보틱의 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

ESP32로 라이다 분석용 코드를 포팅했습니다. 그리고 검증용으로 PC로 x,y 점들을 보내게 했더니 나오네요.

카카오를 안써서 이곳에 영상을 못올립니다. 

영상과 선 연결 정보등은 페북에서 :  www.facebook.com/groups/airoboticskr/posts/1146125082811698/

ESP32 쪽은 현재는 거의 초벌수준인 아주 엉성한 코드입니다.   PC쪽은 자신이 원하는 아무 프로그램으로 직접 만드셔야 합니다.

#define RXD2 16 // This line connects to TX of Lidar 
#define TXD2 17 // is not used

/*  On ESP32, there are D/A pin which emits voltage.
 *  You can set 5th pin of lidar to this D/A pin to change speed.
 *  0V = Maximum speed, 5V is Slowest or Stop.
 */

int machine_pack_type = 40; // only 1 machine out of 4 of mine is 60, rests are 40
const int BUFFER_SIZE = 200;
char buf[BUFFER_SIZE];
String s;
String rs;
int i, j, k, counter, rmax, pack_type, data_length, start_angle, stop_angle;
int diff, length3, dataOk = 0;
int distance, data0, data1, data2, angle ;
float steplength, angle_per_sample, distanceDivided, anglef, distanceInMeter;
bool LadarActive = true;
int FullCircleDataSize = 0;
int ThisRoundCount = 0, maxThisRound = 0, x, y;
float anglePlus;

enum States {
   START1 = 0,
   START2 = 1,
   HEADER = 2,
   DATA = 3
};

float angle_list[120];
float distance_list[120];
int   quality_list[120];

States state;

void serial2Flush(){
  while(Serial2.available() > 0) {
    char t = Serial2.read();
  }
}

void setup() {
   steplength = (PI*2);
   anglePlus  = PI/2;
   state = START1;
  
   Serial.begin(230400);  // To PC
   Serial2.begin(153600, SERIAL_8N1, RXD2, TXD2); // To Ridar
   s.reserve(200);    s = "";
   rs.reserve(260); rs = "";  // string to pass data
}

void loop() { 
   if (LadarActive) {
      if (Serial2.available() > 0) {  // Look for AA55
         if (state == START1) {
            dataOk = Serial2.readBytes(buf, 1);
            // Serial.println(dataOk); // 1 came out
            if (dataOk == 1) { 
               if (buf[0] == 0xAA) {
                  state = START2;
               } 
            }
         } else if (state == START2) {  // Confirm 55
            dataOk = Serial2.readBytes(buf, 1); 
            // Serial.println(dataOk); // 1 came out
            if (dataOk == 1) { 
               if (buf[0] == 0x55) {
                  state = HEADER;
               } else {
                  state = START1;
               }
            }
         } else if (state == HEADER) {   // Read main data
            dataOk = Serial2.readBytes(buf, 8);
            // Serial.println(dataOk);  // 8 came out
            if (dataOk == 8) { 
               pack_type = buf[0];
               data_length = int(buf[1]);
               start_angle = int(buf[3] << 8) + int(buf[2]);
               stop_angle  = int(buf[5] << 8) + int(buf[4]);
            
               diff = stop_angle - start_angle;
               if (stop_angle < start_angle) {
                  diff =  0xB400 - start_angle + stop_angle;
               }

               angle_per_sample = 0;
               if ((diff > 1) and ((data_length-1) > 0)) {
                  angle_per_sample = diff / (data_length-1);
               }

               // Debug 
               s = ("#") + String(counter)
                  + (" t:") + pack_type
                  + (" l:") + data_length 
                  + (" sa:")+ start_angle 
                  + (" ta:")+ stop_angle
                  + (" f:") + diff
                  + (" as:")+ round(angle_per_sample);
               //Serial.println(s);
               
               FullCircleDataSize += data_length;

               counter += 1;
               if (pack_type != machine_pack_type) {  // default = 40, for some device = 60
                  // about every 40 cycles, one full circle done.
                  s = ("Counter : ") + String(counter-1) 
                     + (" Cycle Size : ") + String(FullCircleDataSize*3);
                  //Serial.println(s);
                  //Serial.println("*"); // Send End of 1 Full Cycle
                  counter = 0;
                  FullCircleDataSize = 0;
               }
               state = DATA;  
               rs = ""; 
            }       
         } else if (state == DATA) {   // Read main data
            state = START1;
            length3 = data_length * 3;
            dataOk = Serial2.readBytes(buf, (length3));

            s = " Data_length : " + String(data_length) 
               + "  dataOk :" + String(dataOk);
            // Serial.println(s); // usually 40, 120 came out
            
            if (dataOk == length3) { 
               for (int i = 0; i < data_length; i++) {
                  data0 = int(buf[i*3 + 0]);
                  data1 = int(buf[i*3 + 1]);
                  data2 = int(buf[i*3 + 2]); 
                  distance = (data2 << 8) + data1;

                  angle = (start_angle + angle_per_sample * i);
                  // 0xB400 = 46080, steplength = 2*PI, angle sample 20006
                  anglef = (steplength * angle) / 0xB400; // steplength = 2*PI
                  distanceDivided = distance / 100;

                  if (distanceDivided < 60) {
                     distanceInMeter = (distance/5); // div to convert mm to meter
                     y = distanceDivided * cos(anglef);
                     x = distanceDivided * sin(anglef);
                     // 0:556.00-556~0  // 1739:324.00-324~0
                     //rs += String(anglef) +(":") + String(distanceInMeter) + ("-")
                     //   + String(x) + ("~") + String(y) +(", "); 
                     if ((x != 0) and (y != 0)) { 
                        rs += ("[") + String(x) + (":") + String(y) + ("]");  
                     }
                     if (rs.length() > 240) { 
                         Serial.println(rs);  
                         rs = "";
                     }
                     serial2Flush();
                  }
                    
               //   angle_list[i] = anglef;

               //   distance_list[i] = distance / 1000; // div to convert mm to meter
               //   quality_list[i] = data0;
               }
               Serial.println(rs); // send remaining
               if (pack_type != machine_pack_type) { // default = 40, for some device = 60
                  // Draw stuffs now if OLED is attached
                  Serial.println("*"); // Send End of 1 Full Cycle
               }
               /*if (data_length > 0) {  // Draw data or prepare data to send
                  // Debug 
                  s = " Data_length : " + data_length;
                  //Serial.println(s);
                  for (int i = 0; i < data_length; i++) {
                     s += String(angle_list[i]) + (", ");
                  }
                  //Serial.println(s);
               }*/
            }
            
         }
        
      } 
      
   }
}

Simple pass-thru : (my local folder :  /SHARE/TTGO_Liligo/rider01_path_thru_esp32)

#define RXD2 16  
#define TXD2 17 // is not used

const int BUFFER_SIZE = 500;
char buf[BUFFER_SIZE];

void setup() {
//  Serial.begin(230400);  // To Lazarus App
  Serial.begin(250000);  // To PC
  Serial2.begin(153600, SERIAL_8N1, RXD2, TXD2); // To Ridar
}

void loop() { 
  while (Serial2.available()){
    Serial.print(char(Serial2.read())); 
  }
}