Algorytmy robotyki mobilnej – odometria

Wprowadzenie

Celem zadania jest zapoznanie się z obsługą robotów Pioneer 3DX dostępnych w laboratorium L1.5, dostępem do danych z sensorów bezpośrednio z tematu ROS (ewentualnie za pośrednictwem biblioteki RosAriaDriver – rozwiązanie niezalecane), a następnie implementacja oraz obserwacja własności odometrycznej lokalizacji robota.

Przygotowanie

Przed zajęciami należy zapoznać się z opisem zadania oraz materiałami pomocniczymi w podanym niżej zakresie a także przygotować co następuje:

zapoznać się z instrukcjami BHP: ogólną dla laboratorium oraz przy pracy z robotami Pioneer,
w dokumentacji robota odnaleźć parametry niezbędne do wyznaczania przemieszczenia robota,
przypomnieć potrzebne wzory do wyznaczania przemieszczenia robota metodą odometrii,
przyjrzeć się przykładom załączonym do opisu, zweryfikować możliwość odczytu danych z przykładowych plików i dostępu do potrzebnych wartości.

Część 1 – implementacja

Zweryfikować za pomocą pomiarów wartości uzyskane z dokumentacji robota. W sprawozdaniu oszacować dokładność pomiarów i ich wpływ na dokładność odometrii.
Napisać program do lokalizacji robota za pomocą odometrii. Program powinien wyznaczać pozycję i orientację robota (x,y,θ) na podstawie pomiarów pozycji enkoderów lub prędkości kół.
- Zweryfikować działanie programu przy pomocy plików z danymi, rozpoczynając od elementarnych przesunięć i obrotów, a następnie dla ruchu po kwadracie.
- Uruchomić algorytm dla danych zbieranych online z robota wykorzystując odczyt danych z tematu ROSa, lub bibliotekę RosAriaDriver.
Przeprowadzić eksperyment umożliwiający określenie błędu odometrii.
- Zdefiniować wartości, które powinny być obserwowane/mierzone.
- Zaplanować trajektorię robota.
- Wykonać eksperymenty i zebrać wyniki

Część 2 - weryfikacja

W części weryfikacyjnej można wykorzystać zarówno dane z dołączonych plików, jak i zebrane podczas eksperymentu w części 1.

Porównać pozycję i orientację wyliczoną przez program z części pierwszej z pozycją i orientacją (pose) wyznaczaną przez wewnętrzny układ robota
Zakładając, że wyznaczona przez robota konfiguracja jest dokładna, oszacować błąd odometrii i jego zmianę w czasie.

Dodatkowe informacje

Konfiguracja laboratorium

Komputery w laboratorium połączone są z centalnym serwerem roscore uruchamianym na komputerze prowadzącego. Roboty Pioneer łacząc się z serwerem tworzą własną przestrzeń nazw /PIONIERX, gdzie X jest numerem robota. Listę dostępnych tematów, w których publikowane są dane można uzyskać poleceniem rostopic list a dane publikowane w danym temacie można wyświetlić poleceniem rostopic echo topicname

Dane pomiarowe

W trakcie realizacji ćwiczenia potrzebne są następujące dane pozyskane z czujników robota: odczyty enkoderów (położenia lub prędkości), pozycja robota z wewnętrznego systemu lokalizacji (położenie i orientacja).

Do testowania programów można korzystać z następujących źródeł danych:

Przykładowe dane zapisane w plikach CSV
Przykładowe dane zapisane w plikach rosbag
Dane online z robotów za pośrednictwem RosAriaDriver
- Dane z enkoderów uzyskuje się za pomocą funkcji ReadEncoder() która zwraca tablicę [(posL,posR),(velL,velR)], z licznikiem impulsów enkoderów typu long int oraz prędkościami kół typu double.
- Pozycję robota uzyskujemy za pomocą funkcji ReadPose() która zwraca tablicę [x,y,th], z położeniem w metrach i orientacją w stopniach.
Przykład użycia biblioteki w kodzie załączonym poniżej.
Dane online z subskypcji odpowiedniego tematu ROS.

Sprawozdanie

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

Opis użytego algorytmu do lokalizacji odometrycznej, wraz z zastosowanymi wzorami oraz wartościami wyznaczonych bądź zmierzonych parametrów.
Oszacowanie teoretycznej wartości błędu odometrii (w pojedynczym kroku)
Komentarz do zebranych obserwacji
Podsumowanie weryfikacji algorytmu z części 2.

Przykładowy kod

Pomiar czasu

import datetime
a=datetime.datetime.now()
b=datetime.datetime.now()
c=b-a
c.microseconds
c.seconds
c

Odczyt pliku CSV

import csv

with open('forward.csv') as csvfile:
    reader = csv.DictReader(csvfile, delimiter=';', quotechar='|')
    for row in reader:        
        print row['posL'], row['posR']

Odczyt enkoderów z użyciem biblioteki RosAriaDriver (niezalecane!)

from drive import RosAriaDriver
robot=RosAriaDriver('/PIONIERX') # w miejsce X wstaw numer robota

[(posL,posR),(vL,vR)]=robot.ReadEncoder()
print posL, posR