PreciseNavigateToAction + IR readings subtree about mep3 HOT 7 CLOSED

Treba smisliti kako obraditi to. Ako se robot krece napred a senzori nazad okinu, onda ne treba da stane, dok pri rotaciji mislim da treba da stane ako bilo koji okine (ali ako se rotira blizu ivice terena mozda detektuje osobe van terena). Mozemo citati trenutnu brzinu robota iz /big/odom pa na osnovu toga zakljuciti mozda.
Ovo je predlog za ovu godinu, tu svakako mislim da moramo reorganizovati stvari za narednu.

from mep3.

@angstrem98 To je dobra ideja, samo sto bih ja prije citao Twist, manje suma ima i manje kasni od Odometry.

Treba nam BT uslov koji provjerava da li postoji prepreka:

• BT bi slusao LaserScan, Range, Twist i TFMessage teme.
  • Svaki od LaserScan i Range ima zaglavlje u kome se nalazi frame_id, pa se pozicija svakog senzora moze odrediti u odnosu na map frejm uz pomoc TFa.
• Sve prepreke van terena treba da se ignorisu (baciti pogled na #106).
• Ukoliko se robot okrece (abs(twist.angular.z) > 0) i bilo koji od LaserScan i Range prepozna prepreku onda BT treba da vrati true.
• Zavisno da li se robot krece naprijed ili nazad (twist.linear.x) i bilo koji od LaserScan i Range prepozna prepreku na odgovarajucoj strani onda BT treba da vrati true.

Zatim, treba napraviti BT podstablo (recimo skill_safe_precise_navigate_to) koje bi koristilo BT uslov:

• Mislim da PipelineSequence BT cvor moze da inicira PreciseNavigateToAction, a onda u pozadini provjerava neki uslov:
  

  mep3/mep3_navigation/behavior_trees/navigate_w_recovery_and_replanning_only_if_path_becomes_invalid.xml

  Lines 9 to 20 in 4586e95

  	<PipelineSequence name="NavigateWithReplanning">
  	<Fallback>
  	<Inverter>
  	<GlobalUpdatedGoal />
  	</Inverter>
  	<ComputePathToPose goal="{goal}" path="{path}" planner_id="GridBased" />
  	</Fallback>
  	<RecoveryNode number_of_retries="1" name="FollowPath">
  	<FollowPath path="{path}" controller_id="FollowPath" />
  	<ClearEntireCostmap name="ClearLocalCostmap-Context" service_name="local_costmap/clear_entirely_local_costmap" />
  	</RecoveryNode>
  	</PipelineSequence>

• Ukoliko se prepreka ne ukloni za vise od x sekundi podstablo treba pukne.

from mep3.

@VladimirVincan Predlog je da se vecina koda ne piše unutar BT. Napisati zaseban cvor koji obradjuje senzore, za lidar slusati temu /big/scan_inflated jer su tu vec uklonjene tacke van terena i neprijateljski roboti su uvecani (ovde postoji neki mali bag da taj cvor puca, ali resicu ubrzo).
U tom cvoru predlazem da se TF transformacije obradjuju u posebnom thread-u, imas ovde primer:

Dakle tacke lidara van terena su resene (trenutno sve sto se nalazi na 10 cm ili blize ivici terena se uklanja, zbog tornja npr.), kad se tacka dobijena sa range senzora transformise u map frame onda je izbaciti na slican nacin.

Moj predlog za jednostavnu detekciju prepreka lidarom jeste da gledas tacke ispred i iza robota i prebrojis koliko tacaka je na distanci manjoj od neke konstante safe_distance. Ako imamo ispred robota vise od min_number tacaka na distanci manjoj od safe_distance znaci imamo prepreku ispred.

Na osnovu citanja Twist, sto mozes dobiti sa teme /big/cmd_vel dobijas da li ce doci do kolizije. Ovaj podatak objavljujes na neku temu, moze neka bool ili int poruka. Unutar BT je dovoljno samo prijaviti se na temu i videti da li treba stati ili je bezbedno nastaviti.

from mep3.

Skroz sam zaboravio da /big/scan_inflated ne sadrzi tacke van terena. Ovo pojednostavljuje implementaciju, ne moras da racuns pozicije preko TFa, samo provjeris udaljenosti

from mep3.

Skroz sam zaboravio da /big/scan_inflated ne sadrzi tacke van terena. Ovo pojednostavljuje implementaciju, ne moras da racuns pozicije preko TFa, samo provjeris udaljenosti

Za lidar nece morati, ali za range swnzore hoce :(

from mep3.

@angstrem98 Mozes li ubaciti u ROS cvor za filtriranje lidara i filtriranje IR senzora? S obzirom na poziciju IR senzora msm da nam nece trebati inflacija, samo izbacivanje statickih prepreka

from mep3.

Implementirano u #168.

Doduse fale IR senzori. Treba iskoristit IR Webots drajvere iz #145 ako se odlucimo za koriscenje IR senzora

from mep3.