diff --git a/lab04/README.md b/lab04/README.md index 7f5a788eaed3fc0dd267a1f703ff3eae98c21061..0a069cd151f97d0f77b24fbffc76e232651c597f 100644 --- a/lab04/README.md +++ b/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. + + + + +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**. + + + + +**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. + + + +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). + + + +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. + diff --git a/lab04/img/QgroundControl_plot.png b/lab04/img/QgroundControl_plot.png new file mode 100644 index 0000000000000000000000000000000000000000..5ed65391c3e4523b42b7221042640ff7197bf36f Binary files /dev/null and b/lab04/img/QgroundControl_plot.png differ diff --git a/lab04/img/imu_mes.png b/lab04/img/imu_mes.png new file mode 100644 index 0000000000000000000000000000000000000000..9dbd9ef6dd37096db53cb22c922f488b9de969aa Binary files /dev/null and b/lab04/img/imu_mes.png differ diff --git a/lab04/img/ls_pixhawk.png b/lab04/img/ls_pixhawk.png new file mode 100644 index 0000000000000000000000000000000000000000..942848a09f0755971c3750654b0162bb128fd154 Binary files /dev/null and b/lab04/img/ls_pixhawk.png differ diff --git a/lab04/img/madgwick.png b/lab04/img/madgwick.png new file mode 100644 index 0000000000000000000000000000000000000000..cbb595b8ba40f65a4bc143db0e9169b67caecb2a Binary files /dev/null and b/lab04/img/madgwick.png differ