diff --git a/README.md b/README.md
index dda75812c8a77ce8fe64a3a5cdc6afcb031e8b47..084cb345ad2d4dd8e3aecbc41c49e152506c31e3 100644
--- a/README.md
+++ b/README.md
@@ -18,19 +18,19 @@ Celem tego laboratorium jest zaznajomienie z oprogramowaniem oraz sprzętem wyko
 **Wynik**: Po wykonaniu tego ćwiczenia uczestnik kursu będzie posiadał wiedzę jak tworzy się oprogramowanie dla robotów w środowisku ROS. Ponadto będzie w stanie połączyć elementy fizyczne jak efektory czy sensory z oprogramowaniem komputerowym.  
 
 
-## Laboratorium 3
+## Laboratorium 2
 ### Sensory bezprzewodowej detekcji przeszkĂłd
 **Opis**: W tym ćwiczeniu przedstawione zotaną podstawowe zagadnienia związane z zaawansowanymi sensorami bezprzewodowej detekcji przeszkód. Uczestnik uruchomi sensory przy pomocy jednoukładowego komputera i przetworzy uzyskiwane dane.
 
-**Instrukcja**: [lab03/README.md](lab03/README.md)
+**Instrukcja**: [lab02/README.md](lab02/README.md)
 
 **Wynik**: Po wykonaniu tego ćwiczenia uczestnik kursu będzie posiadał wiedzę jak korzystać z bardziej zaawansowanych sensorów detekcji przeszków oraz jak tworzyć mapy przestrzeni w 2D i 3D. 
 
-## Laboratorium 4
+## Laboratorium 3
 ### Systemy nawigacji zliczeniowej i inercyjnej
 **Opis**: Ostatnie ćwiczenie będzie poświęcone nawigacji zliczeniowej i zagadnienia transformacji układów współrzędnych. Dodatkowo ćwiczenie wykorzystuje platformę pixhawk do uzyskiwania danych inercyjnych. 
 
-**Instrukcja**: [lab04/README.md](lab04/README.md)
+**Instrukcja**: [lab03/README.md](lab03/README.md)
 
 **Wynik**: Po wykonaniu tego ćwiczenia uczestnik kursu będzie posiadał wiedzę jak korzystać z nawigacji zliczeniowej oraz jakie zastosowanie ma w przypadku pełnego systemu nawigacji robota. Ponadto uczestnik zapozna się z platformą pixhawk będącą układem sterowania dla pojazdów naziemnych, wodnych i powietrznych.
 
diff --git a/ROS_setup/README.md b/ROS_setup/README.md
index c29920b504a6b40a7f3242f2d58bf2038758e726..47fce66eabd82205143b49f490d6c7738c2120be 100644
--- a/ROS_setup/README.md
+++ b/ROS_setup/README.md
@@ -57,7 +57,9 @@ sudo apt-key adv --keyserver keys.gnupg.net --recv-key F6E65AC044F831AC80A06380C
 
 sudo add-apt-repository "deb http://realsense-hw-public.s3.amazonaws.com/Debian/apt-repo bionic main" -u
 
+## reinstall librealsense2-dkms if system img was cloned 
 sudo apt-get install librealsense2-dkms
+##
 sudo apt-get install librealsense2-utils
 sudo apt-get install librealsense2-dev
 sudo apt-get install librealsense2-dbg
diff --git a/lab02/README.md b/lab02/README.md
new file mode 100644
index 0000000000000000000000000000000000000000..db28ff5a75bd842ab0cb631c516911843a5c01d4
--- /dev/null
+++ b/lab02/README.md
@@ -0,0 +1,629 @@
+# Połączenie ze zdalnym środowiskiem
+- Połącz się ze zdalnym środowiskiem przy pomocy programu NoMachine oraz Visual Studio code
+ 
+Przypomnienie [lab00](https://git.pg.edu.pl/p962302/ppb_tpa_lab2020/tree/master/lab00)
+ 
+# Przygotowanie przestrzeni roboczej
+Sprawdź czy w przestrzeni roboczej **catkin_ws** znajduje się pakiet, który stworzyłeś na wcześniejszych zajęciach laboratoryjnych. Jeśli pakiet nie znajduje się w folderze **src** to ściągnij pakiet z repozytorium, które stworzyłeś na pierwszym laboratorium.
+- PrzejdĹş do folderu **catkin_ws/src**
+- Ściągnij repozytorium:
+```bash
+git clone <url do repozytorium>
+```
+![git clone](img/git_clone.gif "git clone")
+ 
+# RPLidar S1
+Na potrzeby ćwiczenia zostanie wykorzystany lidar RPlidar S1, który został podłączony do komputera zdalnego przez USB. Więcej informacji o lidarze [link](https://www.slamtec.com/en/Lidar/S1).
+ 
+![Rplidar](img/rplidar_s1.jpg "Rplidar")
+ 
+# Uruchomienie Lidaru RPLiDAR S1
+Tak samo jak w przypadku arduino, aby uruchomić urządzenie podłączone pod usb musimy wiedzieć pod który port zostało ono podłączone. Wywołujemy komendę:
+```bash
+ls -l /dev/serial/by-id
+```
+ID Lidaru będzie rozpoczynało się od nazwy **usb-Silicon_Labs_CP2102_USB_to_UART_Bridge_Controller_**
+ 
+![serial id](img/ls_serial_dev.gif "serial_id")
+ 
+**Zadanie 1:** Zidentyfikuj port ttyUSBx do którego podłączony jest lidar.
+ 
+Aby można było korzystać z urządzeń zewnętrznych potrzebne są sterowniki i programy je obsługujące. Wz przypadku RPlidaru będzie to pakiet rplidar_ros stworzony przez producenta urządzenia [link](https://github.com/Slamtec/rplidar_ros)
+Pakiet został już ściągnięty na komputer zdalny i znajduje się w katalogu **catkin_ws/src/**
+ 
+Zedytuj plik **rplidar_s1.lauch** w folderze **catkin_ws/src/rplidar_ros/launch**
+ 
+Zmień parametr **serial_port** na znaleziony wcześniej port do którego podłączony jest lidar i zapisz zmiany 
+ 
+ 
+ 
+![launch_mod](img/launch_mod.gif "launch_mod")
+ 
+Uruchom lidar wraz z wizualizacją przy pomocy komendy (w tym przypadku nie potrzeba uruchamiać roscore) w terminalu na maszynie zdalnej (przez program noMachine):
+```bash
+roslaunch rplidar_ros view_rplidar_s1.launch
+```
+ 
+Po uruchomieniu komendy powinno pojawić się okno programu RVIZ wraz z wizualizacją danych z lidaru w postaci czerwonych punktów.
+ 
+![Rviz](img/rviz.png "Rviz")
+ 
+Przy pomocy myszki możemy przemieszczać się i zmieniać orientację kamery w wizualizacji. 
+ 
+**Zadanie 2:** Zmień rozmiar wyświetlanych punktów na większe (parametr "size"), zmień kolor wyświetlanych punktów (najpierw zmień parametr "color transformer" na "FlatColor", a następnie parametr "Color")
+ 
+Zamknij program Rviz i zatrzymaj działanie programu w konsoli **Ctrl+c**
+ 
+Jak mogłeś już zauważyć tym razem program w środowisku ROS został uruchomiony przy pomocy innej komendy niż dotychczas. Wcześniej używaliśmy komendy
+```bash
+rosrun <nazwa pakietu> <nazwa aplikacji>
+```
+Tym razem użyliśmy komendy
+```bash
+roslaunch <nazwa pakietu> <nazwa pliku launch>
+```
+Komenda ta uruchamia plik launch, który pozwala uruchomić wiele node'ów naraz wraz z odpowiednimi parametrami. Uruchamia on również roscore jeśli ten nie był włączony wcześniej. Pliki launch bardzo ułatwiają pracę w ROS. Wcześniej, aby uruchomić kilka programów, musieliśmy uruchomić każdy program w oddzielnej konsoli, co przy dużej ilości programów wprowadzało mocne zamieszanie. Przy pomocy plików launch możemy uruchomić wszystko przy pomocy jednej komendy. Przykładowy plik launch do uruchomienia Lidaru wygląda następująco:
+```xml
+<launch>
+  <node name="rplidarNode"          pkg="rplidar_ros"  type="rplidarNode" output="screen">
+  <param name="serial_port"         type="string" value="/dev/ttyUSB1"/>
+  <param name="serial_baudrate"     type="int"    value="256000"/>
+  <param name="frame_id"            type="string" value="laser"/>
+  <param name="inverted"            type="bool"   value="false"/>
+  <param name="angle_compensate"    type="bool"   value="true"/>
+  </node>
+</launch>
+```
+Jak widać jest to plik **rplidar_s1.launch**, który już wcześniej zmodyfikowaliśmy. Pliki launch pisane są w języku xml, który wykorzystuje znaczniki do reprezentowania danych w strukturalny sposób. Plik lauch rozpoczyna się i kończy znacznikiem o tej samej nazwie:
+```xml
+<launch>
+ 
+</launch>
+```
+Wewnątrz tego znacznika możemy użyć wielu innych znaczników, które odpowiednio uruchamiają nody, inne pliki launch lub deklarują parametry (Więcej informacji o znacznikach [link](http://wiki.ros.org/roslaunch/XML)). W przytoczonym pliku launch widzimy jedynie dwa warianty znaczników
+```xml
+<node></node>
+oraz
+<param>
+```
+Znacznik **node** pozwala nam na uruchomienie określonego programu, według poniższego schematu:
+```xml
+<node name="rplidarNode"          pkg="rplidar_ros"  type="rplidarNode" output="screen">
+</node>
+```
+gdzie:
+- name - dowolna nazwa jaką chcemy nadać naszemu node'owi
+- pkg - nazwa pakietu, którym znajduje się node (tak samo jak przy komendzie rosrun)
+- type - nazwa programu (node'a), który chcemy uruchomić (tak samo jak przy komendzie rosrun)
+- output - temu parametrowi możemy nadać dwie wartości "log" lub "screen" ("log" jest wartością domyślną). Pozwala on na wskazanie, gdzie mają być wyświetlane komunikaty wypisywane normalnie w konsoli. Jeśli wybierzemy "screen" wtedy komunikaty będą wyświetlane w terminalu, jeśli "log" to będą zapisywane do pliku log. Ten parametr jest o tyle ważny, że jeśli uruchomimy kilka nodów, które bardzo często wypisują dane w terminalu jak np. pierwsze stworzone node'y podczas pierwszego laboratorium, które wypisywały wysłany i odebrany komunikat, to po uruchomieniu ich w jednym terminalu, komunikaty stają się nieczytelne z powodu ich pojawiającej się ilości. 
+ 
+Kolejne znaczniki **param** odpowiadają za definicje parametrów dla określonego node'a.
+ 
+- serial_port - nazwa portu USB do którego podłączony jest lidar
+- serial_baudrate - szybkość transmisji
+- frame_id - nazwa układu odniesienia dla liadru
+- inverted - Jeśli lidar byłby zamontowany do góry nogami to można by odwrócić jego orientację
+- angle_compensate - tryb kompensacji kąta, który powoduje, że kolejne pomiary są przesunięte względem siebie o ten sam kąt
+ 
+Znaczniki **param** znajdują się pomiędzy znacznikiem **node** co oznacza, że parametry dotyczą tylko tego node'a, aby dowiedzieć jakie parametry obsługuje dany node należy zajrzeć do dokumentacji danego node'a [link](http://wiki.ros.org/rplidar).
+ 
+ 
+Przy uruchamianiu Lidaru wraz z wizualizacją nie uruchomiliśmy jednak pliku **rplidar_s1.launch**, a plik **view_rplidar_s1.launch**. 
+ 
+OtwĂłrz go w VSC.
+ 
+```xml
+<launch>
+  <include file="$(find rplidar_ros)/launch/rplidar_s1.launch" />
+ 
+  <node name="rviz" pkg="rviz" type="rviz" args="-d $(find rplidar_ros)/rviz/rplidar.rviz" />
+</launch>
+```
+Jak widać plik ten posiada znacznik, którego jeszcze nie widzieliśmy, czyli **include**. Znacznik ten pozwala uruchamiać inne pliki launch podając do nich ścieżkę, dzięki czemu możemy grupować i uruchamiać poszczególne funkcjonalności np. jeśli chcielibyśmy uruchomić sam lidar wtedy uruchomilibyśmy tylko plik **rplidar_s1.launch**, natomiast jeśli chcielibyśmy dodać do tego wizualizacje to nie musimy kopiować zawartości pliku **rplidar_s1.launch** do nowego pliku i dodawać do niego uruchomienia wizualizacji tylko wystarczy uruchomić pierwszy plik i wizualizację. Dzięki takiemu podejściu nie powielamy już raz napisanego kodu, co zmniejsza możliwość pojawiania się błędów oraz upraszcza modyfikację.
+ 
+To co jeszcze ciekawego możemy zauważyć w pliku **view_rplidar_s1.launch** to użycie tzw. argumentu podstawienia **$(find rplidar_ros)** w znaczniku **include**:
+```xml
+<include file="$(find rplidar_ros)/launch/rplidar_s1.launch" />
+```
+Dzięki niemu nie musimy znać ścieżki do pakietu rplidar_ros. Przy wywołaniu pliku **launch** ścieżka zostanie znaleziona automatycznie, więc niezależnie od miejsca w którym pakiet został zainstalowany plik zostanie uruchomiony prawidłowo.
+ 
+ 
+**Zadanie 3:** Stwórz nowy plik launch we własnym pakiecie. Pliki launch standardowo znajdują się w folderze launch. Jeśli nie posiadasz takiego w głównym katalogu swojego pakietu to stwórz folder o takiej nazwie. Nazwa pliku launch może być dowolna. Stworzony plik ma uruchamiać dwa node: **turtlesim_node** oraz **turtle_teleop_key**, gdzie pierwszy node ma wypisywać komunikaty do pliku "log", natomiast drugi na terminal. Zapisz plik i uruchom go. (Pakiet nie musi być skompilowany przy takiej operacji). W sprawozdaniu przedstaw zarówno kod pliku launch jak i graf z programu rqt_graph (z uruchomionym plikiem launch).
+ 
+Skoro potrafimy już uruchomić Lidar i zwizualizować z niego dane to możemy teraz przyjrzeć się jak konkretnie te dane wyglądają. Node obsługujący lidar wysyła zbierane dane na topic **/scan** standardowy komendą **rostopic** możemy sprawdzić jakiego typu dane pojawiają się na tym topicu. 
+
+![scan_info](img/scan_info.gif "scan_info")
+ 
+Jak widzimy dane są typu sensor_msgs/LaserScan.msg ([link](http://wiki.ros.org/rplidar) do dokumentacji)
+ 
+![LaserScan](img/laserScan.png "LaserScan")
+ 
+Jest to dość złożony typ danych opisujący scan laserowy dla lidaru 2D. Pola, które będą nas interesować to kolejno
+- angle_min - jest to minimalny kąt, od którego nasz lidar zaczyna pomiar odległości, w tym przypadku będzie to -pi (wartość w radianach)
+- angle_max - jest to maksymalny kąt, na którym nasz lidar kończy pomiar odległości, w tym przypadku będzie to pi (wartość w radianach)
+- angle_increment - odległość kątowa pomiędzy dwoma kolejnymi pomiarami odległości 
+- range_min/max - po przekroczeniu tych wartości dany pojedynczy pomiar odległości jest odrzucany
+- ranges - tablica pomierzonych odległości (pierwszy element tablicy to pomiar dla kąt angle_min, natomiast kolejne to angle_min + angle_increment)
+ 
+ 
+**Zadanie 4:** Wykorzystując poniższy szablon napisz program, który przyjmuje dane z lidaru, a następnie sprawdza czy któraś wartość w tablicy **ranges** przekracza wartość 2. Jeśli tak, to zmień tą wartość na 0. (Wartość range_min jest ustawiona na 0.15, dlatego wartości 0 zostaną zignorowane przy wyświetlaniu). Wskazówka - elementy msg.ranges nie mogą być modyfikowane wprost, dlatego stwórz nową tablicę i przepisz do niej wszystkie wartości, a na koniec przypisz ją do msg.ranges.
+ 
+ 
+```python
+#!/usr/bin/env python
+ 
+import rospy
+from sensor_msgs.msg import LaserScan
+ 
+pub = rospy.Publisher('/scan_filtered', LaserScan, queue_size=10)
+def callback(data):
+    msg = data
+    #################
+    #Dodaj swoj kod
+    #Zamien wszystkie wartosci w zmiennej msg.ranges powyzej 2 na wartosc 0 
+    
+    
+    ###########
+    pub.publish(msg)
+ 
+def laser_filter():
+    rospy.init_node('laser_filter', anonymous=False)
+ 
+    rospy.Subscriber("/scan", LaserScan, callback)
+    rospy.loginfo("Filter started")
+    rospy.spin()
+ 
+ 
+if __name__ == '__main__':
+    try:
+        laser_filter()
+    except rospy.ROSInterruptException:
+        pass
+ 
+```
+Uruchom **view_rplidar_s1.launch** oraz stworzony node. W programie RVIZ dodaj nowy element wizualizacji
+ 
+![scan2](img/scan2.gif "scan2")
+ 
+**Zadanie 5:** Zmień kolor wyświetlania danych z lidaru po filtracji, tak aby można było odróżnić skan przed i po filtracji. 
+ 
+Zapisz konfiguracje Rviza w folderze rviz w swoim pakiecie.
+ 
+![rviz_conf](img/rviz_conf.gif "rviz_conf")
+ 
+ 
+**Zadanie 6:** Wzorując się na pliku **view_rplidar_s1.launch** napisz swój plik launch, w którym uruchomisz **rplidar_s1.launch**, swój node filtrujący oraz program Rviz z zapisaną konfiguracją wyświetlające oba skany. 
+ 
+# Tworzenie mapy pomieszczenia
+Do przedstawienia procesu tworzenia map w postaci siatki zajętości posłużymy się symulacją. Symulacja pozwala na swobodne poruszanie się robotem po stworzonym świecie, a co za tym idzie większy wpływ na działanie algorytmu mapowania. 
+ 
+![TurtleBot3](img/turtlebot3.png "TurtleBot3")
+ 
+Na potrzeby ćwiczenia użyjemy symulacji robota turtlebot3 waffle [(opis)](https://emanual.robotis.com/docs/en/platform/turtlebot3/overview/) w symulatorze [Gazebo](http://gazebosim.org/), który jest domyślnym symulatorem dla środowiska ROS.  
+ 
+Do przeprowadzenia symulacji robota potrzebujemy następujących pakietów:
+- https://github.com/ROBOTIS-GIT/turtlebot3_simulations
+- https://github.com/ROBOTIS-GIT/turtlebot3
+ 
+Pakiety zostały ówcześnie zainstalowane na komputerze zdalnym. 
+Uruchom symulację:
+```bash
+export TURTLEBOT3_MODEL=waffle
+roslaunch turtlebot3_gazebo turtlebot3_world.launch
+```
+![sim_on](img/sim_on.gif "sim_on")
+ 
+**Zadanie 7:** Po uruchomieniu symulacji sprawdź jakie topici pojawiły się w Rosie i jak wygląda rqt_graph.
+ 
+Uruchom program Rviz z odpowiednią konfiguracją
+```bash
+export TURTLEBOT3_MODEL=waffle
+roslaunch turtlebot3_gazebo turtlebot3_gazebo_rviz.launch
+```
+ 
+**Zadanie 8:** Uruchom program do sterowania robotem i przemieść robota z jednego końca mapy na drugi. Obserwuj, jak wygląda symulacja oraz dane w programie RVIZ. Po uruchomieniu programu w konsoli pojawi się opis jak sterować robotem.
+```bash
+export TURTLEBOT3_MODEL=waffle
+roslaunch turtlebot3_teleop turtlebot3_teleop_key.launch
+```
+ 
+---------
+Do stworzenia mapy pomieszczenia potrzebny jest nam algorytm SLAM. Tym razem wykoĹźystamy bardzo popularny i prosty w uĹźyciu [Gmapping](http://wiki.ros.org/gmapping)
+ 
+Do działania tego algorytmu potrzebne są trzy elementy:
+- Dane z Lidaru (scan 2d)
+- Odometria (przemieszczenie)
+- tf (przekształcenie układu współrzędnych pomiędzy startem odometrii, robotem, a lidarem)
+ 
+Zagadnienia związane z odometrią oraz tf zostaną omówione w kolejnym ćwiczeniu laboratoryjnym. 
+ 
+Wszystkie powyższe elementy dostarcza nam symulacja, dzięki czemu możemy skupić się na samym mapowaniu. 
+ 
+Zamknij wszystkie aplikacje związane z ROSem (Gazebo, RVIZ itd.)
+ 
+Uruchom nową symulację robota w innym środowisku.
+```bash
+export TURTLEBOT3_MODEL=waffle
+roslaunch turtlebot3_gazebo turtlebot3_house.launch
+```
+ 
+Uruchom Rviz 
+```bash
+export TURTLEBOT3_MODEL=waffle
+roslaunch turtlebot3_gazebo turtlebot3_gazebo_rviz.launch
+```
+ 
+Najprościej, bez ustawiania parametrów uruchomić gmapping następująco:
+```bash
+rosrun gmapping slam_gmapping
+```
+Przejdź do RVIZa i dodaj nowy element wizualizacji z zakładki "by topic" z topicu /map
+ 
+![sim_map_on](img/sim_map_on.gif "sim_map_on")
+ 
+<!-- Włączyć tf - zobaczyć map-odom -->
+ 
+**Zadanie 9:** Uruchom sterowanie robotem i przemieszczając się po środowisku, obserwuj jak w programie RVIZ tworzy się mapa danego środowiska. Zmapuj co najmniej jedno pomieszczenie. 
+```bash
+export TURTLEBOT3_MODEL=waffle
+roslaunch turtlebot3_teleop turtlebot3_teleop_key.launch
+```
+ 
+Jeśli uznamy, że stworzona mapa jest już kompletna albo wystarczająca to możemy ją zapisać poleceniem (mapa musi zostać zapisana przed zamknięciem terminalu z aplikacją gmapping. Jeśli zamkniemy ją wcześniej to mapa będzie dalej wyświetlana w RVIZ jednak nie będzie już możliwości jej zapisu):
+ 
+```bash
+rosrun map_server map_saver -f <Ścieżka zapisu i nazwa mapy>
+```
+![map_save](img/map_save.gif "map_save")
+ 
+**Zadanie 10:** Zobacz jakie pliki zostały stworzone w podanym katalogu i co zawiera każdy z plików. 
+ 
+ 
+Jak widać uruchomienie algorytmu SLAM było bardzo proste. Jeśli chcemy uruchomić gmapping z innymi parametrami niż domyślne, to najlepiej stworzyć plik launch np.:
+```xml
+<launch>
+  <arg name="scan_topic" default="scan" />
+ 
+  <node pkg="gmapping" type="slam_gmapping" name="slam_gmapping" output="screen">
+    <param name="base_frame" value="base_footprint"/>
+    <param name="odom_frame" value="odom"/>
+    <param name="map_update_interval" value="5.0"/>
+    <param name="maxUrange" value="6.0"/>
+    <param name="maxRange" value="8.0"/>
+    <param name="sigma" value="0.05"/>
+    <param name="kernelSize" value="1"/>
+    <param name="lstep" value="0.05"/>
+    <param name="astep" value="0.05"/>
+    <param name="iterations" value="5"/>
+    <param name="lsigma" value="0.075"/>
+    <param name="ogain" value="3.0"/>
+    <param name="lskip" value="0"/>
+    <param name="minimumScore" value="100"/>
+    <param name="srr" value="0.01"/>
+    <param name="srt" value="0.02"/>
+    <param name="str" value="0.01"/>
+    <param name="stt" value="0.02"/>
+    <param name="linearUpdate" value="0.1"/>
+    <param name="angularUpdate" value="0.5"/>
+    <param name="temporalUpdate" value="-1.0"/>
+    <param name="resampleThreshold" value="0.5"/>
+    <param name="particles" value="30"/>
+    <param name="xmin" value="-5.0"/>
+    <param name="ymin" value="-5.0"/>
+    <param name="xmax" value="5.0"/>
+    <param name="ymax" value="5.0"/>
+ 
+    <param name="delta" value="0.05"/>
+    <param name="llsamplerange" value="0.01"/>
+    <param name="llsamplestep" value="0.01"/>
+    <param name="lasamplerange" value="0.005"/>
+    <param name="lasamplestep" value="0.005"/>
+    <remap from="scan" to="$(arg scan_topic)"/>
+  </node>
+</launch>
+```
+Wyjaśnienie wszystkich parametrów można znaleźć w dokumnetacji [Gmapping](http://wiki.ros.org/gmapping).
+ 
+Jeśli chcielibyśmy użyć tego algorytmu na własnym robocie to musielibyśmy zadbać o trzy elementy, które były wymienione wcześniej. Parametry, które za te elementy odpowiadają to 
+- scan (topic na którym pojawiają się scany 2d z lidaru)
+- odom_frame (nazwa układu współrzędnych odometrii)
+- base_frame (nazwa układu współrzędnych robota)
+ 
+Jeśli robot jest stworzony w sposób standardowy to układy współrzędnych powinny mieć domyślne nazwy, jednak nie zawsze tak jest. 
+ 
+Z parametrów, które warto wymienić to np.
+- maxUrange i maxRange - maksymalny użyteczny i maksymalny rzeczywisty zasięg Lidaru. W przypadku RPLidar S1 maksymalny zasięg wynosi 40 m, natomiast wartość użyteczną dla tego lidaru można ustawić np. na 20 m. 
+- delta - rozdzielczość mapy (domyślnie 1 pixel to kwadrat 5x5 cm)
+- linearUpdate i angularUpdate - odległość liniowa i kątowa po przejechaniu, której zaktualizowana zostanie mapa.
+ 
+**Zadanie 11:** Stwórz plik launch w swoim pakiecie zawierający ustawienia przedstawione powyżej, zmień parametry delta, linearUpdate i map_update_interval. Zaobserwuj zmiany w działaniu algorytmu. 
+ 
+ 
+Zamknij wszystkie aplikacje
+ 
+-------
+ 
+# Kamery Realsense
+ 
+![d435_t265](img/d435_t265.jpg "d435_t265")
+
+Przedmiotem kolejnej części ćwiczenia będzie tworzenie map 3d z wykorzystaniem kamery głębi. Do realizacji celu posłużą nam dwie kamery:
+- realsense t265 - kamera stereowizyjna z układem logicznym realizująca slam wizyjny pozwalająca na uzyskanie pozycji i orientacji kamery. Test kamery [link](https://www.youtube.com/watch?v=mrdegXnG_no).
+ 
+- realsense d435 - Kamera stereowizyjna z projektorem światła podczerwonego. Pozwala na uzyskanie informacji o głębi w obrazie. Test kamery [link](https://www.youtube.com/watch?v=UgwHXH-jT0U).
+ 
+ 
+W odróżnieniu od peryferiów, które były wykorzystywane wcześniej (Lidar, arduino) komunikacja z kamerami nie jest realizowana przez serial port tylko przez sterownik usb. Do obsługi kamer potrzebujemy następującego oprogramowania i sterowników:
+- https://github.com/IntelRealSense/realsense-ros
+ 
+Oprogramowanie jest już zainstalowane, dzięki czemu możemy od razu uruchomić program testowy dostarczony przez producenta. Uruchom go w terminalu:
+```bash
+realsense-viewer
+```
+![realsense_view](img/realsense_view.gif "realsense_view")
+ 
+**Zadanie 12:** Włącz obie kamery i zaobserwuj dane w widoku 2d i 3d.
+ 
+Sprawdź i zapisz numer seryjny obu kamer, które będą potrzebne za chwilę przy uruchamianiu ich w środowisku ros (odczytywanie numeru seryjnego przedstawiono również na powyższym gifie).
+ 
+Aby uruchomić obie kamery w środowisku ROS potrzebujemy pliku launch, takiego jak poniżej:
+```xml
+<launch>
+  <arg name="device_type_camera1"             default="t265"/>
+  <arg name="device_type_camera2"             default="d4.5"/>
+  <arg name="serial_no_camera1"         default=""/>      <!-- Note: Replace with actual serial number -->
+  <arg name="serial_no_camera2"         default=""/>      <!-- Note: Replace with actual serial number -->
+  <arg name="camera1"                   default="t265"/>  
+  <arg name="camera2"                   default="d400"/>    
+  <arg name="tf_prefix_camera1"         default="$(arg camera1)"/>
+  <arg name="tf_prefix_camera2"         default="$(arg camera2)"/>
+  <arg name="initial_reset"             default="false"/>
+  <arg name="enable_fisheye"            default="true"/>
+  <arg name="color_width"               default="640"/>
+  <arg name="color_height"              default="480"/>
+  <arg name="depth_width"               default="640"/>
+  <arg name="depth_height"              default="480"/>
+  <arg name="clip_distance"             default="-2"/>
+  <arg name="topic_odom_in"             default="odom_in"/>
+  <arg name="calib_odom_file"           default=""/>
+ 
+
+
+  <group ns="$(arg camera1)">
+    <include file="$(find realsense2_camera)/launch/includes/nodelet.launch.xml">
+      <arg name="serial_no"             value="$(arg serial_no_camera1)"/>
+      <arg name="device_type"             value="$(arg device_type_camera1)"/>
+      <arg name="tf_prefix"             value="$(arg tf_prefix_camera1)"/>
+      <arg name="initial_reset"         value="$(arg initial_reset)"/>
+      <arg name="enable_fisheye1"       value="$(arg enable_fisheye)"/>
+      <arg name="enable_fisheye2"       value="$(arg enable_fisheye)"/>
+      <arg name="topic_odom_in"         value="$(arg topic_odom_in)"/>
+      <arg name="calib_odom_file"       value="$(arg calib_odom_file)"/>
+      <arg name="fisheye_fps"         default="30"/>
+      <arg name="gyro_fps"            default="200"/>
+      <arg name="accel_fps"           default="62"/>
+      <arg name="enable_gyro"         default="true"/>
+      <arg name="enable_accel"        default="true"/>
+      <arg name="enable_pose"         default="true"/>
+
+      <arg name="enable_sync"           default="false"/>
+
+      <arg name="linear_accel_cov"      default="0.01"/>
+      <arg name="unite_imu_method"      default=""/>
+
+      <arg name="publish_odom_tf"     default="true"/>
+
+    </include>
+  </group>
+ 
+  <group ns="$(arg camera2)">
+    <include file="$(find realsense2_camera)/launch/includes/nodelet.launch.xml">
+      <arg name="serial_no"             value="$(arg serial_no_camera2)"/>
+      <arg name="device_type"             value="$(arg device_type_camera2)"/>
+      <arg name="tf_prefix"             value="$(arg tf_prefix_camera2)"/>
+      <arg name="initial_reset"         value="$(arg initial_reset)"/>
+      <arg name="align_depth"           value="true"/>
+      <arg name="filters"               value="pointcloud"/>
+      <arg name="color_width"           value="$(arg color_width)"/>
+      <arg name="color_height"          value="$(arg color_height)"/>
+      <arg name="depth_width"           value="$(arg depth_width)"/>
+      <arg name="depth_height"          value="$(arg depth_height)"/>
+      <arg name="clip_distance"         value="$(arg clip_distance)"/>
+    </include>
+  </group>
+  <node pkg="tf" type="static_transform_publisher" name="t265_to_d400" args="0 0.0168 0.03 0 0 0 /$(arg tf_prefix_camera1)_link /$(arg tf_prefix_camera2)_link 100"/>
+ 
+   <node pkg="depthimage_to_laserscan" type="depthimage_to_laserscan" name="depthimage_to_laserscan">
+        <remap from="image"     to="/$(arg camera2)/aligned_depth_to_color/image_raw"/>
+        <remap from="camera_info" to="/$(arg camera2)/depth/camera_info"/>
+        <remap from="scan" to="/scan"/>
+        <param name="output_frame_id" value="$(arg camera2)_depth_frame"/>
+        <param name="range_max" type="double" value="4"/>
+        <param name="range_min" type="double" value="0.2"/>
+        <param name="scan_time" type="double" value="0.033"/>
+        <param name="scan_height" type="double" value="100"/>
+    </node>
+ 
+</launch>
+ 
+```
+W powyższym pliku możemy zauważyć nowy znacznik "group". Pozwala on odseparować parametry jednej kamery od drugiej. Jest to konieczne, ponieważ w plikach launch uruchamianych w znacznikach "include" argumenty mają taką samą nazwę np. "serial_no". Gdyby nie grupowanie to po uruchomieniu drugiej kamery parametry pierwszej zostałyby zmienione. 
+ 
+Wszystkie parametry możliwe do ustawienia możemy znaleźć tu [link](https://github.com/IntelRealSense/realsense-ros), w podpunkcie "Launch parameters". 
+ 
+Oprócz uruchomienia kamer, włączany jest "static_transform_publisher", który publikuje w systemie stałą transformację pomiędzy jendą kamerą, a drugą. Dzięki temu wskazujemy jak kamery są przesunięte względem siebie i odometrię, którą uzyskujemy z kamery t265, będziemy mogli odnieść do drugiej kamery (będziemy mogli śledzić położenie również drugiej kamery).
+ 
+Ostatni uruchamiany node "depthimage_to_laserscan" jak sama nazwa wskazuje pozwala uzyskać laserscan z obrazu głębi. Laser skan będzie potrzebny przy tworzeniu mapy 3d. Jeśli nie posiadamy lidaru i używamy samych kamer to w ten sposób możemy go zasymulować. W przeciwnym wypadku uruchomilibyśmy po prostu lidar już bez programu "depthimage_to_laserscan".
+ 
+**Zadanie 13:** Stwórz plik launch uruchamiający obie kamery w swoim pakiecie. Uzupełnij powyższy plik o odpowiednie numery seryjne kamer (w czwartej i piątej linii, gdzie kamera1 to kamera t265, a kamera2 to kamera d400). 
+ 
+Uruchom stworzony plik oraz Rviz. W programie Rviz otwórz konfigurację "test_kamer.rviz" z pakietu "laboratorium_pliki_dodatkowe" w folderze "catkin_ws/src".
+ 
+![rviz_test_kamer](img/rviz_test_kamer.gif "rviz_test_kamer")
+ 
+![konfiguracja_rviz](img/kamery_rviz.png "konfiguracja_rviz")
+ 
+W konfiguracji zostały przedstawione obrazy z dwóch kamer. Jak widać kamera t265 jest wyposażona w obiektyw rybie oko, dzięki czemu ma dużo większe pole widzenia.  W środkowej części programu widoczna jest chmura punktów uzyskana z kamery głębi, którą można obejrzeć z różnych stron zmieniając widok kamery przy pomocy myszki. 
+ 
+**Zadanie 14:** Uruchom program na arduino do sterowania serwem. Ustaw flagę w pozycji pionowej i zobacz czy flaga pojawiła się w chmurze punktów. 
+ 
+W programie Rviz znajduje się narzędzie measure dzięki któremu możemy zmierzyć odległości w scenie. Przy dokonywaniu pomiaru najlepiej zmienić styl wyświetlania chmury punktów. Po zmianie stylu na punkty, krawędź obiektu jest lepiej widoczna i można dokonać lepszego pomiaru. 
+ 
+ 
+![rviz_pomiar](img/rviz_pomiar.gif "rviz_pomiar")
+ 
+**Zadanie 15:** Używając narzędzia measure, zmierz szerokość uniesionej flagi.
+ 
+---------
+ 
+# Tworzenie mapy 3d 
+ 
+Do przygotowania mapy 3d tak samo jak w przypadku mapy 2d potrzebowaliśmy algorytmu slam. Tym razem użyjemy rtabmap:
+- http://introlab.github.io/rtabmap/
+- http://wiki.ros.org/rtabmap_ros
+ 
+Jest to slam przeznaczony typowo dla kamer głębi (RGB-D). Ze względu zdalnej natury przeprowadzanego laboratorium nie ma możliwości na swobodną manipulację kamerami i stworzenia mapy dowolnej przestrzeni, dlatego prezentowane są metody pracy z danym nie pobieranymi z czujników w czasie rzeczywistym. W przypadku lidaru 2d i mapy 2d zaprezentowane było podejście symulacyjne. Tym razem przedstawione zostanie użycie prawdziwych danych z rzeczywistych czujników, które zostały zapisane wcześniej. To podejście pozwala wielokrotnie pracować na tych samych rzeczywistych pomiarach i testować różnego typu algorytmy. 
+ 
+W systemie ROS do zapisu danych używa się plików bag [link](http://wiki.ros.org/rosbag). Rosbag pozwala na nagrywanie i późniejsze odtwarzanie wybranych topiców oraz wiadomości, które się na nich pojawiają. Jak już wcześniej było wspomniane jest to świetne narzędzie do testowania algorytmów oraz analizy działania robota (wyszukiwania błędów itp.).
+ 
+Z plikami bag można pracować przy pomocy dwóch narzędzi do odtwarzania, analizy i nagrywania danych:
+- rqt_bag - narzędzie graficzne 
+- rosbag - narzędzie konsolowe  
+ 
+Uruchom roscore oraz rqt_bag.
+ 
+Stworzony plik bag znajduje się w pakiecie "laboratorium_pliki_dodatkowe" w folderze bagfiles.
+ 
+![rqt_bag](img/rqt_bag.gif "rqt_bag")
+ 
+Po załadowaniu pliku pokazały się wszystkie topici, które zostały zapisane w pliku. Po prawej stronie znajdują się osie czasu po zbliżeniu których widzimy poszczególne wiadomości. Możemy podejrzeć dane na topicu klikając na nim prawym przyciskiem myszki i wybierając opcję view. W zależności od danych jest kilka możliwych wariantów podglądu. 
+- raw - podgląd poszczególnych wiadomości, podobnie jak rostopic echo
+- plot - wykreślenie całego przebiegu wybranych wartości z wiadomości
+- image - w przypadku obrazu z kamery moĹźliwe jest podejrzenie poszczegĂłlnych klatek
+ 
+**Zadanie 16:** Podejrzyj dane na topicu d400/color/image_raw oraz t265/odom/sample. Pierwszy topic podejrzyj w trybie image (view -> image), natomiast drugi w trybie plot. Zmaksymalizuj okno rqt_bag, powiększ okno wykresu plot przeciągając jego krawędź w lewą stronę. W oknie plot po prawej stronie jest wyświetlana struktura wiadomości odom. Wyświetl pozycję x i y (pose -> pose -> position -> x/y). Lewym przyciskiem myszki możesz zmieniać pozycję czasu na osi. Zaobserwuj, jak zmieniają się klatki filmu z kamery oraz jak zmienia się pozycja pionowej czerwonej osi na wykresie plot. Dodatkowo możesz uruchomić odtwarzanie wiadomości w czasie ciągłym klikając zieloną strzałkę będącą 7 opcją na panelu w lewym górnym rogu ekranu  
+ 
+Niestety odtwarzanie plików bag z programu rqt_bag tak aby wiadomości były widziane w systemie ROS nie jest efektywne, dlatego zamknij program rqt_bag. Do odtworzenia posłużymy się narzędziem konsolowym rosbag.
+ 
+Otwórz terminal i przejdź do katalogu zawierającego plik bag, który otwierałeś przed chwilą. Tak samo jak przy pomocy rqt_bag i przy użyciu rosbag możemy dowiedzieć się podstawowych informacji o pliku. 
+UĹźyj komendy:
+```bash
+rosbag info <nazwa pliku>
+```
+ 
+![rosbag_info](img/rosbag_info.gif "rosbag_info")
+ 
+Jak widzimy otrzymaliśmy informacje o czasie trwania pliku, kiedy był nagrywany, wielkości pliku, ale co najważniejsze widzimy jakie topici zostały nagrane oraz jakiego typu wiadomości się na nich znajdują.
+ 
+Jeśli chcemy otworzyć plik bag ze wszystkimi topicami to musimy użyć komendy:
+```bash
+rosbag play <nazwa pliku>
+```
+![rosbag_play](img/rosbag_play.gif "rosbag_play")
+ 
+Używając przycisku **spacja** możemy zatrzymywać odtwarzanie pliku.
+ 
+W przypadku kiedy chcemy aby odtwarzały się jedynie wybrane topici wtedy komenda powinna wyglądać następująco 
+```bash
+rosbag play <nazwa pliku> --topics <topic 1> <topic 2 ...>
+```
+ 
+rosbag posiada jeszcze wiele różnych możliwości jak przyspieszanie i opóźnianie odtwarzania plików. Wszystkie dostępne możliwości można sprawdzić w dokumnetacji [link](http://wiki.ros.org/rosbag/Commandline)
+ 
+**Zadanie 17:** Odtwórz plik bag, który otwierałeś w rqt_bag, przy pomocy rosbag. Podczas odtwarzania, otwórz program RVIZ i podejrzyj obraz z kamery (topic - /t265/fisheye1/image_raw) i zmieniającą się pozycję kamery (topic - /t265/odom/sample). Oba elementy możesz dodać do widoku RVIZa przez Add -> by topic -> nazwa topica
+ 
+-----
+ 
+Posiadając już dane na których możemy użyć algorytmu slam, możemy zająć się przygotowaniem pliku launch do jego uruchomienia. Plik launch do uruchamiania samego algorytmu wygląda następująco:
+ 
+```xml
+<launch>
+    <arg name="camera1" default="t265"/>  
+    <arg name="camera2" default="d400"/>
+    <node name="rtabmap" pkg="rtabmap_ros" type="rtabmap" output="screen" args="--delete_db_on_start">
+          <param name="frame_id" type="string" value="/$(arg camera2)_link"/>
+ 
+          <param name="subscribe_depth" type="bool" value="true"/>
+          <param name="subscribe_scan" type="bool" value="true"/>
+ 
+          <remap from="odom" to="/$(arg camera1)/odom/sample"/>
+          <remap from="scan" to="/scan"/>
+ 
+          <remap from="rgb/image"       to="/$(arg camera2)/color/image_raw"/>
+          <remap from="depth/image"     to="/$(arg camera2)/aligned_depth_to_color/image_raw"/>
+          <remap from="rgb/camera_info" to="/$(arg camera2)/color/camera_info"/>
+ 
+          <param name="queue_size" type="int" value="200"/>
+ 
+          <!-- RTAB-Map's parameters -->
+          <param name="RGBD/ProximityBySpace"     type="string" value="false"/>
+          <param name="RGBD/AngularUpdate"        type="string" value="0.01"/>
+          <param name="RGBD/LinearUpdate"         type="string" value="0.01"/>
+          <param name="RGBD/OptimizeFromGraphEnd" type="string" value="false"/>
+          <param name="Reg/Force3DoF"             type="string" value="true"/>
+          <param name="Vis/MinInliers"            type="string" value="12"/>
+          <param name="RGBD/OptimizeMaxError"     type="string" value="0.1"/>
+    </node>
+ 
+</launch>
+```
+Dzięki tak stworzonemu plikowi moglibyśmy uruchomić algorytm rtabmap przy użyciu rzeczywistych sensorów. Jeśli używamy plik bag musimy zadbać o jeszcze jedną rzecz. Jeśli uruchomilibyśmy powyższy plik z danymi odtwarzanymi z pliku bag to algorytm zwróciłby błąd o braku aktualnych danych. Jest to spowodowane różnicą pomiędzy aktualnym czasem, a czasem zawartym w wiadomościach. Większość wiadomości w systemie ROS posiada nagłówek, w którym zawarta jest informacja w jakiej chwili dany pomiar/wiadomość został wysłany/stworzony/zmierzony tzw. timestamp. Skoro odtwarzamy nasze wiadomości to ich timestamp może być nawet z przed kilku dni. Natomiast algorytm wymaga, aby dane były nie starsze niż 5s. Aby rozwiązać ten problem użyjemy czasu symulowanego. Podczas symulacji oraz odtwarzania danych symulacja czasu pozwala nie tylko przyspieszać czy zwalniać czas, ale również rozpoczynać wielokrotnie daną sekwencję od stałego punktu w czasie. 
+ 
+W systemie ROS, gdy chcemy używać czasu symulowanego potrzebujemy dwóch rzeczy:
+- ustawić parametr "use_sim_time" na true
+- oraz zapewnić program, który będzie publikował symulowany czas na topic /clock
+ 
+Dzięki parametrowi "use_sim_time" ROS wie, że każdy node powinien używać czasu z topicu /clock, a nie czasu systemowego (rzeczywistego). W naszym przypadku programem publikującym czas, będzie rosbag. Wystarczy przy jego uruchamianiu dodać flagę ```--clock```
+ 
+Jeśli uruchamiali byśmy wszystko po kolei w terminalu, komendy wyglądałby następująco (każda kolejna komenda w osobnym terminalu):
+```bash
+roscore
+rosparam set /use_sim_time true
+rosbag play <nazwa pliku> --clock
+```
+![rosbag_play_sim_time](img/rosbag_play_sim_time.gif "rosbag_play_sim_time")
+ 
+Jaki widać po ustawieniu parametru /use_sim_time oraz argumentu --clock, pojawił się topic /clock. Dodatkowo w programie Rviz w dolnej części okna można zauważyć, że czas ROS Time oraz Wall Time różnią się od siebie. 
+- ROS Time - Czas uĹźywany przez system ROS (w tym przypadku symulowany)
+- Wall Time - Czas rzeczywisty pobrany z systemu operacyjnego
+ 
+Do realizacji mapy 3d posłużymy się jednak plikiem launch, żeby uprościć proces uruchamiania całości
+```xml
+<launch>
+    <arg name="bag_file_dir" default="$(find laboratorium_pliki_dodatkowe)/bagfiles/lab_kor.bag"/>
+    <param name="use_sim_time" type="bool" value="true"/>
+    <node name="bag_file" pkg="rosbag" type="play" output="screen" args="--clock $(arg bag_file_dir)"/>
+</launch>
+```
+Najpierw jako argument definiujemy ścieżkę do naszego pliku bag. Następnie ustawiamy parametr "use_sim_time" aby ROS używał czasu symulowanego. Na końcu uruchamiamy nasz plik bag dodając argumenty zawierające parametr **--clock** powodujący publikowanie czasu symulacji oraz ścieżkę do pliku bag.
+ 
+ 
+**Zadanie 18:** Stwórz plik lauch uruchamiający odtwarzanie pliku bag oraz algorytm rtabmap. 
+ 
+Po uruchomieniu stworzonego pliku, zatrzymaj go klawiszem **spacja**. W konsoli powinien pojawić się komunikat **[PAUSED]**. Następnie otwórz program RVIZ i załaduj konfigurację **mapowanie_wizualizacja.rviz** z pakietu **laboratorium_pliki_dodatkowe**.
+ 
+Konfiguracja RVIZ przedstawia:
+- MapCloud - wizualizacja chmur punktów składających się na mapę 3d
+- Tf - wizualizacja układów współrzędnych
+- Widok kameru ustawiony w taki sposób, aby podążał za jednym z układów współrzędnych.
+ 
+W konsoli z uruchomionym plikiem launch użyj ponownie klawisza **spacja** aby dane znów były odtwarzane. Przy pomocy myszki możesz zmieniać orientację kamery i odległość od układu współrzędnych.
+ 
+![rosbag_play_map](img/rosbag_play_map.gif "rosbag_play_map")
+ 
+ 
+Plik zawiera dwa przejazdy po korytarzu. Poczekaj, aż stworzy się cała mapa. Podczas drugiego przejazdu zaobserwujesz mocne skoki kamery. Skoki te zostaną wyjaśnione w laboratorium nr 4. Gdy pojawi się skakanie kamery przełącz "Target Frame" na map lub Fixed frame. Opcje zmiany widoku znajdują się po prawej stronie okna. 
+ 
+**Zadanie 19:** Zrób zrzut ekranu, na którym będzie widoczna cała stworzona mapa. 
+ 
+Po zatrzymaniu mapowania (Ctrl+c w terminalu z uruchomionym plikiem launch), mapa zapisuje się w **/home/robot/.ros/rtabmap.db**. Możemy ją otworzyć i przeglądać dzięki programowi 
+```bash
+rtabmap
+```
+![rtabmap](img/rtabmap.gif "rtabmap")
+ 
+Niestety przy połączeniu zdalnym program **rtabmap** nie radzi sobie z poprawnym wyświetlaniem danych. 
+ 
+Stworzoną mapę można również wygenerować w standardowym formacie .ply czy .pcd do dalszej analizy czy obróbki.
+ 
+![map_export](img/map_export.png "map_export")
\ No newline at end of file
diff --git a/lab03/img/d435_t265.jpg b/lab02/img/d435_t265.jpg
similarity index 100%
rename from lab03/img/d435_t265.jpg
rename to lab02/img/d435_t265.jpg
diff --git a/lab03/img/git_clone.gif b/lab02/img/git_clone.gif
similarity index 100%
rename from lab03/img/git_clone.gif
rename to lab02/img/git_clone.gif
diff --git a/lab03/img/kamery_rviz.png b/lab02/img/kamery_rviz.png
similarity index 100%
rename from lab03/img/kamery_rviz.png
rename to lab02/img/kamery_rviz.png
diff --git a/lab03/img/laserScan.png b/lab02/img/laserScan.png
similarity index 100%
rename from lab03/img/laserScan.png
rename to lab02/img/laserScan.png
diff --git a/lab03/img/launch_mod.gif b/lab02/img/launch_mod.gif
similarity index 100%
rename from lab03/img/launch_mod.gif
rename to lab02/img/launch_mod.gif
diff --git a/lab03/img/ls_serial_dev.gif b/lab02/img/ls_serial_dev.gif
similarity index 100%
rename from lab03/img/ls_serial_dev.gif
rename to lab02/img/ls_serial_dev.gif
diff --git a/lab03/img/map_export.png b/lab02/img/map_export.png
similarity index 100%
rename from lab03/img/map_export.png
rename to lab02/img/map_export.png
diff --git a/lab03/img/map_save.gif b/lab02/img/map_save.gif
similarity index 100%
rename from lab03/img/map_save.gif
rename to lab02/img/map_save.gif
diff --git a/lab03/img/map_save.png b/lab02/img/map_save.png
similarity index 100%
rename from lab03/img/map_save.png
rename to lab02/img/map_save.png
diff --git a/lab03/img/realsense_view.gif b/lab02/img/realsense_view.gif
similarity index 100%
rename from lab03/img/realsense_view.gif
rename to lab02/img/realsense_view.gif
diff --git a/lab03/img/rosbag_info.gif b/lab02/img/rosbag_info.gif
similarity index 100%
rename from lab03/img/rosbag_info.gif
rename to lab02/img/rosbag_info.gif
diff --git a/lab03/img/rosbag_play.gif b/lab02/img/rosbag_play.gif
similarity index 100%
rename from lab03/img/rosbag_play.gif
rename to lab02/img/rosbag_play.gif
diff --git a/lab03/img/rosbag_play_map.gif b/lab02/img/rosbag_play_map.gif
similarity index 100%
rename from lab03/img/rosbag_play_map.gif
rename to lab02/img/rosbag_play_map.gif
diff --git a/lab03/img/rosbag_play_sim_time.gif b/lab02/img/rosbag_play_sim_time.gif
similarity index 100%
rename from lab03/img/rosbag_play_sim_time.gif
rename to lab02/img/rosbag_play_sim_time.gif
diff --git a/lab03/img/rplidar_s1.jpg b/lab02/img/rplidar_s1.jpg
similarity index 100%
rename from lab03/img/rplidar_s1.jpg
rename to lab02/img/rplidar_s1.jpg
diff --git a/lab03/img/rqt_bag.gif b/lab02/img/rqt_bag.gif
similarity index 100%
rename from lab03/img/rqt_bag.gif
rename to lab02/img/rqt_bag.gif
diff --git a/lab03/img/rtabmap.gif b/lab02/img/rtabmap.gif
similarity index 100%
rename from lab03/img/rtabmap.gif
rename to lab02/img/rtabmap.gif
diff --git a/lab03/img/rviz.png b/lab02/img/rviz.png
similarity index 100%
rename from lab03/img/rviz.png
rename to lab02/img/rviz.png
diff --git a/lab03/img/rviz_conf.gif b/lab02/img/rviz_conf.gif
similarity index 100%
rename from lab03/img/rviz_conf.gif
rename to lab02/img/rviz_conf.gif
diff --git a/lab03/img/rviz_pomiar.gif b/lab02/img/rviz_pomiar.gif
similarity index 100%
rename from lab03/img/rviz_pomiar.gif
rename to lab02/img/rviz_pomiar.gif
diff --git a/lab03/img/rviz_test_kamer.gif b/lab02/img/rviz_test_kamer.gif
similarity index 100%
rename from lab03/img/rviz_test_kamer.gif
rename to lab02/img/rviz_test_kamer.gif
diff --git a/lab03/img/scan2.gif b/lab02/img/scan2.gif
similarity index 100%
rename from lab03/img/scan2.gif
rename to lab02/img/scan2.gif
diff --git a/lab03/img/scan_info.gif b/lab02/img/scan_info.gif
similarity index 100%
rename from lab03/img/scan_info.gif
rename to lab02/img/scan_info.gif
diff --git a/lab03/img/sim_map_on.gif b/lab02/img/sim_map_on.gif
similarity index 100%
rename from lab03/img/sim_map_on.gif
rename to lab02/img/sim_map_on.gif
diff --git a/lab03/img/sim_on.gif b/lab02/img/sim_on.gif
similarity index 100%
rename from lab03/img/sim_on.gif
rename to lab02/img/sim_on.gif
diff --git a/lab03/img/turtlebot3.png b/lab02/img/turtlebot3.png
similarity index 100%
rename from lab03/img/turtlebot3.png
rename to lab02/img/turtlebot3.png
diff --git a/lab03/README.md b/lab03/README.md
index db28ff5a75bd842ab0cb631c516911843a5c01d4..af7b581e8869b4765450fbad7b2d0829a02bf533 100644
--- a/lab03/README.md
+++ b/lab03/README.md
@@ -1,629 +1,303 @@
-# Połączenie ze zdalnym środowiskiem
-- Połącz się ze zdalnym środowiskiem przy pomocy programu NoMachine oraz Visual Studio code
- 
-Przypomnienie [lab00](https://git.pg.edu.pl/p962302/ppb_tpa_lab2020/tree/master/lab00)
- 
 # Przygotowanie przestrzeni roboczej
-Sprawdź czy w przestrzeni roboczej **catkin_ws** znajduje się pakiet, który stworzyłeś na wcześniejszych zajęciach laboratoryjnych. Jeśli pakiet nie znajduje się w folderze **src** to ściągnij pakiet z repozytorium, które stworzyłeś na pierwszym laboratorium.
-- PrzejdĹş do folderu **catkin_ws/src**
-- Ściągnij repozytorium:
-```bash
-git clone <url do repozytorium>
-```
-![git clone](img/git_clone.gif "git clone")
- 
-# RPLidar S1
-Na potrzeby ćwiczenia zostanie wykorzystany lidar RPlidar S1, który został podłączony do komputera zdalnego przez USB. Więcej informacji o lidarze [link](https://www.slamtec.com/en/Lidar/S1).
- 
-![Rplidar](img/rplidar_s1.jpg "Rplidar")
- 
-# Uruchomienie Lidaru RPLiDAR S1
-Tak samo jak w przypadku arduino, aby uruchomić urządzenie podłączone pod usb musimy wiedzieć pod który port zostało ono podłączone. Wywołujemy komendę:
-```bash
-ls -l /dev/serial/by-id
-```
-ID Lidaru będzie rozpoczynało się od nazwy **usb-Silicon_Labs_CP2102_USB_to_UART_Bridge_Controller_**
- 
-![serial id](img/ls_serial_dev.gif "serial_id")
- 
-**Zadanie 1:** Zidentyfikuj port ttyUSBx do którego podłączony jest lidar.
- 
-Aby można było korzystać z urządzeń zewnętrznych potrzebne są sterowniki i programy je obsługujące. Wz przypadku RPlidaru będzie to pakiet rplidar_ros stworzony przez producenta urządzenia [link](https://github.com/Slamtec/rplidar_ros)
-Pakiet został już ściągnięty na komputer zdalny i znajduje się w katalogu **catkin_ws/src/**
- 
-Zedytuj plik **rplidar_s1.lauch** w folderze **catkin_ws/src/rplidar_ros/launch**
- 
-Zmień parametr **serial_port** na znaleziony wcześniej port do którego podłączony jest lidar i zapisz zmiany 
+Połącz się ze zdalnym środowiskiem i ściągnij swoje repozytorium do przestrzeni roboczej catkin_ws.
  
  
+# Transformacje pomiędzy układami współrzędnych
+Przed przystąpieniem do omawiania nawigacji ważnym jest, aby rozumieć jak wiele układów współrzędnych znajduje się w platformie robotycznej, jak są one ze sobą połączone oraz jak określać położenie i orientację względem poszczególnych układów współrzędnych. 
  
-![launch_mod](img/launch_mod.gif "launch_mod")
+Działanie zależności pomiędzy układami współrzędnych zostanie zwizualizowane na przykładzie manipulatora o 5 stopniach swobody. 
  
-Uruchom lidar wraz z wizualizacją przy pomocy komendy (w tym przypadku nie potrzeba uruchamiać roscore) w terminalu na maszynie zdalnej (przez program noMachine):
-```bash
-roslaunch rplidar_ros view_rplidar_s1.launch
-```
- 
-Po uruchomieniu komendy powinno pojawić się okno programu RVIZ wraz z wizualizacją danych z lidaru w postaci czerwonych punktów.
- 
-![Rviz](img/rviz.png "Rviz")
+![model_katana](img/model_katana.png "model_katana")
  
-Przy pomocy myszki możemy przemieszczać się i zmieniać orientację kamery w wizualizacji. 
+W systemie ROS możemy wizualizować roboty i elementy środowiska przy pomocy opisu URDF. Na niniejszym laboratorium jednak nie zostanie on wyjaśniony. Jeśli chcesz dowiedzieć jak tworzy się pliki URDF i jak stworzyć model własnego robota, to tu znajdziesz więcej informacji [link](http://wiki.ros.org/urdf/Tutorials) (tutorial opisuje, jak stworzyć prosty model robota R2D2).
  
-**Zadanie 2:** Zmień rozmiar wyświetlanych punktów na większe (parametr "size"), zmień kolor wyświetlanych punktów (najpierw zmień parametr "color transformer" na "FlatColor", a następnie parametr "Color")
+W skrócie pliki URDF pozwalają opisać jak złożony jest robot. Z jakich składa się elementów i jak są one połączone ze sobą. 
  
-Zamknij program Rviz i zatrzymaj działanie programu w konsoli **Ctrl+c**
+Aby uruchomić naszą wizualizację potrzebujemy pliku launch.
  
-Jak mogłeś już zauważyć tym razem program w środowisku ROS został uruchomiony przy pomocy innej komendy niż dotychczas. Wcześniej używaliśmy komendy
-```bash
-rosrun <nazwa pakietu> <nazwa aplikacji>
-```
-Tym razem użyliśmy komendy
-```bash
-roslaunch <nazwa pakietu> <nazwa pliku launch>
-```
-Komenda ta uruchamia plik launch, który pozwala uruchomić wiele node'ów naraz wraz z odpowiednimi parametrami. Uruchamia on również roscore jeśli ten nie był włączony wcześniej. Pliki launch bardzo ułatwiają pracę w ROS. Wcześniej, aby uruchomić kilka programów, musieliśmy uruchomić każdy program w oddzielnej konsoli, co przy dużej ilości programów wprowadzało mocne zamieszanie. Przy pomocy plików launch możemy uruchomić wszystko przy pomocy jednej komendy. Przykładowy plik launch do uruchomienia Lidaru wygląda następująco:
-```xml
-<launch>
-  <node name="rplidarNode"          pkg="rplidar_ros"  type="rplidarNode" output="screen">
-  <param name="serial_port"         type="string" value="/dev/ttyUSB1"/>
-  <param name="serial_baudrate"     type="int"    value="256000"/>
-  <param name="frame_id"            type="string" value="laser"/>
-  <param name="inverted"            type="bool"   value="false"/>
-  <param name="angle_compensate"    type="bool"   value="true"/>
-  </node>
-</launch>
-```
-Jak widać jest to plik **rplidar_s1.launch**, który już wcześniej zmodyfikowaliśmy. Pliki launch pisane są w języku xml, który wykorzystuje znaczniki do reprezentowania danych w strukturalny sposób. Plik lauch rozpoczyna się i kończy znacznikiem o tej samej nazwie:
 ```xml
 <launch>
+  <param name="robot_description" command="$(find xacro)/xacro.py '$(find katana_description)/urdf/katana_300_6m180.urdf.xacro'" />
+    <node pkg="joint_state_publisher_gui" type="joint_state_publisher_gui" name="joint_state_publisher">
+    </node>
+ 
+    <node pkg="robot_state_publisher" type="robot_state_publisher" name="robot_state_publisher">
+        <param name="ignore_timestamp" value="false"/>
+    </node>
  
+    <node pkg="tf" type="static_transform_publisher" name="katana_base_link_to_box" args="0 0.5 0 0 0 0 /katana_base_link /box 100"/>
 </launch>
 ```
-Wewnątrz tego znacznika możemy użyć wielu innych znaczników, które odpowiednio uruchamiają nody, inne pliki launch lub deklarują parametry (Więcej informacji o znacznikach [link](http://wiki.ros.org/roslaunch/XML)). W przytoczonym pliku launch widzimy jedynie dwa warianty znaczników
-```xml
-<node></node>
-oraz
-<param>
-```
-Znacznik **node** pozwala nam na uruchomienie określonego programu, według poniższego schematu:
-```xml
-<node name="rplidarNode"          pkg="rplidar_ros"  type="rplidarNode" output="screen">
-</node>
-```
-gdzie:
-- name - dowolna nazwa jaką chcemy nadać naszemu node'owi
-- pkg - nazwa pakietu, którym znajduje się node (tak samo jak przy komendzie rosrun)
-- type - nazwa programu (node'a), który chcemy uruchomić (tak samo jak przy komendzie rosrun)
-- output - temu parametrowi możemy nadać dwie wartości "log" lub "screen" ("log" jest wartością domyślną). Pozwala on na wskazanie, gdzie mają być wyświetlane komunikaty wypisywane normalnie w konsoli. Jeśli wybierzemy "screen" wtedy komunikaty będą wyświetlane w terminalu, jeśli "log" to będą zapisywane do pliku log. Ten parametr jest o tyle ważny, że jeśli uruchomimy kilka nodów, które bardzo często wypisują dane w terminalu jak np. pierwsze stworzone node'y podczas pierwszego laboratorium, które wypisywały wysłany i odebrany komunikat, to po uruchomieniu ich w jednym terminalu, komunikaty stają się nieczytelne z powodu ich pojawiającej się ilości. 
- 
-Kolejne znaczniki **param** odpowiadają za definicje parametrów dla określonego node'a.
- 
-- serial_port - nazwa portu USB do którego podłączony jest lidar
-- serial_baudrate - szybkość transmisji
-- frame_id - nazwa układu odniesienia dla liadru
-- inverted - Jeśli lidar byłby zamontowany do góry nogami to można by odwrócić jego orientację
-- angle_compensate - tryb kompensacji kąta, który powoduje, że kolejne pomiary są przesunięte względem siebie o ten sam kąt
+Został on już stworzony w pakiecie "laboratorium_pliki_dodatkowe". 
  
-Znaczniki **param** znajdują się pomiędzy znacznikiem **node** co oznacza, że parametry dotyczą tylko tego node'a, aby dowiedzieć jakie parametry obsługuje dany node należy zajrzeć do dokumentacji danego node'a [link](http://wiki.ros.org/rplidar).
+Po pierwsze musimy załadować opis naszego robota jako parametr pod nazwą **robot_description**. 
  
+Następnie uruchamiamy trzy node'y. Pierwszy odpowiada za publikowanie stanu poszczególnych przegubów robota. Po uruchomieniu zobaczysz okno z interfejsem, który pozwoli na zmianę pozycji poszczególnych przegubów robota. Jeśli posiadalibyśmy prawdziwego robota, którego chcielibyśmy wizualizować, wtedy jego sterownik publikowałby aktualne stany poszczególnych przegubów.  
  
-Przy uruchamianiu Lidaru wraz z wizualizacją nie uruchomiliśmy jednak pliku **rplidar_s1.launch**, a plik **view_rplidar_s1.launch**. 
+Kolejny node na podstawie pliku urdf oraz danych publikowanych przez **joint_state_publisher** uaktualnia pozycje wizualizowanych elementĂłw robota.
  
-OtwĂłrz go w VSC.
+Ostatni node **static_transform_publisher** publikuje stałe przekształcenie (odsunięcie o 0.5 m w osi y) pomiędzy układem współrzędnych **katana_base_link**, a **box**. Ma on symulować położenie przykładowego elementu i posłuży do lepszego wyjaśnienia omawianego zagadnienia.  
  
-```xml
-<launch>
-  <include file="$(find rplidar_ros)/launch/rplidar_s1.launch" />
+Uruchom plik launch **wizualizacja_urdf** z paketu **laboratorium_pliki_dodatkowe**. Następnie uruchom program RVIZ i dodaj wizualizacje modelu robota, tf oraz zmień stały układ współrzędnych na **katana_base_link**.
  
-  <node name="rviz" pkg="rviz" type="rviz" args="-d $(find rplidar_ros)/rviz/rplidar.rviz" />
-</launch>
-```
-Jak widać plik ten posiada znacznik, którego jeszcze nie widzieliśmy, czyli **include**. Znacznik ten pozwala uruchamiać inne pliki launch podając do nich ścieżkę, dzięki czemu możemy grupować i uruchamiać poszczególne funkcjonalności np. jeśli chcielibyśmy uruchomić sam lidar wtedy uruchomilibyśmy tylko plik **rplidar_s1.launch**, natomiast jeśli chcielibyśmy dodać do tego wizualizacje to nie musimy kopiować zawartości pliku **rplidar_s1.launch** do nowego pliku i dodawać do niego uruchomienia wizualizacji tylko wystarczy uruchomić pierwszy plik i wizualizację. Dzięki takiemu podejściu nie powielamy już raz napisanego kodu, co zmniejsza możliwość pojawiania się błędów oraz upraszcza modyfikację.
- 
-To co jeszcze ciekawego możemy zauważyć w pliku **view_rplidar_s1.launch** to użycie tzw. argumentu podstawienia **$(find rplidar_ros)** w znaczniku **include**:
-```xml
-<include file="$(find rplidar_ros)/launch/rplidar_s1.launch" />
-```
-Dzięki niemu nie musimy znać ścieżki do pakietu rplidar_ros. Przy wywołaniu pliku **launch** ścieżka zostanie znaleziona automatycznie, więc niezależnie od miejsca w którym pakiet został zainstalowany plik zostanie uruchomiony prawidłowo.
+![urdf_rviz](img/urdf_rviz.gif "urdf_rviz")
  
  
-**Zadanie 3:** Stwórz nowy plik launch we własnym pakiecie. Pliki launch standardowo znajdują się w folderze launch. Jeśli nie posiadasz takiego w głównym katalogu swojego pakietu to stwórz folder o takiej nazwie. Nazwa pliku launch może być dowolna. Stworzony plik ma uruchamiać dwa node: **turtlesim_node** oraz **turtle_teleop_key**, gdzie pierwszy node ma wypisywać komunikaty do pliku "log", natomiast drugi na terminal. Zapisz plik i uruchom go. (Pakiet nie musi być skompilowany przy takiej operacji). W sprawozdaniu przedstaw zarówno kod pliku launch jak i graf z programu rqt_graph (z uruchomionym plikiem launch).
+**Zadanie 1:** Przy pomocy **joint_state_publisher** obróć poszczególne przeguby robota i zobacz, jak zmienia się wizualizacja. Zmień stały układ współrzędnych (Fixed frame) na wybrany układ zaczynający się od nazwy **katana_motor**. Sprawdź jak teraz zmienia się wizualizacja przy ruchach przegubów. 
  
-Skoro potrafimy już uruchomić Lidar i zwizualizować z niego dane to możemy teraz przyjrzeć się jak konkretnie te dane wyglądają. Node obsługujący lidar wysyła zbierane dane na topic **/scan** standardowy komendą **rostopic** możemy sprawdzić jakiego typu dane pojawiają się na tym topicu. 
-
-![scan_info](img/scan_info.gif "scan_info")
  
-Jak widzimy dane są typu sensor_msgs/LaserScan.msg ([link](http://wiki.ros.org/rplidar) do dokumentacji)
+![model_tf](img/model_tf.png "model_tf")
  
-![LaserScan](img/laserScan.png "LaserScan")
+Jak widać robot złożony jest z wielu układów współrzędnych połączonych ze sobą. Każdy kolejny układ współrzędnych jest zaczepiony w poprzednim i względem niego jest określana jego pozycja. Taki sposób organizacji wszystkich układów współrzędnych pozwala nam w łatwy sposób wyliczyć, gdzie np. znajduje się końcówka robota względem podstawy lub dowolnego innego obiektu np. pudełka o układzie współrzędnych **box**. Wszystkie połączenia pomiędzy układami współrzędnych są zorganizowane w postaci struktury drzewa. Aktualną strukturę możemy zobaczyć przy pomocy narzędzia rqt i TF Tree.
  
-Jest to dość złożony typ danych opisujący scan laserowy dla lidaru 2D. Pola, które będą nas interesować to kolejno
-- angle_min - jest to minimalny kąt, od którego nasz lidar zaczyna pomiar odległości, w tym przypadku będzie to -pi (wartość w radianach)
-- angle_max - jest to maksymalny kąt, na którym nasz lidar kończy pomiar odległości, w tym przypadku będzie to pi (wartość w radianach)
-- angle_increment - odległość kątowa pomiędzy dwoma kolejnymi pomiarami odległości 
-- range_min/max - po przekroczeniu tych wartości dany pojedynczy pomiar odległości jest odrzucany
-- ranges - tablica pomierzonych odległości (pierwszy element tablicy to pomiar dla kąt angle_min, natomiast kolejne to angle_min + angle_increment)
+**Zadanie 2:** Uruchom program rqt przy pomocy komendy ``` rqt ``` w terminalu. Następnie włącz wizualizacje TF Tree (Plugins -> Visualization -> TF Tree).
  
+Po uruchomieniu TF Tree możesz zobaczyć jak cała struktura jest zorganizowana. Dodatkowo, dzięki temu narzędziu możemy sprawdzić czy na pewno wszystko jest ze sobą połączone. Jeśli któryś z elementów nie będzie połączony z resztą to dane z takiego elementu nie mogą być odniesione do innych np. jeśli nie wprowadzimy jak zamontowany jest lidar na robocie to będziemy wiedzieć jak daleko znajduje się przeszkoda od lidaru, ale nie będziemy wiedzieć jak blisko znajduje się od np. przedniego zderzaka robota. 
  
-**Zadanie 4:** Wykorzystując poniższy szablon napisz program, który przyjmuje dane z lidaru, a następnie sprawdza czy któraś wartość w tablicy **ranges** przekracza wartość 2. Jeśli tak, to zmień tą wartość na 0. (Wartość range_min jest ustawiona na 0.15, dlatego wartości 0 zostaną zignorowane przy wyświetlaniu). Wskazówka - elementy msg.ranges nie mogą być modyfikowane wprost, dlatego stwórz nową tablicę i przepisz do niej wszystkie wartości, a na koniec przypisz ją do msg.ranges.
+![tf_tree](img/tf_tree.png "tf_tree")
  
+W dowolnej chwili możemy sprawdzić aktualne przesunięcie (transformacje) pomiędzy dwoma dowolnymi układami współrzędnych. Możemy to zrobić przy pomocy komendy:
+```bash
+rosrun tf tf_echo <nazwa układu współrzędnych> <nazwa układu współrzędnych>
+```
  
-```python
-#!/usr/bin/env python
+![tf_echo](img/tf_echo.gif "tf_echo")
  
-import rospy
-from sensor_msgs.msg import LaserScan
+![tf_echo](img/tf_echo.png "tf_echo")
  
-pub = rospy.Publisher('/scan_filtered', LaserScan, queue_size=10)
-def callback(data):
-    msg = data
-    #################
-    #Dodaj swoj kod
-    #Zamien wszystkie wartosci w zmiennej msg.ranges powyzej 2 na wartosc 0 
-    
-    
-    ###########
-    pub.publish(msg)
+Program tf co sekundę pokazuje nam odległość i zmianę orientacji pomiędzy dwoma układami współrzędnych. Przesunięcie określone jest kolejno w osiach X,Y,Z. Natomiast zmiana orientacji podana jest w trzech różnych jednostkach.
  
-def laser_filter():
-    rospy.init_node('laser_filter', anonymous=False)
  
-    rospy.Subscriber("/scan", LaserScan, callback)
-    rospy.loginfo("Filter started")
-    rospy.spin()
+**Zadanie 3:** Uruchom tf_echo pomiędzy układami współrzędnych **katana_gripper_tool_frame** i **box**, a następnie ustaw końcówkę robota na tyle blisko układu współrzędnych **box**, aby odległość w żadnej osi nie przekraczała 0.2 m.
  
+-----------
  
-if __name__ == '__main__':
-    try:
-        laser_filter()
-    except rospy.ROSInterruptException:
-        pass
+# Odometria
  
-```
-Uruchom **view_rplidar_s1.launch** oraz stworzony node. W programie RVIZ dodaj nowy element wizualizacji
+Przechodząc do pojazdów, standardowa struktura układów współrzędnych i ich nazewnictwo przedstawia się następująco:
  
-![scan2](img/scan2.gif "scan2")
+![mobile_robot_standard_tf](img/mobile_robot_standard_tf.png "mobile_robot_standard_tf")
  
-**Zadanie 5:** Zmień kolor wyświetlania danych z lidaru po filtracji, tak aby można było odróżnić skan przed i po filtracji. 
+Standardowo wszystkie elementy umieszczone na robocie są ostatecznie połączone z układem **base_link**. Następnie układ **base_link** jest połączony z układem **base_footprint**, który jest rzutem na płaszczyznę układu **base_stabilized**, ale zwykle pomija się układ **base_stabilized** i zwykle się go nie tworzy.
  
-Zapisz konfiguracje Rviza w folderze rviz w swoim pakiecie.
+Kolejne połączenie jest tworzone pomiędzy **base_footprint**, a **odom**. Tym razem jest to już połączenie odpowiedzialne za lokalizację. Tak jak poprzednie układy współrzędnych były raczej statyczne względem siebie, tak **base_footprint** i **odom** już nie. Transformacja ta obrazuje przesunięcie robota względem punktu początkowego (początkiem układu współrzędnych odom) i zwykle wykorzystywane są do tego metody przyrostowe. Co ważne w systemie ROS przyjęte jest, że w tej transformacji nie mogą występować przeskoki w pozycji (pozycja musi zmieniać się w sposób ciągły). 
  
-![rviz_conf](img/rviz_conf.gif "rviz_conf")
+Ostatnim zaprezentowanym połączeniem jest pomiędzy **odom** i **map**. To połączenie pozwala pokazać, gdzie robot znajduje się na mapie. Tu mogą już występować przeskoki w pozycji tzn. transformacja pomiędzy **map** i **odom** jest tak dobierana, aby końcowa pozycja robota względem mapy była zgodna z rzeczywistością (dokonuje tego zwykle algorytm SLAM). 
  
+Czasami stosuje się jeszcze układ współrzędnych **earth**, która jest już ostatecznym układem współrzędnych i zwykle odnosi się do koordynatów GPS. Ten układ współrzędnych pozwala nam na odniesienie względem siebie kilku map i przypisać im rzeczywistą pozycję na "Świecie"
  
-**Zadanie 6:** Wzorując się na pliku **view_rplidar_s1.launch** napisz swój plik launch, w którym uruchomisz **rplidar_s1.launch**, swój node filtrujący oraz program Rviz z zapisaną konfiguracją wyświetlające oba skany. 
+------
  
-# Tworzenie mapy pomieszczenia
-Do przedstawienia procesu tworzenia map w postaci siatki zajętości posłużymy się symulacją. Symulacja pozwala na swobodne poruszanie się robotem po stworzonym świecie, a co za tym idzie większy wpływ na działanie algorytmu mapowania. 
+Jeśli znamy już całą konwencję tworzenia i nazewnictwa układów współrzędnych, możemy przejść do nawigacji zliczeniowej.
+Zwykle pozycja w nawigacji zliczeniowej robotów mobilnych jest określana na podstawie enkoderów umieszczonych na kołach. Wiedząc jak obróciły się koła możemy obliczyć pozycję robota na podstawie poniższych wzorów:
  
-![TurtleBot3](img/turtlebot3.png "TurtleBot3")
+![odom_wzor](img/odom_wzor.png "odom_wzor")
  
-Na potrzeby ćwiczenia użyjemy symulacji robota turtlebot3 waffle [(opis)](https://emanual.robotis.com/docs/en/platform/turtlebot3/overview/) w symulatorze [Gazebo](http://gazebosim.org/), który jest domyślnym symulatorem dla środowiska ROS.  
  
-Do przeprowadzenia symulacji robota potrzebujemy następujących pakietów:
-- https://github.com/ROBOTIS-GIT/turtlebot3_simulations
-- https://github.com/ROBOTIS-GIT/turtlebot3
+W systemie ROS obliczaniem odometri zakmuje się program obsługujący fizyczne enkodery, natomiast już w systemie posługujemy się wadomością Odometry ([dokumnetacja](http://docs.ros.org/en/melodic/api/nav_msgs/html/msg/Odometry.html)). Zawiera ona zarówno aktualną pozycję jak i prędkość. Dodatkowo oba parametry są opatrzone macierzą kowariancji, która reprezentuje niepewność każdego z pomiarów. 
  
-Pakiety zostały ówcześnie zainstalowane na komputerze zdalnym. 
-Uruchom symulację:
+Uruchom symulację robota turtlebot:
 ```bash
 export TURTLEBOT3_MODEL=waffle
 roslaunch turtlebot3_gazebo turtlebot3_world.launch
 ```
-![sim_on](img/sim_on.gif "sim_on")
  
-**Zadanie 7:** Po uruchomieniu symulacji sprawdź jakie topici pojawiły się w Rosie i jak wygląda rqt_graph.
- 
-Uruchom program Rviz z odpowiednią konfiguracją
+Uruchom RVIZ
 ```bash
 export TURTLEBOT3_MODEL=waffle
 roslaunch turtlebot3_gazebo turtlebot3_gazebo_rviz.launch
 ```
  
-**Zadanie 8:** Uruchom program do sterowania robotem i przemieść robota z jednego końca mapy na drugi. Obserwuj, jak wygląda symulacja oraz dane w programie RVIZ. Po uruchomieniu programu w konsoli pojawi się opis jak sterować robotem.
+Uruchom program sterujący robotem
 ```bash
 export TURTLEBOT3_MODEL=waffle
 roslaunch turtlebot3_teleop turtlebot3_teleop_key.launch
 ```
  
----------
-Do stworzenia mapy pomieszczenia potrzebny jest nam algorytm SLAM. Tym razem wykoĹźystamy bardzo popularny i prosty w uĹźyciu [Gmapping](http://wiki.ros.org/gmapping)
- 
-Do działania tego algorytmu potrzebne są trzy elementy:
-- Dane z Lidaru (scan 2d)
-- Odometria (przemieszczenie)
-- tf (przekształcenie układu współrzędnych pomiędzy startem odometrii, robotem, a lidarem)
- 
-Zagadnienia związane z odometrią oraz tf zostaną omówione w kolejnym ćwiczeniu laboratoryjnym. 
+**Zadanie 4:** Uruchom program rqt i zobacz, jak wyglądają zależności pomiędzy układami współrzędnych w tym robocie.
  
-Wszystkie powyższe elementy dostarcza nam symulacja, dzięki czemu możemy skupić się na samym mapowaniu. 
+Przejdź do programu RVIZ, zmień ustawienie widoku na **ThirdPersonFollower**, zbliż kamerę do robota. Włącz wizualizację TF i włącz widoczność tylko następujących układów **base_link**, **base_scan**, **wheel_right_link**, **wheel_left_link**, **odom**. Zmniejsz rozmiar zwizualizowanych układów współrzędnych "Marker scale", tak aby nie przysłaniały modelu robota. 
  
-Zamknij wszystkie aplikacje związane z ROSem (Gazebo, RVIZ itd.)
+![sim_rviz_setup](img/sim_rviz_setup.gif "sim_rviz_setup")
  
-Uruchom nową symulację robota w innym środowisku.
-```bash
-export TURTLEBOT3_MODEL=waffle
-roslaunch turtlebot3_gazebo turtlebot3_house.launch
-```
- 
-Uruchom Rviz 
-```bash
-export TURTLEBOT3_MODEL=waffle
-roslaunch turtlebot3_gazebo turtlebot3_gazebo_rviz.launch
-```
+**Zadanie 5:** Podejrzyj co dzieje się na topicu **/odom**. Obserwując robota jak i topic **/odom** przejedź około 1 m do przodu symulowanym robotem. 
  
-Najprościej, bez ustawiania parametrów uruchomić gmapping następująco:
-```bash
-rosrun gmapping slam_gmapping
-```
-Przejdź do RVIZa i dodaj nowy element wizualizacji z zakładki "by topic" z topicu /map
+**Zadanie 6:** Zmień stały układ współrzędnych "Fixed Frame" z **odom** na **base_scan**. Przejedź ponownie robotem jakąś odległość w dowolnym kierunku. Jak teraz prezentowane są dane w programie RVIZ?
  
-![sim_map_on](img/sim_map_on.gif "sim_map_on")
+Podczas tworzenia mapy 3d na laboratorium nr 3 mogłeś doświadczyć dopasowywania układu współrzędnych **odom** do **map**. Możesz poniżej zobaczyć odpowiadający temu fragment mapowania. W początkowej fazie widzimy jak jedna ze ścian korytarza zbiega się do drugiej, tak jakby korytarz się zwężał. Na kamerze możemy zobaczyć, że algorytm widzi tylko jedną ścianę. W momencie, gdy na obrazie pojawiają się obie, algorytm uznaje, że znajduje się w początkowej pozycji i naprawia mapę. Tym samym zmieniając transformację pomiędzy mapą, a odometrią. Później kamery cały czas widzą znajomą przestrzeń, dlatego cały czas próbują dopasować swoją pozycję i mapę, co skutkuje skakaniem układu współrzędnych **t265_odom_frame** w stosunku do układu **map**
  
-<!-- Włączyć tf - zobaczyć map-odom -->
+![tf_jumping](img/tf_jumping.gif "tf_jumping")
  
-**Zadanie 9:** Uruchom sterowanie robotem i przemieszczając się po środowisku, obserwuj jak w programie RVIZ tworzy się mapa danego środowiska. Zmapuj co najmniej jedno pomieszczenie. 
-```bash
-export TURTLEBOT3_MODEL=waffle
-roslaunch turtlebot3_teleop turtlebot3_teleop_key.launch
-```
+------
  
-Jeśli uznamy, że stworzona mapa jest już kompletna albo wystarczająca to możemy ją zapisać poleceniem (mapa musi zostać zapisana przed zamknięciem terminalu z aplikacją gmapping. Jeśli zamkniemy ją wcześniej to mapa będzie dalej wyświetlana w RVIZ jednak nie będzie już możliwości jej zapisu):
+# Orientacja
  
-```bash
-rosrun map_server map_saver -f <Ścieżka zapisu i nazwa mapy>
-```
-![map_save](img/map_save.gif "map_save")
+![orientation](img/orientation.png "orientation")
  
-**Zadanie 10:** Zobacz jakie pliki zostały stworzone w podanym katalogu i co zawiera każdy z plików. 
+Oprócz pozycji często potrzebujemy również orientacji robota. Orientację w trzech osiach możemy uzyskać dzięki żyroskopowi, akcelerometrowi i magnetometrowi. Magnetometr jest bardzo podatny na zakłócenia, szczególnie w pomieszczeniach i wymaga dobrej kalibracji, aby działał poprawnie. Z tego powodu zwykle używa się jedynie żyroskopu i akcelerometru. Akcelerometr pozwala nam wyliczyć dokładny kąt Pitch i Roll na podstawie wektora grawitacji jednak tylko przy wolniejszych zmianach. Dzięki żyroskopowi jesteśmy w stanie natomiast nadążać, za szybkimi zmianami jednak wyliczanie orientacji na jego podstawie polega na całkowaniu prędkości kątowych w każdej osi, co skutkuje ciągle narastającym błędem. Łącząc dane z akcelerometru i żyroskopu możemy śledzić szybkie zmiany orientacji, a zarazem zniwelować narastające błędy dzięki wektorowi grawitacji, ale niestety odnosi się to tylko do kątów Roll i Pitch, ponieważ obrót w osi Z (kąt Yaw) nie wpływa na odczyt wektora grawitacji. Z tego powodu kąt Yaw uzyskuje się jedynie z żyroskopu i jest on obarczony narastającym błędem. Aby zniwelować ten błąd używa się magnetometru, który wskazuje biegun magnetyczny ziemi. Można również wykorzystać inne metody określania kąta Yaw, jak np. użycie dwóch odbiorników GPS zamontowanych na dwóch krańcach pojazdu. 
  
+W następnym ćwiczeniu posłużymy się sterownikiem pixhawk, który został podłączony do komputera przewodem USB.
  
-Jak widać uruchomienie algorytmu SLAM było bardzo proste. Jeśli chcemy uruchomić gmapping z innymi parametrami niż domyślne, to najlepiej stworzyć plik launch np.:
-```xml
-<launch>
-  <arg name="scan_topic" default="scan" />
- 
-  <node pkg="gmapping" type="slam_gmapping" name="slam_gmapping" output="screen">
-    <param name="base_frame" value="base_footprint"/>
-    <param name="odom_frame" value="odom"/>
-    <param name="map_update_interval" value="5.0"/>
-    <param name="maxUrange" value="6.0"/>
-    <param name="maxRange" value="8.0"/>
-    <param name="sigma" value="0.05"/>
-    <param name="kernelSize" value="1"/>
-    <param name="lstep" value="0.05"/>
-    <param name="astep" value="0.05"/>
-    <param name="iterations" value="5"/>
-    <param name="lsigma" value="0.075"/>
-    <param name="ogain" value="3.0"/>
-    <param name="lskip" value="0"/>
-    <param name="minimumScore" value="100"/>
-    <param name="srr" value="0.01"/>
-    <param name="srt" value="0.02"/>
-    <param name="str" value="0.01"/>
-    <param name="stt" value="0.02"/>
-    <param name="linearUpdate" value="0.1"/>
-    <param name="angularUpdate" value="0.5"/>
-    <param name="temporalUpdate" value="-1.0"/>
-    <param name="resampleThreshold" value="0.5"/>
-    <param name="particles" value="30"/>
-    <param name="xmin" value="-5.0"/>
-    <param name="ymin" value="-5.0"/>
-    <param name="xmax" value="5.0"/>
-    <param name="ymax" value="5.0"/>
- 
-    <param name="delta" value="0.05"/>
-    <param name="llsamplerange" value="0.01"/>
-    <param name="llsamplestep" value="0.01"/>
-    <param name="lasamplerange" value="0.005"/>
-    <param name="lasamplestep" value="0.005"/>
-    <remap from="scan" to="$(arg scan_topic)"/>
-  </node>
-</launch>
-```
-Wyjaśnienie wszystkich parametrów można znaleźć w dokumnetacji [Gmapping](http://wiki.ros.org/gmapping).
+![pixhawk](img/pixhawk.png "pixhawk")
  
-Jeśli chcielibyśmy użyć tego algorytmu na własnym robocie to musielibyśmy zadbać o trzy elementy, które były wymienione wcześniej. Parametry, które za te elementy odpowiadają to 
-- scan (topic na którym pojawiają się scany 2d z lidaru)
-- odom_frame (nazwa układu współrzędnych odometrii)
-- base_frame (nazwa układu współrzędnych robota)
+Na kontrolerze pixhawk zainstalowane jest oprogramowanie Ardupilot, pozwalające na sterowanie pojazdami autonomicznymi ([dokumentacja](https://ardupilot.org/ardupilot/)). 
  
-Jeśli robot jest stworzony w sposób standardowy to układy współrzędnych powinny mieć domyślne nazwy, jednak nie zawsze tak jest. 
+Istnieje wiele akcesoriów do układu pixhawk, które zwiększają jego możliwości jak:
+- odbiornik RC
+- GPS
+- Zewnętrzny magnetometr
+- Radiokomunikacja 
+- ...
  
-Z parametrów, które warto wymienić to np.
-- maxUrange i maxRange - maksymalny użyteczny i maksymalny rzeczywisty zasięg Lidaru. W przypadku RPLidar S1 maksymalny zasięg wynosi 40 m, natomiast wartość użyteczną dla tego lidaru można ustawić np. na 20 m. 
-- delta - rozdzielczość mapy (domyślnie 1 pixel to kwadrat 5x5 cm)
-- linearUpdate i angularUpdate - odległość liniowa i kątowa po przejechaniu, której zaktualizowana zostanie mapa.
+Na pokładzie pixhawka mamy do dyspozycji żyroskop, akcelerometr, magnetometr oraz barometr. Na potrzeby tego ćwiczenia wykorzystamy akcelerometr i żyroskop, aby określić orientację kontrolera.
  
-**Zadanie 11:** Stwórz plik launch w swoim pakiecie zawierający ustawienia przedstawione powyżej, zmień parametry delta, linearUpdate i map_update_interval. Zaobserwuj zmiany w działaniu algorytmu. 
+Aby odczytać dane z kontrolera możemy wykorzystać dwie metody:
+- program stacji bazowej np. [QGroundControl](http://qgroundcontrol.com/) czy [Mission Planner](https://ardupilot.org/planner/). W przypadku systemu Windows zalecam używanie programu Mission Planner, ponieważ posiada on dużo użytecznych funkcjonalności. Niestety jest on dostępny jedynie na systemy Windows, dlatego na potrzeby laboratorium użyjemy QGroundControl. Nie mniej jednak program QGroundControl jest również bardzo użytecznym programem, a zainstalować możemy go nawet na telefonie z systemem Android. 
+- bezpośrednio przy pomocy protokołu mavlink. Tę metodę stosuje się, gdy do sterownika podłączamy komputer, który ma nadzorować jego pracę i wydawać mu komendy, tak jak w naszym przypadku. 
  
  
-Zamknij wszystkie aplikacje
+Najpierw rozpoczniemy pracę od programu QGroundControl. Zwykle używa się go przy początkowej konfiguracji pojazdu, a następnie do zdalnego monitorowania autonomicznej misji misji.
  
--------
+Uruchom program QGroundControl Znajdujący się na pulpicie komputera zdalnego.
  
-# Kamery Realsense
+![qgroundcontrol](img/qgroundcontrol.gif "qgroundcontrol")
  
-![d435_t265](img/d435_t265.jpg "d435_t265")
-
-Przedmiotem kolejnej części ćwiczenia będzie tworzenie map 3d z wykorzystaniem kamery głębi. Do realizacji celu posłużą nam dwie kamery:
-- realsense t265 - kamera stereowizyjna z układem logicznym realizująca slam wizyjny pozwalająca na uzyskanie pozycji i orientacji kamery. Test kamery [link](https://www.youtube.com/watch?v=mrdegXnG_no).
+Najpierw program łączy się z kontrolerem Pixhawk i ściąga jego aktualną konfigurację. W naszym przypadku uzyskamy komunikat, iż pojazd jest nie zdolny do działania, ponieważ nie zostały podłączone kluczowe komponenty. Do prawidłowego działania potrzebne jest zdalne połączenie przez Radio RC oraz GPS. 
  
-- realsense d435 - Kamera stereowizyjna z projektorem światła podczerwonego. Pozwala na uzyskanie informacji o głębi w obrazie. Test kamery [link](https://www.youtube.com/watch?v=UgwHXH-jT0U).
+PrzejdĹş do widoku mapy. 
+![qgroundcontrol_map](img/qgroundcontrol_map.gif "qgroundcontrol_map")
  
+Jeśli mielibyśmy podłączony GPS, wiedzielibyśmy aktualną pozycję naszego pojazdu. W prawym górnym rogu można zauważyć kilka podstawowych parametrów jak orientacja w trzech osiach, wysokość, prędkość czy czas lotu. 
  
-W odróżnieniu od peryferiów, które były wykorzystywane wcześniej (Lidar, arduino) komunikacja z kamerami nie jest realizowana przez serial port tylko przez sterownik usb. Do obsługi kamer potrzebujemy następującego oprogramowania i sterowników:
-- https://github.com/IntelRealSense/realsense-ros
+Możemy też dodać szybki podgląd dowolnego parametru klikając na koło zębate koło napisu **Valuse**
  
-Oprogramowanie jest już zainstalowane, dzięki czemu możemy od razu uruchomić program testowy dostarczony przez producenta. Uruchom go w terminalu:
-```bash
-realsense-viewer
-```
-![realsense_view](img/realsense_view.gif "realsense_view")
+**Zadanie 7:** Dodaj do podglądu trzy parametry Roll, Pitch i Heading. 
  
-**Zadanie 12:** Włącz obie kamery i zaobserwuj dane w widoku 2d i 3d.
  
-Sprawdź i zapisz numer seryjny obu kamer, które będą potrzebne za chwilę przy uruchamianiu ich w środowisku ros (odczytywanie numeru seryjnego przedstawiono również na powyższym gifie).
+Parametry możemy również wyświetlać w formie wykresów, przechodząc do zakładki Analyze
  
-Aby uruchomić obie kamery w środowisku ROS potrzebujemy pliku launch, takiego jak poniżej:
-```xml
-<launch>
-  <arg name="device_type_camera1"             default="t265"/>
-  <arg name="device_type_camera2"             default="d4.5"/>
-  <arg name="serial_no_camera1"         default=""/>      <!-- Note: Replace with actual serial number -->
-  <arg name="serial_no_camera2"         default=""/>      <!-- Note: Replace with actual serial number -->
-  <arg name="camera1"                   default="t265"/>  
-  <arg name="camera2"                   default="d400"/>    
-  <arg name="tf_prefix_camera1"         default="$(arg camera1)"/>
-  <arg name="tf_prefix_camera2"         default="$(arg camera2)"/>
-  <arg name="initial_reset"             default="false"/>
-  <arg name="enable_fisheye"            default="true"/>
-  <arg name="color_width"               default="640"/>
-  <arg name="color_height"              default="480"/>
-  <arg name="depth_width"               default="640"/>
-  <arg name="depth_height"              default="480"/>
-  <arg name="clip_distance"             default="-2"/>
-  <arg name="topic_odom_in"             default="odom_in"/>
-  <arg name="calib_odom_file"           default=""/>
+![QgroundControl_plot](img/qgroundcontrol_plot.gif "QgroundControl_plot")
  
-
-
-  <group ns="$(arg camera1)">
-    <include file="$(find realsense2_camera)/launch/includes/nodelet.launch.xml">
-      <arg name="serial_no"             value="$(arg serial_no_camera1)"/>
-      <arg name="device_type"             value="$(arg device_type_camera1)"/>
-      <arg name="tf_prefix"             value="$(arg tf_prefix_camera1)"/>
-      <arg name="initial_reset"         value="$(arg initial_reset)"/>
-      <arg name="enable_fisheye1"       value="$(arg enable_fisheye)"/>
-      <arg name="enable_fisheye2"       value="$(arg enable_fisheye)"/>
-      <arg name="topic_odom_in"         value="$(arg topic_odom_in)"/>
-      <arg name="calib_odom_file"       value="$(arg calib_odom_file)"/>
-      <arg name="fisheye_fps"         default="30"/>
-      <arg name="gyro_fps"            default="200"/>
-      <arg name="accel_fps"           default="62"/>
-      <arg name="enable_gyro"         default="true"/>
-      <arg name="enable_accel"        default="true"/>
-      <arg name="enable_pose"         default="true"/>
-
-      <arg name="enable_sync"           default="false"/>
-
-      <arg name="linear_accel_cov"      default="0.01"/>
-      <arg name="unite_imu_method"      default=""/>
-
-      <arg name="publish_odom_tf"     default="true"/>
-
-    </include>
-  </group>
- 
-  <group ns="$(arg camera2)">
-    <include file="$(find realsense2_camera)/launch/includes/nodelet.launch.xml">
-      <arg name="serial_no"             value="$(arg serial_no_camera2)"/>
-      <arg name="device_type"             value="$(arg device_type_camera2)"/>
-      <arg name="tf_prefix"             value="$(arg tf_prefix_camera2)"/>
-      <arg name="initial_reset"         value="$(arg initial_reset)"/>
-      <arg name="align_depth"           value="true"/>
-      <arg name="filters"               value="pointcloud"/>
-      <arg name="color_width"           value="$(arg color_width)"/>
-      <arg name="color_height"          value="$(arg color_height)"/>
-      <arg name="depth_width"           value="$(arg depth_width)"/>
-      <arg name="depth_height"          value="$(arg depth_height)"/>
-      <arg name="clip_distance"         value="$(arg clip_distance)"/>
-    </include>
-  </group>
-  <node pkg="tf" type="static_transform_publisher" name="t265_to_d400" args="0 0.0168 0.03 0 0 0 /$(arg tf_prefix_camera1)_link /$(arg tf_prefix_camera2)_link 100"/>
- 
-   <node pkg="depthimage_to_laserscan" type="depthimage_to_laserscan" name="depthimage_to_laserscan">
-        <remap from="image"     to="/$(arg camera2)/aligned_depth_to_color/image_raw"/>
-        <remap from="camera_info" to="/$(arg camera2)/depth/camera_info"/>
-        <remap from="scan" to="/scan"/>
-        <param name="output_frame_id" value="$(arg camera2)_depth_frame"/>
-        <param name="range_max" type="double" value="4"/>
-        <param name="range_min" type="double" value="0.2"/>
-        <param name="scan_time" type="double" value="0.033"/>
-        <param name="scan_height" type="double" value="100"/>
-    </node>
+Po dodaniu Parametru do wykresu możemy zauważyć, że jego zmiany są bardzo skokowe. Jest to spowodowane małą częstotliwością wysyłania tego parametry przez kontroler. Aktualną częstotliwość odświeżania możemy zobaczyć po lewej stronie przy nazwie grupy parametrów.
  
-</launch>
  
-```
-W powyższym pliku możemy zauważyć nowy znacznik "group". Pozwala on odseparować parametry jednej kamery od drugiej. Jest to konieczne, ponieważ w plikach launch uruchamianych w znacznikach "include" argumenty mają taką samą nazwę np. "serial_no". Gdyby nie grupowanie to po uruchomieniu drugiej kamery parametry pierwszej zostałyby zmienione. 
+![qgroundcontrol_plot](img/qgroundcontrol_plot.png "qgroundcontrol_plot")
  
-Wszystkie parametry możliwe do ustawienia możemy znaleźć tu [link](https://github.com/IntelRealSense/realsense-ros), w podpunkcie "Launch parameters". 
+Częstotliwość odświeżania możemy zmienić w ustawieniach w zakładce MAVLink:
  
-Oprócz uruchomienia kamer, włączany jest "static_transform_publisher", który publikuje w systemie stałą transformację pomiędzy jendą kamerą, a drugą. Dzięki temu wskazujemy jak kamery są przesunięte względem siebie i odometrię, którą uzyskujemy z kamery t265, będziemy mogli odnieść do drugiej kamery (będziemy mogli śledzić położenie również drugiej kamery).
+![QgroundControl_hz](img/qgroundcontrol_hz.gif "QgroundControl_hz")
  
-Ostatni uruchamiany node "depthimage_to_laserscan" jak sama nazwa wskazuje pozwala uzyskać laserscan z obrazu głębi. Laser skan będzie potrzebny przy tworzeniu mapy 3d. Jeśli nie posiadamy lidaru i używamy samych kamer to w ten sposób możemy go zasymulować. W przeciwnym wypadku uruchomilibyśmy po prostu lidar już bez programu "depthimage_to_laserscan".
+(Jeśli jest wyłączony to uruchom wiatrak na stronie zarządzania niniejszym laboratorium [link](http://153.19.49.102:5000) (Zakładka sterowanie. Po włączeniu wiatraka sprawdź na kamerze), aby moduły zaczęły się poruszać. Wyłącz go po zakończeniu ćwiczenia)
+
+**Zadanie 8:** Wyświetl na wykresie trzy prędkości kątowe (xgyro, ygyro, zgyro) i zaobserwuj, jak wyglądają wskazania przed zmianą i po zmianie częstotliwości odświeżania. 
  
-**Zadanie 13:** Stwórz plik launch uruchamiający obie kamery w swoim pakiecie. Uzupełnij powyższy plik o odpowiednie numery seryjne kamer (w czwartej i piątej linii, gdzie kamera1 to kamera t265, a kamera2 to kamera d400). 
  
-Uruchom stworzony plik oraz Rviz. W programie Rviz otwórz konfigurację "test_kamer.rviz" z pakietu "laboratorium_pliki_dodatkowe" w folderze "catkin_ws/src".
  
-![rviz_test_kamer](img/rviz_test_kamer.gif "rviz_test_kamer")
+Ostatnim ważnym aspektem możliwość programu QGroundControl jest możliwość konfiguracji ustawień pojazdu. Poniżej zaprezentowano zmianę parametru opisującego typ używanych sterowników silników **MOT_PWM_TYPE**. **Umiejętność konfiguracji pojazdu będzie prawdopodobnie niezbędna przy realizacji zadań projektowych.**
  
-![konfiguracja_rviz](img/kamery_rviz.png "konfiguracja_rviz")
  
-W konfiguracji zostały przedstawione obrazy z dwóch kamer. Jak widać kamera t265 jest wyposażona w obiektyw rybie oko, dzięki czemu ma dużo większe pole widzenia.  W środkowej części programu widoczna jest chmura punktów uzyskana z kamery głębi, którą można obejrzeć z różnych stron zmieniając widok kamery przy pomocy myszki. 
+![QgroundControl_param](img/qgroundcontrol_param.gif "QgroundControl_param")
  
-**Zadanie 14:** Uruchom program na arduino do sterowania serwem. Ustaw flagę w pozycji pionowej i zobacz czy flaga pojawiła się w chmurze punktów. 
  
-W programie Rviz znajduje się narzędzie measure dzięki któremu możemy zmierzyć odległości w scenie. Przy dokonywaniu pomiaru najlepiej zmienić styl wyświetlania chmury punktów. Po zmianie stylu na punkty, krawędź obiektu jest lepiej widoczna i można dokonać lepszego pomiaru. 
+Zamknij program QGroundControl
  
+----------
  
-![rviz_pomiar](img/rviz_pomiar.gif "rviz_pomiar")
+Po doświadczeniach z programem QGroundControl teraz połączymy się z kontrolerem Pixhawk przy pomocy środowiska ROS i protokołu MavLink. 
  
-**Zadanie 15:** Używając narzędzia measure, zmierz szerokość uniesionej flagi.
+Aby było to możliwe potrzebujemy pakietu mavros ([dokumntacja](http://wiki.ros.org/mavros)), który został już zainstalowwany na komputerach zdalnych. 
  
----------
+Przykładowy plik launch pozwalający uruchomić połączenie z pixhawkiem wygląda następująco:
  
-# Tworzenie mapy 3d 
+```xml
+<launch>
+        <arg name="fcu_url" default="/dev/ttyACM0:921600" />
+        <arg name="gcs_url" default="" />
+        <arg name="tgt_system" default="1" />
+        <arg name="tgt_component" default="1" />
+        <arg name="log_output" default="screen" />
+        <arg name="fcu_protocol" default="v2.0" />
+        <arg name="respawn_mavros" default="false" />
+ 
+        <include file="$(find mavros)/launch/node.launch">
+                <arg name="pluginlists_yaml" value="$(find mavros)/launch/apm_pluginlists.yaml" />
+                <arg name="config_yaml" value="$(find mavros)/launch/apm_config.yaml" />
+                <arg name="fcu_url" value="$(arg fcu_url)" />
+                <arg name="gcs_url" value="$(arg gcs_url)" />
+                <arg name="tgt_system" value="$(arg tgt_system)" />
+                <arg name="tgt_component" value="$(arg tgt_component)" />
+                <arg name="log_output" value="$(arg log_output)" />
+                <arg name="fcu_protocol" value="$(arg fcu_protocol)" />
+                <arg name="respawn_mavros" value="$(arg respawn_mavros)" />
+        </include>
+</launch>
+```
  
-Do przygotowania mapy 3d tak samo jak w przypadku mapy 2d potrzebowaliśmy algorytmu slam. Tym razem użyjemy rtabmap:
-- http://introlab.github.io/rtabmap/
-- http://wiki.ros.org/rtabmap_ros
+To co pojawia się przy każdym podłączeniu nowego urządzenia do komputera to konieczność sprawdzenia portu, do którego zostało ono przypisane. Sprawdź port do którego został podpięty pixhawk. Nazwa powinna rozpoczynać się od **usb-ArduPilot_fmuv3**.
  
-Jest to slam przeznaczony typowo dla kamer głębi (RGB-D). Ze względu zdalnej natury przeprowadzanego laboratorium nie ma możliwości na swobodną manipulację kamerami i stworzenia mapy dowolnej przestrzeni, dlatego prezentowane są metody pracy z danym nie pobieranymi z czujników w czasie rzeczywistym. W przypadku lidaru 2d i mapy 2d zaprezentowane było podejście symulacyjne. Tym razem przedstawione zostanie użycie prawdziwych danych z rzeczywistych czujników, które zostały zapisane wcześniej. To podejście pozwala wielokrotnie pracować na tych samych rzeczywistych pomiarach i testować różnego typu algorytmy. 
+![ls_pixhawk](img/ls_pixhawk.png "ls_pixhawk")
  
-W systemie ROS do zapisu danych używa się plików bag [link](http://wiki.ros.org/rosbag). Rosbag pozwala na nagrywanie i późniejsze odtwarzanie wybranych topiców oraz wiadomości, które się na nich pojawiają. Jak już wcześniej było wspomniane jest to świetne narzędzie do testowania algorytmów oraz analizy działania robota (wyszukiwania błędów itp.).
  
-Z plikami bag można pracować przy pomocy dwóch narzędzi do odtwarzania, analizy i nagrywania danych:
-- rqt_bag - narzędzie graficzne 
-- rosbag - narzędzie konsolowe  
+**Zadanie 9:** Stwórz plik launch we własnym pakiecie zmieniając jedynie parametr **fcu_url** zgodnie ze znalezionym portem przy pomocy komendy **ls** (zostaw liczbę 921600 po znaku ":", jest to prędkość przesyłania danych). 
  
-Uruchom roscore oraz rqt_bag.
  
-Stworzony plik bag znajduje się w pakiecie "laboratorium_pliki_dodatkowe" w folderze bagfiles.
+Po uruchomieniu stworzonego pliku powinno pojawić się wiele topiców udostępnionych przez mavros z danymi z naszego kontrolera. Wszystkie są opisane w [dokumntacji](http://wiki.ros.org/mavros).
  
-![rqt_bag](img/rqt_bag.gif "rqt_bag")
+![mavros_list](img/mavros_list.gif "mavros_list")
  
-Po załadowaniu pliku pokazały się wszystkie topici, które zostały zapisane w pliku. Po prawej stronie znajdują się osie czasu po zbliżeniu których widzimy poszczególne wiadomości. Możemy podejrzeć dane na topicu klikając na nim prawym przyciskiem myszki i wybierając opcję view. W zależności od danych jest kilka możliwych wariantów podglądu. 
-- raw - podgląd poszczególnych wiadomości, podobnie jak rostopic echo
-- plot - wykreślenie całego przebiegu wybranych wartości z wiadomości
-- image - w przypadku obrazu z kamery moĹźliwe jest podejrzenie poszczegĂłlnych klatek
+Posiadając dostęp do danych możemy zająć się przedmiotem tego ćwiczenia, czyli obliczeniem orientacji naszego kontrolera, a potencjalnie robota/pojazdu. Po pierwsze potrzebne są nam dane z żyroskopu i akcelerometru. Są one dostępne na topicu **/mavros/imu/data_raw**. Wiadomości na tym topicu są typu [sensor_msgs/Imu](http://docs.ros.org/en/melodic/api/sensor_msgs/html/msg/Imu.html) i standardowo zawierają orientację (w tym przypadku jej nie ma, ponieważ są wysyłane na tym topicu tylko dane z żyroskopu i akcelerowmetru), przyspieszenia liniowe w trzech osiach oraz prędkości kątowe w trzech osiach.
  
-**Zadanie 16:** Podejrzyj dane na topicu d400/color/image_raw oraz t265/odom/sample. Pierwszy topic podejrzyj w trybie image (view -> image), natomiast drugi w trybie plot. Zmaksymalizuj okno rqt_bag, powiększ okno wykresu plot przeciągając jego krawędź w lewą stronę. W oknie plot po prawej stronie jest wyświetlana struktura wiadomości odom. Wyświetl pozycję x i y (pose -> pose -> position -> x/y). Lewym przyciskiem myszki możesz zmieniać pozycję czasu na osi. Zaobserwuj, jak zmieniają się klatki filmu z kamery oraz jak zmienia się pozycja pionowej czerwonej osi na wykresie plot. Dodatkowo możesz uruchomić odtwarzanie wiadomości w czasie ciągłym klikając zieloną strzałkę będącą 7 opcją na panelu w lewym górnym rogu ekranu  
+![imu_mes](img/imu_mes.png "imu_mes")
  
-Niestety odtwarzanie plików bag z programu rqt_bag tak aby wiadomości były widziane w systemie ROS nie jest efektywne, dlatego zamknij program rqt_bag. Do odtworzenia posłużymy się narzędziem konsolowym rosbag.
+Przy przetwarzaniu tego typu danych ważna jest częstotliwość publikowania danych. Możemy ją sprawdzić przy pomocy komendy:
  
-Otwórz terminal i przejdź do katalogu zawierającego plik bag, który otwierałeś przed chwilą. Tak samo jak przy pomocy rqt_bag i przy użyciu rosbag możemy dowiedzieć się podstawowych informacji o pliku. 
-UĹźyj komendy:
 ```bash
-rosbag info <nazwa pliku>
+rostopic hz <nazwa topicu>
 ```
+![mavros_hz](img/mavros_hz.gif "mavros_hz")
  
-![rosbag_info](img/rosbag_info.gif "rosbag_info")
+Sprawdź jak często pojawiają się wiadomości na tym topicu. Domyślnie dane wysyłane są z częstotliwością 2 Hz. Aby algorytm określający działał poprawnie potrzebujemy danych znacznie częściej. Ideałem byłyby informacje dostarczane z częstotliwością 200 Hz lub wyższą. Wtedy śledzenie orientacji byłoby bardzo płynne i pojawiałyby się mniejsze błędy. Pixhawk, z domyślnym firmwarem ardupilot rover jest w stanie wysyłać dane z sensorów z maksymalną częstotliwością ok 40 Hz. Nie jest to wartość idealna jednak już wystarczająca, aby wyznacza orientacja była użyteczna. 
  
-Jak widzimy otrzymaliśmy informacje o czasie trwania pliku, kiedy był nagrywany, wielkości pliku, ale co najważniejsze widzimy jakie topici zostały nagrane oraz jakiego typu wiadomości się na nich znajdują.
+Aby zmienić częstotliwość wysyłanych danych musimy użyć komendy:
  
-Jeśli chcemy otworzyć plik bag ze wszystkimi topicami to musimy użyć komendy:
 ```bash
-rosbag play <nazwa pliku>
+rosrun mavros mavsys rate <nazwa grupy danych> <częstotliwość>
 ```
-![rosbag_play](img/rosbag_play.gif "rosbag_play")
  
-Używając przycisku **spacja** możemy zatrzymywać odtwarzanie pliku.
+Aby zmienić częstotliwość wysyłania surowych danych z sensorów musimy użyć komendy:
  
-W przypadku kiedy chcemy aby odtwarzały się jedynie wybrane topici wtedy komenda powinna wyglądać następująco 
 ```bash
-rosbag play <nazwa pliku> --topics <topic 1> <topic 2 ...>
+rosrun mavros mavsys rate --raw-sensors 40
 ```
  
-rosbag posiada jeszcze wiele różnych możliwości jak przyspieszanie i opóźnianie odtwarzania plików. Wszystkie dostępne możliwości można sprawdzić w dokumnetacji [link](http://wiki.ros.org/rosbag/Commandline)
- 
-**Zadanie 17:** Odtwórz plik bag, który otwierałeś w rqt_bag, przy pomocy rosbag. Podczas odtwarzania, otwórz program RVIZ i podejrzyj obraz z kamery (topic - /t265/fisheye1/image_raw) i zmieniającą się pozycję kamery (topic - /t265/odom/sample). Oba elementy możesz dodać do widoku RVIZa przez Add -> by topic -> nazwa topica
- 
------
- 
-Posiadając już dane na których możemy użyć algorytmu slam, możemy zająć się przygotowaniem pliku launch do jego uruchomienia. Plik launch do uruchamiania samego algorytmu wygląda następująco:
- 
-```xml
-<launch>
-    <arg name="camera1" default="t265"/>  
-    <arg name="camera2" default="d400"/>
-    <node name="rtabmap" pkg="rtabmap_ros" type="rtabmap" output="screen" args="--delete_db_on_start">
-          <param name="frame_id" type="string" value="/$(arg camera2)_link"/>
- 
-          <param name="subscribe_depth" type="bool" value="true"/>
-          <param name="subscribe_scan" type="bool" value="true"/>
- 
-          <remap from="odom" to="/$(arg camera1)/odom/sample"/>
-          <remap from="scan" to="/scan"/>
- 
-          <remap from="rgb/image"       to="/$(arg camera2)/color/image_raw"/>
-          <remap from="depth/image"     to="/$(arg camera2)/aligned_depth_to_color/image_raw"/>
-          <remap from="rgb/camera_info" to="/$(arg camera2)/color/camera_info"/>
- 
-          <param name="queue_size" type="int" value="200"/>
- 
-          <!-- RTAB-Map's parameters -->
-          <param name="RGBD/ProximityBySpace"     type="string" value="false"/>
-          <param name="RGBD/AngularUpdate"        type="string" value="0.01"/>
-          <param name="RGBD/LinearUpdate"         type="string" value="0.01"/>
-          <param name="RGBD/OptimizeFromGraphEnd" type="string" value="false"/>
-          <param name="Reg/Force3DoF"             type="string" value="true"/>
-          <param name="Vis/MinInliers"            type="string" value="12"/>
-          <param name="RGBD/OptimizeMaxError"     type="string" value="0.1"/>
-    </node>
- 
-</launch>
-```
-Dzięki tak stworzonemu plikowi moglibyśmy uruchomić algorytm rtabmap przy użyciu rzeczywistych sensorów. Jeśli używamy plik bag musimy zadbać o jeszcze jedną rzecz. Jeśli uruchomilibyśmy powyższy plik z danymi odtwarzanymi z pliku bag to algorytm zwróciłby błąd o braku aktualnych danych. Jest to spowodowane różnicą pomiędzy aktualnym czasem, a czasem zawartym w wiadomościach. Większość wiadomości w systemie ROS posiada nagłówek, w którym zawarta jest informacja w jakiej chwili dany pomiar/wiadomość został wysłany/stworzony/zmierzony tzw. timestamp. Skoro odtwarzamy nasze wiadomości to ich timestamp może być nawet z przed kilku dni. Natomiast algorytm wymaga, aby dane były nie starsze niż 5s. Aby rozwiązać ten problem użyjemy czasu symulowanego. Podczas symulacji oraz odtwarzania danych symulacja czasu pozwala nie tylko przyspieszać czy zwalniać czas, ale również rozpoczynać wielokrotnie daną sekwencję od stałego punktu w czasie. 
+**Zadanie 10:** Zmień częstotliwość wysyłania danych z sensorów i sprawdź komendą ```rostopic hz```, czy dane rzeczywiście przychodzą częściej.
  
-W systemie ROS, gdy chcemy używać czasu symulowanego potrzebujemy dwóch rzeczy:
-- ustawić parametr "use_sim_time" na true
-- oraz zapewnić program, który będzie publikował symulowany czas na topic /clock
+Do obliczenia orientacji robota posłużymy się filtrem madgwica, który był omawiany podczas wykładów [więcej_informacji](https://x-io.co.uk/open-source-imu-and-ahrs-algorithms/). Zwykle większość popularnych filtrów/algorytmów jest już dostępna w formie pakietów w systemie ROS i nie inaczej jest w tym przypadku. Filtr madwicka znajdziemy w pakiecie imu_tools->imu_filter_madgwick [dokumentacja](http://wiki.ros.org/imu_filter_madgwick?distro=melodic). 
  
-Dzięki parametrowi "use_sim_time" ROS wie, że każdy node powinien używać czasu z topicu /clock, a nie czasu systemowego (rzeczywistego). W naszym przypadku programem publikującym czas, będzie rosbag. Wystarczy przy jego uruchamianiu dodać flagę ```--clock```
+![madgwick](img/madgwick.png "madgwick")
  
-Jeśli uruchamiali byśmy wszystko po kolei w terminalu, komendy wyglądałby następująco (każda kolejna komenda w osobnym terminalu):
-```bash
-roscore
-rosparam set /use_sim_time true
-rosbag play <nazwa pliku> --clock
-```
-![rosbag_play_sim_time](img/rosbag_play_sim_time.gif "rosbag_play_sim_time")
+Po przeczytaniu dokumentacji możemy zauważyć, iż algorytm do działania oczekuje danych na topicu **imu/data_raw** oraz **imu/mag**. W naszym przypadku nie będziemy używać magnetometru, dlatego będziemy używać tylko topicu **imu/data_raw**.
  
-Jaki widać po ustawieniu parametru /use_sim_time oraz argumentu --clock, pojawił się topic /clock. Dodatkowo w programie Rviz w dolnej części okna można zauważyć, że czas ROS Time oraz Wall Time różnią się od siebie. 
-- ROS Time - Czas uĹźywany przez system ROS (w tym przypadku symulowany)
-- Wall Time - Czas rzeczywisty pobrany z systemu operacyjnego
+Plik launch uruchamiający filtr madgwicka został stworzony w pakiecie **laboratorium_pliki_dodatkowe** i przedstawia się następująco:
  
-Do realizacji mapy 3d posłużymy się jednak plikiem launch, żeby uprościć proces uruchamiania całości
-```xml
 <launch>
-    <arg name="bag_file_dir" default="$(find laboratorium_pliki_dodatkowe)/bagfiles/lab_kor.bag"/>
-    <param name="use_sim_time" type="bool" value="true"/>
-    <node name="bag_file" pkg="rosbag" type="play" output="screen" args="--clock $(arg bag_file_dir)"/>
+  <node pkg="imu_filter_madgwick" name="imu_filter_node" type="imu_filter_node"  >
+        <rosparam>
+            use_mag: false
+            fixed_frame: base_link
+            publish_tf: false
+        </rosparam>
+      <remap from="/imu/data_raw" to="/mavros/imu/data_raw"/>
+    </node>
 </launch>
-```
-Najpierw jako argument definiujemy ścieżkę do naszego pliku bag. Następnie ustawiamy parametr "use_sim_time" aby ROS używał czasu symulowanego. Na końcu uruchamiamy nasz plik bag dodając argumenty zawierające parametr **--clock** powodujący publikowanie czasu symulacji oraz ścieżkę do pliku bag.
- 
- 
-**Zadanie 18:** Stwórz plik lauch uruchamiający odtwarzanie pliku bag oraz algorytm rtabmap. 
  
-Po uruchomieniu stworzonego pliku, zatrzymaj go klawiszem **spacja**. W konsoli powinien pojawić się komunikat **[PAUSED]**. Następnie otwórz program RVIZ i załaduj konfigurację **mapowanie_wizualizacja.rviz** z pakietu **laboratorium_pliki_dodatkowe**.
+W powyższym pliku pojawiają się dwie nowe informacje. Po pierwsze możliwy jest inny sposób definiowania parametrów w znaczniku **rosparam**. Druga dotyczy znacznika **remap**. Tak jak było wspomniane wcześniej filtr oczekuje danych na topicu **/imu/data_raw** natomiast nasz kontroler wystawia je na topicu **/mavros/imu/data_raw**. Dzięki znacznikowi **remap** możemy zmienić nazwę wszystkich topiców z jakich korzysta dany node. Jest to bardzo przydatne w takiej sytuacji jaka zaszła teraz. Nie musimy zmieniać nic w kodzie jednego czy drugiego programu, tylko wystarczy dodać jedną linijkę w pliku launch. 
  
-Konfiguracja RVIZ przedstawia:
-- MapCloud - wizualizacja chmur punktów składających się na mapę 3d
-- Tf - wizualizacja układów współrzędnych
-- Widok kameru ustawiony w taki sposób, aby podążał za jednym z układów współrzędnych.
+Po uruchomieniu pliku launch na topicu **/imu/data** powinny pojawić się dane tym razem wzbogacone o orientacje. Pakiet imu_tools dostarcza również plugin rozszerzający możliwość programu RVIZ o wyświetlanie orientacji obiektu. 
  
-W konsoli z uruchomionym plikiem launch użyj ponownie klawisza **spacja** aby dane znów były odtwarzane. Przy pomocy myszki możesz zmieniać orientację kamery i odległość od układu współrzędnych.
- 
-![rosbag_play_map](img/rosbag_play_map.gif "rosbag_play_map")
- 
- 
-Plik zawiera dwa przejazdy po korytarzu. Poczekaj, aż stworzy się cała mapa. Podczas drugiego przejazdu zaobserwujesz mocne skoki kamery. Skoki te zostaną wyjaśnione w laboratorium nr 4. Gdy pojawi się skakanie kamery przełącz "Target Frame" na map lub Fixed frame. Opcje zmiany widoku znajdują się po prawej stronie okna. 
- 
-**Zadanie 19:** Zrób zrzut ekranu, na którym będzie widoczna cała stworzona mapa. 
- 
-Po zatrzymaniu mapowania (Ctrl+c w terminalu z uruchomionym plikiem launch), mapa zapisuje się w **/home/robot/.ros/rtabmap.db**. Możemy ją otworzyć i przeglądać dzięki programowi 
-```bash
-rtabmap
-```
-![rtabmap](img/rtabmap.gif "rtabmap")
+![mavros_mad_wiz](img/mavros_mad_wiz.gif "mavros_mad_wiz")
  
-Niestety przy połączeniu zdalnym program **rtabmap** nie radzi sobie z poprawnym wyświetlaniem danych. 
+**Zadanie 11:** Uruchom filtr madgwicka plikiem launch z pakietu **laboratorium_pliki_dodatkowe** i uruchom wizualizacje w programie RVIZ. 
+ (Jeśli jest wyłączony to uruchom wiatrak na stronie zarządzania niniejszym laboratorium [link](http://153.19.49.102:5000) (Zakładka sterowanie. Po włączeniu wiatraka sprawdź na kamerze), aby moduły zaczęły się poruszać. Wyłącz go po zakończeniu ćwiczenia)
  
-Stworzoną mapę można również wygenerować w standardowym formacie .ply czy .pcd do dalszej analizy czy obróbki.
  
-![map_export](img/map_export.png "map_export")
\ No newline at end of file
diff --git a/lab04/img/imu_mes.png b/lab03/img/imu_mes.png
similarity index 100%
rename from lab04/img/imu_mes.png
rename to lab03/img/imu_mes.png
diff --git a/lab04/img/ls_pixhawk.png b/lab03/img/ls_pixhawk.png
similarity index 100%
rename from lab04/img/ls_pixhawk.png
rename to lab03/img/ls_pixhawk.png
diff --git a/lab04/img/madgwick.png b/lab03/img/madgwick.png
similarity index 100%
rename from lab04/img/madgwick.png
rename to lab03/img/madgwick.png
diff --git a/lab04/img/mavros_hz.gif b/lab03/img/mavros_hz.gif
similarity index 100%
rename from lab04/img/mavros_hz.gif
rename to lab03/img/mavros_hz.gif
diff --git a/lab04/img/mavros_list.gif b/lab03/img/mavros_list.gif
similarity index 100%
rename from lab04/img/mavros_list.gif
rename to lab03/img/mavros_list.gif
diff --git a/lab04/img/mavros_mad_wiz.gif b/lab03/img/mavros_mad_wiz.gif
similarity index 100%
rename from lab04/img/mavros_mad_wiz.gif
rename to lab03/img/mavros_mad_wiz.gif
diff --git a/lab04/img/mobile_robot_standard_tf.png b/lab03/img/mobile_robot_standard_tf.png
similarity index 100%
rename from lab04/img/mobile_robot_standard_tf.png
rename to lab03/img/mobile_robot_standard_tf.png
diff --git a/lab04/img/model_katana.png b/lab03/img/model_katana.png
similarity index 100%
rename from lab04/img/model_katana.png
rename to lab03/img/model_katana.png
diff --git a/lab04/img/model_tf.png b/lab03/img/model_tf.png
similarity index 100%
rename from lab04/img/model_tf.png
rename to lab03/img/model_tf.png
diff --git a/lab04/img/odom_wzor.png b/lab03/img/odom_wzor.png
similarity index 100%
rename from lab04/img/odom_wzor.png
rename to lab03/img/odom_wzor.png
diff --git a/lab04/img/orientation.png b/lab03/img/orientation.png
similarity index 100%
rename from lab04/img/orientation.png
rename to lab03/img/orientation.png
diff --git a/lab04/img/pixhawk.png b/lab03/img/pixhawk.png
similarity index 100%
rename from lab04/img/pixhawk.png
rename to lab03/img/pixhawk.png
diff --git a/lab04/img/qgroundcontrol.gif b/lab03/img/qgroundcontrol.gif
similarity index 100%
rename from lab04/img/qgroundcontrol.gif
rename to lab03/img/qgroundcontrol.gif
diff --git a/lab04/img/qgroundcontrol_hz.gif b/lab03/img/qgroundcontrol_hz.gif
similarity index 100%
rename from lab04/img/qgroundcontrol_hz.gif
rename to lab03/img/qgroundcontrol_hz.gif
diff --git a/lab04/img/qgroundcontrol_map.gif b/lab03/img/qgroundcontrol_map.gif
similarity index 100%
rename from lab04/img/qgroundcontrol_map.gif
rename to lab03/img/qgroundcontrol_map.gif
diff --git a/lab04/img/qgroundcontrol_param.gif b/lab03/img/qgroundcontrol_param.gif
similarity index 100%
rename from lab04/img/qgroundcontrol_param.gif
rename to lab03/img/qgroundcontrol_param.gif
diff --git a/lab04/img/qgroundcontrol_plot.gif b/lab03/img/qgroundcontrol_plot.gif
similarity index 100%
rename from lab04/img/qgroundcontrol_plot.gif
rename to lab03/img/qgroundcontrol_plot.gif
diff --git a/lab04/img/qgroundcontrol_plot.png b/lab03/img/qgroundcontrol_plot.png
similarity index 100%
rename from lab04/img/qgroundcontrol_plot.png
rename to lab03/img/qgroundcontrol_plot.png
diff --git a/lab04/img/sim_rviz_setup.gif b/lab03/img/sim_rviz_setup.gif
similarity index 100%
rename from lab04/img/sim_rviz_setup.gif
rename to lab03/img/sim_rviz_setup.gif
diff --git a/lab04/img/tf_echo.gif b/lab03/img/tf_echo.gif
similarity index 100%
rename from lab04/img/tf_echo.gif
rename to lab03/img/tf_echo.gif
diff --git a/lab04/img/tf_echo.png b/lab03/img/tf_echo.png
similarity index 100%
rename from lab04/img/tf_echo.png
rename to lab03/img/tf_echo.png
diff --git a/lab04/img/tf_jumping.gif b/lab03/img/tf_jumping.gif
similarity index 100%
rename from lab04/img/tf_jumping.gif
rename to lab03/img/tf_jumping.gif
diff --git a/lab04/img/tf_tree.png b/lab03/img/tf_tree.png
similarity index 100%
rename from lab04/img/tf_tree.png
rename to lab03/img/tf_tree.png
diff --git a/lab04/img/urdf_rviz.gif b/lab03/img/urdf_rviz.gif
similarity index 100%
rename from lab04/img/urdf_rviz.gif
rename to lab03/img/urdf_rviz.gif
diff --git a/lab04/README.md b/lab04/README.md
deleted file mode 100644
index af7b581e8869b4765450fbad7b2d0829a02bf533..0000000000000000000000000000000000000000
--- a/lab04/README.md
+++ /dev/null
@@ -1,303 +0,0 @@
-# Przygotowanie przestrzeni roboczej
-Połącz się ze zdalnym środowiskiem i ściągnij swoje repozytorium do przestrzeni roboczej catkin_ws.
- 
- 
-# Transformacje pomiędzy układami współrzędnych
-Przed przystąpieniem do omawiania nawigacji ważnym jest, aby rozumieć jak wiele układów współrzędnych znajduje się w platformie robotycznej, jak są one ze sobą połączone oraz jak określać położenie i orientację względem poszczególnych układów współrzędnych. 
- 
-Działanie zależności pomiędzy układami współrzędnych zostanie zwizualizowane na przykładzie manipulatora o 5 stopniach swobody. 
- 
-![model_katana](img/model_katana.png "model_katana")
- 
-W systemie ROS możemy wizualizować roboty i elementy środowiska przy pomocy opisu URDF. Na niniejszym laboratorium jednak nie zostanie on wyjaśniony. Jeśli chcesz dowiedzieć jak tworzy się pliki URDF i jak stworzyć model własnego robota, to tu znajdziesz więcej informacji [link](http://wiki.ros.org/urdf/Tutorials) (tutorial opisuje, jak stworzyć prosty model robota R2D2).
- 
-W skrócie pliki URDF pozwalają opisać jak złożony jest robot. Z jakich składa się elementów i jak są one połączone ze sobą. 
- 
-Aby uruchomić naszą wizualizację potrzebujemy pliku launch.
- 
-```xml
-<launch>
-  <param name="robot_description" command="$(find xacro)/xacro.py '$(find katana_description)/urdf/katana_300_6m180.urdf.xacro'" />
-    <node pkg="joint_state_publisher_gui" type="joint_state_publisher_gui" name="joint_state_publisher">
-    </node>
- 
-    <node pkg="robot_state_publisher" type="robot_state_publisher" name="robot_state_publisher">
-        <param name="ignore_timestamp" value="false"/>
-    </node>
- 
-    <node pkg="tf" type="static_transform_publisher" name="katana_base_link_to_box" args="0 0.5 0 0 0 0 /katana_base_link /box 100"/>
-</launch>
-```
-Został on już stworzony w pakiecie "laboratorium_pliki_dodatkowe". 
- 
-Po pierwsze musimy załadować opis naszego robota jako parametr pod nazwą **robot_description**. 
- 
-Następnie uruchamiamy trzy node'y. Pierwszy odpowiada za publikowanie stanu poszczególnych przegubów robota. Po uruchomieniu zobaczysz okno z interfejsem, który pozwoli na zmianę pozycji poszczególnych przegubów robota. Jeśli posiadalibyśmy prawdziwego robota, którego chcielibyśmy wizualizować, wtedy jego sterownik publikowałby aktualne stany poszczególnych przegubów.  
- 
-Kolejny node na podstawie pliku urdf oraz danych publikowanych przez **joint_state_publisher** uaktualnia pozycje wizualizowanych elementĂłw robota.
- 
-Ostatni node **static_transform_publisher** publikuje stałe przekształcenie (odsunięcie o 0.5 m w osi y) pomiędzy układem współrzędnych **katana_base_link**, a **box**. Ma on symulować położenie przykładowego elementu i posłuży do lepszego wyjaśnienia omawianego zagadnienia.  
- 
-Uruchom plik launch **wizualizacja_urdf** z paketu **laboratorium_pliki_dodatkowe**. Następnie uruchom program RVIZ i dodaj wizualizacje modelu robota, tf oraz zmień stały układ współrzędnych na **katana_base_link**.
- 
-![urdf_rviz](img/urdf_rviz.gif "urdf_rviz")
- 
- 
-**Zadanie 1:** Przy pomocy **joint_state_publisher** obróć poszczególne przeguby robota i zobacz, jak zmienia się wizualizacja. Zmień stały układ współrzędnych (Fixed frame) na wybrany układ zaczynający się od nazwy **katana_motor**. Sprawdź jak teraz zmienia się wizualizacja przy ruchach przegubów. 
- 
- 
-![model_tf](img/model_tf.png "model_tf")
- 
-Jak widać robot złożony jest z wielu układów współrzędnych połączonych ze sobą. Każdy kolejny układ współrzędnych jest zaczepiony w poprzednim i względem niego jest określana jego pozycja. Taki sposób organizacji wszystkich układów współrzędnych pozwala nam w łatwy sposób wyliczyć, gdzie np. znajduje się końcówka robota względem podstawy lub dowolnego innego obiektu np. pudełka o układzie współrzędnych **box**. Wszystkie połączenia pomiędzy układami współrzędnych są zorganizowane w postaci struktury drzewa. Aktualną strukturę możemy zobaczyć przy pomocy narzędzia rqt i TF Tree.
- 
-**Zadanie 2:** Uruchom program rqt przy pomocy komendy ``` rqt ``` w terminalu. Następnie włącz wizualizacje TF Tree (Plugins -> Visualization -> TF Tree).
- 
-Po uruchomieniu TF Tree możesz zobaczyć jak cała struktura jest zorganizowana. Dodatkowo, dzięki temu narzędziu możemy sprawdzić czy na pewno wszystko jest ze sobą połączone. Jeśli któryś z elementów nie będzie połączony z resztą to dane z takiego elementu nie mogą być odniesione do innych np. jeśli nie wprowadzimy jak zamontowany jest lidar na robocie to będziemy wiedzieć jak daleko znajduje się przeszkoda od lidaru, ale nie będziemy wiedzieć jak blisko znajduje się od np. przedniego zderzaka robota. 
- 
-![tf_tree](img/tf_tree.png "tf_tree")
- 
-W dowolnej chwili możemy sprawdzić aktualne przesunięcie (transformacje) pomiędzy dwoma dowolnymi układami współrzędnych. Możemy to zrobić przy pomocy komendy:
-```bash
-rosrun tf tf_echo <nazwa układu współrzędnych> <nazwa układu współrzędnych>
-```
- 
-![tf_echo](img/tf_echo.gif "tf_echo")
- 
-![tf_echo](img/tf_echo.png "tf_echo")
- 
-Program tf co sekundę pokazuje nam odległość i zmianę orientacji pomiędzy dwoma układami współrzędnych. Przesunięcie określone jest kolejno w osiach X,Y,Z. Natomiast zmiana orientacji podana jest w trzech różnych jednostkach.
- 
- 
-**Zadanie 3:** Uruchom tf_echo pomiędzy układami współrzędnych **katana_gripper_tool_frame** i **box**, a następnie ustaw końcówkę robota na tyle blisko układu współrzędnych **box**, aby odległość w żadnej osi nie przekraczała 0.2 m.
- 
------------
- 
-# Odometria
- 
-Przechodząc do pojazdów, standardowa struktura układów współrzędnych i ich nazewnictwo przedstawia się następująco:
- 
-![mobile_robot_standard_tf](img/mobile_robot_standard_tf.png "mobile_robot_standard_tf")
- 
-Standardowo wszystkie elementy umieszczone na robocie są ostatecznie połączone z układem **base_link**. Następnie układ **base_link** jest połączony z układem **base_footprint**, który jest rzutem na płaszczyznę układu **base_stabilized**, ale zwykle pomija się układ **base_stabilized** i zwykle się go nie tworzy.
- 
-Kolejne połączenie jest tworzone pomiędzy **base_footprint**, a **odom**. Tym razem jest to już połączenie odpowiedzialne za lokalizację. Tak jak poprzednie układy współrzędnych były raczej statyczne względem siebie, tak **base_footprint** i **odom** już nie. Transformacja ta obrazuje przesunięcie robota względem punktu początkowego (początkiem układu współrzędnych odom) i zwykle wykorzystywane są do tego metody przyrostowe. Co ważne w systemie ROS przyjęte jest, że w tej transformacji nie mogą występować przeskoki w pozycji (pozycja musi zmieniać się w sposób ciągły). 
- 
-Ostatnim zaprezentowanym połączeniem jest pomiędzy **odom** i **map**. To połączenie pozwala pokazać, gdzie robot znajduje się na mapie. Tu mogą już występować przeskoki w pozycji tzn. transformacja pomiędzy **map** i **odom** jest tak dobierana, aby końcowa pozycja robota względem mapy była zgodna z rzeczywistością (dokonuje tego zwykle algorytm SLAM). 
- 
-Czasami stosuje się jeszcze układ współrzędnych **earth**, która jest już ostatecznym układem współrzędnych i zwykle odnosi się do koordynatów GPS. Ten układ współrzędnych pozwala nam na odniesienie względem siebie kilku map i przypisać im rzeczywistą pozycję na "Świecie"
- 
-------
- 
-Jeśli znamy już całą konwencję tworzenia i nazewnictwa układów współrzędnych, możemy przejść do nawigacji zliczeniowej.
-Zwykle pozycja w nawigacji zliczeniowej robotów mobilnych jest określana na podstawie enkoderów umieszczonych na kołach. Wiedząc jak obróciły się koła możemy obliczyć pozycję robota na podstawie poniższych wzorów:
- 
-![odom_wzor](img/odom_wzor.png "odom_wzor")
- 
- 
-W systemie ROS obliczaniem odometri zakmuje się program obsługujący fizyczne enkodery, natomiast już w systemie posługujemy się wadomością Odometry ([dokumnetacja](http://docs.ros.org/en/melodic/api/nav_msgs/html/msg/Odometry.html)). Zawiera ona zarówno aktualną pozycję jak i prędkość. Dodatkowo oba parametry są opatrzone macierzą kowariancji, która reprezentuje niepewność każdego z pomiarów. 
- 
-Uruchom symulację robota turtlebot:
-```bash
-export TURTLEBOT3_MODEL=waffle
-roslaunch turtlebot3_gazebo turtlebot3_world.launch
-```
- 
-Uruchom RVIZ
-```bash
-export TURTLEBOT3_MODEL=waffle
-roslaunch turtlebot3_gazebo turtlebot3_gazebo_rviz.launch
-```
- 
-Uruchom program sterujący robotem
-```bash
-export TURTLEBOT3_MODEL=waffle
-roslaunch turtlebot3_teleop turtlebot3_teleop_key.launch
-```
- 
-**Zadanie 4:** Uruchom program rqt i zobacz, jak wyglądają zależności pomiędzy układami współrzędnych w tym robocie.
- 
-Przejdź do programu RVIZ, zmień ustawienie widoku na **ThirdPersonFollower**, zbliż kamerę do robota. Włącz wizualizację TF i włącz widoczność tylko następujących układów **base_link**, **base_scan**, **wheel_right_link**, **wheel_left_link**, **odom**. Zmniejsz rozmiar zwizualizowanych układów współrzędnych "Marker scale", tak aby nie przysłaniały modelu robota. 
- 
-![sim_rviz_setup](img/sim_rviz_setup.gif "sim_rviz_setup")
- 
-**Zadanie 5:** Podejrzyj co dzieje się na topicu **/odom**. Obserwując robota jak i topic **/odom** przejedź około 1 m do przodu symulowanym robotem. 
- 
-**Zadanie 6:** Zmień stały układ współrzędnych "Fixed Frame" z **odom** na **base_scan**. Przejedź ponownie robotem jakąś odległość w dowolnym kierunku. Jak teraz prezentowane są dane w programie RVIZ?
- 
-Podczas tworzenia mapy 3d na laboratorium nr 3 mogłeś doświadczyć dopasowywania układu współrzędnych **odom** do **map**. Możesz poniżej zobaczyć odpowiadający temu fragment mapowania. W początkowej fazie widzimy jak jedna ze ścian korytarza zbiega się do drugiej, tak jakby korytarz się zwężał. Na kamerze możemy zobaczyć, że algorytm widzi tylko jedną ścianę. W momencie, gdy na obrazie pojawiają się obie, algorytm uznaje, że znajduje się w początkowej pozycji i naprawia mapę. Tym samym zmieniając transformację pomiędzy mapą, a odometrią. Później kamery cały czas widzą znajomą przestrzeń, dlatego cały czas próbują dopasować swoją pozycję i mapę, co skutkuje skakaniem układu współrzędnych **t265_odom_frame** w stosunku do układu **map**
- 
-![tf_jumping](img/tf_jumping.gif "tf_jumping")
- 
-------
- 
-# Orientacja
- 
-![orientation](img/orientation.png "orientation")
- 
-Oprócz pozycji często potrzebujemy również orientacji robota. Orientację w trzech osiach możemy uzyskać dzięki żyroskopowi, akcelerometrowi i magnetometrowi. Magnetometr jest bardzo podatny na zakłócenia, szczególnie w pomieszczeniach i wymaga dobrej kalibracji, aby działał poprawnie. Z tego powodu zwykle używa się jedynie żyroskopu i akcelerometru. Akcelerometr pozwala nam wyliczyć dokładny kąt Pitch i Roll na podstawie wektora grawitacji jednak tylko przy wolniejszych zmianach. Dzięki żyroskopowi jesteśmy w stanie natomiast nadążać, za szybkimi zmianami jednak wyliczanie orientacji na jego podstawie polega na całkowaniu prędkości kątowych w każdej osi, co skutkuje ciągle narastającym błędem. Łącząc dane z akcelerometru i żyroskopu możemy śledzić szybkie zmiany orientacji, a zarazem zniwelować narastające błędy dzięki wektorowi grawitacji, ale niestety odnosi się to tylko do kątów Roll i Pitch, ponieważ obrót w osi Z (kąt Yaw) nie wpływa na odczyt wektora grawitacji. Z tego powodu kąt Yaw uzyskuje się jedynie z żyroskopu i jest on obarczony narastającym błędem. Aby zniwelować ten błąd używa się magnetometru, który wskazuje biegun magnetyczny ziemi. Można również wykorzystać inne metody określania kąta Yaw, jak np. użycie dwóch odbiorników GPS zamontowanych na dwóch krańcach pojazdu. 
- 
-W następnym ćwiczeniu posłużymy się sterownikiem pixhawk, który został podłączony do komputera przewodem USB.
- 
-![pixhawk](img/pixhawk.png "pixhawk")
- 
-Na kontrolerze pixhawk zainstalowane jest oprogramowanie Ardupilot, pozwalające na sterowanie pojazdami autonomicznymi ([dokumentacja](https://ardupilot.org/ardupilot/)). 
- 
-Istnieje wiele akcesoriów do układu pixhawk, które zwiększają jego możliwości jak:
-- odbiornik RC
-- GPS
-- Zewnętrzny magnetometr
-- Radiokomunikacja 
-- ...
- 
-Na pokładzie pixhawka mamy do dyspozycji żyroskop, akcelerometr, magnetometr oraz barometr. Na potrzeby tego ćwiczenia wykorzystamy akcelerometr i żyroskop, aby określić orientację kontrolera.
- 
-Aby odczytać dane z kontrolera możemy wykorzystać dwie metody:
-- program stacji bazowej np. [QGroundControl](http://qgroundcontrol.com/) czy [Mission Planner](https://ardupilot.org/planner/). W przypadku systemu Windows zalecam używanie programu Mission Planner, ponieważ posiada on dużo użytecznych funkcjonalności. Niestety jest on dostępny jedynie na systemy Windows, dlatego na potrzeby laboratorium użyjemy QGroundControl. Nie mniej jednak program QGroundControl jest również bardzo użytecznym programem, a zainstalować możemy go nawet na telefonie z systemem Android. 
-- bezpośrednio przy pomocy protokołu mavlink. Tę metodę stosuje się, gdy do sterownika podłączamy komputer, który ma nadzorować jego pracę i wydawać mu komendy, tak jak w naszym przypadku. 
- 
- 
-Najpierw rozpoczniemy pracę od programu QGroundControl. Zwykle używa się go przy początkowej konfiguracji pojazdu, a następnie do zdalnego monitorowania autonomicznej misji misji.
- 
-Uruchom program QGroundControl Znajdujący się na pulpicie komputera zdalnego.
- 
-![qgroundcontrol](img/qgroundcontrol.gif "qgroundcontrol")
- 
-Najpierw program łączy się z kontrolerem Pixhawk i ściąga jego aktualną konfigurację. W naszym przypadku uzyskamy komunikat, iż pojazd jest nie zdolny do działania, ponieważ nie zostały podłączone kluczowe komponenty. Do prawidłowego działania potrzebne jest zdalne połączenie przez Radio RC oraz GPS. 
- 
-PrzejdĹş do widoku mapy. 
-![qgroundcontrol_map](img/qgroundcontrol_map.gif "qgroundcontrol_map")
- 
-Jeśli mielibyśmy podłączony GPS, wiedzielibyśmy aktualną pozycję naszego pojazdu. W prawym górnym rogu można zauważyć kilka podstawowych parametrów jak orientacja w trzech osiach, wysokość, prędkość czy czas lotu. 
- 
-Możemy też dodać szybki podgląd dowolnego parametru klikając na koło zębate koło napisu **Valuse**
- 
-**Zadanie 7:** Dodaj do podglądu trzy parametry Roll, Pitch i Heading. 
- 
- 
-Parametry możemy również wyświetlać w formie wykresów, przechodząc do zakładki Analyze
- 
-![QgroundControl_plot](img/qgroundcontrol_plot.gif "QgroundControl_plot")
- 
-Po dodaniu Parametru do wykresu możemy zauważyć, że jego zmiany są bardzo skokowe. Jest to spowodowane małą częstotliwością wysyłania tego parametry przez kontroler. Aktualną częstotliwość odświeżania możemy zobaczyć po lewej stronie przy nazwie grupy parametrów.
- 
- 
-![qgroundcontrol_plot](img/qgroundcontrol_plot.png "qgroundcontrol_plot")
- 
-Częstotliwość odświeżania możemy zmienić w ustawieniach w zakładce MAVLink:
- 
-![QgroundControl_hz](img/qgroundcontrol_hz.gif "QgroundControl_hz")
- 
-(Jeśli jest wyłączony to uruchom wiatrak na stronie zarządzania niniejszym laboratorium [link](http://153.19.49.102:5000) (Zakładka sterowanie. Po włączeniu wiatraka sprawdź na kamerze), aby moduły zaczęły się poruszać. Wyłącz go po zakończeniu ćwiczenia)
-
-**Zadanie 8:** Wyświetl na wykresie trzy prędkości kątowe (xgyro, ygyro, zgyro) i zaobserwuj, jak wyglądają wskazania przed zmianą i po zmianie częstotliwości odświeżania. 
- 
- 
- 
-Ostatnim ważnym aspektem możliwość programu QGroundControl jest możliwość konfiguracji ustawień pojazdu. Poniżej zaprezentowano zmianę parametru opisującego typ używanych sterowników silników **MOT_PWM_TYPE**. **Umiejętność konfiguracji pojazdu będzie prawdopodobnie niezbędna przy realizacji zadań projektowych.**
- 
- 
-![QgroundControl_param](img/qgroundcontrol_param.gif "QgroundControl_param")
- 
- 
-Zamknij program QGroundControl
- 
-----------
- 
-Po doświadczeniach z programem QGroundControl teraz połączymy się z kontrolerem Pixhawk przy pomocy środowiska ROS i protokołu MavLink. 
- 
-Aby było to możliwe potrzebujemy pakietu mavros ([dokumntacja](http://wiki.ros.org/mavros)), który został już zainstalowwany na komputerach zdalnych. 
- 
-Przykładowy plik launch pozwalający uruchomić połączenie z pixhawkiem wygląda następująco:
- 
-```xml
-<launch>
-        <arg name="fcu_url" default="/dev/ttyACM0:921600" />
-        <arg name="gcs_url" default="" />
-        <arg name="tgt_system" default="1" />
-        <arg name="tgt_component" default="1" />
-        <arg name="log_output" default="screen" />
-        <arg name="fcu_protocol" default="v2.0" />
-        <arg name="respawn_mavros" default="false" />
- 
-        <include file="$(find mavros)/launch/node.launch">
-                <arg name="pluginlists_yaml" value="$(find mavros)/launch/apm_pluginlists.yaml" />
-                <arg name="config_yaml" value="$(find mavros)/launch/apm_config.yaml" />
-                <arg name="fcu_url" value="$(arg fcu_url)" />
-                <arg name="gcs_url" value="$(arg gcs_url)" />
-                <arg name="tgt_system" value="$(arg tgt_system)" />
-                <arg name="tgt_component" value="$(arg tgt_component)" />
-                <arg name="log_output" value="$(arg log_output)" />
-                <arg name="fcu_protocol" value="$(arg fcu_protocol)" />
-                <arg name="respawn_mavros" value="$(arg respawn_mavros)" />
-        </include>
-</launch>
-```
- 
-To co pojawia się przy każdym podłączeniu nowego urządzenia do komputera to konieczność sprawdzenia portu, do którego zostało ono przypisane. Sprawdź port do którego został podpięty pixhawk. Nazwa powinna rozpoczynać się od **usb-ArduPilot_fmuv3**.
- 
-![ls_pixhawk](img/ls_pixhawk.png "ls_pixhawk")
- 
- 
-**Zadanie 9:** Stwórz plik launch we własnym pakiecie zmieniając jedynie parametr **fcu_url** zgodnie ze znalezionym portem przy pomocy komendy **ls** (zostaw liczbę 921600 po znaku ":", jest to prędkość przesyłania danych). 
- 
- 
-Po uruchomieniu stworzonego pliku powinno pojawić się wiele topiców udostępnionych przez mavros z danymi z naszego kontrolera. Wszystkie są opisane w [dokumntacji](http://wiki.ros.org/mavros).
- 
-![mavros_list](img/mavros_list.gif "mavros_list")
- 
-Posiadając dostęp do danych możemy zająć się przedmiotem tego ćwiczenia, czyli obliczeniem orientacji naszego kontrolera, a potencjalnie robota/pojazdu. Po pierwsze potrzebne są nam dane z żyroskopu i akcelerometru. Są one dostępne na topicu **/mavros/imu/data_raw**. Wiadomości na tym topicu są typu [sensor_msgs/Imu](http://docs.ros.org/en/melodic/api/sensor_msgs/html/msg/Imu.html) i standardowo zawierają orientację (w tym przypadku jej nie ma, ponieważ są wysyłane na tym topicu tylko dane z żyroskopu i akcelerowmetru), przyspieszenia liniowe w trzech osiach oraz prędkości kątowe w trzech osiach.
- 
-![imu_mes](img/imu_mes.png "imu_mes")
- 
-Przy przetwarzaniu tego typu danych ważna jest częstotliwość publikowania danych. Możemy ją sprawdzić przy pomocy komendy:
- 
-```bash
-rostopic hz <nazwa topicu>
-```
-![mavros_hz](img/mavros_hz.gif "mavros_hz")
- 
-Sprawdź jak często pojawiają się wiadomości na tym topicu. Domyślnie dane wysyłane są z częstotliwością 2 Hz. Aby algorytm określający działał poprawnie potrzebujemy danych znacznie częściej. Ideałem byłyby informacje dostarczane z częstotliwością 200 Hz lub wyższą. Wtedy śledzenie orientacji byłoby bardzo płynne i pojawiałyby się mniejsze błędy. Pixhawk, z domyślnym firmwarem ardupilot rover jest w stanie wysyłać dane z sensorów z maksymalną częstotliwością ok 40 Hz. Nie jest to wartość idealna jednak już wystarczająca, aby wyznacza orientacja była użyteczna. 
- 
-Aby zmienić częstotliwość wysyłanych danych musimy użyć komendy:
- 
-```bash
-rosrun mavros mavsys rate <nazwa grupy danych> <częstotliwość>
-```
- 
-Aby zmienić częstotliwość wysyłania surowych danych z sensorów musimy użyć komendy:
- 
-```bash
-rosrun mavros mavsys rate --raw-sensors 40
-```
- 
-**Zadanie 10:** Zmień częstotliwość wysyłania danych z sensorów i sprawdź komendą ```rostopic hz```, czy dane rzeczywiście przychodzą częściej.
- 
-Do obliczenia orientacji robota posłużymy się filtrem madgwica, który był omawiany podczas wykładów [więcej_informacji](https://x-io.co.uk/open-source-imu-and-ahrs-algorithms/). Zwykle większość popularnych filtrów/algorytmów jest już dostępna w formie pakietów w systemie ROS i nie inaczej jest w tym przypadku. Filtr madwicka znajdziemy w pakiecie imu_tools->imu_filter_madgwick [dokumentacja](http://wiki.ros.org/imu_filter_madgwick?distro=melodic). 
- 
-![madgwick](img/madgwick.png "madgwick")
- 
-Po przeczytaniu dokumentacji możemy zauważyć, iż algorytm do działania oczekuje danych na topicu **imu/data_raw** oraz **imu/mag**. W naszym przypadku nie będziemy używać magnetometru, dlatego będziemy używać tylko topicu **imu/data_raw**.
- 
-Plik launch uruchamiający filtr madgwicka został stworzony w pakiecie **laboratorium_pliki_dodatkowe** i przedstawia się następująco:
- 
-<launch>
-  <node pkg="imu_filter_madgwick" name="imu_filter_node" type="imu_filter_node"  >
-        <rosparam>
-            use_mag: false
-            fixed_frame: base_link
-            publish_tf: false
-        </rosparam>
-      <remap from="/imu/data_raw" to="/mavros/imu/data_raw"/>
-    </node>
-</launch>
- 
-W powyższym pliku pojawiają się dwie nowe informacje. Po pierwsze możliwy jest inny sposób definiowania parametrów w znaczniku **rosparam**. Druga dotyczy znacznika **remap**. Tak jak było wspomniane wcześniej filtr oczekuje danych na topicu **/imu/data_raw** natomiast nasz kontroler wystawia je na topicu **/mavros/imu/data_raw**. Dzięki znacznikowi **remap** możemy zmienić nazwę wszystkich topiców z jakich korzysta dany node. Jest to bardzo przydatne w takiej sytuacji jaka zaszła teraz. Nie musimy zmieniać nic w kodzie jednego czy drugiego programu, tylko wystarczy dodać jedną linijkę w pliku launch. 
- 
-Po uruchomieniu pliku launch na topicu **/imu/data** powinny pojawić się dane tym razem wzbogacone o orientacje. Pakiet imu_tools dostarcza również plugin rozszerzający możliwość programu RVIZ o wyświetlanie orientacji obiektu. 
- 
-![mavros_mad_wiz](img/mavros_mad_wiz.gif "mavros_mad_wiz")
- 
-**Zadanie 11:** Uruchom filtr madgwicka plikiem launch z pakietu **laboratorium_pliki_dodatkowe** i uruchom wizualizacje w programie RVIZ. 
- (Jeśli jest wyłączony to uruchom wiatrak na stronie zarządzania niniejszym laboratorium [link](http://153.19.49.102:5000) (Zakładka sterowanie. Po włączeniu wiatraka sprawdź na kamerze), aby moduły zaczęły się poruszać. Wyłącz go po zakończeniu ćwiczenia)
- 
-