Algorytmy robotyki mobilnej - lokalizacja względem znaczników

Wprowadzenie

Celem zadania jest zapoznanie się z przetwarzaniem danych ze skanera laserowego, a następnie implementacja wybranej metody wykrywania linii i na jej podstawie lokalizacji robota.

Przygotowanie

Przed zajęciami należy zapoznać się z opisem zadania oraz materiałami pomocniczymi w podanym niżej zakresie:

przypomnieć przekształcenia współrzędnych między układami: biegunowym i kartezjańskim;
przypomnieć formuły przekształceń współrzędnych między przesuniętymi i obróconymi względem siebie układami współrzędnych;
na podstawie opracowanych materiałów na seminarium zaproponować algorytm wykrywania linii;
zaproponować metodę oceny dokładności metod lokalizacji z zadania bieżącego i poprzedniego

Część 1 - odczyt danych ze skanera laserowego

Dostosować i uruchomić przykład subscriber_scan.py.
Na podstawie odległości odczytanych ze skanera wyliczyć i narysować położenie przeszkód w układzie lokalnym robota (kartezjańskim).

Część 2 - wykrywanie linii

Zaimplementować jedną z wybranych metod wykrywania linii.
Dobrać parametry metody
Korzystając z danych zapisanych w plikach ocenić skuteczność i poprawność wykrywania linii.

Uwaga: Dla uproszczenia zadania można ograniczyć zakres analizowanych danych ze skanera przez zawężenie obserwowanego kąta i ograniczenie maksymalnej odległości.

Część 3 - lokalizacja robota

Dla wybranej konfiguracji środowiska zdefiniować globalny układ współrzędnych.
Wyznaczyć pozycję i orientację robota (x,y,θ) względem tak zdefiniowanego układu globalnego.
Ocenić poprawność uzyskanych rezultatów

Uwaga: oceny poprawności wyników dla danych odczytanych z robota można dokonać przez pomiar rzeczywistego położenia robota.

Część 4 - weryfikacja

Porównać pozycję i orientację wyliczone metodami: lokalizacji odometrycznej (z poprzedniego laboratorium), z wykorzystaniem znaczników (linii) z konfiguracją (pose) wyznaczaną przez wewnętrzny układ robota

Dodatkowe informacje

Dane pomiarowe

line_detection_1

line_detection_2

line_localization_1

Sprawozdanie

Sprawozdanie w formacie PDF należy przesłać pocztą elektroniczną przed terminem rozpoczęcia kolejnego zadania laboratoryjnego. Sprawozdanie powinno zawierać:

Krótki opis użytego algorytmu wykrywania linii
Przykładowe wyniki wykrywania linii
Opis metody lokalizowania robota na podstawie wyznaczonych linii
Przykładowe wyniki lokalizacji
Podsumowanie porównujące wyniki lokalizacji

Przykładowy kod

Odczyt danych ze skanera na robocie (subscriber_scan.py)


#!/usr/bin/env python

# run: python subscriber_scan.py 5  (where 5 is robot number)
import sys
import rospy
from sensor_msgs.msg import LaserScan
#import tf

def callback_scan(msg):
    ## extract ranges from message
    scan=list(msg.ranges)
    #print(" angle min %f max %f" % (msg.angle_min, msg.angle_max)
    #print("  Scan min: %f  front: %f" % ( min(scan),scan[256]))
    print(scan)

    
def listener():
    rospy.init_node('atr', anonymous=True)

    # Subscribe topics and bind with callback functions
    rospy.Subscriber("/PIONIER"+nr+"/scan", LaserScan, callback_scan)

    # spin() simply keeps python from exiting until this node is stopped
    rospy.spin()

if __name__ == '__main__':
    if len(sys.argv) < 2:
        sys.stderr.write('Usage: sys.argv[0] \n')
        sys.exit(1)

    nr=sys.argv[1]
    listener()

Przetwarzanie skanu: odczyt pliku json i wizualizacja

import json
##wczytanie danych z pliku
json_data = open('line_detection_1.json')
data = json.load(json_data)

### wykres danych ze skanera w ukladzie biegunowym
import matplotlib.pyplot as plt
import numpy as np

x = np.arange(0,512)
theta = (np.pi/512 )*(x-256)  # kat w radianach

# Wczytanie skanu z pierwszego zestawu danych
skan=data[0]["scan"]

fig1 = plt.figure()
ax1 = fig1.add_axes([0.1,0.1,0.8,0.8],polar=True)
line, = ax1.plot(theta,skan,lw=2.5)
ax1.set_ylim(0,2)  # zakres odleglosci
plt.show()

Zapis pliku json

Rysowanie linii

import numpy as np
import pylab as pl
from matplotlib import collections  as mc

lines = [[(0, 1), (1, 1)], [(2, 3), (3, 3)], [(1, 2), (1, 3)]]
c = np.array([(1, 0, 0, 1), (0, 1, 0, 1), (0, 0, 1, 1)])
lc = mc.LineCollection(lines, colors=c, linewidths=2)

lines2 = [[(0.1, 1.1), (1.1, 1.1)], [(2.1, 3.1), (3.1, 3.1)], [(1.1, 2.1), (1.1, 3.1)]]
lc2 = mc.LineCollection(lines2, colors=[0,0,0],linewidths=1)

fig, ax = pl.subplots()
ax.add_collection(lc)
ax.add_collection(lc2)
ax.autoscale()
ax.margins(0.1)

pl.show()