Nowoczesne Sensory w Robotyce

Politechnika Poznańska, Instytut Robotyki i Inteligencji Maszynowej

Laboratorium 11: Obsługa danych GPS (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 wizualizacja oraz porównanie danych z odbiornika GPS i DGPS (Differential Global Positioning System). Studenci nauczą się konwertować współrzędne geograficzne do układu metrycznego (UTM), publikować trajektorie w ROS2 oraz korzystać z narzędzia evo do analizy błędów pozycjonowania.

Wstęp

GPS (Global Positioning System) to satelitarny system nawigacyjny umożliwiający wyznaczanie pozycji na powierzchni Ziemi. Standardowy odbiornik GPS osiąga dokładność rzędu kilku metrów. DGPS (Differential GPS) wykorzystuje dodatkową stację referencyjną do korekcji błędów, osiągając dokładność na poziomie centymetrów.

UTM (Universal Transverse Mercator) to układ współrzędnych, w którym pozycje wyrażone są w metrach, co ułatwia obliczenia odległości i wizualizację trajektorii.

Przygotowanie środowiska

Proszę pobrać docker image

docker pull stanislawkuczma/ros2-humble

💡 Tip: Dane do zajęć (folder nswr_dgps_rosbag2) należy pobrać z Chmury PP.

Uruchomienie

IMAGE_NAME="stanislawkuczma/ros2-humble:latest"
CONTAINER_NAME="<your_index>"

DOMAIN=${CONTAINER_NAME: -2}

xhost +local:root
XAUTH=/tmp/.docker.xauth
if [ ! -f $XAUTH ]
then
    xauth_list=$(xauth nlist :0 | sed -e 's/^..../ffff/')
    if [ ! -z "$xauth_list" ]
    then
        echo $xauth_list | xauth -f $XAUTH nmerge -
    else
        touch $XAUTH
    fi
    chmod a+r $XAUTH
fi
docker stop $CONTAINER_NAME || true && docker rm $CONTAINER_NAME || true
docker run -it \
    --env="DISPLAY=$DISPLAY" \
    --env="QT_X11_NO_MITSHM=1" \
    --env="ROS_DOMAIN_ID=$DOMAIN" \
    --volume="/tmp/.X11-unix:/tmp/.X11-unix:rw" \
    --env="XAUTHORITY=$XAUTH" \
    --volume="$XAUTH:$XAUTH" \
    --volume="/dev:/dev" \
    --volume="$PWD:/root/ros2_ws" \
    --privileged \
    --network=host \
    --name="$CONTAINER_NAME" \
    $IMAGE_NAME \
    /bin/bash

Uruchamiamy kontener kodem powyżej.

./run.sh

Proszę zwrócić uwagę, na tą linikę w skrypcie:

--volume="$PWD:/root/ros2_ws" \

Oznacza ona, że aktualny katalog (gdzie jest skrypt) będzie dostępny w kontenerze pod ścieżką /root/ros2_ws. Dlatego ważne jest, aby skrypt run.sh znajdował się w katalogu, gdzie są dane do zajęć (folder nswr_dgps_rosbag2).

Proszę też doinstalować bibliotekę utm, która będzie potrzebna do konwersji współrzędnych geograficznych do układu UTM.

sudo apt install python3-pip
pip install utm

W drugim terminalu sprawdzić zawartość nagranych danych i je odtworzyć:

# Sprawdzenie topic'ów w pliku rosbag
ros2 bag info nswr_dgps_rosbag2

# Odtworzenie danych (opcjonalnie: -r 2 dla 2x prędkości)
ros2 bag play nswr_dgps_rosbag2

📝 Pytanie: Jakie tematy (topics) zawiera plik rosbag? Jakiego typu są wiadomości GPS?

Zadanie 1: Wizualizacja w Foxglove

Przy pomocy narzędzia Foxglove Studio można sobie wygodnie wyświetlić dane z Baga. W tym celu proszę uruchomić Foxglove Studio, zalogować się, a następnie zamiast tworzenia nowego połączenia, wybrać opcję “Open from file” i wskazać plik nswr_dgps_rosbag2. Po załadowaniu danych, można dodać wykresy dla tematów gps_tapas i dgps_magellan, aby porównać trajektorie obu odbiorników, również na mapie od OpenStreetMap. Na dole jest możliwość zatrzymania oraz przewijania danych.

Aby w ten sposób wyświetlić dane, nie trzeba ich odtwarzać za pomocą ros2 bag play, ponieważ Foxglove odczytuje dane bezpośrednio z pliku rosbag. Nie trzeba też w ogóle uruchamiać kontenera.

Zadanie 2: Wizualizacja trajektorii GPS w RViz

Korzystając z dostarczonego szablonu programu, należy zwizualizować trajektorie obydwóch odbiorników GPS w programie RViz.

import nav_msgs
import rclpy
from rclpy.node import Node
from nav_msgs.msg import Odometry
from std_msgs.msg import Header
from sensor_msgs.msg import NavSatFix, NavSatStatus
import utm
import math


class GPS(Node):

    def __init__(self):
        super().__init__('nswr_gps')

        #  GPS Fix subscribers
        # self.subscription_tapas = self.create_subscription()
        
        # Create topic for odometry publishers
        # self.publisher_tapas_odom = self.create_publisher()

        self.easting_offset = 0
        self.northing_offset = 0
        self.alt_offset = 0
        
        # Initalize empty file
        with open("tapas.txt", 'w') as file:
            file.close()

    def tapas_callback(self, fix):

        if fix.status.status == NavSatStatus.STATUS_NO_FIX:
            return

        # Convert latitude and longitude to UTM coordinates
        # u = utm.from_latlon()

        # Declare odometry message
        odom = Odometry()

        # Fill odometry message time and frames
        odom.header.stamp = fix.header.stamp
        odom.header.frame_id = "map"
        odom.child_frame_id = fix.header.frame_id

        #  Fill "position" in odom message (https://docs.ros2.org/foxy/api/nav_msgs/msg/Odometry.html)
        #  easting: the x coordinate
        #  northing: the y coordinate
        #  fix.altitude: the z coordinate





        # Publish odometry message
        self.publisher_tapas_odom.publish(odom)


def main(args=None):

    rclpy.init(args=args)
    gps = GPS()

    rclpy.spin(gps)

    gps.destroy_node()
    rclpy.shutdown()


if __name__ == '__main__':
    main()

Wymagania:

1. Odczytać współrzędne geograficzne z dwóch tematów:
   • gps_tapas — odbiornik GPS
   • dgps_magellan — odbiornik DGPS
2. Przekonwertować współrzędne geograficzne (latitude, longitude) do układu UTM za pomocą funkcji utm.from_latlon().
3. Utworzyć i opublikować wiadomość typu nav_msgs/msg/Odometry dla każdego odbiornika.
   • Pola związane z orientacją ustawić na kwaternion jednostkowy: (0, 0, 0, 1).
   • Pola kowariancji można pominąć.
4. Trajektoria musi być wyrażona relatywnie do pierwszej odczytanej pozycji (pierwsza wiadomość → współrzędne (0, 0, 0)), aby łatwo zlokalizować trajektorię w RViz.

⚠️ Uwaga: Część podpunktów jest już zaimplementowana w szablonie — należy uzupełnić brakujące fragmenty kodu.

Rozwiązanie

Zrzut ekranu z RViz przedstawiający obydwie trajektorie jednocześnie.

Zadanie 3: Zapis trajektorii do pliku

Zapisać trajektorie obydwóch odbiorników do osobnych plików tekstowych w formacie TUM (kompatybilnym z narzędziem evo).

Format pliku (TUM):

Każda linia odpowiada jednej pozycji:

timestamp tx ty tz qx qy qz qw

💡 Wskazówka: Aby uzyskać poprawny znacznik czasu (timestamp) dla formatu TUM, należy połączyć sekundy i nanosekundy z nagłówka wiadomości: timestamp = sec + (nanosec * 1e-9).

Rozwiązanie

Dwa pliki tekstowe: tapas.txt i magellan.txt.

Zadanie 4: Analiza błędu APE z użyciem evo

Korzystając z narzędzia evo obliczyć APE (Absolute Pose Error) między obydwoma trajektoriami oraz wygenerować wykresy porównawcze.

Instalacja evo:

pip install evo --upgrade --no-binary evo

Zadanie 5: Obliczenie długości trajektorii

Obliczyć całkowitą długość każdej z trajektorii GPS, uwzględniając zmianę wysokości (3D).

Wymagania:

• Dla każdej kolejnej pary punktów obliczyć odległość euklidesową w 3D:

\[d = \sqrt{\Delta x^2 + \Delta y^2 + \Delta z^2}\]

• Zsumować odległości dla całej trajektorii.
• Wyświetlić wynik w metrach w konsoli.

💡 Pytanie: Jak duża jest różnica w długości trajektorii między GPS a DGPS? Czym może być spowodowana?

Rozwiązanie

Zrzut ekranu z konsoli z obliczonymi długościami obu trajektorii.

Komendy:

# Wizualizacja obu trajektorii
evo_traj tum tapas.txt magellan.txt --plot

# Obliczenie APE i wygenerowanie wykresów
evo_ape tum tapas.txt magellan.txt --plot

Co na eKursy:

1. Zrzut ekranu z Foxglove z mapą i położeniem odbiorników (Zadanie 1)
2. Zrzut ekranu z RViz z trajektoriami (Zadanie 2)
3. Pliki tapas.txt i magellan.txt (Zadanie 3)
4. Cztery wykresy z evo (Zadanie 4)
5. Zrzut ekranu z konsoli z długościami trajektorii (Zadanie 5)