문제는 첫 실험 코딩이 matplotlib를 사용해서 엄청 느리기 때문에 PyQtGraph 라이브러리로 바꿔서 코딩해야 할 듯 합니다.
이건 나의 실험 케이스 :
Platform : Orange Pi 5, on UART #0 ( /dev/ttyS0 ) @ 230400 bps.
Speed control : PWM1_M2, pin # 26, which uses /sys/class/pwm/pwmchip0/
Then use the following command to make pwm1 output a 50Hz square wave (please switch to the root user first, and then execute the following command) root@o5bot:~# echo 0 > /sys/class/pwm/pwmchip0/export root@o5bot:~# echo 20000000 > /sys/class/pwm/pwmchip0/pwm0/period root@o5bot:~# echo 1000000 > /sys/class/pwm/pwmchip0/pwm0/duty_cycle root@o5bot:~# echo 1 > /sys/class/pwm/pwmchip0/pwm0/enable
Orange Pi 5에서 모터와 기타 단순 관리용 서브모듈로 사용하는 Arduino Nano와의 시리얼 통신용 코드.
이 방식은 아래와 같이 형태의 4글자 명령어와 최대 4개의 파라메터를 함께 보낼 수 있는 방식이다. 기체의 두뇌인 Orange Pi 5에서 온갖 명령어를 보내서 단순 작업들을 아두이노 나노에게 실행시키는 방법이다. <ESPC:3,4,-29334,4> <ESCT:0>
Parses.h
#define BUFFER_SIZE 50
int P2B_NEWLINE = 11; // < -- Beginning of Command to Remote Control that controls R/C Car
int P2B_COM_TYPE = 12; // 0..9 -- First 2 letters following "<"
// Total Command Types : 10 x 10 = 100
int P2B_DATA = 12; // Data section
int P2B_ENDING = 13; // > -- End of Message Signal
int P2B_STAT_NONE = 1; // Do nothing until '<' has received
int P2B_STAT_CMD = 2; // Accepting Command
int P2B_STAT_DATA = 3; // Accepting Command data
int P2B_STAT_END = 4; // '>' has receieve, it is the end of command;
// PI to Nano command string
char p2b_command[5];
char p2b_data[BUFFER_SIZE];
int p2b_paramcount = 0;
int p2b_status = 0;
int i = 0; // dumb int
int j = 0; // dumb int
int k = 0; // dumb int
int n = 0; // dumb int
int lp =0; // dumb last position of input command string
int r = 0; // Simple Counter to replace delay() in the main loop
String s; // Temporary string for general purpose
String rs;
String cmd; // used on ExecuteLogicalCommand()
boolean debugSerial = true;
boolean ContionousMove = true;
char inputString[200]; // a string to hold incoming data
boolean stringComplete = false; // whether the string is complete
char inChar;
String tempStr;
char buffer[12]; // buffer used for itoa (int to string conversion)
int inlen = 0; // Length of inputString
char RemoteString[50];
// value from PC Command (integer params 1 .. 4 max)
int paramcount = 0; // How many params in input String
int param1 = -1; // value from PC Command (Param 1)
int param2 = -1; // value from PC Command (Param 2)
int param3 = -1; // value from PC Command (Param 3)
int param4 = -1; // value from PC Command (Param 4)
int tempVal = 0;
int nPos = 0;
int sign = 1; // 1 = plus, -1 = minus
float fparam1 = 0.0;
float fparam2 = 0.0;
float fparam3 = 0.0;
float fparam4 = 0.0;
boolean SecondParam = false; // Now gettign second parameter (after ,)
// NEW PINS 18 = cam
const int CAMPin = 5;// the number of the LED pin
int freq = 5000;
int resolution = 8;
int dist = 0;
int StartingSpeed = 0; // 0 - 255 (8 bit)
int targetHeight = 0;
int msAi = 0;
int msBi = 0;
int msCi = 0;
int msDi = 0;
int maxSpeed = 50;
int height = 0;
int targetLR = 0; // Target Left/Right
int targetFB = 0; // Target Forward / Backward
int targetTN = 0; // Target Turn (+ CW, - CCW)
boolean mStop = false; // MS, Stop Motors
boolean mGoInAngle = false; // MG, Set Motor Speeds and Directions
boolean contGetAll = false; //
boolean AutoBalancing = false;
boolean NoOffset = false;
////////////////////////////////////////////////////////////////
void ClearCommand() {
p2b_command[0] = 0; // null
for (int i=0; i <= 49; i++){
p2b_data[i] = 0; // null
}
// set as none, 0 could be value
param1 = -1; //
param2 = -1; //
param3 = -1; //
param4 = -1; //
SecondParam = false;
}
void ClearRemoteString() {
for (i=0;i<=49;i++) {
RemoteString[i] = 0;
};
}
void ClearParamsAndInputstring(){
// clear the string:
for (i=0;i<=49;i++) {
inputString[i] = 0;
};
i = 0;
j = 0;
paramcount = 0; // How many params in input String
param1 = -1;
param2 = -1;
param3 = -1;
param4 = -1;
fparam1 = 0.0;
fparam2 = 0.0;
fparam3 = 0.0;
fparam4 = 0.0;
stringComplete = false;
}
MAR01AR.ino
/* This is written by Henry Kim, as a M.A.R. (Machine of Attack & Return)
Currently testing level @ Oct. 1, 2023
3 ESC for Propellers
- Center Tail Propeller
- Left Wing Propeller
- Right Wing Propeller
3 Servo 메인 모터 180도 회전
- for 180 degree rotation of 3 Propeller Motors
10 LED
1 A/D Battery Level Monitor
1 A/D Light Sensor
3 A/D Sound Sensor
1 A/D Temperature Sensor
*/
#include <Servo.h>
#include "Parses.h"
// ---------------------------------------------------------------------------
// Customize here pulse lengths as needed
#define MIN_PULSE_LENGTH 1000 // Minimum pulse length in µs (1 ms : min speed)
#define MAX_PULSE_LENGTH 2000 // Maximum pulse length in µs (2 ms : max speed)
// ---------------------------------------------------------------------------
Servo escLeft1, escRight2, escCenter3;
Servo motLeft4, motRight5, motCenter6;
// ---------------------------------------------------------------------------
void setup() {
ClearCommand();
Serial.begin(115200);
Serial.println("#MAR-NANO>");
// 3 main ESC for Props
escLeft1.attach(4, MIN_PULSE_LENGTH, MAX_PULSE_LENGTH);
escRight2.attach(5, MIN_PULSE_LENGTH, MAX_PULSE_LENGTH);
escCenter3.attach(6, MIN_PULSE_LENGTH, MAX_PULSE_LENGTH);
// Servos - Change angles of Prop motors
motLeft4.attach(7);
motRight5.attach(8);
motCenter6.attach(9);
// Prevent motor run at the start
stopAllProps(); // set in MIN_PULSE_LENGTH
// Servos heading top initially
basePosition4Servos(); // Heading Top
}
void loop() {
r++; //
if (r > 300000) { r = 0; } // Reset r if it is too large
if ((r % 20000) == 0) { // Do some sensor reading
//
}
while (Serial.available()) {
inChar = (char)Serial.read();
// get the new byte:
//Serial.print(inChar); // for debut
// add it to the inputString:
inputString[i] = inChar;
i++;
// if the incoming character is a newline, set a flag
if (inChar == '<') {
p2b_status = P2B_STAT_CMD;
ClearCommand();
j = i-1;
} else if (inChar == '>') { // so the main loop can do something about it:
p2b_command[0] = inputString[j+1]; // Get Command Character
p2b_command[1] = inputString[j+2]; // Get Command Character
p2b_command[2] = inputString[j+3]; // Get Command Character
p2b_command[3] = inputString[j+4]; // Get Command Character
// O : Orientation
// D : Distance
p2b_paramcount = int(inputString[j+6])-48; // take out 0
j = j + 8; // now j is pointing param starting position
if (p2b_paramcount > 0) {
tempVal = 0;
nPos = 0;
sign = 1; // 1 = plus, -1 = minus
k = 1;
for (n=j;n<=lp+1;n++){
// end of param
if ((inputString[n] == ',') or (inputString[n] == '>')){
if (k == 1){
param1 = tempVal * sign;
}
if (k == 2){
param2 = tempVal * sign;
}
if (k == 3){
param3 = tempVal * sign;
}
if (k == 4){
param4 = tempVal * sign;
}
tempVal = 0;
k++; // Param No.
nPos = 0;
sign = 1; // 1 = plus, -1 = minus
} else {
if (inputString[n] == '-'){
sign = -1;
} else {
nPos++;
if (nPos == 1) {
tempVal = (byte(inputString[n]) - 48);
} else {
tempVal = tempVal * 10 + (byte(inputString[n]) - 48);
}
}
}
}
} // end of if command is for this Machine
Serial.println("ESCT TEST 00");
ExecuteLogicalCommand();
i = 100; // refresh counter
stringComplete = true;
p2b_status = P2B_STAT_NONE;
} else {
lp = i-1; // set last position on each entry;
}
} /* while (Serial.available()) { */
if (stringComplete) {
uPrint(); // Return for debug
ClearParamsAndInputstring();
}
}
void ExecuteLogicalCommand() {
cmd = (char)p2b_command[0]+(char)p2b_command[1]+(char)p2b_command[2]+(char)p2b_command[3];
if (cmd == "ESCT"){ // Test ESC for Prop Motors
testESC();
};
if (cmd == "SRVT"){ // Test All Servos
testServos();
}
}
void basePosition4Servos(){
// Servos heading top initially
motLeft4.write(180); // Heading Top
motRight5.write(180); // Heading Top
motCenter6.write(180); // Heading Top
}
void testServos() // 3 Servos : escLeft1, escRight2, escCenter3
{
// Heading top to bottom
for (int i = 0; i <= 180; i += 10) {
motLeft4.write(180-i);
motRight5.write(180-i);
motCenter6.write(180-i);
delay(100);
// Heading bottom to top
for (int i = 0; i <= 180; i += 10) {
motLeft4.write(i);
motRight5.write(i);
motCenter6.write(i);
delay(100);
}
basePosition4Servos();
}
void stopAllProps() {
escLeft1.writeMicroseconds(MIN_PULSE_LENGTH);
escRight2.writeMicroseconds(MIN_PULSE_LENGTH);
escCenter3.writeMicroseconds(MIN_PULSE_LENGTH);
}
void testESC() // 3 ESC Test : escLeft1, escRight2, escCenter3
{
for (int i = MIN_PULSE_LENGTH; i <= MAX_PULSE_LENGTH-700; i += 40) {
// Serial.print("Pulse length = ");
// Serial.println(i);
escLeft1.writeMicroseconds(i);
escRight2.writeMicroseconds(i);
escCenter3.writeMicroseconds(i);
delay(200);
}
stopAllProps();
}
2. For now, you cannot use SSH log-in. So, attach monitor, keyboard, mouse.
3. After login,
sudo passwd root
(enter default password "odroid" for sudo)
(for new password, "111" and confirm) - enter extremely easy password. Save yourself from password hell.
sudo passwd odroid
(same thing. change all password to "111")
// This steps are needed for NPU to work
sudo apt update
sudo apt upgrade
sudo apt-get install nano
sudo nano /etc/ssh/sshd_config
// Allow enable SSH login option, so "root" can be remotely log-in
# Authentication:
#LoginGraceTime 2m
PermitRootLogin yes (<== change right here)
#StrictModes yes
#MaxAuthTries 6
#MaxSessions 10
sudo reboot
// After reboot, make sure to log-in as root via SSH.
// The user id "odroid" will not be used anymore.
// ************ IP RESERVATION with Router *************
// Try to reserve IP address for Odroid-M1 on your home or office router's configuration.
// If this device's MAC address is reserved with fixed local IP address, then this device will always have
// same local IP address everytime it boot-up. In my case, I have set its IP to 192.168.1.96
// now log-in from PC using SSH as root, 111 (My case, I use Snowflake on Ubuntu. With Snowflake, you can have file manager between PC & bot, text editor that can edit files on bot from PC, and endless number of terminals.)
// Install LXDE
apt-get install lxde
- during setting, it will as for gdm or lightdm, choose lightdm.
'''
Read Gyro and Accelerometer by Interfacing Raspberry Pi with MPU6050 using Python
http://www.electronicwings.com
'''
import smbus #import SMBus module of I2C
from time import sleep #import
#some MPU6050 Registers and their Address
PWR_MGMT_1 = 0x6B
SMPLRT_DIV = 0x19
CONFIG = 0x1A
GYRO_CONFIG = 0x1B
INT_ENABLE = 0x38
ACCEL_XOUT_H = 0x3B
ACCEL_YOUT_H = 0x3D
ACCEL_ZOUT_H = 0x3F
GYRO_XOUT_H = 0x43
GYRO_YOUT_H = 0x45
GYRO_ZOUT_H = 0x47
def MPU_Init():
#write to sample rate register
bus.write_byte_data(Device_Address, SMPLRT_DIV, 7)
#Write to power management register
bus.write_byte_data(Device_Address, PWR_MGMT_1, 1)
#Write to Configuration register
bus.write_byte_data(Device_Address, CONFIG, 0)
#Write to Gyro configuration register
bus.write_byte_data(Device_Address, GYRO_CONFIG, 24)
#Write to interrupt enable register
bus.write_byte_data(Device_Address, INT_ENABLE, 1)
def read_raw_data(addr):
#Accelero and Gyro value are 16-bit
high = bus.read_byte_data(Device_Address, addr)
low = bus.read_byte_data(Device_Address, addr+1)
#concatenate higher and lower value
value = ((high << 8) | low)
#to get signed value from mpu6050
if(value > 32768):
value = value - 65536
return value
bus = smbus.SMBus(5) # or bus = smbus.SMBus(0) for older version boards
Device_Address = 0x68 # MPU6050 device address
MPU_Init()
print (" Reading Data of Gyroscope and Accelerometer")
while True:
#Read Accelerometer raw value
acc_x = read_raw_data(ACCEL_XOUT_H)
acc_y = read_raw_data(ACCEL_YOUT_H)
acc_z = read_raw_data(ACCEL_ZOUT_H)
#Read Gyroscope raw value
gyro_x = read_raw_data(GYRO_XOUT_H)
gyro_y = read_raw_data(GYRO_YOUT_H)
gyro_z = read_raw_data(GYRO_ZOUT_H)
#Full scale range +/- 250 degree/C as per sensitivity scale factor
Ax = acc_x/16384.0
Ay = acc_y/16384.0
Az = acc_z/16384.0
Gx = gyro_x/131.0
Gy = gyro_y/131.0
Gz = gyro_z/131.0
print ("Gx=%.2f" %Gx, u'\u00b0'+ "/s", "\tGy=%.2f" %Gy, u'\u00b0'+ "/s", "\tGz=%.2f" %Gz, u'\u00b0'+ "/s", "\tAx=%.2f g" %Ax, "\tAy=%.2f g" %Ay, "\tAz=%.2f g" %Az)
sleep(1)
Code 실행 결과
Reading Data of Gyroscope and Accelerometer
Gx=0.00 °/s Gy=0.00 °/s Gz=0.00 °/s Ax=0.00 g Ay=0.00 g Az=0.00 g
Gx=-0.25 °/s Gy=-0.05 °/s Gz=-0.04 °/s Ax=-0.18 g Ay=0.56 g Az=0.72 g
Gx=-0.28 °/s Gy=-0.10 °/s Gz=-0.02 °/s Ax=-0.17 g Ay=0.56 g Az=0.74 g
Gx=21.01 °/s Gy=-46.73 °/s Gz=-66.13 °/s Ax=-0.60 g Ay=0.53 g Az=1.98 g
Gx=-1.12 °/s Gy=0.08 °/s Gz=-2.44 °/s Ax=0.91 g Ay=0.25 g Az=-0.42 g
Gx=-20.89 °/s Gy=22.92 °/s Gz=-12.68 °/s Ax=0.57 g Ay=0.91 g Az=-0.08 g
Gx=-8.25 °/s Gy=6.10 °/s Gz=-2.79 °/s Ax=-0.14 g Ay=-0.28 g Az=0.79 g
Gx=0.19 °/s Gy=-1.33 °/s Gz=0.41 °/s Ax=-0.27 g Ay=-0.19 g Az=0.89 g
Gx=-1.12 °/s Gy=0.74 °/s Gz=-1.82 °/s Ax=0.01 g Ay=-0.03 g Az=0.93 g
Gx=-16.98 °/s Gy=-8.69 °/s Gz=-0.35 °/s Ax=0.53 g Ay=0.11 g Az=0.41 g
Gx=-0.86 °/s Gy=-3.69 °/s Gz=-3.56 °/s Ax=0.41 g Ay=-0.85 g Az=1.86 g
Gx=8.50 °/s Gy=-13.91 °/s Gz=-2.17 °/s Ax=1.00 g Ay=0.22 g Az=-0.11 g
Gx=1.95 °/s Gy=-15.69 °/s Gz=-6.44 °/s Ax=0.88 g Ay=0.49 g Az=0.22 g
Gx=-0.90 °/s Gy=-19.22 °/s Gz=4.11 °/s Ax=0.79 g Ay=0.20 g Az=0.20 g
Gx=2.63 °/s Gy=-0.53 °/s Gz=3.50 °/s Ax=0.57 g Ay=0.22 g Az=0.47 g
Gx=7.98 °/s Gy=-5.44 °/s Gz=-4.53 °/s Ax=0.16 g Ay=-0.44 g Az=1.19 g
Gx=-1.49 °/s Gy=2.40 °/s Gz=-1.51 °/s Ax=0.50 g Ay=-0.40 g Az=0.69 g
Gx=9.06 °/s Gy=-0.34 °/s Gz=11.00 °/s Ax=-0.20 g Ay=-0.04 g Az=0.61 g
Gx=8.08 °/s Gy=-0.40 °/s Gz=-2.18 °/s Ax=-0.12 g Ay=0.08 g Az=0.93 g
Gx=-0.26 °/s Gy=-0.06 °/s Gz=-0.07 °/s Ax=-0.04 g Ay=0.12 g Az=0.94 g
Gx=-0.23 °/s Gy=-0.02 °/s Gz=-0.08 °/s Ax=-0.04 g Ay=0.12 g Az=0.94 g
Gx=-0.25 °/s Gy=-0.08 °/s Gz=-0.09 °/s Ax=-0.06 g Ay=0.15 g Az=0.93 g
Gx=-0.23 °/s Gy=-0.05 °/s Gz=-0.03 °/s Ax=-0.06 g Ay=0.17 g Az=0.93 g
Gx=-0.29 °/s Gy=-0.06 °/s Gz=-0.05 °/s Ax=-0.07 g Ay=0.15 g Az=0.92 g
Gx=-0.40 °/s Gy=0.06 °/s Gz=-0.11 °/s Ax=-0.07 g Ay=0.16 g Az=0.94 g
Gx=-0.28 °/s Gy=-0.04 °/s Gz=-0.08 °/s Ax=-0.08 g Ay=0.17 g Az=0.93 g
Gx=-0.25 °/s Gy=-0.07 °/s Gz=-0.04 °/s Ax=-0.06 g Ay=0.14 g Az=0.94 g
Gx=-0.27 °/s Gy=-0.05 °/s Gz=-0.02 °/s Ax=-0.06 g Ay=0.16 g Az=0.93 g
Gx=6.27 °/s Gy=8.89 °/s Gz=2.79 °/s Ax=0.06 g Ay=-0.07 g Az=0.90 g
Gx=4.08 °/s Gy=36.98 °/s Gz=-14.33 °/s Ax=0.28 g Ay=0.08 g Az=0.66 g
Gx=-2.27 °/s Gy=-2.36 °/s Gz=-1.52 °/s Ax=-0.13 g Ay=0.01 g Az=0.95 g
Gx=0.41 °/s Gy=-0.18 °/s Gz=-0.15 °/s Ax=-0.09 g Ay=0.30 g Az=0.89 g
^CTraceback (most recent call last):
File "/root/o/2.py", line 82, in <module>
sleep(1)
(나중에 추가) 아래 코드는 원작자도 에라가 있는 걸 못 고친 듯 합니다. 알고보니 원본 데모 영상에도 똑같은 에러가 있더군요.
별로 가치가 없는 내용이었습니다.
===============================================
알고보틱의 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()
