Algorytmy robotyki mobilnej - nawigacja

Wprowadzenie

Celem zadania jest praktyczna weryfikacja mapy przygotowanej w poprzednim zadaniu przez wykorzystanie jej do planowania ruchu robota oraz przećwiczenie synchronizacji zadań związanych z planowaniem i wykonywaniem ruchu.

Przygotowanie

Przed zajęciami należy zapoznać się z opisem zadania oraz materiałami pomocniczymi w podanym niżej zakresie:

Metoda propagacji fali jako sposób wyznaczania ścieżki robota.
Przeanalizować dobór parametrów przy budowie mapy (np. wielkość pojedynczej komórki) oraz sposób interpretacji mapy na potrzeby metody planowania ścieżki.

Część 1 - planowanie ścieżki na mapie rastrowej

Przygotować mapę w postaci siatki zajętości.
Zdefiniować położenie robota oraz punktu docelowego na mapie.
Korzystając z metody propagacji fali (wavefront) wyznaczyć ścieżkę od robota do punktu docelowego.
Uwaga: Można optymalizować zaplanowaną ścieżkę, np. minimalizując liczbę skrętów.

Część 2 - nawigacja na podstawie mapy

Zrealizować ruch robota wzdłuż wyznaczonej ścieżki, korzystając z biblioteki RosAriaDriver lub bezpośredniego publikowania tematów ROS-a.
Uwaga 1: W przypadku realizacji ruchu w laboratorium, należy przed planowaniem ścieżki powiększyć przeszkody na mapie (np. metodą dylatacji obrazu) by uniknąć zatrzymywania robota przez laboratoryjny system stopu bezpieczeństwa.

Zadania opcjonalne

Ruch wzdłuż ścieżki może zostać dodatkowo uzupełniony o zabezpieczenie przed kolizją z przeszkodami.
W wersji podstawowej robot realizuje całą zaplanowaną ścieżkę. Jako rozszerzenie można przyjąć, że w trakcie przejazdu nowe pomiary służą do aktualizacji mapy i - w razie potrzeby - modyfikacji ścieżki.

Sprawozdanie

Sprawozdanie w formacie PDF należy przesłać pocztą elektroniczną przed terminem rozpoczęcia kolejnego zadania laboratoryjnego. Sprawozdanie powinno zawierać:

Krótki opis użytych algorytmów.
Przykładowe wyniki planowania ruchu.
Podsumowanie z obserwacjami i wnioskami.

Metoda propagacji fali

Algorytm propagacji fali:

wartość komórki celu ustawić na 0, przeszkód – na +∞, pozostałe komórki są nieoznaczone
i=0
dopóki istnieją nieoznaczone komórki (lub osiągnięty został punkt startowy)
- zwiększ numer iteracji i,
- każdej nieoznaczonej komórce, która sąsiaduje z komórką o wartości i-1 nadaj wartość równą i.
Wyznacz ścieżkę docelową od punktu startowego do celu jako ciąg sąsiadujących komórek o malejących wartościach.

Przykład ze strony Society of robots
Recursive WaveFront Propagation

Przykładowy kod

Sterowanie robotem

Zadawanie prędkości wymaga uruchomienia klienta systemu bezpieczeństwa i połączenia z wybranym robotem (opis przy zadaniu 1: Nowy system bezpieczeństwa).

import rospy
from geometry_msgs.msg import Twist
import time

def set_vel(v,w):
   global pub
   twist = Twist()
   twist.linear.x = v
   twist.angular.z =w
   print twist
   pub.publish(twist)

def start():
   global pub
   pub = rospy.Publisher('/PIONIER2/RosAria/cmd_vel', Twist)
   rospy.init_node('vel_controller', anonymous=True)
   
if __name__ == '__main__':
    start()
    time.sleep(2) # delay to complete node connection with ROS
    set_vel(0,0.2)
    time.sleep(2)
    set_vel(0.1,0)
    time.sleep(2)