Začíname s ROS-om (2)

Obr. 11 Akcelerometer MMA8452Q

Sériová komunikácia

Na rozbehanie sériovej komunikácie pod ROS-om možno použiť aj tutoriál dostupný na wiki.ros.org. Pre lepšie pochopenie fungovania ROS-u však nie je vhodné začínať hneď s vytváraním „subscribera“ alebo „publishera“. Napriek tomu sa odporúča neskôr si ho prejsť. Ide o vytvorenie programu „Hello World!“, kde je na komunikáciu použitá štandardná správa typu „string“. V tomto článku ukážeme, ako si vytvoriť vlastnú správu s vlastnými údajovými typmi. Výsledkom je program, ktorý slúži na testovanie sériovej komunikácie. V tomto článku bude využitý na komunikáciu s mikrokontrolérom ATmega164, ktorý komunikuje cez I2C s akcelerometrom MMA8452Q (obr. 11).

Na začiatku si treba vytvoriť vlastný balík, v ktorom budú priečinky tak, ako sú na obr. 12. Balík bude postupne implementovaný a budú sa doň pridávať správy, servisy a na záver spúšťací súbor („launchfile“).

Definovanie a vytvorenie správy

V priečinku „msg“ treba vytvoriť súbor serial_data.msg. Tu sa definujú používané údajové typy. Vložením dvoch riadkov ich definujeme takto:

uint8[] data_write
int8 length_read

Parameter „data_writev“ je v jazyku C++ definovaný ako std::vector a budú tam uložené údaje, resp. inštrukcie, ktoré budú posielané na sériovú komunikáciu. V parametri „length_read“ sa bude posielať počet bajtov, ktoré budú čítané zo sériovej komunikácie [2].

Vytvorenie subscribera

V priečinku „src“ treba vytvoriť C++ zdrojový kód „serial_subscriber.cpp“, zapísaný takto:

1. #include<ros/ros.h>
2. #include<serial/serial.h>
3. #include „my_package/serial_data.h”
4. serial::Serial* my_serial;
// prilinkuje hlavnú knižnicu ROS-u a knižnicu na sériovú komunikáciu; knižnicu my_package/serial_data.h vygeneruje CmakeList pri kompilácii; definuje sa tu aj smerník na triedu serial, ktorý bude inicializovaný v main
5. void serial Callback(constmapping::serial_data::ConstPtr&msg){
6. std::vector<uint8_t>my_list = msg->write_data;
7. intlength = msg->length_read;
8. uint8_t write[my_list.size()];
9. uint8_t result_data[length];
10. for(int i=0; i <my_list.size(); i++) write[i] = my_list[i];
11. my_serial->write(write,my_list.size());
12. if (length> 0){
13. my_serial->read(result_data,length);
14. for (int i = 0;i<length;i++) ROS_INFO(„%x „,result_data[i]); } }
//implementovaná metóda, ktorá prečíta údaje zo správy (riadky 6, 7), zapíše na port (riadok 11) a následne prečíta odpoveď zo zariadenia (riadky 13, 14).
15. int main(intargc, char **argv) {
16. ros::init(argc, argv, „serial_test”);
17. ros::NodeHandle n;
18. my_serial= new serial::Serial(„/dev/ttyUSB0”, 9600, serial::Timeout::simpleTimeout(1000));
19. ros::Subscribersub = n.subscribe(„write_to_port”, 10, serialCallback);
20. ros::spin(); }

V metóde „main“ je vidno inicializáciu uzla, ktorý musí mať unikátny názov (riadok 16). V riadku 17 je vytvorený aktuálne používaný „handler“ na uzol. Pomocou neho sú volané funkcie na vytvorenie „subscribera“, „publishera“ alebo mnohé iné, ktoré sú dostupné v ROS-e. Prebieha tu otvorenie portu (riadok 18), ktorý bude mať základné nastavenie. Programátor si ho musí nastaviť podľa vlastných požiadaviek, preto sa odporúča preštudovať si dokumentáciu ku knižnici. V riadku 19 sa vytvorí objekt „subscriber“. Prvý parameter je unikátny názov témy, druhý parameter je veľkosť zásobníka. Keď je dosiahnutá táto veľkosť, najstaršia správa sa vymaže. Tretí parameter je metóda, ktorá sa bude vykonávať. Riadok 20 spôsobuje, že sa bude metóda volať vždy, keď príde správa.

Kompilácia a spustenie

Pred spustením kompilácie treba upraviť package.xml, a to pridaním závislostí:

<build_depend>serial</build_depend>
<run_depend>serial</run_depend>
<build_depend>message_generation</build_depend>
<run_depend>message_runtime</run_depend>

V CmakeList.txt treba pridať do find_package závislosti serial, message_generation, zrušiť komentáre na funkcii add_message_files a pridať vytvorený súbor so správami (serial_data.msg). Ďalej treba odkomentovať celú funkciu generate_messages, aby Cmake vedel vygenerovať knižnicu serial_data.h. Na záver treba ešte napísať, ktoré zdrojové kódy budú spustiteľné, a to vpísaním riadkov:

add_executable(subscriber src/subscriber.cpp)
target_link_libraries(subscriber ${catkin_LIBRARIES})

Všetko je pripravené a balík sa môže skompilovať použitím catkin_make. Treba zapnúť roscore a vytvorený uzol. Príkazom rostopicpub /write_to_portmy_package/serial_data [data1,data2…] length_read možno zapisovať na tému a vytvorený uzol si to prečíta [3].

Vytvorenie publishera

Opäť treba v priečinku „src“ vytvoriť C++ zdrojový kód serial_publisher.cpp:

1. #include „ros/ros.h”
2. #include „my_package/serial_data.h“
3. int main(int argc, char **argv){
4. ros::init(argc, argv, „entry“);
5. ros::NodeHandle n;
6. ros::Publisher pub = n.advertise<my_package::serial_ data>(„write_to_port”, 10);
7. my_package::serial_data msg;
8. uint8_t data[] = {0x06};
9. std::vector<uint8_t>my_list(data,data + sizeof(data)) ;
10. while (ros::ok()){
11. msg.length_read = 7; msg.data_write = my_list;
12. pub.publish(msg);
13. sleep(1);
14. } return 0; }

Vytvorenie „publishera“ je dosť podobné ako pri „subscriberi“. Pomocou n.advertise (riadok 6) sa vytvorí objekt „publishera“, ktorý bude posielať na tému write_to_port definovanú správu. Zásobník bude mať veľkosť 10; keď bude zaplnený, správa sa neodošle. Ďalej je vidno definovanie objektu správy (riadok 7), samotnú inicializáciu (riadok 12) a následné odoslanie pub.publish(msg) (riadok 12). Pri kompilácii treba označiť v CMakeList.txt „publishera“ ako „executable“. Tento „publisher“ odosiela na tému len konštantné údaje. Pri použití je ideálne vytvoriť konkrétny vstup, či už z klávesnice, džojstika, alebo z nejakého výpočtu.

Servisy

Servisy predstavujú veľmi významný druh komunikácie v ROS-e. Na rozdiel od „publisherov“ a „subscriberov“, ktoré sú určené hlavne na veľkokapacitnú komunikáciu alebo publikovanie veľkého množstva dát, servisy predstavujú skôr komunikáciu typu žiadosť/odpoveď („request/response“). Tá je definovaná dvojicou správ. Tieto vlastnosti komunikácie sa veľmi často vyžadujú v distribuovaných systémoch. Uzol ROS ponúka vytvorenie servisu pod servisným názvom, pričom jeden uzol väčšinou predstavuje takzvaný servisný server, ktorý spracúva žiadosti od klientov (iných uzlov) a odosiela im odpovede.

Príklad použitia servisu

Tak ako v predchádzajúcom prípade, aj tu si ukážeme využitie servisu s použitím seriálovej komunikácie. Servis je určený na komunikáciu s robotickým ramenom KV-01 a jeho meničmi Faulhaber (obr. 15). V tomto prípade je využitie servisu na takéto účely komunikácie rozhodne efektívnejšie ako využívanie „subscriberov“ a „publisherov“.

Podobne ako v predchádzajúcej časti, aj tu treba vytvoriť v balíčku nový priečinok, v tomto prípade „srv“ (obr. 12) a vložiť doň súbor typu serial_data2.srv. Do tohto súboru sa potom vpisujú premenné, ktoré sa budú v servisoch používať. Premenná nad čiarou („spravaIn“) predstavuje žiadosti od klientov, premenná pod čiarou („spravaOut“) je odpoveď, ktorú servis vygeneruje. V našom prípade bude súbor serial_data2.srv vyzerať takto:

uint8[15] spravaIn
---
uint8[15] spravaOut

Vytvorenie servisu

Následne treba vytvoriť dva uzly. Jeden sa bude volať „serial_port.cpp“ a bude reprezentovať vytvorený servis, druhý bude mať názov „client.cpp“ a bude reprezentovať klienta, ktorý posiela žiadosti.

Súbor serial_port.cpp obsahuje tento kód:

1. #include<ros/ros.h>
2. #include<serial/serial.h>
3. #include „my_package/serial_data2.h”
4. serial::Serial* my_serial;
5. bool WriteAndRead(my_package::serial_data2::Request &req, my_package::serial_data2::Response &res) {
6. uint8_t *wBuffer = new uint8_t[15];
7. uint8_t *rBuffer = new uint8_t[15];
8. for (int i = 0; i < sizeof(req.spravaIn);i++)
9. wBuffer[i]=req.spravaIn[i];
10. int bytes_wrote =my_serial->write(wBuffer, sizeof(req.spravaIn)-1);
11. if (my_serial->available() > 0)
12. int bytes_read = my_serial->read(rBuffer,15);
13. for (int i = 0; i <sizeof(rBuffer); i++)
14. res.spravaOut[i]= rBuffer[i];
15. return true;
16. }
17. int main(int argc, char **argv){
18. ros::init(argc, argv, „serial_port”);
19. ros::NodeHandle n;
20. my_serial= new serial::Serial(„/dev/ttyUSB0”,115200, serial::Timeout::simpleTimeout(20));
21. ros::ServiceServer service = n.advertiseService(„serial_communication”, WriteAndRead);
22. ros::spin();
23. return 0; }

Takto by približne vyzeral servis na komunikáciu cez sériový port. Kód je, samozrejme, skrátený a uvedený v čo najjednoduchšej verzii. Nezaoberá sa presným uzatvorením portu, kontrolou otvorenia portu ani ničím podobným, keďže dôraz sa kladie hlavne na funkcionalitu servisu.
V riadkoch 18 a 19 sa inicializuje uzol s unikátnym názvom „serial_port” a vytvorí sa „handler“ na tento uzol. Následne sa v riadku 21 vytvorí ROS servis s unikátnym názvom „serial_communication“, ktorý čaká na príchod nejakej žiadosti. Po príchode žiadosti prebehne funkcia WriteAndRead, ktorá spracuje danú žiadosť a vráti odpoveď. Následne prebehne v riadkoch 10 až 12 prečítanie odpovede zo sériovej komunikácie a zápis do res.spravaOut, čo predstavuje odpoveď pre žiadateľa. Ak všetko prebehlo korektne, funkcia vráti žiadateľovi odpoveď true. Aby program neskončil po jedinom zavolaní, treba použiť funkciu ros::spin(), ktorá vždy čaká na prijatie novej žiadosti.

Vytvorenie klienta

1. #include<ros/ros.h>
2. #include<serial/serial.h>
3. #include „my_package/serial_data2.h”
4. int main(int argc, char **argv){
5. uint8_t *rBuffer = new uint8_t[15];
6. uint8_t *message = new uint8_t[15]; // naplnim tym, co chcem poslat
7. ros::init(argc, argv, „client”);
8. ros::NodeHandle n;
9. ros::ServiceClient client = n.serviceClient<my_package::serial_data2>(„serial_communication”);
10. my_package::serial_data2 srv;
11. for(int i=0;i<sizeof(message) ;i++)
12. srv.request.spravaIn[i] = message[i];
13. if (client.call(srv)){
14. for(int i=0;i< sizeof(srv.response.spravaOut);i++)
15. rBuffer[i]=srv.response.spravaOut[i];
16. }
17. return 0; }

Takto vytvorený klient je schopný posielať žiadosti servisu. Riadky 7 a 8 sú už dobre známe a sú potrebné vždy pri vytváraní uzla. Podstatné sú však riadky 9 a 10, kde sa inicializuje klient a vytvára sa premenná „srv“, ktorá bude obsahovať údaje z vytvorenej knižnice serial_data2.srv. Následne treba už len jednoducho naplniť žiadosť správou, ktorú treba poslať (riadok 11), a potom v riadku 13 zavolať servis, nech žiadosť obslúži. Ten podľa toho vráti z už spomínanej funkcie WriteAndRead odpoveď, či sa žiadosť obslúžila. Ak áno, možno z premennej spravaOut vyčítať správu, ktorú servis naplnil odpoveďou od meničov.

Kompilácia

Na skompilovanie projektu treba v CMakeLists.txt doplniť generovanie súborov pre servis, a to tak, že sa do funkcie add_service_files pridá serial_data2.srv (celý CmakeList.txt je vidno na obr. 16, aj so správami z predchádzajúcej časti článku). Tiež treba nastaviť, ktoré zdrojové kódy sa budú spúšťať:

add_executable(serial_port src/ serial_port.cpp)
target_link_libraries(serial_port ${catkin_LIBRARIES})
add_executable(client src/ client.cpp)
target_link_libraries(client ${catkin_LIBRARIES})

Parametrový server a príkaz „rosparam“

Parametrový server je určený na uchovávanie premenných rôznych údajových typov, využívajú ho všetky uzly a možno k nemu pristupovať aj zo siete. Spúšťa sa súčasne so zapnutím „rosmastra“. Uzly používajú tento server na uchovávanie a získavanie parametrov počas svojho behu. Nie je vhodný na časté používanie v cykloch, ale skôr na uchovávanie nastavení jednotlivých uzlov. Ak sa používa správne, umožňuje jednoduchý prístup k nastaveniam uzlov a ich prehľadné zobrazenie. Parametrový server podporuje premenné typu 32-bit integer, bool, string, double, iso8601 date, list, base64-encoded binary data. Názvy používaných parametrov možno získať príkazom „rosparam list“. Tiež možno zistiť hodnoty týchto parametrov príkazom „rosparam get názov_parametra“ alebo zmeniť hodnoty parametrov príkazom „rosparam set meno_parametra nová_hodnota“. Ak sa spustí roscore, zadaním príkazu „rosparam list“ sa zobrazia parametre /rosdistro, /rosversion a ešte niektoré ďalšie. Následne sa dajú prezerať hodnoty uložené v týchto premenných pomocou príkazov „rosparam get /rosdistro“ a “rosparam get /rosversion“. Výstupom môže byť ‚indigo’ a ‚1.11.16’ (obr. 18).

Pristupovanie k parametrom v kóde

Roscpp podporuje všetky typy premenných, ktoré môžu byť uchovávané v parametrovom serveri. Avšak práca je pomerne jednoduchá iba s premennými typu string, integer, double a boolean. K dispozícii sú funkcie na:

získanie parametra (parameter bude zapísaný do premennej hodnota)

ros::NodeHandle::getParam(“meno_parametra”,hodnota);

nastavenie parametra (premenná hodnota bude zapísaná do parametra)

ros::NodeHandle::setParam(“meno_parametra”,hodnota);

zistenie, či parameter existuje (ak existuje, vracia true)

ros::NodeHandle::hasParam(“memo_parametra”);

zmazanie parametra

ros::NodeHandle::deleteParam(“meno_parametra”);

Vytvorenie spúšťacieho súboru (launchfile)

Spúšťací súbor slúži na spustenie jedného alebo viacerých uzlov naraz s nahraním parametrov do parametrového servera. Každý spúšťací súbor by mal byť vytvorený v adresári ~/catkin_ws/src/meno_balíčka/launch a jeho meno musí mať koncovku. launch. Každý spúšťací súbor sa začína výrazom <launch> a končí výrazom </launch>. Medzi týmito výrazmi na nachádza zoznam uzlov, ktoré sa majú spustiť, a to tak, že sa každý uzol začína výrazom <nodename=”názov_uzlu” pkg=”názov_balíčka” type=”názov_uzlu”> a končí výrazom </node>. Vnútri sa nachádzajú parametre pre uzol, ktorý obsahuje tri časti a má nasledujúcu syntax:

<paramname=”názov_parametra” type=”dátový_typ” value=”hodnota”/>

Na obr. 19 je príklad súboru, ktorý spúšťa uzly „subscriber“ a „serial_publisher“, nachádzajúce sa vo vytvorenom balíčku my_package. Prvý uzol je spúšťaný s dvomi parametrami (ďalšie tri parametre sú ponechané ako základné nastavenie uzlom) a druhý bez parametrov. Podobný spúšťací súbor možno vytvoriť pre akékoľvek iné uzly, či už pre vlastné, alebo pre uzly nachádzajúce sa v nainštalovaných balíčkoch. Opis jednotlivých parametrov uzlov, ktoré sú nainštalované, možno nájsť na stránke balíčka.

Záver

Tento článok predstavil základné a najjednoduchšie druhy komunikácie v ROS-e, ktoré sú zároveň najviac využívané. Uviedli sme jednoduché príklady použitia a vysvetlili funkcionalitu. Okrem toho sa článok zaoberal aj vytvorením spúšťacieho súboru, ktorý uľahčuje prácu a pomáha spúšťať viacero uzlov naraz s rôznymi parametrami. V ďaľšej časti seriálu, ktorá bude uverejnená v ATP Journal 3/2016, bude analyzovaná a implementovaná tvorba modelu robota v súbore urdf, následné využitie tf v rvize, ako aj skutočné použitie ROS-u s reálnym robotickým systémom a možnosti a balíčky, ktoré ponúka v tejto oblasti.

Literatúra

[1] Woodal, W. – Harrison, J.: Creating a ROS msg and srv. [online]. Citované 16. 12. 2015. Dostupné na: https://github.com/wjwwood/serial.
[2] Autor neuvedený: Creating a ROS msg and srv. [online]. Citované 16. 12. 2015. Dostupné na: http://wiki.ros.org/ROS/Tutorials/CreatingMsgAndSrv . 
[3] Autor neuvedený: Writing a Simple Publisher and Subscriber (C++). [online]. Citované 16. 12. 2015. Dostupné na: http://wiki.ros.org/ROS/Tutorials/WritingPublisherSubscriber%28c%2B%2B%29. 
[4] Autor neuvedený: Roscpp/Overview/ Parameter Server. [online]. Citované 16. 12. 2015. Dostupné na: http://wiki.ros.org/roscpp/Overview/Parameter%20Server. 
[5] Autor neuvedený: Parameter Server. [online]. Citované 16. 12. 2015. Dostupné na: http://wiki.ros.org/roscpp/Overview/Parameter%20Server. 
[6] Autor neuvedený: Rosparam. [online]. Citované 16. 12. 2015. Dostupné na: http://wiki.ros.org/rosparam#rosparam_list. 
[7] Autor neuvedený: Creating a ROSmsg and srv. [online]. Citované 17. 12. 2015. Dostupné na: http://wiki.ros.org/ROS/Tutorials/CreatingMsgAndSrv .
[8] Autor neuvedený: Energie Technik Faulhaber. [online]. Dostupné na: http://www.energie-und-technik.de/bilder/?gid=3237&cp=4.

Miroslav Kohút
Matej Bartošovič
Michal Dobiš
doc. Ing. František Duchoň, PhD.
Ing. Andrej Babinec, PhD.
STU Bratislava
Fakulta elektrotechniky a informatiky
Ústav robotiky a kybernetiky
Ilkovičova 3, 812 19 Bratislava
frantisek.duchon@stuba.sk 
www.urk.fei.stuba.sk