Czas na czwarty artykuł z cyklu o Jetson TK1, którego tematem jest przykładowa aplikacja użycia układu w robocie mobilnym.
Najprawdopodobniej nie jest to typowy artykuł, jaki można znaleźć na blogu, mimo to zapraszam na przygodę od instalacji narzędzi w ramach ROS do prawdziwej autonomicznej jazdy.
Poznaliśmy już układ od strony instalacji wymaganych narzędzi oraz konfiguracji do wygodnej pracy, zaznajomiliśmy się również z różnymi niskopoziomowymi interfejsami komunikacyjnymi.
Jednak na razie funkcjonalność niczym nie różni się znacząco od np. Raspberry Pi, a układ posiada moc, o której inne komputery wbudowane mogą pomarzyć i to postaram się pokazać w niniejszej części artykułu.
Na początku tego rozdziału przyjrzymy się dostępnej konfiguracji sprzętowej - przedstawię mobilną platformę, której będziemy nadawać drugie życie (albo raczej drugi mózg?). Następnie zaplanuję w jaki sposób można ulepszyć robota z wykorzystaniem układu Jetson TK1, po czym przejdziemy do rzeczywistych prac implementacyjnych.
Na początku omówię instalację systemu ROS, gdyż bez tego framework’a dzisiejsza robotyka byłaby biblijną wieżą Babel. Następnie skonfiguruję wszystkie gotowe moduły ROS do wspólnej pracy, a na koniec zostanie nam wisienka na torcie, czyli program wykorzystujący CUDA.
Platforma mobilna
Platformą mobilną, w którą tchniemy drugą młodość będzie robot firmy MobileRobots Pioneer 3-DX. Poniżej podano podstawowe informacje na temat owej platformy.
Robot firmy MobileRobots Pioneer 3-DX.
Robot w podstawowej wersji jest dość ubogi, jeżeli chodzi o system informatyczny. Ma wbudowaną płytkę z 32-bitowym mikroprocesorem, która realizuje wszystkie podstawowe zadania związane z lokomocją robota oraz podstawowymi sensorami takimi jak dołączone czujniki ultradźwiękowe oraz enkodery.
Platforma ma wiele zalet takich jak stosunkowo duża moc silników umożliwiająca poruszanie się nawet w trudnym terenie, dobrą odometrię i dużą społeczność. Widać jednak sporo ograniczeń w tego sprzętu, na przykład brak możliwości dołączenia jakiegokolwiek urządzenia o bardziej zaawansowanym interfejsie niż komunikacja na wejściach-wyjściach cyfrowych.
Zastosowany mikroprocesor nie ma wyprowadzonych żadnych innych interfejsów programowalnych oprócz kilku wejść wyjść cyfrowych i jeszcze mniej analogowych.
Dodatkowym problemem jest fakt, że płytka z mikroprocesorem nie może być wykorzystywana jako wysokopoziomowy sterownik robota – nie ma możliwości implementacji żadnego programu na ten sterownik. Działa on jedynie jako urządzenie typu SLAVE przyjmujące i realizujące konkretne rozkazy dla silników. Producent oferuje również możliwość dokupienia modułów umożliwiających zdalną komunikację ze sterownikiem robota, jednak cena jest zaporowa i skutecznie motywuje do realizacji tej funkcji na własną rękę.
Projekt PioneerV2
Wygląda na to, że płytka sterująca robotem jest przeznaczona do realizacji funkcji sterownika silników i tak ją będziemy traktować. Możemy zatem albo prowadzać robota na smyczy w postaci długiego kabla RS232 podłączonego do komputera stacjonarnego, albo dorobić garb Pioneerowi w postaci laptopa, który będzie mózgiem operacji, albo...
Pewnie już wszyscy wiedzą, o co chodzi. Albo dołożyć na pokład komputer z linuksem o niskim poborze prądu, a stosunkowo dużej mocy obliczeniowej umożliwiającej zarówno realizację całego programu już na robocie, jak i realizację sieci rozproszonej w filozofii ROS. Całe szczęście, że firma Mobile Robots wyprowadziła na płytce zasilającej dwa poziomy napięciowe: 5V (1.5A) oraz 12V (2.5A), a dla przypomnienia kto pamięta, jakiego zasilania potrzebuje Jetson TK1?
Wcześniej nie mogliśmy nic podłączyć do Pioneera, teraz większość sensorycznego świata stoi przed nami otworem, dlatego dołożymy jeszcze moduł IMU, by móc dokładniej śledzić pozycję robota w przestrzeni. Dokładamy jeszcze to, co powinien mieć każdy szanujący się robot, czyli skaner laserowy oraz Porsche wśród sensorów: ASUS Xtion Pro Live, by robot zaczął widzieć w 3D. Dla wygodnego sterowania robotem dodamy jeszcze bezprzewodowy pad od Xboxa.
Schemat blokowy rozbudowanej wersji robota.
Powyżej zamieszczono schemat systemu elektronicznego robota. Jest to schemat ogólny i w niniejszym zastosowaniu nie będziemy wykorzystywać IMU, ani skanera laserowego, czy joysticka - jedynie sensor RBG-D.
Bardzo ważną sprawą, nad którą spędziłem kilka dni główkowania, jest fakt, że wewnętrzny port RS232 dedykowanej płytki mikroprocesorowej, wymaga logicznej jedynki na sygnale DTR (pin 4 wtyczki DB9), czyli napięcia +5V.
Aby to wymaganie spełnić, połączono pin 4 oraz pin 9 (RI)
po stronie sterownika (domyślnie było tam właśnie napięcie +5V).
Na koniec warto powiedzieć, że istnieje możliwość podłączenia dwóch sensorów RGB-D do Jetsona, z tym że każdy z nich wymaga podłączenia na oddzielnej szynie USB, z powodu dużego wymaganego transferu danych (około 17 MB/s).
Krok 1: Instalacja ROS
Skoro już mamy projekt gotowy, to zabieramy się za implementację. Należy zainstalować ROS na Jetsonie. Wszystkie poniższe kroki zostały zaczerpnięte Wiki ROS.
Włączanie programu nasłuchującego stan klawiszy na padzie realizujemy poprzez włączenie node'a o nazwie joy:
Shell
1
ubuntu@tegra-ubuntu:~$rosrun joy joy_node
Aby sprawdzić, czy wszystko działa poprawnie możemy w kolejnej konsoli posłuchać, czy na topicu /joy publikowane są odpowiednie komendy oznaczające stan wszystkich przycisków pada:
Shell
1
ubuntu@tegra-ubuntu:~$rostopic echo/joy
Po wciśnięciu jakiegokolwiek klawisza powinny pokazać się wiadomość podobna do tej:
To oznacza, że ujarzmiliśmy pada i możemy przejść do kolejnego peryferium.
Krok 3: Obsługa skanera laserowego
Tutaj również jest gotowa paczka umożliwiająca wykorzystanie lasera hokuyo w systemie ROS (czy wspominałem już, że ROS jest cudowny?). Instalujemy potrzebną paczkę:
Bardzo ważnym urządzeniem, które można dołączyć do robota mobilnego, jest sensor głębi. Można dzięki niemu zobaczyć o jeden wymiar więcej niż we wcześniej przedstawionym skanerze laserowym, ile to jeden wymiar? Dużo. Gdybyśmy byli istotami 4-wymiarowymi podróże w czasie, byłyby dla nas równie łatwe co wyjście do sklepu po bułki!
Korzystamy z gotowej paczki o nazwie openni2, instalujemy ją za pomocą polecenia:
Następnie w kolejnej konsoli nasłuchujemy topic przykładowo /camera/rgb/image_raw, jednak nie będę tutaj wklejał tego, co wyskoczyło w konsoli (30 klatek na sekundę, każda o wymiarach 640x480 oraz 3 kanałach). Wiadomo, że sensor działa poprawnie i możemy go wykorzystywać do ambitnych zastosowań.
Krok 5: Komunikacja ze sterownikiem robota
Robot jest skomplikowanym układem, zawiera szereg parametrów zapisanych w nieulotnej pamięci flash, różne tryby komunikacji i samej pracy. Biblioteka do komunikacji z robotem musi być odpowiednio skomplikowana, by umożliwiała dostęp do wszystkich możliwości robota, nie należy jednak się obawiać, ponieważ podstawowa konfiguracja jest bardzo prosta i na nasze potrzeby zupełnie wystarczająca.
Na początku instalujemy potrzebne pakiety za pomocą apt-get.
Aby sprawdzić, czy sprzętowo wszystko działa poprawnie, możemy wykorzystać demo, czyli jeden ze ściągniętych pakietów. Program zapisany jest w katalogu /usr/bin, jednak ścieżka ta jest dodana do zmiennej systemowej $PATH, dlatego możemy w każdym miejscu go wywołać z parametrem -rpo wartości /dev/ttyS0, czyli specyfikacji portu gdzie podłączony jest robot:
C
1
ubuntu@tegra-ubuntu:~$aria-demo-rp/dev/ttyS0
Można zauważyć wynik działania programu:
C
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
Could notconnect tosimulator,connecting torobot through serial port/dev/ttyS0.
ArACTS_1_2:Could notconnect toACTS running on localhost:5001(Connection refused.)
You can dothese actions with these keys:
quit:escape
help:'h'or'H'or'?'or'/'
You can switchtoother modes with these keys:
teleop mode:'t'or'T'
unguarded teleop mode:'u'or'U'
wander mode:'w'or'W'
laser mode:'l'or'L'
bumps mode:'b'or'B'
sonar mode:'s'or'S'
position mode:'p'or'P'
camera mode:'c'or'C'
command mode:'d'or'D'
report robot config mode:'o'or'O'
io mode:'i'or'I'
tcm2 mode:'m'or'M'
acts mode:'a'or'A'
You are in'teleop'modecurrently.
Teleop modewill drive under your joystick orkeyboard control.
It will notallow you todrive into obstacles it can see,
though ifyou are presistent you may be able torun into something.
Forjoystick,hold inthe trigger button andthenmove the joystick todrive.
Forkeyboard control these are the keys andtheir actions:
up arrow:speed up ifforward orno motion,slow down ifgoing backwards
down arrow:slow down ifgoing forwards,speed up ifbackward orno motion
left arrow:turn left
right arrow:turn right
space bar:stop
e:(re)enable motors
transVel rotVelxyth volts
00000.013.6
00000.013.6
Czyli wszystko dobrze, teraz możemy sterować robotem za pomocą strzałek, swoją drogą dobra zabawa, ale to tylko demo i aby mieć pełną kontrolę nad robotem możemy wykorzystać paczkę rosaria, czyli pomostu między ROS, a ARIA:
Bardzo ważne jest /RosAria/cmd_vel, czyli miejsce gdzie wysyłamy komendy prędkości robota. Użycie jest bardzo podobne do kontroli innych robotów w filozofii ROS (przykładowo turtlebot'a z tutoriala na stronie ROS_TUTORIALS).
Koniec konfiguracji.
Teraz warto przemyśleć założenia pierwszego programu.
Krok 6: Własny program: założenia i odrobina teorii
Sporo myślałem nad tym, jaki program zaproponować, by był on ciekawy zarówno od strony algorytmicznej, jak i użytkowej. Wybór padł na zadanie śledzenia znacznika z wykorzystaniem sensora RGB-D i krótko chciałbym opowiedzieć Wam, dlaczego jest to ciekawe z obu perspektyw.
Każda technologia ma swoją niszę, gdzie gra pierwsze skrzypce. Jeżeli chcemy pokazać siłę technologii CUDA, możemy spróbować zrealizować jakieś zadanie wizyjne. Wizja komputerowa jest o tyle skomplikowana, że zawiera dużą ilość informacji - każda klatka ma wymiary kilkaset na kilkaset pikseli i aby obraz był płynny, liczba klatek na sekundę musi być jak największa.
Procesor CPU preferuje trudne zadania obliczeniowe,
a nie kilkaset tysięcy małych zadanek.
Wizyjnie będziemy bazować głównie na obrazie RGB, z którego wydobędziemy informację o położeniu znacznika o określonym kolorze, a następnie skorzystamy z informacji o głębi w tym miejscu. Nie będziemy mocno skupiać się na samej metodzie wizyjnej, ponieważ nie jest to sedno tego artykułu. Krótko można zaznaczyć, jak będzie wyglądał tor przetwarzania informacji:
RGB: Konwersja obrazu do przestrzeni HSV
RGB: Progowanie obrazu wedle wcześniej wyznaczonych parametrów znacznika
RGB: Rozmycie oraz morfologiczne zamknięcie obrazu
RGB: Znalezienie pozycji XY największego konturu (znacznika)
DEPTH: Znalezienie odległości Z w pozycji z punktu 4tego
Następnie zrealizujemy prosty regulator P, gdzie odległość pozycji XY znacznika od środka obrazu RGB będzie uchybem do regulatora kierunku, zaś odległość Z do znacznika będzie uchybem regulatora prędkości. Wszystko jest dokładnie opisane w kodzie: JETSON_FOLLOWER.
Krok 7: Własny program: znacznik oraz konfiguracja
Zanim przejdziemy do omówienia właściwego programu, zaczniemy od znalezienia odpowiedniego znacznika. W prawdziwych zastosowaniach robotycznych profesjonalnie byłoby rozpoznawać ludzkie nogi, jako naturalny znacznik. Nie jest to jednak nasz główny cel, dlatego zastosujemy prostsze rozwiązanie ze sztucznym znacznikiem.
Dobór znacznika ma tutaj duże znaczenie. Nasza metoda będzie bazowała na znalezieniu największego elementu o danym kolorze, więc warto postarać się, by wybrany przez nas znacznik miał wystarczająco oryginalny kolor. Aby spełnić to założenie, wybrałem się do pobliskiego sklepu i kupiłem opaskę odblaskową w żółtym kolorze.
Opaska okazała się zbyt błyszcząca, więc zdarłem wierzchnią warstwę materiału i pomalowałem markerem permanentnym w kolorze niebieskim.
Opaska wykorzystywana podczas dalszej części projektu.
Kolejnym krokiem było napisanie prostego programu, w którym moglibyśmy przesuwać suwakami we wszystkich trzech kolorach H, S, V (po konwersji). Wykorzystałem do tego technologię ROS + Qt + OpenCV. Instalacja paczki oraz uruchomienie programu kalibrującego przebiega następująco:
Kalibracja została przeprowadzona na poniższej scenie.
Kalibracja systemu.
Po włączeniu programu ukazuje się okno z domyślnie ustawionymi na sztywno parametrami (po lewej). Po odpowiednim dobraniu parametrów, tak aby na biało był widoczny jedynie marker klikamy zapisz i parametry HSV zostają zapisane do pliku w katalogu config paczki (po prawej).
Tak ustawione parametry są jak najbardziej w porządku mimo występujących na znaczniku dziur - po to właśnie jest dokonywana operacja zamknięcia, by takie błędy zminimalizować.
Krok 8: Własny program: implementacja szeregowa
Właściwy program jest podzielony na dwa moduły: w pliku main_follower.cpp znajduje się część programu sterująca robotem na podstawie danych otrzymanych z modułu drugiego, czyli właściwego programu znajdującego pozycję znacznika.
Program sterujący został wyciągnięty do pliku main, by ewentualnie ktoś chcący powielić funkcjonalność na swoim robocie, mógł dobrać parametry sterowań wedle własnych potrzeb. Samo sterowanie to dwa proste regulatory P, gdzie prędkość liniowa jest regulowana na podstawie uchybu odległości do znacznika, zaś prędkość obrotowa na podstawie uchybu pozycji znacznika względem środka obrazu.
Wszystkie parametry, czyli wzmocnienie, martwa strefa oraz tematy do publikowania każdy musiałby uzupełnić wedle własnych potrzeb.
Właściwa część programu to klasa Follower, gdzie użytkownik ma do dyspozycji jedną metodę: findMarker. Wewnętrznie są wczytywane parametry HSV oraz pobierane obrazy RGB i głębi z urządzenia, zaś po wywołaniu wcześniej wspomnianej metody następuje właściwe szukanie największego obiektu o danym kolorze.
Dla wygody użytkownika są wyprowadzone dwa parametry w pliku follower.cpp: ITERATIONS (liczba iteracji, na podstawie której obliczany jest czas działania programu - więcej w kroku 9) oraz DEBUG (parametr pozwalający wyświetlić podgląd poszczególnych etapów przetwarzania obrazu: progowanie w przestrzeni HSV, operacje morfologiczne, znaleziony największy kontur).
Krok 9: Własny program: implementacja równoległa
Programowanie równoległe ma to do siebie, że wydajne programy pisze się bardzo długo i zrównoleglenie całego kodu rozpoznającego zajęłoby bardzo dużo czasu, dlatego na potrzeby demonstracji zdecydowałem się ograniczenie zasięgu, gdzie będziemy używać technologii CUDA.
Zajmiemy się pierwszą częścią przetwarzania, czyli konwersję obrazu RGB do obrazu w przestrzeni HSV, a następnie jego progowaniem w zależności od wczytanych parametrów.
Sama technologia CUDA jest dość skomplikowana i nie chcę wchodzić w dokładne szczegóły sprzętowe oraz implementacyjne - to temat na zupełnie oddzielny cykl artykułów. Krótko ujmując, model programistyczny biblioteki CUDA zakłada, że procesor CPU jest elementem nadrzędnym (host) względem jednostki obliczeniowej GPU (device).
Gospodarz ma dwa duże zadania: pierwszym jest przygotowanie danych do obróbki oraz skopiowanie ich do pamięci urządzenia, zaś drugim wysłanie instrukcji wywołania jądra obliczeniowego (kernel), czyli programu realizowanego w technologii równoległej już na procesorze GPU. Po przetworzeniu zachodzi potrzeba skopiowania pamięci z urządzenia do gospodarza oraz dealokacja pamięci GPU (wszystko inicjowane na rozkaz CPU).
Jądro jest pisane w taki sposób, jakby było wykonywane w sposób szeregowy przez pojedynczy wątek. Każde wywołanie jądra jest realizowane przez wiele równoległych wątków, które są uporządkowane w bloku (blocks), a następnie w siatkę (grid).
Ideowy schemat organizacji wątków, bloków oraz siatki bloków można obejrzeć poniżej:
Przechodząc do konkretów, obraz, który będzie przetwarzany jest rozmiaru 640x480 pikseli, gdzie każdy piksel zajmuje 3 bajty, zaś wyjściem algorytmu będzie obraz o takiej samej wielkości, ale jednokanałowy. Według tego, co zostało powiedziane powyżej, na początku musimy zaalokować pamięć na urządzeniu. Realizujemy to za pomocą dostarczonych przez CUDA funkcji podobnych do funkcji mallocznanych z języka C:
Póki co jest dość intuicyjnie. Teraz następuje deklaracja konfiguracji, w jakiej będzie uruchamiane jądro obliczeniowe. Blok definiujemy jako kwadrat o boku 32 wątków, gdzie jeden wątek będzie obsługiwał jeden piksel obrazu. Aby przetworzyć cały obraz musimy zdefiniować tyle bloków wątków, by pokryć cały obraz:
Pierwszy kernel dokonuje konwersji RGB->HSV, gdzie obliczone wartości pikseli w przestrzeni hsv są zapisywane tam gdzie dane wejściowe. Drugi zaś stanowi prostą realizację operacji progowania na podstawie danych parametrów HSV - wyjście jest okrojone o dwa kanały, dlatego zapisywane jest w miejscu d_data_out.
Po obliczeniu wynikowy, sprogowany obraz trzeba skopiować z powrotem do pamięci CPU:
Na końcu dealokacja pamięci zaalokowanej na urządzeniu:
C
1
2
3
// Deallocation
cudaFree((void*)d_data_in);
cudaFree((void*)d_data_out);
Od razu bardzo mocno muszę zaznaczyć, że niektóre pokazane mechanizmy z punktu widzenia efektywności działania programu nie mają sensu i mają jedynie charakter dydaktyczny.
Przykładowo alokacja oraz dealokacja pamięci na GPU mogłaby być dokonywana raz przy inicjacji systemu oraz przy zamknięciu programu, a nie podczas przetwarzania każdej klatki jak w pokazanym przykładzie. To oczywiście nie jedyna możliwość przyspieszenia działania, a po szczegóły odsyłam do ReferenceManual'a.
Krok 10: Własny program: demonstracja
Teraz coś co tygryski lubią najbardziej, czyli demonstracja. Zakładam, że parametry HSV mamy już dobrane, więc możemy włączyć program na robocie. Kilka słów komentarza. Nie oceniajcie możliwości Jetsona po braku płynności jazdy robota - jest to kwestia bardzo prostego regulatora, nasz układ sobie poradziłby bez żadnego problemu z zaawansowanymi sposobami sterowania, ale to nie był główny temat tego artykułu.
Drugim problemem, przy takiej realizacji jest również wąski kąt widzenia kamery ASUS. Bardzo łatwo było wyjść znacznikiem poza zakres widzenia, a ponieważ nie zaimplementowałem żadnych zaawansowanych technik sterowania, znajdowany był jakikolwiek inny kontur i robot podążał w jego kierunku. To wszystko są szczegóły techniczne, ważne z punktu widzenia prawdziwego projektu, jednak z demonstracyjnej strony możemy być w pełni zadowoleni z takiego sposobu działania:
Teraz dla wiadomości można porównać szybkość wykonywania tej samej operacji na CPU oraz GPU na Jetsonie. Czas jest uśredniany poprzez daną liczbę iteracji (ja wybrałem liczbę 100) - wersja szeregowa zajęła około 60.1 ms, zaś wersja równoległa około 21.3 ms.
Jak widać, już na etapie najprostszej implementacji równoległej
algorytm potrzebuje 3-krotnie mniej czasu.
Podsumowanie
Cieszę się, że dotarliście ze mną do końca cyklu i mam nadzieję, że przekonałem Was o możliwościach, które otwierają się przed robotykami dzięki układowi Jetson TK1. Jest on nie tylko Linuxem wraz ze wszystkimi dobrami z tym związanymi, ale również potężnym układem do przetwarzania danych. Dla mnie jest to najlepszy wybór (brak lokowania produktu).
Warto jednak zauważyć, że nie każde zadanie jest ze swojej natury dostosowane do realizacji w sposób równoległy i jest to bardzo ważny element, który trzeba mieć na uwadze projektując każdy system robotyczny. Oczywiście układ może być wykorzystany w innych dziedzinach techniki, z powodzeniem można go użyć do zestawienia serwera domowego, układu do kompresji audio/wideo, czy jako komputer do dydaktyki. A jakie Wy widzicie zastosowanie dla Jetsona?
Dołącz do 20 tysięcy osób, które otrzymują powiadomienia o nowych artykułach! Zapisz się, a otrzymasz PDF-y ze ściągami (m.in. na temat mocy, tranzystorów, diod i schematów) oraz listę inspirujących DIY na bazie Arduino i Raspberry Pi.
To nie koniec, sprawdź również
Przeczytaj powiązane artykuły oraz aktualnie popularne wpisy lub losuj inny artykuł »
Dołącz do 20 tysięcy osób, które otrzymują powiadomienia o nowych artykułach! Zapisz się, a otrzymasz PDF-y ze ściągami (m.in. na temat mocy, tranzystorów, diod i schematów) oraz listę inspirujących DIY z Arduino i RPi.
Trwa ładowanie komentarzy...