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.

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 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.

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.

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.

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.

Na początku dodajemy źródła apt-get:

Dodajemy klucze apt-get:

Upewniamy się, że lista pakietów jest odświeżona:

Nie potrzebujemy całego ROSa (wiele paczek nie będzie działało lub w ogóle nie jest zbudowanych dla architektury arm) instalujemy samą bazę ROSa:

Na koniec należy jeszcze zainicjować narzędzie rosdep:

Oraz ustawić automatyczne eksportowanie zmiennych środowiskowych ROS dla każdej nowo włączonej konsoli:

Krok 2: Obsługa joysticka

W ramach ROSa jest gotowa paczka umożliwiająca wykorzystanie pada. Należy ją ściągnąć za pomocą komendy:

Włączanie programu nasłuchującego stan klawiszy na padzie realizujemy poprzez włączenie node'a o nazwie joy:

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:

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ę:

Następnie możemy włączyć program komunikujący się z laserem i publikujący w systemie ROS informacje o tym, co widzi skaner:

Następnie w kolejnej konsoli nasłuchujemy topic /scan:

Powinniśmy zobaczyć coś na kształt takiej informacji:

Co oznacza, że i skaner działa poprawnie!

Krok 4: Obsługa sensora RGB-D Asus Xtion Pro Live

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 możemy włączyć program komunikujący się z sensorem i publikujący w systemie ROS informacje o tym, co widzi Asus:

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 -rp o wartości /dev/ttyS0, czyli specyfikacji portu gdzie podłączony jest robot:

Można zauważyć wynik działania programu:

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:

Teraz należy jedynie zaktualizować informację o dostępnych programach wewnątrz naszego workspace'a (o ile wcześniej tego nie zrobiliśmy):

Teraz wielka chwila, czyli włączamy program wewnątrz ROSa, który będzie nasłuchiwał komend m.in. prędkości oraz wysyłał stan czujników itd.:

Możemy podejrzeć publikowane wiadomości:

Następnie przykładowo dane z sensorów ultradźwiękowych:

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).

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.

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:

1. RGB:     Konwersja obrazu do przestrzeni HSV
2. RGB:     Progowanie obrazu wedle wcześniej wyznaczonych parametrów znacznika
3. RGB:     Rozmycie oraz morfologiczne zamknięcie obrazu
4. RGB:     Znalezienie pozycji XY największego konturu (znacznika)
5. 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.

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.

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.

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.

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 malloc znanych z języka C:

Następnie kopiowanie pamięci z ramu do pamięci urządzenia:

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:

Następnie wywołanie samych kerneli przez:

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:

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 Reference Manual'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.

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?

Autor kursu: Daniel (danioto) Koguciuk
Redakcja: Damian (Treker) Szymański

jetson, kursTK1, Pioneer, tk1