From b2e8fd9a7daf281755e848e8e7d5286f398e3d33 Mon Sep 17 00:00:00 2001
From: HappyZ <zhuyanzi@gmail.com>
Date: Sun, 5 May 2019 16:12:24 -0500
Subject: [PATCH] add function to combine trace with map

---
 example/20190505_154653_map.png | Bin 0 -> 9717 bytes
 libs/parser.py                  |   2 +-
 libs/parser_post.py             | 110 ++++++++++++++++++++++++++++++++
 libs/vacuum_controller.py       |   2 +-
 4 files changed, 112 insertions(+), 2 deletions(-)
 create mode 100644 example/20190505_154653_map.png
 create mode 100644 libs/parser_post.py

diff --git a/example/20190505_154653_map.png b/example/20190505_154653_map.png
new file mode 100644
index 0000000000000000000000000000000000000000..af8e27019767af718ad2fc7af192101c251e0aef
GIT binary patch
literal 9717
zcmeHL`#;lP{6Cw`T_O^>RdjK`B&%fW>Y5~%GPlpCqM}8V!t71DC?Vx8d@6FuEy<mA
zUF9-GE|E=($z5*4Y~Qob=lf55AK&)Nw)gv-*X8+ozAooP+uK=4i7SZ%0Hmxe4>$mT
zfj==oR2Y6yRJ5-Fpjl^iV6WqqjPdWU5*}AQ2lipC77Cq0kF4B!vgx@7#u6FGxD`9{
za<5m&z$Zs76G^!9&*u*Ue-QYCz#jzuPa;4)#QJP1uzYMyHm&P}yYbvo%Ay$jkJpyG
zNEEIyK?N6%-u;}_5jq_}(T_P9Mhc_k&#<O#@W<GR{dZ_7%eMJ50QtZgUJsDi_X9Jv
zdh*KVMw5FYyPV0Mu`HaqYFt_%YuPQNE_<P6atDYXu#rUyQzc^6qPvE4RvMZ$m8?sW
zi#Jd`bB9vV)N$^lK$7rlqv<P$`0^N@Qw?8bhPynv@{93mbiaT9kEmBO4{$QmqWrfD
z_QoDT+Ue*%&dEe`Z;r>vWIo$;ru#X){&zq!kAdNXcxICfVq=-ud!z(AS#)cVq1CkL
z*+nv$$W3o3{l(ugw<NP51bo;MvPhGt5N~SEExYQ_VgJ<HxQdh3pkobJh>)oXfSX-`
ztk-1d>KAbBCKRBawk@B?9|T3-H{wLcTFwapT}uG`x5Yih**AWQsXIl;^brE5W#LBW
z62x8}fT=qg093sw#Izp!GN6?xS>!&*R|Fvm05G)@9`ZxygjV0my2(@nBLYG4o_m>c
z>pUvd0HDeWq<njT88)=hx#w?I&Ik5K&;V(Q7MeM`6M<6wec*~d>$q?b*mWl0S?a`v
zOfjG%uyQa;3fPoL?N3(>bApoe<Ds_ax!HJbFy}sFkiQxv-G@&SUSHzpTO*yQCtApg
zC=cbO0;8{|y^4nd6Sh#3O}h7;V@{x`M6;(YtA_%P|Es=96ZJZ)$Px{Jk0w;5`#=j7
ze%g7_WuiQ}mJ$xSt>I2$&trW=3#y0_IDfaJ6*M|y{attz;I$j-APpwp2U-#U(S@%$
zibZIqgKYLY&WlU)ZzTa&9IgmI)I=~y#;o?syu+6UXDp!1UTaNM_+Q(gJ4yn#T{AG<
z<!aOf-@bsWkq0Ef3xP4???(A^LI4yC%)7DRH^VtKzj!<o4_<6R0dOQg`<S{oaJ!BH
zAo=qnKgu^TugP$r8BC1zpPdm#3I#zia>d5x=OViAPiMlQEZqZ;;XTk5;0i%Rq8zbQ
znP_CUlbYoVK=6cy#LKvVJUB;%r$VW6PZEFyqg~$W86Z&LON0ITP^38YGJ9VJ=FC0G
zl)}wbHWd+X#lhHqXrk72DU=i3*LVp2Su1*tx7K(zICYSI?*Ix}fqbCuE4tR{D9c<G
zje3O#V>WP=^XfYEf9sva?*rl*ba2YFv{g2s@}L&4Yun=Mi9i}~RUwGDX#blLk0*1s
zK<fBQ!Ux@XrwO5R&@EgnRJz^Exi*%K0W^V+hcK0Znra0AVSfU+nkfa81<Ho)rlbK4
zas{!jkXpjj0;=L777KQL{>+by5(OF81S2J=LP6Qt>jhd=q0^9<Lhy^A;LS%d-2}eh
z??;t__2BqEDDsTmMKoxI0R`dlf@hE};POk8EBMeU%KWkrQkwv^J-o}psa_2~5=j9#
zeSzU8n$%BFr9g+^rOIK{GVAO(&gU6AJ_aCx0`p?$MQ>yK0YM5b<{b4YK*vV`WJd6&
zHAI-kI#x1`LPiAveAujs?%XUMn@PqmEdhdn@wo1e)&9z~fWrnw(3ZRnkSV)UB*s`w
z%BUVHbn$v92Ji&2*!wygzls-Es|}b1+(;ByiP?W`@>_Wl$Hoy*FC2y2R<=vTV|xMB
z8M@y__Z<pv(&2Tem$v9ul)3ENxYYMFL>Nr@3J@tbxaGe710x^pB^jkJour4RZvJ<V
z#mUC}U!EJQiq?;5SRoz@hBIz<(>*9=@}dzIQF(_VrK~$1G9eR!;pfvYC7luhZqfQK
z07p&AUiff;P)IdfP{i;+Tb%w0h*_1K`ds)n(ahbIl|`tUo^Tj;eK&xM==g$sDYMW(
zy~K>zn5%)doBEz$IPWr<A2eprv4HJ1MwOay6sClz(>y^%20Yunzva4bqIe135k50_
z79@eosr3ZF*%09173V9?L@|{dAwD0UpeGEb@HH~(U@HC8ED9HGkV3{%8<m;i4`vpI
zGa0=zNh`$wDEtGnIdd(hNG-rStiRg3RxOSm_O7e9g#KkL^9|;8ekK-78AFtY3ywYu
zDMl)J#(Qq8T2M8Wz<|7e#MD7hM|ng)E5C~as1<oIzrw-|b04P0P<BkD^Ll25%<ls=
zEXuf0ZmpUK;F`k-&zEWBU68+L*QHFOH&)7#i{H|0-AipgtSsH4;{)Tulr!TVH|@g$
zrk_CT;O-}VvSAnU#YO3NX{i%uC*};IS2yrOCPxd@)aGY8E`Pq<orhO_xugZ@S%ro2
zL$=<0;2XJEv^VfDlC{g+OphDtwwuo#<#3v@vlH#6hnbb9PQYT`F0J<*!xV{`Y>Z*d
z4ToAJ*N2GPI-wM9C*dVjBY&d-lbsLA@z~H^qI7>7mqFo6@wqefnH`aYdRj$SOwYz`
z(*>66`svl%NqrI<j;^#legyd|!k7lL2CMcv@>G-CCNFPEmE|Q_Asy`e6MvWfRQ<sU
zw2`}*xHgAnya160#qW}LfjwcHfol-cv)45H<uAsG9PjaCS-ul>8Ltf?4gGZ+I#9y{
z55chFnWvF`ZaJ??u)NUaytV6Y?3IS;uIxDiW~bayrMZ3eUw18<NQtsSE$_BJdj8M6
zxoeEE<V>wfSRE|Kz~vE)*0NP1i`_}D>^4#FE52~n=&za~nlt6(Qc7m0+op<B>dM-4
zI`fv^0NhM~vBXtpn{`c*3Tt(QK9~qMlP7A;6hPI^e}j1XP$TWxtcrKdn^b;gtvVc3
z_Ch9gc3LD=&&?BaysIX0z2pVDG%$tLo@$<y^hp0~XXUK<#F=6=VIOR5g8nsg+;~4Z
z7cu`@o4v3ysKmQInZwg%iXgqP3VNyiwZBGnZ@{^m5os(}t@HBTGih()`^VLjV=T`H
z;ObWaw<HX#)`cC%OK1DP70nHym%hY=wkDmj{1uXak<XbNxaw2z1b`Y?YP|4)d%(H3
zC_g>_qMn)XUzb8LpNKx+1L+&S+8dgT;#GX-)&4Vy{Oo%qiR8RIIz^jlJ0nqdB__u@
zI!f0n!ozrSpsHGfFhYjy8&b2|qL^EED8KCc=pe1D@xB$(N7j*gdrWFBNn__r;h(;d
z^A2kWRxm<l5HIU*c1JEeG^ArX_fDxDDrH%i5Y}ZpRhrX1dn>v3P>EazZRq9CLMwHh
zq9@0R*HOSN(h(0dS=}4$`;3<P!3s9l_;KZoH;%2Jj&wEGdn&PSF>Bs6lYc1Uvx*q<
zfVmOoe0=+s+wR|NOZ2LARkqighR=ztdz6W)&OyKHT~-ZTa9B$7+3AAH=Kimv8ML;I
zVx^}K)jjHABz(@q(|Q6~r%}FJ%S3=3?10FwYA;;oqFoV%Q6ulk>o$>DvdBWlo+EuV
z++mLdwwel;y`=ac4WL0Xj4+|TD$2~n#-sfN<**Zh#|gl(cFk=P8pBOVC%Zq$tcP)M
zOM~HR8m#H7%vdb8k&XFG*Oc)qc+3ndf9<eZ#YIfnUCe&Xuo^iIP)QKq6c^&I{@iDc
zBPmt0yy{}gQ4_+;0MGN4{7Dqu-8<`BZd(!p{MW-2X&yB!bYFkPS-60O(>p7fqPkU?
zGSTnw@me{iz1l-^U^$<KLTq6F$@VA?3LCc#Zn+w$xmfZ~()bC!$#P>5vF;mlNi_eo
zxXB*al>S@TXG@ysPRNNk6V@`fckG32wxgW^-g=v2f=%q0;uVSCDC8FO=29+V#8bRe
znL$*#HRR3m^(dSzTkt&0YW(fRC>iwbjU%nPa$^Jom<G_y7xE$vHtE7c1rk3D@qcp?
zefi05_?@mB<MqiC{-bX-yUuD>USiFv0{^pcwVbYKF=KS(jRqm#L4BeBcHp^m@V-_g
z3_E6$h&7=-Fek#9YZFIL_uvOjEVLSCO!g@&$L`Iy54X&^V4nKEL~*Nz3_3)9Bt|T=
zra5-FD0bGXqkF^w@*T3Wt=WZIn|nXJZqQ3P`=t&)j^_DXd++NY{dQx~LfM39M6>Js
zh6%dC&SP*+7#8rs`+ZdgZ}J^itop?4RK@KRzrGIg6Is%4;*YG;u@u#yxm}bdcIfOZ
zJ*fU)q+2v>`h!Kal4rLazZx<5DJp84j7&d$*33M!rE@cq^BsFGO)nSDa!8XPGO30p
zf%q;j(#a`R;qG>uf#G+)x#*?p<rAxQZqhdqRmyTvp6AZK-9Fzp7Jl}oa5T)T?J%#F
zT+1=)!F~gE2Hmj<sJO;Ti6-$M2M8mXMhmV5<{x&TxOLN)oMdg1jNdNgO{;;vgjhI#
z8tu>ypnDGN3Fl14`1teIMHV0hbDVr6Y;U|dmY1IcJ}ytzM{QW39Ibgez*<BBg=`2w
z_)up{$+&d9*2B2>(UD!h*ePjcYP7PtO@zbI<lj3OO>4{xP_KQ*dqfQq``T9200~Z&
zh<PrdylZV>z-ppG2F)~KbV%W$ljrC7!v@V+9I|v#7BM9#@60(-;C2V{Y5cnJY@ijQ
zyO(tC5IHq&5H+|-`{ifiCS_w%Ti5Sh)#t?E<k@!vt|b55^_A8sTyx%=)~=9g<1AOh
zE=e0d64SoNz-37qRKkM?!T4itaFNe*hQhG%<?|=P#-Ae&rCFmp+tR<lR;dZmbG>Tv
zZT$_OE!?i%+Sh)LXj3+$t%)=5yo0eE^MM(*>|YK~e5xtdVS51cvIcC%C31bmV&U58
z?knpX&gBeU@%0%~AbF-Z5f9t%VmyPE2@3K`g}B)<RM6%rGWQ^#WnUY&;?@}1amiKq
zw+V`yWG-IK!vgAgIK#EMRBa-(CNND`1<jN*PR^Ex-Mhk${IW+!tjbquxxzADNAYo2
zXAGd`3;4?D)CAE~?(}3MWj}v;vTJd=hR1e57Kd?(&$j9ciN0538Sy!NndH?;&&DgE
z9VDA2HKg->Nu!_YbS;bjqJc6_5(=Oor;MroYw=@C_+Yi^dRocT1o1sWGaVfHuCVYO
zjoN^E5Kb{(YImC$<*3lX<2@7nERhyHPma<Sg)>hJ;Fz`&aB(9qbmU47oUnaiqRu+4
z2^#Xwdt=yO7wz4DjO~=cH-Az((df_bS-wS=rsNi&lZ1!zKizadYq%Yf*@1Zhq5PK`
zdhc=m9yP+|lIMJ{vD};po7>1FdEEHgQfnv8S3zn6bev4-j@2b+jn*}mKt&`gNGCnj
zIEyw(ul;MNwxwxObqWu<ZnmJ_+(v$O6C;Q2HpzPzxv``{-<?@JD7yP5J`bGP5puA+
z;q`RDFaFe`JEvjcLF#F->QOnG?{vn+tlI6QgL4s^VIkFpsd$DI)AWVy{n*#1@Yo=C
z+UQ!1?)1eAo%$kqRcyBGwSs{i^iFoc+Rgdw9lHCE$9KbHLo!5~cH$f6La{cbxVk^s
ze)p<h8r0+Uxs8{y>u70Qj#5r;6g&e9;_X?iOtxB6=Vsj4m&UiXC#ED)=FlHVhU?<o
z2TG&%`H`N&sqZ&LXSh)ARB?}R$qPE!I$UgDvA9=aM;gCJ7fmOW6F`;VT(NYnM<}~T
zx4r<p=gh7BA$~18wLdg^Zy%Vt1uqJyMgQtsH$0RiG%Idr+$8xr2b62_@8~5zT%mYA
zd7=DB!FUDuheBA#Hu*7=C8|QVcWV1pR9+$TY#r2B@$Rq^(-cXDm1|zhd=vppxeheC
z&9$L<<xB-p>ISWK92c*W`}YbZjxFm#eA}gEax^Bv;MEssKN@!8HqRE1nR=TJmg0gN
zl2vn4z7{Qad41k`%ALL|)%43r7%9;9l^@%H0;IpcT?n^X3>~TKvqKM*4d?`~<Iqa<
z#oj(WNyJR*3Qc{P`9(etm>p1RX~~yPiit0q#kwdZ9B9n-Ra#x_X4-hhCBix4O2M~B
zFWQkg$Vov*oK4g&JAxgtN&lV0bJwEak8R=hrE%9f(Al<GDgYM+gSPSj-P+-RWDXA3
zzt_WdR!CtNWKB;$SO@P5%BG}|miO76^##l1t@01%4P2t*L+UpZ^u90jb?2W9xs9jW
z%HME@1EOnKzEGR16DI4smcK7r-K5q%ExM#<?FG_PM3ab}w+s6DQ&#aM3WM}c<*cH5
zG(4gknr^E9rFFf`_0m{Yp{OVWUo+Nt10Gzfu(0=jP`vr_6=QoRPV%}Yy2$%|C}zNf
z)#0{q%VeQ*Nr*ZQ>wvvcYtbOZY^_CC+&>pn32SUBmE4Sn`q>Lr#XsSQkqJH9);`Af
zbHt@@Sl53S<+Eyu+w-PBVxjMG&q?Qoj*|llNm~GDf&7U7EgByC#=cV3ELrlFXE=BH
zLQuuS-2?1m;x|RFVi~V<&5nqgsruMT&#_)OnW8}DhB;~KMiwqs`xEa?JKi%o9lSi(
zk=zi%<9xBh<lKnNIY0NKYe)$*FZsbYvq+FtXW{T}dG%lf3wL9b+lc1Ke?$VBvCSsL
zpI?I(7!DI2>!b8jojiZ@j`|6Sf@HRh3X*ZElDCZ+XN;1NMP-R04hIR(934Ds6TR+S
zTJ+VDPa>&xyS4--3MQ3yA%Hya({Me`=7*32oTzS3d0$)?(?%lFK1F_GbqSAj_6h+J
z*k097w>y)M8D(q9#1j?Ngy*A?Fr}Km*F-j-#$OmWu?zdYcz(6%cZ<KNlRX?~8%%3y
z2RzBdKrG$OV7+mak2g1v$35-*+9h046xk>IDwr!d<iYP2FY9>V|Ggz|A3)LtP5cgy
z>b`xyhYZJ?R_OQKg^V{o%(G^h1>16PLnq{AOr7&;oLvfp{!4P)AzbesNyaM@skiR2
zPnkmNCw%bHTOOar?BbV0wQ~%U9pNbrFQ|DZf7UFrdG6x&zC!Fzn0;ox{F-oF>!;t`
zo`9!au3L5wT}G|u3Q-3aUy7)2sh098>bo3ayk$>`S-HE1-$y$zz4N1uhAhaL&ZLDa
z==^N-E@wSW<LiE}_mb!ZXzJ;FXnM38jXylsw>GkRudjZ?Se>r%XuCLX-@$!9jy?!>
zy2Wr`x(aWIAZtpkzpkZ3x676c)()_(2a-MD{R^eI%cDTtYlPuqt5!!^DHr+sVlCB&
z$xl=n{>=6w!E)t)6=vl#qr~|VTTDmvdMruNM?5?W&sf7TzCR;Tyi#*u+riZyq)!)~
zOjL?Qn1epStepPVmz%DE2{CzfN8gzHj#W)u<w+qo(+rZ0w`$J<oyKm$MC0aX@!h|B
z0CG$afPBaH#)DQ7XL1_~4O5D;K21L+9-iN_4!?>+>RkBz;exBYLC$({S0}UPZ6;Q7
z5VO-TrP|y{TM)ndi<v0@;^)~rrXI%Lp?x&LDrfye;X2a~T@SoSC&xXJ0RHQsBde0m
zOm&%X?#|cIJx2}A(iiTY?8w)z%<+2E;}ygVj$Zku%kQ+F-_r)KDkcc<5Z&-TK=~$(
zHT@xRJdTAcKHkn((ypy%jIP8<=+!j~NB1n{DGkVM-)TD`YA@NbW{>1;-<3C^zZR9>
zvyGxk9*Q<YD^)BPVVOBswv3ZB7K_*<3tlycrvS_I{Dq@fdt;*KF7J{a-c(3F50gq1
zOTfQj24g{$Bs>_8>3VKxtL5VB4*3@aPi!!3$DjH!S0r|4QzZXw<5P5;;q<pzdTX#K
z7<&uP;TB7F!egI>nB>oK$?xNKrzxKN<$OKV8TVG4zN5Ns!<7qDwP@gWLlpq!Und+r
zx89!&nQKHi@_h403pf5Ur<NQ>E7!M%AO3;{RdNuk){`abCdqD=mlmZbe_e8Go5S}z
zF<teRUjb&U223zFqt3YXU4yGKcIzonkMLXXs>GhKw#^{#AQRuba@=nLm=-WR{^r$)
zxk<aK@Jv2SmdHE323uBbjW{R8q}Q-<J7pN7OB(Nd8P_V{O)DE-mr?J1x_5TmZ@DkP
z2E8&>QGa>c@m||fxhd1CF19lwxFivUz$l+pwkcnpl{vqd+>7-Jp-e49&m#4MK<xIU
z$Rn}xw!CFB3g|H@77W21Vp~>>Ij}IC<L~A+z&j(RgbLzmswwNhIQv(^tKfd49Jb)@
z@uvM@!QwxkKM4Fm;12?S5cq?@|2G7v-?{h$$fv@~4Kq6hzFQr%J5apOEAoE;?tPvo

literal 0
HcmV?d00001

diff --git a/libs/parser.py b/libs/parser.py
index 3c70a93..8e2cfa1 100644
--- a/libs/parser.py
+++ b/libs/parser.py
@@ -285,4 +285,4 @@ if __name__ == "__main__":
 
     args, __ = parser.parse_known_args()
 
-    test(args)
\ No newline at end of file
+    test(args)
diff --git a/libs/parser_post.py b/libs/parser_post.py
new file mode 100644
index 0000000..6777f7d
--- /dev/null
+++ b/libs/parser_post.py
@@ -0,0 +1,110 @@
+import io
+import os
+from PIL import Image
+from PIL import ImageDraw
+from PIL import ImageChops
+
+
+
+def build_map(slam_data, map_image_data):
+    '''
+    draws the path into the map. Returns the new map as a BytesIO
+
+    from https://github.com/dgiese/dustcloud/blob/71f7af3e2b9607548bcd845aca251326128f742c/dustcloud/build_map.py
+
+    '''
+    map_image = Image.open(io.BytesIO(map_image_data))
+    map_image = map_image.convert('RGBA')
+
+    # calculate center of the image
+    center_x = map_image.size[0] / 2
+    center_y = map_image.size[0] / 2
+
+    # rotate image by -90°
+    # map_image = map_image.rotate(-90)
+
+    red = (255, 0, 0, 255)
+    green = (0, 255, 0, 255)
+    grey = (125, 125, 125, 255)  # background color
+    transparent = (0, 0, 0, 0)
+
+    # prepare for drawing
+    draw = ImageDraw.Draw(map_image)
+
+    # loop each loc
+    prev_pos = None
+    for line in slam_data:
+        tmp = line.rstrip().split(",")
+        robotime = float(tmp[1])
+        epoch = int(tmp[2])
+        x = float(tmp[3])
+        y = float(tmp[4])
+        yaw = float(tmp[5])
+
+        # set x & y by center of the image
+        # 20 is the factor to fit coordinates in in map
+        x = center_x + (x * 20)
+        y = center_y + (-y * 20)
+        pos = (x, y)
+
+        if prev_pos:
+            draw.line([prev_pos, pos], red)
+
+        prev_pos = pos
+
+    # rotate image back by 90°
+    # map_image = map_image.rotate(90)
+
+    # crop image
+    bgcolor_image = Image.new('RGBA', map_image.size, grey)
+    cropbox = ImageChops.subtract(map_image, bgcolor_image).getbbox()
+
+    # and replace background with transparent pixels
+    pixdata = map_image.load()
+    for y in range(map_image.size[1]):
+        for x in range(map_image.size[0]):
+            if pixdata[x, y] == grey:
+                pixdata[x, y] = transparent
+
+    temp = io.BytesIO()
+    map_image.save(temp, format="png")
+    return temp
+
+
+def test(args):
+    if args.loc and args.map:
+        with open(args.loc) as f:
+            # skip the first line which is coumn names
+            slam_data = f.readlines()[1:]
+        with open(args.map, 'rb') as f:
+            map_image_data = f.read()
+        augmented_map = build_map(slam_data, map_image_data)
+        filepath, ext = os.path.splitext(args.map)
+        with open("{}.png".format(filepath), 'wb') as f:
+            f.write(augmented_map.getvalue())
+
+
+if __name__ == "__main__":
+    import argparse
+
+    parser = argparse.ArgumentParser(
+        description='post processing test'
+    )
+
+    parser.add_argument(
+        '-l', '--location',
+        dest='loc',
+        default=None,
+        help='Specify location file path'
+    )
+
+    parser.add_argument(
+        '-m', '--map',
+        dest='map',
+        default=None,
+        help='Specify map file path'
+    )
+
+    args, __ = parser.parse_known_args()
+
+    test(args)
diff --git a/libs/vacuum_controller.py b/libs/vacuum_controller.py
index b3ae19c..811ed6c 100644
--- a/libs/vacuum_controller.py
+++ b/libs/vacuum_controller.py
@@ -438,4 +438,4 @@ def init_controller(ip, token):
             print("Cannot connect to vacuum!")
             exit(-1)
 
-    return c
\ No newline at end of file
+    return c