OSGAR
Open Source Garden Autonomous Robot
OSGAR is a long term project which will probably incorporate more than one robotic platform in the future. The project started in 2014 when we decided to modify school garden tractor John Deere X300R into autonomous robot. The project is carried on in cooperation with the Czech University of Life Science Prague. Blog update: 14/03/2025 — Hello OSGAR!
21st October, 2018 — OSGAR v0.0.1 released!
The OSGAR package was released today for the first time — see
https://pypi.org/project/osgar/. It contains the basic functionality for
logging of various data sources together with replay feature. This initial
release supports John Deere and Spider3 (both quite uncommon, I am
afraid), but more will come soon, I hope. If you would like to try it on your
robot, please let us know. The step 1 is to describe your inputs and outputs
(both the devices configuration as well as the communication protocol used).
You can give it a try:
python3 -m pip install osgar
26th March, 2023 — Web refactoring
Note, that there was a "minor" web refactoring as OSGAR was further developed
(mainly for DARPA Subterranean Challenge. The original page was split
into 3 and we will track them separately:
12. března 2025 — základní použití
Kamarádi mne často s OSGARem provokují, že je sice fajn, ale nemá žádnou dokumentaci. Není to úplně pravda i když to co jsem před mnoha lety
sepsal asi moc jako dokumentace nevypadá. Možná vznikl i pokus o „vědecký” článek v době, kdy jsem potřeboval „čárku” … to už je ale dávno.
Z lenosti přepínám do češtiny, takže tento příspěvek bude zase pár let trošku neviditelný, ale to napravit bude až další krok. Dnes mám v plánu
sepsat základní použití OSGARa včetně nejužitečnějších nástrojů pro nové projekty.
Základní nástroje
Jako základ je trojice nástrojů:
- osgar.record — nahrávání dat (pouštění robota)
- osgar.logger — analýza logů (nahraných dat)
- osgar.replay — debug a přehrávání modulů z nahraných dat
Pořadí použítí je typicky nejprve osgar.record, následovaný opakovaným osgar.logger a pro vývojáře někdy osgar.replay.
osgar.record
První úloha je typicky „nahrát data”. Někdo by mohl rozporovat, že je úkolem spíše naprogramovat robota, ale daleko snazší je
fake it until you make it, tj. jak nasbírat data, jakoby se robot v dané scéně řízeně pohyboval. Modul osgar.record má jako jediný
povinný parametr konfigurační soubor (JSON formát).
Konfigurační soubor popisuje jednak které moduly se mají spustit a dále komunikační sǐt
tj. jak jsou propojeny jednotlivé vstupy a výstupy běžících modulů. Viz například jednoduchý příklad robota Matty,
který má ujet 1m —
config/matty-go.json, kde jsou 4 moduly: app, platform,
timer a serial. app je naše „aplikace” jízdy rovně, platform zajišťuje nízkoúrovňové řízení a parsování dat (odometrie,
nárazníky, stav baterie), timer je časovač, který pravidelně „připomíná” modulu platform ať pošle nový příkaz a konečně serial
je napojení na HW (pro windows např. COM4, pro linux /dev/ttyUSB0).
osgar.logger
Pokud máte data již nahraná, je třeba ověřit, zda komunikace probíhala podle očekávání. K tomu je užitečný osgar.logger, který má jediný povinný
vstup a to jméno souboru (logu). Výstupem je přehledová tabulka jednotlivých kanálů (streams), kolik daných zpráv bylo posláno, celková
velikost a průměrná frekvence posílaných zpráv.
Pokud vás zajímají data z konkrétního kanálu, tak pomocí parametru --stream vyberete jméno, resp. seznam jmen, jejichž obsah se má vypsat.
Někdy je užitečné i výstup přímo formátovat s použitím --format.
osgar.replay
Pro samotný vývoj aplikace je klíčový osgar.replay, který přehraje vybraný modul s použitím již nahraných dat. Pro OSGAR je zásadní, že
přehrání je plně deterministické a modul pro dané vstupy vždy odpoví stejnými výstupy ve stejném pořadí. Pokud tomu tak není, dostanete
AssertionError s výpisem jaký výstup posílá při přehrávání a jaký dáváte na výstup vy.
Reálná nahrávka je vždy unikátní — někde něco problikne, spojení se na chvíli přeruší nebo nárazník je už na startu zamáčknutý s čímž
váš kód nepočítal. Na druhou stranu přehrání (replay) dopadne vždy stejně, můžete si to pustit 10x, přidat výstupy nebo i zobrazování
mezivýsledků, nebo v debuggeru dát breakpoint — toto na běžícím robotovi rozhodně nedoporučuji (rád bych napsal, že to není možné, ale
ono to možné je, jen to nedopadne dobře).
14. března 2025 — Hello OSGAR!
Dnes bych se rád věnoval úvodu do programování vlastního Python kódu s použítím OSGARa. Probereme dvě úlohy, kdy první je zpracování a vizualizace
již nasbíraných dat a druhá je implementace vlastního modulu zapojitelného do řízení robota.
Aplikace na analýzu logu
Začal bych s jednoduchou aplikaci na „analýzu” dat z hloubkové kamery. Základní kostra vypadá takto:
from datetime import timedelta from osgar.logger import LogReader, lookup_stream_id from osgar.lib.serialize import deserialize def read_logfile(logfile, stream): only_stream = lookup_stream_id(logfile, stream) with LogReader(logfile, only_stream_id=only_stream) as log: for timestamp, stream_id, data in log: if timedelta(seconds=3) <= timestamp < timedelta(seconds=4): depth = deserialize(data) print(timestamp, depth[400//2][640//2]) if __name__ == "__main__": import argparse parser = argparse.ArgumentParser(description='Extract data from logfile') parser.add_argument('logfile', help='recorded log file') parser.add_argument('–stream', help='stream ID or name', default='oak.depth') args = parser.parse_args() read_logfile(args.logfile, args.stream)
Pro pochopení asi bude rozumné nahlédnout hloběji, co nahraná data obsahují. Použijeme-li již zmíněný osgar.logger dostaneme například:
python -m osgar.logger matty-go-250313_193339.log k name bytes | count | freq Hz 0 sys 3225 | 16 | 1.6Hz 1 app.desired_speed 5 | 1 | 0.1Hz 2 app.desired_steering 5 | 1 | 0.1Hz 3 platform.esp_data 894 | 106 | 10.6Hz 4 platform.emergency_stop 0 | 0 | 0.0Hz 5 platform.pose2d 552 | 84 | 8.4Hz 6 platform.bumpers_front 2 | 2 | 0.2Hz 7 platform.bumpers_rear 2 | 2 | 0.2Hz 8 platform.gps_serial 1176 | 16 | 1.6Hz 9 gps.position 88 | 8 | 0.8Hz 10 gps.rel_position 0 | 0 | 0.0Hz 11 gps.nmea_data 1432 | 8 | 0.8Hz 12 timer.tick 900 | 100 | 10.0Hz 13 serial.raw 4745 | 189 | 19.0Hz Total time 0:00:09.964834
Je vidět, že nahrávka obsahuje jednotlivé streamy nebo „kanály”, což jsou fakticky výstupy jednotlivých modulů. Pak tedy app.desired_speed
je výstup modulu app, který se jmenuje desired_speed a má identifikační číslo 1. Jedinou výjimkou je stream 0, který se používá pro logování
systémových událostí.
Zpět ke kódu — pokud chceme vyčítat data z nějakého kanálu, tak je třeba znát jeho pořadí. Snazší je jeho jméno, pro který funkce lookup_stream_id()
pak číslo dohledá (pokud je na vstupu číslo, tak zase vráti to samé). LogReader pak vrací iterátor, pomocí kterého můžeme data číst. Jedná
se vždy o trojici timestamp, stream_id, data. timestamp je časová známka, kdy byla zpráva uložena do souboru od začátku nahrávání. stream_id
je číslo (pokud bychom četli zmíněné app.desired_speed, tak to bude 1). Vyčítáme-li pouze jeden jediný stream, tak jeho stream_id můžeme
ignorovat — bude pořád stejné a odpovídat našemu požadavku, ale pokud výčítáme současně vícero streamů, tak je podle stream_id rozlišujeme.
Konečně data jsou serializovaná data. Jedná se spíše o historický přežitek, že rozbalování něco stojí a my chceme jenom některá speciální.
V našem umělém příkladě výše nás zajimají hlouboková data pouze ve třetí sekundě nahrávky, která chceme vypsat.
A to je vše. Nějaké dotazy?
Modul pro nahrávání
Druhý příklad je o něco složitější, běží v reálném čase a vedle vstupů má i výstupy. Nechť se jedná o zjednodušený příklad, který má zastavit
robota při detekci překážky.
import math from osgar.node import Node class StopAndGo(Node): def __init__(self, config, bus): super().__init__(config, bus) bus.register('desired_steering') self.max_speed = config.get('max_speed', 0.2) def on_depth(self, data): dist = data[400//2][640//2] print(self.time, dist) if 0 < dist <= 500: self.send_speed_cmd(0, 0) else: self.send_speed_cmd(self.max_speed, 0) def send_speed_cmd(self, speed, steering_angle): return self.publish( 'desired_steering', [round(speed*1000), round(math.degrees(steering_angle)*100)] )
První pozorování je, že potřebujeme třidu (class), která dědí z osgar.node.Node. Fakticky je to spíše pro pohodlnost, ale to pro teď přeskočme.
Náš modul má při vzniku (__init__()) k dispozici konfigurační soubor (config) a komunikační sběrnici (bus). V našem případě se z konfiguračního souboru
vyčte maximální povolená rychlost (pokud není v JSON konfiguraci, tak se použije 0.2 m/s). A v síti se registruje nový výstup desired_steering.
Node zajištuje volání „callback” funkci — v našem příkladu on_depth(). Vstupní parametr jsou data, zde depth hloubková mapa. Pokud
modul potřebuje vědět „kolik je hodin?”, tak může použít self.time.
Konečně pro výstup z modulu se používá self.publish() s parametry jméno kanálu a samotná data (zde dvojice rychlost v milimetrech za sekundu a úhel
v setinách stupňů).