/* Ċ Ġ* ĠThis ĠSource ĠCode ĠForm Ġis Ġsubject Ġto Ġthe Ġterms Ġof Ġthe ĠMozilla ĠPublic Ċ Ġ* ĠLicense , Ġv . Ġ 2 . 0 . ĠIf Ġa Ġcopy Ġof Ġthe ĠM PL Ġwas Ġnot Ġdistributed Ġwith Ġthis Ċ Ġ* Ġfile , ĠYou Ġcan Ġobtain Ġone Ġat Ġhttps :// mozilla . org / MPL / 2 . 0 /. Ċ Ġ* Ċ Ġ* ĠCopyright Ġ 2 0 1 9 , ĠGreen ĠHope ĠF al cons Ċ Ġ*/ Ċ Ċ package Ġorg . gh robot ics . lib . vision Ċ Ċ import Ġedu . wpi . first . wp ilib j . Median Filter Ċ import Ġedu . wpi . first . wp ilib j . Timer Ċ import Ġedu . wpi . first . wp ilib j . geometry . Pose 2 d Ċ import Ġedu . wpi . first . wp ilib j . geometry . Rotation 2 d Ċ import Ġedu . wpi . first . wp ilib j . geometry . Translation 2 d Ċ import Ġorg . gh robot ics . lib . debug . F alcon Dashboard Ċ import Ġorg . gh robot ics . lib . mat hematic s . units . Meter Ċ import Ġorg . gh robot ics . lib . mat hematic s . units . SI Unit Ċ import Ġorg . gh robot ics . lib . mat hematic s . units . Second Ċ import Ġorg . gh robot ics . lib . mat hematic s . units . seconds Ċ Ċ /** Ċ Ġ* ĠClass Ġused Ġto Ġtrack Ġthe Ġlocations Ġof Ġvarious Ġvision Ġtargets Ġthroughout Ċ Ġ* Ġthe Ġfield . Ċ Ġ* Ċ Ġ* ĠThis Ġclass Ġshould Ġbe Ġover rid en Ġto Ġadd Ġyear - specific Ġf unt ion ality Ġalong Ċ Ġ* Ġwith Ġa Ġmethod Ġto Ġobtain Ġthe Ġ" best Ġtarget ". ĠA Ġ" best Ġtarget " Ġmethod Ġis Ġprovided Ċ Ġ* Ġby Ġdefault ; Ġhowever , Ġthis Ġuses Ġabsolute Ġpose Ġand Ġnot Ġthe Ġrobot Ġpose . ĠThe Ġbest Ċ Ġ* Ġtarget Ġbased Ġon Ġthe Ġrobot Ġpose Ġmay Ġvary Ġdue Ġto Ġrobot Ġgeometry Ġ( pas sthrough , Ġt urret ), Ċ Ġ* Ġetc . Ċ Ġ*/ Ċ open Ġclass ĠTarget Tracker ( private Ġval Ġconstants : ĠTarget Tracker Constants ) Ġ{ ĊĊĠĠĠ Ġ// ĠStores Ġall Ġtracked Ġtargets . ĊĠĠĠ Ġprotected Ġval Ġtargets Ġ= Ġmutable Set Of < Tracked Target >() ĊĊĠĠĠ Ġ/** ĊĠĠĠĠ Ġ* ĠUpdates Ġall Ġtargets Ġthat Ġare Ġstored Ġin Ġthe ĠTarget Tracker . ĊĠĠĠĠ Ġ*/ ĊĠĠĠ Ġfun Ġupdate () Ġ{ ĊĠĠĠĠĠĠĠ Ġ// ĠGet Ġthe Ġcurrent Ġtime . ĊĠĠĠĠĠĠĠ Ġval Ġtime Ġ= ĠTimer . getF PG AT ime stamp (). seconds ĊĊĠĠĠĠĠĠĠ Ġ// ĠUpdate Ġand Ġremove Ġold Ġtargets . ĊĠĠĠĠĠĠĠ Ġtargets . remove If Ġ{ Ġit . update ( time ); Ġ! it . is Alive Ġ} ĊĊĠĠĠĠĠĠĠ Ġ// ĠPublish Ġtargets Ġto ĠF alcon ĠDashboard . ĊĠĠĠĠĠĠĠ ĠF alcon Dashboard . vision Targets Ġ= Ġtargets . as Sequence () ĊĠĠĠĠĠĠĠĠĠĠĠ Ġ. filter Ġ{ Ġit . is Real Ġ} ĊĠĠĠĠĠĠĠĠĠĠĠ Ġ. map Ġ{ Ġit . average Pose Ġ} ĊĠĠĠĠĠĠĠĠĠĠĠ Ġ. toList () ĊĠĠĠ Ġ} ĊĊĠĠĠ Ġ/** ĊĠĠĠĠ Ġ* ĠAdds Ġ( a ) Ġnew Ġsample ( s ) Ġto Ġthe ĠTarget ĠTracker . ĊĠĠĠĠ Ġ* ĊĠĠĠĠ Ġ* Ġ@ param Ġcapture Time ĠThe Ġtime Ġof Ġcapture . ĊĠĠĠĠ Ġ* Ġ@ param Ġpo ses ĠThe Ġfield - relative Ġpo ses Ġof Ġthe Ġtargets . ĊĠĠĠĠ Ġ*/ ĊĠĠĠ Ġfun Ġadd Samples ( capture Time : ĠSI Unit < Second >, Ġpo ses : ĠIterable < Pose 2 d >) Ġ{ ĊĠĠĠĠĠĠĠ Ġ// ĠRemove Ġsamples Ġfrom Ġthe Ġ" future ". ĊĠĠĠĠĠĠĠ Ġif Ġ( capture Time Ġ> ĠTimer . getF PG AT ime stamp (). seconds ) Ġ{ ĊĠĠĠĠĠĠĠĠĠĠĠ Ġreturn ĊĠĠĠĠĠĠĠ Ġ} ĊĊĠĠĠĠĠĠĠ Ġ// ĠFind Ġthe Ġtarget Ġto Ġadd Ġthis Ġsample Ġto . ĊĠĠĠĠĠĠĠ Ġfor Ġ( pose Ġin Ġpo ses ) Ġ{ ĊĠĠĠĠĠĠĠĠĠĠĠ Ġ// ĠGet Ġthe Ġclosest Ġtarget Ġto Ġthis Ġpose . ĊĠĠĠĠĠĠĠĠĠĠĠ Ġval Ġclosest Target Ġ= Ġtargets . min By Ġ{ Ġit . average Pose . translation . get Distance ( pose . translation ) Ġ} ĊĊĠĠĠĠĠĠĠĠĠĠĠ Ġ// ĠCreate Ġthe Ġsample . ĊĠĠĠĠĠĠĠĠĠĠĠ Ġval Ġsample Ġ= ĠTrack ed Target Sample ( capture Time , Ġpose ) ĊĊĠĠĠĠĠĠĠĠĠĠĠ Ġ// ĠDec ide Ġon Ġwhether Ġto Ġadd Ġto Ġthe Ġclosest Ġtarget Ġor Ġadd Ġto Ġa Ġnew Ġtarget . ĊĠĠĠĠĠĠĠĠĠĠĠ Ġif Ġ( closest Target Ġ== Ġnull Ġ|| Ġclosest Target . average Pose . translation . get Distance ( pose . translation ) Ġ> ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġconstants . k Target Tracking Distance Error Tolerance . value ĊĠĠĠĠĠĠĠĠĠĠĠ Ġ) Ġ{ ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġ// ĠCreate Ġa Ġnew Ġtarget Ġbecause Ġnothing Ġis Ġnear Ġthe Ġtolerance . ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġtargets Ġ+= ĠTrack ed Target ( sample , Ġconstants ) ĊĠĠĠĠĠĠĠĠĠĠĠ Ġ} Ġelse Ġ{ ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġ// ĠWe Ġare Ġwithin Ġtolerance Ġof Ġthe Ġclosest Ġtarget . ĠWe Ġcan Ġadd Ġthis Ġsample ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġ// Ġto Ġthat Ġtarget . ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġclosest Target . add Sample ( sample ) ĊĠĠĠĠĠĠĠĠĠĠĠ Ġ} ĊĠĠĠĠĠĠĠ Ġ} ĊĠĠĠ Ġ} ĊĊĠĠĠ Ġ/** ĊĠĠĠĠ Ġ* ĠReturns Ġthe Ġclosest Ġtarget Ġto Ġthe Ġabsolute Ġtranslation Ġon Ġthe Ġfield . ĊĠĠĠĠ Ġ* ĊĠĠĠĠ Ġ* Ġ@ param Ġtranslation ĠThe Ġabsolute Ġtranslation Ġof Ġthe Ġpose Ġon Ġthe Ġfield . ĊĠĠĠĠ Ġ* Ġ@ return ĠThe Ġtracked Ġtarget Ġor Ġnull Ġif Ġthere Ġis Ġno Ġnearby Ġbest Ġtarget . ĊĠĠĠĠ Ġ*/ ĊĠĠĠ Ġopen Ġfun Ġget Absolute Target ( translation : ĠTranslation 2 d ): ĠTrack ed Target ? Ġ{ ĊĠĠĠĠĠĠĠ Ġreturn Ġtargets . as Sequence () ĊĠĠĠĠĠĠĠĠĠĠĠ Ġ. filter Ġ{ ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġit . is Real Ġ&& Ġtranslation . get Distance ( it . average Pose . translation ) Ġ< ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġconstants . k Target Tracking Distance Error Tolerance . value ĊĠĠĠĠĠĠĠĠĠĠĠ Ġ}. min By Ġ{ Ġit . average Pose . translation . get Distance ( translation ) Ġ} ĊĠĠĠ Ġ} ĊĊĠĠĠ Ġ/** ĊĠĠĠĠ Ġ* ĠRepresents Ġone Ġtarget Ġon Ġthe Ġfield . ĊĠĠĠĠ Ġ* ĊĠĠĠĠ Ġ* Ġ@ param Ġinitial Sample ĠThe Ġinitial Ġtarget Ġsample Ġfor Ġthis Ġtarget . ĊĠĠĠĠ Ġ*/ ĊĠĠĠ Ġclass ĠTrack ed Target ( ĊĠĠĠĠĠĠĠ Ġprivate Ġval Ġinitial Sample : ĠTrack ed Target Sample , ĊĠĠĠĠĠĠĠ Ġprivate Ġval Ġconstants : ĠTarget Tracker Constants ĊĠĠĠ Ġ) Ġ{ ĊĠĠĠĠĠĠĠ Ġ// ĠThe Ġsamples Ġthat Ġmake Ġup Ġthis Ġtarget . ĊĠĠĠĠĠĠĠ Ġprivate Ġval Ġsamples Ġ= Ġmutable Set Of < Tracked Target Sample >() ĊĊĠĠĠĠĠĠĠ Ġ// ĠThe Ġaverage Ġpose Ġof Ġthis Ġtarget . ĊĠĠĠĠĠĠĠ Ġvar Ġaverage Pose Ġ= Ġinitial Sample . pose ĊĠĠĠĠĠĠĠĠĠĠĠ Ġprivate Ġset ĊĊĠĠĠĠĠĠĠ Ġ// Ġa Ġfilter Ġto Ġaverage Ġthe Ġpose Ġof Ġthis Ġtarget ĊĠĠĠĠĠĠĠ Ġprivate Ġval Ġx Filter Ġ= ĠMed ian Filter ( constants . k Median Window Size ) ĊĠĠĠĠĠĠĠ Ġprivate Ġval Ġy Filter Ġ= ĠMed ian Filter ( constants . k Median Window Size ) ĊĠĠĠĠĠĠĠ Ġprivate Ġval Ġtheta Filter Ġ= ĠMed ian Filter ( constants . k Median Window Size ) ĊĊĠĠĠĠĠĠĠ Ġ// ĠThe Ġtarget Ġis Ġalive Ġwhen Ġit Ġhas Ġat Ġleast Ġone Ġdata Ġpoint Ġfor Ġa ĊĠĠĠĠĠĠĠ Ġ// Ġcertain Ġamount Ġof Ġtime . ĊĠĠĠĠĠĠĠ Ġvar Ġis Alive Ġ= Ġtrue ĊĠĠĠĠĠĠĠĠĠĠĠ Ġprivate Ġset ĊĊĠĠĠĠĠĠĠ Ġ// ĠThe Ġtarget Ġis Ġreal Ġwhen Ġit Ġhas Ġreceived Ġa Ġcertain Ġnumber Ġof ĊĠĠĠĠĠĠĠ Ġ// Ġdata Ġpoints Ġfor Ġa Ġcertain Ġperiod Ġof Ġtime . ĊĠĠĠĠĠĠĠ Ġvar Ġis Real Ġ= Ġtrue ĊĠĠĠĠĠĠĠĠĠĠĠ Ġprivate Ġset ĊĊĠĠĠĠĠĠĠ Ġ// ĠAdd Ġthe Ġinitial Ġsample Ġto Ġthe Ġlist Ġof Ġsamples . ĊĠĠĠĠĠĠĠ Ġinit Ġ{ ĊĠĠĠĠĠĠĠĠĠĠĠ Ġadd Sample ( initial Sample ) ĊĠĠĠĠĠĠĠ Ġ} ĊĊĠĠĠĠĠĠĠ Ġfun Ġreset Filters () Ġ{ ĊĠĠĠĠĠĠĠĠĠĠĠ Ġx Filter . reset () ĊĠĠĠĠĠĠĠĠĠĠĠ Ġy Filter . reset () ĊĠĠĠĠĠĠĠĠĠĠĠ Ġtheta Filter . reset () ĊĠĠĠĠĠĠĠ Ġ} ĊĊĠĠĠĠĠĠĠ Ġ/** ĊĠĠĠĠĠĠĠĠ Ġ* ĠAdds Ġa Ġsample Ġto Ġthe Ġtracked Ġtarget . ĊĠĠĠĠĠĠĠĠ Ġ*/ ĊĠĠĠĠĠĠĠ Ġfun Ġadd Sample ( sample : ĠTrack ed Target Sample ) Ġ{ ĊĠĠĠĠĠĠĠĠĠĠĠ Ġsamples . add ( sample ) ĊĠĠĠĠĠĠĠ Ġ} ĊĊĠĠĠĠĠĠĠ Ġ/** ĊĠĠĠĠĠĠĠĠ Ġ* ĠUpdates Ġthe Ġtarget Ġstate . ĊĠĠĠĠĠĠĠĠ Ġ* ĊĠĠĠĠĠĠĠĠ Ġ* Ġ@ param Ġtime ĠThe Ġcurrent Ġtime . ĊĠĠĠĠĠĠĠĠ Ġ*/ ĊĠĠĠĠĠĠĠ Ġfun Ġupdate ( time : ĠSI Unit < Second >) Ġ{ ĊĠĠĠĠĠĠĠĠĠĠĠ Ġ// ĠRemove Ġexpired Ġsamples . ĊĠĠĠĠĠĠĠĠĠĠĠ Ġsamples . remove If Ġ{ Ġtime Ġ- Ġit . capture Time Ġ>= Ġconstants . k Max Target Tracking Lifetime Ġ} ĊĊĠĠĠĠĠĠĠĠĠĠĠ Ġ// ĠUpdate Ġstate . ĊĠĠĠĠĠĠĠĠĠĠĠ Ġis Alive Ġ= Ġsamples . isNotEmpty () ĊĠĠĠĠĠĠĠĠĠĠĠ Ġis Real Ġ= Ġsamples . size Ġ> Ġ 2 ĊĊĠĠĠĠĠĠĠĠĠĠĠ Ġ// ĠUpdate Ġaverage Ġpose . ĊĠĠĠĠĠĠĠĠĠĠĠ Ġreset Filters () ĊĠĠĠĠĠĠĠĠĠĠĠ Ġvar Ġaverage X Ġ= Ġ 0 . 0 ĊĠĠĠĠĠĠĠĠĠĠĠ Ġvar Ġaverage Y Ġ= Ġ 0 . 0 ĊĠĠĠĠĠĠĠĠĠĠĠ Ġvar Ġaverage Theta Ġ= Ġ 0 . 0 ĊĊĠĠĠĠĠĠĠĠĠĠĠ Ġsamples . forEach Ġ{ ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġaverage X Ġ= Ġx Filter . calculate ( it . pose . translation . x ) ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġaverage Y Ġ= Ġy Filter . calculate ( it . pose . translation . y ) ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġaverage Theta Ġ= Ġtheta Filter . calculate ( it . pose . rotation . radians ) ĊĠĠĠĠĠĠĠĠĠĠĠ Ġ} ĊĊĠĠĠĠĠĠĠĠĠĠĠ Ġaverage Pose Ġ= ĠP ose 2 d ( average X , Ġaverage Y , ĠRotation 2 d ( average Theta )) ĊĠĠĠĠĠĠĠ Ġ} ĊĠĠĠ Ġ} ĊĊĠĠĠ Ġ/** ĊĠĠĠĠ Ġ* ĠRepresents Ġone Ġsample Ġof Ġa Ġtarget Ġon Ġthe Ġfield . ĊĠĠĠĠ Ġ* ĊĠĠĠĠ Ġ* Ġ@ param Ġcapture Time ĠThe Ġtime Ġat Ġwhich Ġthe Ġframe Ġcontaining Ġthe Ġtarget Ġwas Ġcaptured . ĊĠĠĠĠ Ġ* Ġ@ param Ġpose ĠThe Ġfield - relative Ġpose Ġof Ġthe Ġtarget . ĊĠĠĠĠ Ġ*/ ĊĠĠĠ Ġdata Ġclass ĠTrack ed Target Sample ( val Ġcapture Time : ĠSI Unit < Second >, Ġval Ġpose : ĠP ose 2 d ) ĊĊĠĠĠ Ġ/** ĊĠĠĠĠ Ġ* ĠRepresents Ġconstants Ġfor Ġthe Ġtarget Ġtracker . ĊĠĠĠĠ Ġ* ĊĠĠĠĠ Ġ* Ġ@ param Ġk Max Target Tracking Lifetime ĠThe Ġmaximum Ġamount Ġof Ġtime Ġthat Ġa Ġtarget ĊĠĠĠĠ Ġ* Ġshould Ġbe Ġtracked Ġafter Ġa Ġspecific Ġdata Ġpoint Ġhas Ġbeen Ġreceived . ĊĠĠĠĠ Ġ* Ġ@ param Ġk Target Tracking Distance Error Tolerance ĠThe Ġdistance Ġtolerance Ġto ĊĠĠĠĠ Ġ* Ġcreate Ġa Ġnew Ġtarget Ġgiven Ġa Ġsample . ĊĠĠĠĠ Ġ* Ġ@ param Ġk Median Window Size ĠThe Ġnumber Ġof Ġsamples Ġto Ġaverage Ġthe Ġpose Ġover . ĊĠĠĠĠ Ġ*/ ĊĠĠĠ Ġdata Ġclass ĠTarget Tracker Constants ( ĊĠĠĠĠĠĠĠ Ġval Ġk Max Target Tracking Lifetime : ĠSI Unit < Second >, ĊĠĠĠĠĠĠĠ Ġval Ġk Target Tracking Distance Error Tolerance : ĠSI Unit < Meter >, ĊĠĠĠĠĠĠĠ Ġval Ġk Median Window Size : ĠInt ĊĠĠĠ Ġ) Ċ } Ċ