Przeszukaj forum

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