Nowoczesne Sensory w Robotyce

Politechnika Poznańska, Instytut Robotyki i Inteligencji Maszynowej

Laboratorium 8: Obsługa danych ze skanera laserowego (ROS2)

Powrót do wyboru zajęć

Ważne uwagi organizacyjne

Ze sprzętem laboratoryjnym należy obchodzić się ostrożnie. Nie odłączać kabli sieciowych od komputerów.

Cel zajęć

Celem zajęć jest odczytanie chmury punktów 3D z sensora LiDAR (sensor_msgs/msg/PointCloud2), a następnie przetworzenie jej do płaskiego, dwuwymiarowego obrazu (projekcja sferyczna) wizualizującego intensywność odbicia oraz głębię (odległość).

Wstęp

Zadanie dotyczy obsługi danych ze skanera laserowego Ouster OS1-64. Można je realizować zarówno na danych zarejestrowanych podczas poprzednich zajęć, jak i na strumieniu danych przesyłanych na żywo z sensora.

Eksploracja danych

ROS 2 bag z którego można skorzystać na dzisiejszych zajęciach.

Proszę uruchomić odtwarzanie danych i przeanalizować je za pomocą dostępnych narzędzi ROS2:

1. Otwórz RViz2: Jak te dane wyglądają w przestrzeni 3D? Jaki jest ich frame_id?
2. Surowe Dane: Użyj terminala, aby podejrzeć strukturę przesyłanych wiadomości. Wywołaj polecenie ros2 topic echo /ouster/points. Zauważysz, że terminal zostanie zalany ścianą liczb.
3. Filtrowanie: Przerwij poprzednie polecenie (Ctrl+C) i wywołaj je ponownie, dodając flagę --no-arr (ros2 topic echo /ouster/points –no-arr). Zastanów się, co zmieniła ta flaga i dlaczego w kontekście chmur punktów jest ona tak przydatna?

Architektura wiadomości `sensor_msgs/msg/PointCloud2

Wykorzystując flagę --no-arr, ukryłeś zawartość pola data, odsłaniając metadane wiadomości. W ROS 2 chmury punktów są przesyłane za pomocą zoptymalizowanego typu PointCloud2. W przeciwieństwie do prostej listy obiektów 3D, jest to surowa tablica bajtów (msg.data).

Otwórz definicję wiadomości PointCloud2 i porównaj ją z tym co się wyświetliło w terminalu.

• Jaka jest rozdzielczość tej chmury punktów?
• Jakie fizyczne wartości (tzw. fields) kryją się wewnątrz surowych bajtów?
• Co dokładnie definiuje parametr point_step i offset?

Jak to rozczytać?

Próba wyciągnięcia wartości z tablicy msg.data za pomocą zwykłej pętli for w Pythonie drastycznie spowolni działanie programu (przetworzenie jednej klatki może zająć nawet kilkanaście sekund). Aby zdekodować dane w ułamku sekundy, skorzystamy z mechanizmu wektoryzacji w bibliotece NumPy.

Idea polega na stworzeniu “szablonu” odczytu (struktury danych), a następnie nałożeniu tego szablonu na surowe bajty wiadomości.

import numpy as np

# Zdefiniuj strukturę na podstawie metadanych z terminala
cloud_dtype = np.dtype({
    'names': ['x', 'y', 'z', 'intensity'],
    'formats': [ ..., ..., ..., ... ], 
    'offsets': [ ..., ..., ..., ... ], 
    'itemsize': ...                   
})

Jakie jeszcze dane jesteśmy wstanie wydobyć z tablicy? W sensie co moglibyśmy wpisać za intensity?

points_1d = np.frombuffer(msg.data, dtype=cloud_dtype)

Zmienna points_1d pozwala na łatwy dostęp do pól (np. points_1d['intensity']), ale nadal jest to płaska, jednowymiarowa tablica. Ponieważ chmura z LiDARu Ouster jest “zorganizowana”, powinieneś zmienić jej kształt na dwuwymiarową macierz za pomocą funkcji .reshape(), wstawiając jako argumenty wymiary odczytane z metadanych wiadomości.

Zadanie 1: Obraz intensywności (Intensity Image)

Korzystając z biblioteki OpenCV należy przekonwertować pojedynczy skan do obrazu, w którym wartości pikseli będą odczytanymi ze skanera wartościami pola intensity danego punktu.

Wymagania:

• Zdefiniować w kodzie strukturę pojedynczego punktu chmury (odpowiednie ułożenie bajtów i przesunięcia - offset).
• Za pomocą biblioteki NumPy wyciągnąć z surowych danych (msg.data) wyłącznie wartości odpowiadające za intensywność.
• Zmienić kształt wyciągniętej, jednowymiarowej tablicy na obraz o rozmiarze Width × Height (korzystając z metadanych wiadomości).
• Skrajne, odstające wartości intensywności obciąć (np. do 99. percentyla), a całość znormalizować do formatu 8-bitowego (0-255), tak aby uczytelnić obraz.
• Dla punktów, których wartość intensity >= 255, należy przyjąć wartość piksela równą 255
• Zapisać obraz do pliku .png

Rozwiązanie

Jako rozwiązanie proszę przesłać zapisany obraz. Przykład pokazujący jak taki obraz intensity powinien wyglądać przedstawiono poniżej:

Zadanie 2: Obraz głębi (Depth Image)

W analogiczny sposób jak w Zadaniu 1 należy utworzyć obraz głębi, w którym wartość każdego piksela będzie odległością danego punktu od skanera.

Wymagania:

• Odczytać z pamięci chmury punktów współrzędne przestrzenne x, y oraz z.
• Obliczyć odległość euklidesową (w 3D) każdego punktu od środka sensora.
• Utworzyć obraz o rozmiarze Width × Height
• Dla punktów, których odległość jest większa od 50 m należy przyjąć wartość piksela równą 50
• Obraz należy znormalizować z zakresu 0-50 do 0-255
• Zapisać obraz do pliku .png

Wzór na odległość euklidesową w 3D:

\(d = \sqrt{x^2 + y^2 + z^2}\)

Rozwiązanie

Jako rozwiązanie również proszę przesłać zapisany obraz w formacie .png. Przykład pokazujący przykładowy obraz głębi przedstawiono poniżej:

Co na eKursy:

1. intensity.png
2. depth.png
3. Kod który przetwarza chmurę punktów do obrazów.
4. Odpowiedzi na pytania zadane w instrukcji w formacie txt.

Appendix: W codziennej pracy projektowej rzadko jednak parsujemy te bajty ręcznie. Ekosystem ROS2 w Pythonie dostarcza dedykowany moduł sensor_msgs_py.point_cloud2. Posiada on wbudowaną funkcję read_points, która pod spodem automatycznie analizuje metadane (offsety, point_step) i dekoduje bajty za nas.