lab04 update

lab04/README.md

@@ -147,3 +147,145 @@ Na pokładzie pixhawka mamy do dyspozycji żyroskop, akcelerometr, magnetometr o
 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.
+ 
+@@@@@@@
+ 
+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. 
+@@@@@@
+ 
+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
+ 
+@@@@@@@@
+ 
+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 w ustawieniach w zakładce MAVLink:
+ 
+@@@@@@@@
+ 
+**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 silników. **Umiejętność konfiguracji pojazdu będzie prawdopodobnie niezbędna przy realizacji zadań projektowych.**
+ 
+@@@@@@@@@
+ 
+ 
+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).
+ 
+@@@@@@@
+ 
+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 niema 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>
+```
+@@@@@ 
+ 
+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ęstotliowść>
+```
+ 
+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. 
+ 
+@@@@@@
+ 
+**Zadanie 11:** Uruchom filtr madgwicka plikiem launch z pakietu **laboratorium_pliki_dodatkowe** i uruchom wizualizacje w programie RVIZ. 
+