Wizualizer Robot Wizualizer - wizualizacja robota Micromouse (ROS i RViZ)

[Wirgiliusz]

W trakcie budowy ostatniego robota zdałem sobie sprawę jak bardzo przydałoby mi się coś, co pozwalałoby wizualizować to co dzieje się w robocie w trakcie jego testowania. Ze względu, że mój robot Micromouse (którego budowę opisałem tutaj: Micromouse Robot - robot typu micromouse) wyposażony był w wyświetlacz LCD oraz moduł Bluetooth to był to idealny powód aby ich użyć.

W taki sposób zrodził się pomysł na drugi projekt, który nazwałem "Robot Wizualizer - wizualizacja robota Micromouse". 

Projekt składa się z 2 części:

1. Wyświetlacz LCD - na wyświetlaczu chciałem ukazać aktualne parametry robota takie jak: ustawiona prędkość na silnikach, pozycja w labiryncie i odczyty z czujników. Dodatkowo chciałem aby wyświetlacz wizualizował labirynt, który poznaje i przeszukuje robot.
2. Wizualizacja komputerowa z środowiskiem ROS i RViZ - na podstawie danych przesyłanych przez robota do komputera miała powstać 3-wymiarowa wizualizacja labiryntu wraz z aktualnym położeniem robota.

WYŚWIETLACZ LCD

Ze względu, że płytką którą użyłem przy konstrukcji robota było STM32F429I-DISC1 to użytym wyświetlaczem był QVGA TFT LCD wbudowany w płytkę. 

Schemat elektroniczny projektu wygląda następująco:

Jak widać, jeśli chodzi o wyświetlacz LCD to jest on podpięty w płytce wewnętrznie, jedyne co należy zrobić to użyć odpowiednich pinów. Reszta zadania polegała na podpięciu modułu bluetooth (HC-06) oraz w moim przypadku użycia prostego stabilizatora L7805 aby zasilić płytkę 5V.

Interfejsy, które wykorzystałem do obsługi wyświetlacza to: DMA2D z Color Mode ustawionym na RGB565 w celu komunikacji pomiędzy mikrokontrolerem a wyświetlaczem, LTDC z rozdzielczością ustawioną na 320x240 do obsługi wyświetlacza i SPI do jego konfiguracji. Wszystkie elementy skonfigurowałem w środowisku STM32CubeIDE.

Biblioteką, którą użyłem do sterowania wyświetlaczem jest sterownik STM32F429I_DISCOVERY_LCD dostarczany przez ST, który działa na podstawie sterownika wyświetlacza ILI9341.

Użycie powyższej biblioteki sprawia, że praca z wyświetlaczem LCD staje się prosta, gdyż do dyspozycji mamy wiele funkcji, którymi możemy obsługiwać wyświetlacz. Po wykonaniu inicjalizacji oraz włączeniu wyświetlacza:

wyświetlacz obsługujemy funkcjami typu:

Pierwsze z nich pozwalają na rysowanie prostych kształtów o wybranym kolorze, w tym przypadku białego prostokąta w pozycji 0,0 i wielkości 239x239. Natomiast drugi zestaw odpowiada za wybranie białego koloru czcionki i wyświetleniu napisu w zadanej pozycji (70,260) wyrównanego do lewej. 

Niestety nie udało mi się znaleźć dokumentacji, która opisywałaby wszystkie funkcje. Osobiście znalazłem przydatne dla siebie funkcje przeszukując plik nagłówkowy biblioteki i testując ich działanie. Ich nazwy są całkiem adekwatne do tego co robią, więc nie było to niemożliwe zadanie.

Po przetestowaniu działania wyświetlacza przyszedł czas na zaplanowanie elementów graficznych, które zostaną przedstawione na wyświetlaczu. Ze względu, że nie jestem wyrafinowanym grafikiem to zdecydowałem się na schematyczne ukazanie pożądanych informacji:

Kolejne pola labiryntu zostały oznaczone kwadratami a możliwe ścieżki pomiędzy nimi to po prostu linie łączące kolejne pola. Dodatkowo pole, w którym aktualnie znajduje się robot miało być zaznaczane innym kolorem.

Po podziale wyświetlacza na równe części i zastosowaniu odrobiny matematyki udało mi się napisać następujące funkcje:

Pierwsza z nich rysuje bezbarwne pole dla zadanego położenia robota, natomiast druga pole kolorowe. Położenie robota zdefiniowałem jako 2 współrzędne (x,y) gdzie kolejne wartości całkowite (0, 1, .., n-1; gdzie nxn to rozmiar labiryntu) wskazują na pola labiryntu. Układ współrzędnych umieszczony jest w lewym górnym rogu, więc jeśli z tego punktu robot zacznie swoją trasę i przemieści się dwa razy na wschód, a potem raz na południe to jego współrzędne będą wynosiły (2,1). Zastosowanie współczynnika wielkości (WSP_WIELKOSCI) pozwala wykorzystać moje funkcje rysujące dla labiryntu dowolnej wielkości - podczas zwiększania się wymiarów labiryntu, rysowane pola są mniejsze, jednak nadal mieszczą się na wyświetlaczu.

Następnie zaimplementowałem funkcję rysującą istniejące połączenia dla danego pola, dzięki czemu robot wraz z przejeżdżaniem labiryntu może od razu wizualizować, które ścieżki odkrył:

W tym przypadku elementy także są skalowane w zależności od wielkości labiryntu.

W tym momencie zauważyłem, że kwadratowa plansza labiryntu wykorzystuje tylko część prostokątnego wyświetlacza i nadal pozostaje na nim wolne miejsce - dlatego postanowiłem je wykorzystać. Stworzyłem kolejne funkcje, tym razem działające na tekście, które wypisywały informacje o robocie (prędkość, pozycja, czujniki). Implementacja przykładowej z nich wygląda następująco:

Jak widać cały czas posługuje się funkcjami, które przytoczyłem wcześniej i które zapewnia użyta biblioteka do wyświetlacza. Na końcu dodałem jeszcze ramkę aby oddzielić część wizualną labiryntu od reszty danych i o to efekt (zdjęcie 1: labirynt 4x4, zdjęcie 2: labirynt 16x16):

WIZUALIZACJA RVIZ

Ta sekcja będzie mocno programistyczna i składać się będzie z 3 głównych programów: robota, skryptu w Pythonie oraz programu w C++. Jeśli ktoś jest zainteresowany dokładniejszym zrozumieniem działania każdego z nich, to polecam w trakcie czytania śledzenia poniższego diagramu, na którym przedstawione zostały schematy blokowe właśnie tych programów oraz relacje między nimi:

ROBOT - wysyłanie danych

Drugą część projektu rozpocząłem od skonfigurowania interfejsu USART w celu komunikacji mikrokontrolera z modułem bluetooth.

Na podstawie skonfigurowanego interfejsu napisałem funkcje przesyłające przez niego dane do modułu:

Dodatkowo użyłem przerwania wywoływanego przez interfejs w przypadku otrzymania informacji. W ten sposób jeśli otrzymaną informacją była np. komenda stopu to robot mógł zareagować natychmiastowo, bez potrzeby kończenia aktualnie wykonywanej akcji.

Kolejnym krokiem była instalacja środowiska ROS oraz RViZ na moim laptopie. Wszystko odbyło się przy pomocy instrukcji ze strony producenta: https://www.ros.org/.

W pierwszej kolejności moją uwagę poświęciłem stworzeniu prostego modelu robota. Do tego celu potrzebna okazała się wiedza o budowie plików .urdf wykorzystywanych w środowisku. Po zebraniu wiedzy z kilku poradników powstał pierwszy model:

PYTHON - przekonwertowanie i przekazanie danych

Następnym krokiem było wyrysowanie wszystkich elementów graficznych na scenie. W tym celu pierwszym krokiem stało się odebranie danych od robota przez komputer. Aby spełnić to zadanie posłużyłem się językiem Python i biblioteką pybluez, która umożliwia komunikację przez bluetooth. 

Napisany skrypt, w pierwszej kolejności tworzy wydawcę (publishera), który pozwala przesyłać dane do oprogramowania ROS. Następnie pyta użytkownika o tryb pracy: manualny czy zdalny. Tryb manualny polega na ciągłym odczycie komend wpisanych przez użytkownika, natomiast tryb zdalny łączy się z modułem bluetooth i cyklicznie odczytuje otrzymane dane z modułu. Niezależnie od wybranego trybu skrypt przesyła otrzymane dane do środowiska ROS. Wycinki skryptu, które realizują powyższe zadania wyglądają następująco:

C++ - odebranie i wykorzystanie danych

Kolejnym ogniwem był program napisany w języku C++, który pozwalałby wizualizować otrzymane dane w programie RViZ. W związku z tym tworzy on subskrybenta (subscribera), który łączy się z publisherem wcześniej stworzonym w skrypcie tworząc węzeł komunikacyjny. Następnie program tworzy znaczniki (markery) umożliwiające wizualizację w RViZie. Postanowiłem użyć dwóch rodzajów markerów: linii do rysowania ścian labiryntu oraz sześcianu do reprezentowania robota. Końcowo program w pętli oczekuje na odbiór danych, jeśli to się stanie to następuje przerwanie i w zależności od rodzaju danych wykonywane są odpowiednie funkcje aktualizujące pozycję lub orientację modeli. Wycinek opisanych funkcji wygląda następująco:

Końcowy efekt prezentuje się następująco (na zdjęciu widoczne użycie w trybie manualnym):

PODSUMOWANIE

Myślę, że projekt ten wyszedł całkiem nieźle, w szczególności, że była to moja pierwsza styczność z wyświetlaczem LCD oraz oprogramowaniem ROS. Spora jego część jest czysto programistyczna, jednak mam nadzieję, że i tak kogoś zaciekawił.

Jeśli kogoś interesują szczegóły to zapraszam do repozytorium https://github.com/Wirgiliusz/Micromouse-Robot-Wizualizer, w którym można znaleźć wszystkie wspomniane przeze mnie pliki oraz projekt w STM32CubeIDE, który zawiera zarówno konfiguracje samego robota Micromouse jak i wyświetlacza LCD.

Na koniec jeszcze filmik pokazujący działanie wszystkich elementów w czasie rzeczywistym:

 

[Treker (Damian Szymański)]

@Wirgiliusz bardzo fajny projekt i poradniki - dzięki za opis!

Jeśli się nie mylę to jest to jedno z pierwszych praktycznych zastosowań dla ROS, jakie pojawiło się na naszym forum 🙂

[wn2001]

Duże WOW 🙂 zarówno za samego robota jak i część programistyczną z ROS-em 🙂 Mam pytanie, biblioteka do LCD to ta - https://stm32f4-discovery.net/2014/04/library-08-ili9341-lcd-on-stm32f429-discovery-board/ czy może inna? Nie wiedziałem, że może być to takie proste, nie wiem skąd fałszywe przekonanie, że trzeba używać programów typu GFX Designer i znać się na pseudo-wątkowości (stąd płytka poszła w kąt), a tu można użyć tego wyświetlacza tak jak biblioteki w Arduino 🙂

Odnośnie ROS-a; to chyba jeden z pierwszych projektów, ale jak napisał @Treker - był też ten 😉

Mimo to pierwszy raz w życiu widzę ROS w MM 😄 Czy jego wykorzystanie ma charakter edukacyjny i wyłącznie do wizualizacji, czy planujesz dalej rozwijać ten wątek? Wybacz może naiwne pytanie, ale czy ROS sam stworzył sobie model labiryntu? Ponieważ na filmie tego nie widać i ma się wrażenie, jakby był wprowadzony "z góry" 🙂

Edytowano Październik 28, 2020 przez wn2001

[Wirgiliusz]

10 godzin temu, wn2001 napisał:

Mam pytanie, biblioteka do LCD to ta - https://stm32f4-discovery.net/2014/04/library-08-ili9341-lcd-on-stm32f429-discovery-board/ czy może inna?

Szczerze mówiąc nie jestem do końca pewny, ale wydaje mi się, że użyłem po prostu biblioteki BSP, którą można zainstalować bezpośrednio z CubeIDE dla tej płytki. Jednak to co wysłałeś wygląda bardzo podobnie, obstawiam, że też będzie działało. Wydaje mi się że dużo osób tworzy jakieś pochodne biblioteki dla tego wyświetlacza ale działają one na tej samej zasadzie.

 

10 godzin temu, wn2001 napisał:

Czy jego wykorzystanie ma charakter edukacyjny i wyłącznie do wizualizacji, czy planujesz dalej rozwijać ten wątek? Wybacz może naiwne pytanie, ale czy ROS sam stworzył sobie model labiryntu? Ponieważ na filmie tego nie widać i ma się wrażenie, jakby był wprowadzony "z góry" 🙂

Projekt miał głównie charakter edukacyjny i jak na razie nie zamierzam go rozwijać. Jeśli chodzi o labirynt to tak, jest on wgrany "z góry" - na początku miałem plan aby rysował się on na podstawie odczytów z robota, jednak samo nauczenie się zrobienia czegokolwiek w ROSie i RViZie zajęło o wiele więcej czasu i wysiłku niż się spodziewałem, więc potem zacząłem upraszczać wizualizację.

Nie zmienia to faktu, że jest to możliwe i pewnie nie aż tak bardzo trudne w porównaniu do tego co zrobiłem, jednak mi po prostu zabrakło już sił 😄 

[Polecacz 101]

Produkcja i montaż PCB - wybierz sprawdzone PCBWay!
   • Darmowe płytki dla studentów i projektów non-profit
   • Tylko 5$ za 10 prototypów PCB w 24 godziny
   • Usługa projektowania PCB na zlecenie
   • Montaż PCB od 30$ + bezpłatna dostawa i szablony
   • Darmowe narzędzie do podglądu plików Gerber
Zobacz również » Film z fabryki PCBWay