From 9b6cd832b2c44d4c238b627c79df1439d0e59b89 Mon Sep 17 00:00:00 2001 From: Project Guideline Team Date: Mon, 19 Feb 2024 22:54:07 -0800 Subject: [PATCH] Internal changes PiperOrigin-RevId: 608488784 --- project_guideline/audio/BUILD | 2 + project_guideline/audio/assets/BUILD | 2 + project_guideline/audio/assets/turn.ogg | Bin 0 -> 14987 bytes project_guideline/audio/assets/turn_fast.ogg | Bin 0 -> 7998 bytes project_guideline/audio/legacy_sound_pack.cc | 41 ++++ project_guideline/audio/legacy_sound_pack.h | 3 + project_guideline/environment/BUILD | 13 +- .../environment/control_signal.h | 5 + .../environment/guidance_system.cc | 9 +- .../environment/guidance_system.h | 2 +- .../environment/path_planning.cc | 33 ++- project_guideline/environment/path_planning.h | 7 +- project_guideline/logging/BUILD | 33 +++ project_guideline/logging/debug_signal.h | 31 +++ .../logging/debug_signal_provider.cc | 198 ++++++++++++++++++ .../logging/debug_signal_provider.h | 119 +++++++++++ .../proto/guideline_engine_config.proto | 11 +- .../visualization/ml_output_renderer.cc | 59 +++++- .../visualization/ml_output_renderer.h | 9 +- 19 files changed, 553 insertions(+), 24 deletions(-) create mode 100644 project_guideline/audio/assets/turn.ogg create mode 100644 project_guideline/audio/assets/turn_fast.ogg create mode 100644 project_guideline/logging/debug_signal.h create mode 100644 project_guideline/logging/debug_signal_provider.cc create mode 100644 project_guideline/logging/debug_signal_provider.h diff --git a/project_guideline/audio/BUILD b/project_guideline/audio/BUILD index 8123337..e12b7c6 100644 --- a/project_guideline/audio/BUILD +++ b/project_guideline/audio/BUILD @@ -151,6 +151,8 @@ cc_library( "//project_guideline/audio/assets:legacy_v4_2_warning_embed", "//project_guideline/audio/assets:legacy_warning_embed", "//project_guideline/audio/assets:obstacle_embed", + "//project_guideline/audio/assets:turn_embed", + "//project_guideline/audio/assets:turn_fast_embed", "//project_guideline/audio/assets:warning_pitch_shift_2x_embed", "//project_guideline/environment:control_signal", "//project_guideline/logging:guideline_logger", diff --git a/project_guideline/audio/assets/BUILD b/project_guideline/audio/assets/BUILD index a5ab494..3acea53 100644 --- a/project_guideline/audio/assets/BUILD +++ b/project_guideline/audio/assets/BUILD @@ -45,6 +45,8 @@ ALL_OGG_ASSETS = [ "steering_on_course_impulse_tick", "stopstopstop", "straight", + "turn", + "turn_fast", "warning_pitch_shift_2x", ] diff --git a/project_guideline/audio/assets/turn.ogg b/project_guideline/audio/assets/turn.ogg new file mode 100644 index 0000000000000000000000000000000000000000..107f22d532b58b2651c6c3959f677ee04d5517f2 GIT binary patch literal 14987 zcmb`uby!s2*Ec*^fJjJ-q*6mlHwdUm%h0WKNDd(kp#st&3=ECL&>`KRNJuwG%%C9M z-SD14zu({Qe*bvi=f1AzT!)!;_F8-Gy+3R1wf5d;P_?vF2VDdGy|&u$pI}Jbc4JyB zS}ZpQXA^5ErU2`$B<2eQdZ~hS^__>MimCbUg{g_**u-$a2*ob{`}%qPj~;e_tz+$I z#iiwG>dlLWygagPu z|B&@O1gR5YVV%nCnZcTv!B$Xa-BrfXUA9`sbyfX22=qdYw9Sze`>LK92!!*JH6&9w z;a zra*`dZj;MU-<742>66Vv`uzk4Fqsai7fRzWcx_4?qG6kVOh5zzy&@G3de^~-!{Bd1 z%JRf1n;xL%8FjimmKbqRenc`g$*}HPpvDyxz!<;K`xD6kGQbBo3l4b>&W<+;0e4VC zdw4qN{qFECeTq>L?aYc+5$$5kjDXmJD@oOrO6SQ&Gp(WB_(=eWP%Kv;kyn+E3 zFww1hW>G)~{YkHsIs^Ks{|pOYV&(w#QwXD+GoxHIV`nsne2jQ^Ip$U#d7@0FIjU(l zeqd*OU^g>rK679`HEM1cXO610GaNHC9C!Vn_tntf-T=Lg_zj>A+|MJjp5MMQP81f1 z2=q2nIArcQV2=nU-Dqa(I7^SX9FN4@VA?AYz%T%6!1Xrc%503faoN_XIT&N-=Z9qH z<>%WZW?TP%-`CO^3V>-00?~9hQOh||cQ|22b%ikfbr3xWB&$aK)S06rnqLtZe~bW#V6G0RIB<6YPyj@Xxco4`^8ewD0rnaQa6am`co;+>uTT0p4~wN( zJ9R~T5&@Zvk{+j^Sr8?2QO6KiCMq*g;d>NMhz%I2jswm^!UuTTv$DtZ*pL;pfbs5v zFrfv9p`FMyi02lA9}!PGy$jKUc544SGPfAuqzaw%{=_o!A5HJbU;|tTZn_zf445Gn zFzGdF;E%%}=*ta|(=`x|Odmr#{gW_h9HuN%nde}WJ2DH&@E#E`5s5C!WZfU!Uw zOCX~_;}2I*;EaRo^(4m(Ny{Pqq9F22Rnd?zrq~|}3OsRelh>T>lO}}%K#e~P&MGSX zFafzRX+RawI1=j2|XFm6bni zSo7L2Z6GQ2!z!zO*sNt$kVArj?wW*_j*aORrNb!|uNn{I8JjjK6ag6WOzqK-P|hj? zpz)kBXFE=S(Ie1q04u6o9MLPvtJDJ+9aPZ5$_2DEFmhsjpbbD3XS<#(xb&*=wyVZ< zpsW~Ge;aS7gsK#c0t|slEn7eprR~}uRn~E( zsD%11ve$4@`sxc$RJ5g`{eWBCIAcxrhxM>nf8g`Fa;1P$TJ}U>oOrVfcXCI zI_U9R{6yl=>!eg=#2_485IqqjglCZ+9t}~rE2AWGi@_fPvEc+vy+G~%LP4QOQ;7$l z04;fy3Owz}K==WJE8~E0j41;n$9Nb1lp#m-uz?Vy05mkFw~C2KU}V;ap6yi`G7GL3 z%JZiz8e+>62f@^ZLu@(QAqrp&rC8J!50F4zpo~%-pn$riZIq?}1Y~ zQUYM+P@47txkx>XX8}k^7zU7)FpPiF6@knZg`JLJ(EATy7(_CFt#ZF$f#|23@Zl6FdF}>M@lh3 z7(kB44Pz7Jm!zw>u{0T50=M}edK@amtW!nol zwf`*)>i4zeguzsJ9Qq%A{H$gMegE;()fj*M8Dj?kFT^JPJ%Bb-)__cAJjPT2l)Ot> zSsH-fw_ygF1%Ut*5Th{e7zRLo<)S8Gm6*YEw&|D_U5&P=lF}9*V}{o}V~N1r2D~_| zbSzO3s0)bHE*JnquF|nB5&*&kV46hRzQ&v}6O0ISqY(s(!NvyV-neh6BQMHMjn8$3 z!+p?O`c`0knCy#5?pKraih@HFvVf`n55pe-;}b9|aR93*fbacfD6k^^iAAp%xK|9M z3lI(d%tb(M`D(@RhXEhLr~+(z^~i*2<1YiE@Shq0_rDq7e^p_s{Z|bz5)%XdRe+n0 z>DAvy;(sZijOo7^5TLvN_4rR;uFC#f0pvCaB#N=;ql_V$HU7D-hmKDezTMEU`mO+? z2NKug8=`;Ux(gWdiVBLyK=TK}|1q+E`t+aP{a>a?L9PkEK$H@%=I*e|TLxr)4fs%a z12hCod{e-^!QZpWf%O-@DUl3zia)7ME>rdg9vlW2j9XRdKyRgI0TGh3UB|Ci0&q1T zk^;ga6mvjSM~;{jR4!77ZCr(HjK6I(S`Ekpb6*d-A%Sfy4ul85{UosI!X9P7<4g)m z$EdyTKBH7qP~>b+g!ML3n))k%+x1|5wTlK}cO{YJrp2{~v|d~-N3%fgIGpt(l*D-z z7R9sh1G?2nVH!r*^4$Q+)j~NJ`l#iI{&i6;ws9Ds&(v#E;OG;Krv>L>81R9D4n92s zX6!RcV}C4A?Og&;5Z*Ilh~+&kDyKPI$_G)v+`|I#05Ka_i)2LsU<0xJe=IwbuOcZ3 z^yLO7)3N`E)zzz1n;wxj+LQiCB5K0N`y0Y$;rw9fBb7}l@!G&``>ktNp$v2n1p1u? zhwE`t$#h9u3j}?qpJ!}id|(>&e&~Hp6i<+R7mev{hMReA13UqwnN*P536R@1`63@8 zcyf7IenD`Nw*b!}0@2emg1&&Hzuo~o5`Ii6LbQZK?`U-$bmz%;rI#;o@CkUaeque4 zVFNu^dI9Hhk;b{36-pn9&6QlU-HOlRV&!+W{Cf*}O9*=Fe=R7;!lJlk_S!>PQ@_k_ zbJrQ4m@!@nJO=#)mXi!v4>A%G3O?6-`9ARD=hD^@2>0$iAXxpqu3f_{t}z=JU@>;> z@4k`YstAPHH)6QK)1#yJGg1+V!ouomc!FU@>Zkb3f`pup@YuA>PpNTFBov+{CuwfF zrxyPd7nmLGkIhLHrBO+d^cLMRZgT+-ehoD_zavDVO%|Hu+qe2;_s!gX(#HCfkb3#t zI9upk2=#QCTo-k^_x_k>iCf}%2;cZi*=usYmm@EdSyG!VdJ|3Qe9yK!=q``?k8>{a zR1H`jS^6$4CZ{!Buv}bj#;ccXuX&$ORmTeS(bfav<&jc87o{Dvr$=+Ea6h34VVC+J zxD6*csWqa@{gv*yvt2&Xsg+3OsX1S)5kIeH_Hn4zx@ai2Sk_aY)}RWsf9qO(nW`6OOv^o~_wW)V zUeYu_?ZEw&pq}AX^Daeo9f#%mxnl~NTg1ZKf3k0gQ5mAGnWvBs(R^v7zQp$NzMiE* z85+EC#_Nmzj(*i%v0vgGy|&54Ru3zniRf3K>2^j?w@k`^7lyY>c$@3w^~wJF`AF-x z@p=Q7QYwr7x@*7g!&CUESo3G*RUMIdnCsDu%3I@^hC{XO(J29y=2P8f-F04ryadnQ zI~V7*q#z3sa*&LDyfpTDU1R;CyAS#0Vs(nq^Av&)>GA?Tlu~;XcZ>>IeM`EE9FHXU zP4+xiU|uT>gFj;wDXmU#C4hTal3ttM4groiu3xTcR40Pg93N%HrllYDP^} zM?U5bIDl2OJ9eJOW|r5KX`Q<8Q;3qeRbTs@8@e`FG&8!Y^`Y$9<}t{iBrg#vnT!A# ztezzr#=UAppY5BfHX0C@xX-8M6s~x*%BA`oF2@=NKBUbraZzn1KlY>L6muPt*Yp=n zyPXgbIw{074PFhXwneTs(^`^LKfBnkYns`+b9uSa+<;ngxL`I;HJ;f#4>ztCZ4%8v zFpo2RS&$7JIeG2c;G;M(6oa})EB>ZprBcXVZPUx^`Y}y)8v1y9W6A-I_FbIoZIo)@ z+*)Gs6a$IpfqO*g0;WK38zzw}govGO_mRGmdE0fpb87Yxu6e>!6Q#q8`OLI&_25{c$?6WF(@SIIg~mbrWe0Hy`as<4d{y$G?$CO@ z)>}mPpmAq98Lg<8d^vn_?Z_WLuvdt`fKFe^YhK?nPI643`g}nBj)CO9+rdLs^zg>M z{;Hg$^x@>?8uEM~<{Edic}zG<^7TaPT2kSs-$0S>QXK)`9x;At6ML1gu<(98#M#Dq zr@CBXiIQClRd2qC)>e1kds&r1Y29S(>WBIjy;?)b?X#kEdss6p`)>WwW(k+U_2$C7 zd^O1;$e~MK>~UOP-4xR9!S*(rWR~7Bx^;Lde=X;1?_+F>T8@yKKj+j691pv@J1^6E zZbe!EvXLJl>ghXhG@OXad~h@R8xg8p=<9IMfq|8(`|+l)sIN<08BK-FMvk%#2xK&N z^d1|eM0t7vqxgclQ7SIv;a+VBki+xDw2{XkmYL zzRs_Blc%d`*S8ETTk!gTwc6iE2x*S;1q5I<&={ye-HUQHQntdfCH8Gd7qWX}5`=Z>QL%k@H&yzc|V@qVQDK`H@G_%~p%Cbz; zR(T=C8UxltrPB$=bMk6cvV-xd%O0jSXH_h>9h+ZcF+__ff$3Y54=ngwGID`SaCD4!Y=y>!EjO zDfmam>W8xlHP_oWQa377SftVunR;Se4VFhdm%ZLRZ=4yAhM}W7d&1lw*ih4E+EIW& z|3BF)5B0y!UXfov+=%!-Om{A#D(>5xD9|B!aWoexb+Mhct9q`UR-^K=@1mz)M9TYI z_3UznC&y@R;`F52f!#J)!VM6&1d`pEX{5x#cE>mv+kx8Yf1AA^6=*{3qJzsGRr5!$ zSk4E|#eH1Jk_Mn&JNt?E;>7S~?w4N9)$TYH&2UlyDfnA+S9g1w)aAvcuD-6W?y40h zN>rL z&p|(h>Vyx;oFLy`OPE@QGMoD9z*U=Yrk3%g44 zQI?w}X=wumI?!d|p|rhJD%r(fYWu;9)@@S@8xDyZb}2ii3c8ldvtzTP1y78wfedmI zLIV4mr4ZM?aCBUI{2iZ9dANA09_F)Xx$WySWq}LH@q1+zE#R}at^c8|HDLGJEp9b< zv-VS5B+Fda@gzg7gs9JR{x{~5t(msV@5&Ar=$iL~#7;@1 zc5BAbw!UZUvMcp;7p;*c!6oOv=+Mj*krzEI_MfOv0H%3V8;SI zDXI#IaTqOVi~15JhE=byO*0P}T{K$TWu++DlC?8Vj$dD)J+MxRDMtrpRaP4Hew~_| z8yrgWt&Yz0?Vr)=W6w;hjuE0czpTEX*5-~3FFs2!Xvm{IPn)hE)$Z@7?aGLPw6>EPNznNH?h9=g|!LN{`JFJkKOjZZUYJPRat%cTxunp|TJKKkxJ z)3Q*PGrl{QQ&~IT`>*j+&`V!PgsTiZUcjPn!=WqKd-sm6XiC58M@2{);a0)P�=g z(HC`+N>jp94c=n0qaM(B@BK^Hr)Q3PE24%CiC$3-?&qs=3+$`ZQcYh=@ja~i)XRqa%glo@;g4qKgC$VzY7Ri$S@~&Mt?`v8mfB1(*Am z7p?0kvCFM(eDQ0bHA=HbbjcMit{R$g-OEfau1-F_j)u}Jx23|Io%YX(#zRTGElYiV z=pJfr(9p1VjO@<0a!>b-4z9%@P-!pZgBS zaoUd=tEZ`|BE4Hk#=6fPyt<3{7N$ESe_iuEJgIJ&|17bK5gXsyqdOkc z|27|aKCP;kmUi**@(>K;mfV42=`CRLkjOh1&^nT)$jdYxO*YVN{cu6D^>$X5i0bC zCbB5n3wQU~UeFWAnT!mAL$5KT(N`P8wR*b)!qj^k!EyHEwhJ*nwSxN$!Mt}i7|n5O z2I~_IXUjhR$e%p;`88|)#{=+>rWkI%{P%i`9}I-dYBS?q?k?dxcsKFJ+Ua}64G>7{ zs!zi72Jw!Wl*)?kgq_=HZdr#91n(UztOnPe>0drumLVCh`Yo#K78v$TfNUdG4yEBT zQS%@viMN=%7Z=KPJw_~B;zrx1gPXK1M;N<)uAAAdB4hnQu^WhD^X*PARjMvoSNSd` zjiEv##gr3T<%C3`%oCwc?MjjFc__Ht@-Dx~-g)Po7WR1{ev{XW;+@JSmb=c*lRpMJpYsjU-2{3pFuublS{ zlO^XoYdERh8Bx#qmN%TEihWTaY>_i2>=-Zgt+&;oZFV7TS9DMu{pK_qc6!h|vUcS9 zzRl?=f&Iio^N4{a&((ZwF}zbUOIkmCT~i*&HH(nzT$6_Gb{LDyaCM;`v! z9p1a;LZ}(H891;>SYu=`KNPabN za<7Zr__otInq_@Jo#r(aOVO1Q5UBr8f@P;7`I}(-(|mm|3yTU13tcPXiX&s!w!^+& zN_EV4Gz)ZGCD>{p!P-0%Ja4}*^rde9D#6CE+ZBEE1&Np6?K+-rydk!B>%p<%xWC^8 z;YDeo^fSStDCO&RPv^*H=IMjlNyqcE%UwvtngRoTc0zoQ_#suRhMrAB&1HC&`}fy7 zP^B?1t*J{07Yv!x&Ik3H>Q@~1<2!6m+veU5m)Ree&$vs%sMAPkGh=*d0=m))edd#n z8rDX;T7PtQpC*O$7wxEFZK0Wcd{!mSu~8{YV$0`|*SIfP{+HXtjH_*8?#Vyf#Iq=? zEd{xtRhlG}#hgpk;awlsI{J3Qg;7yE9yGk%-l)%8)%vM!_YU>+N-4`q5bk;0tcwdr zq=txSD7$T?s$9iD9;*vGD75Z7t>JF5Q8h1Iz_TCw&q>Bx;6()H!klDW2fkIlMnmqW z=I3_BCBPAg=$MbOh}f7o1Uv$f5FHm67H&{fl|({ED+qEd+BN*(%Go_XKXch%pu&v` zi4gP>aQ9gLEepJ4<41?jxw3E!FW@T2n)%b`OKY=wpQ zBXV;JjJ(Tt&Dcv8JP3L{i;|`_5Q0(eYueh4K3S4lwYqH`h<2e77>awwz`Mf98$BeN zQqnK!p{&h#^}kW3MbugRepax zcSBztroDcX=qv7U$d>86WbNBe<1rGj3b$JiHhOW=$M{-0JU;|gJR@s#huY;@m0(V` zF0>NfO@ctx!~G3!Lhv6G_VqAp^RwP}CR>7#v{JZ_;^To@vw9DPznW)wi#G483Q}gCE z&&blI?f%PSsmsNrW`^MX{i{-h;ihy)J5UBxNL&1X(@Kdjuhc7|)hei4%?@?A5(iig z*73P{r{*PNV_F53qc&uj>UJ|ysYYJn=fWu}IgRo$BsnVk|D?;ey@v5TVLT_XqZ;j- zeJexj)T8Q1MCW>TY*uXHGu~4xrBS>62rc`wdH~v2jE~X_h{H?-$Pr%^qYKb5q59Z!Dd&Ar)d!Y`kDr|d5=&?4+r<)W~Bde@W$&a80f*AqqR z0>?5H+cbIX=CXeEUmT&!s%yZnW{ld8WNv`4{qNWF$PV6~*DE4W{-LVt=8cWAv9Ad< zsLbWn>y1M*9`vGkGOF7*tCUs@XKivfL!intW!x1fy}bD+S*6^&3;f|-?zITv^kt7i z9pc;W@+f5+0rwX8#KJ-TGSuouCx7-qk|eKWZk*&HNqPBh?M3`dE%)syELfLvYJQrF zxrXGr@8f(=v41;+q(gBMjizG@;Kqrqi=z<9SpNxh@EHe_b!if>m!qz`%slK-sae<;I=J1$lN;>XrHGd zv25u7zQ#jJn#NY+$6m33FGnL49p%x{Glw zj0Jy8`*vlJ6Dry#+g74)Cjm+M#W6_eE?w(1h@)vg=J`9OI#0PKi3?#hF&_Ddqs%7YjZxSD)D4j_w7~u z-N3hAJ5MN#9uG`5JT6$U&J6*JQ(&EVrSe6_Vr?*!e3Ud~3+yveJA0P%VatZ7wLnmx zrUDjA#I`oRF_llFkLGlU&2z_5x8~~^7?LHewi_q6w?urM>e`+{E~I*Sq}=O+$zIAN z%=3&YpYoc9^P<1md^v?Qp7JKF^Irc865eM#Q|RsB>B>{o#3sS|U?z`C%jf#%)XnvG zkpTv{`vj2w#GClr-%7)b=s)wtk@pShhmC8+csDVqK^4Mm8qsC1@2@QOY(Zz9Zr$=_ zM@Z$=T!-t9pOj_Dx#N46jXK(wR%_m0hosDmIeKDCJoo^fJdAD#rISc2w_(k1ylJc? zMm)D!Imj_-KLq8VjPB8JP#Wv!TZYf?DCA6-+>s==KHctJ z#w)vB>z|Lkt->U~E9tqBsu**t&9wx5ZcbXN*PHv%Jqu_DF4)_qO-|wYizQFdL#wHOrjcWCNud5 zq809$x^^g{G(G<7HuoFt43m=m97YpkI`%5^E^;2VXrS6miiuy0s|c>J3!$snZ2C4N zl^3+PUT_Kd_(ynDb;9!5;I>Ln5ufy`-u+pD*_87WUv&o)m z`*GYcaoy^EB46&JKuY`jgkY{xF!UEfFKretNBqOw$RrTb9qea$CAazJcikoWTx^Oo z%vOaM`ZJ|fI)t-0pK_(^9*Tr^I!O2?sP^_POb%UFRM=N@m~~HiI{C$K5N-^Ho#vYF#P%mjLQe7m7|9s3SjKik`HmRZs|y$;O_wfDep&$1L;?)8%g zz-qrbcYeIql0c*RTAOw_Y2$m=I-%v`jVQCs(FoA*rrz?I+7icR5A6X-!O$Ico=Q(M z+m9=^?ESmfziSzci%W7^9!vxw$!BdsZ{Cu7(C$6RiqEi~d(V|^)GT$Z;iltFDJ{3N zlkxdSDvd5Ip@gw4R8lJFNqcrdxA3ghXMA~Se(C3p-5E{_*f#xcHvvaS1BnmgQ!ViU z?=z=#hR7>t?}pFV3AZgFWeF0TUMa}5p_hrJOq981pq_6VCRA$}i6#;uM&t|?L!a(u z*mFuO>xaX!U)_`Pt$nj?6_DmxZ?RhyGh8oYY}_Pfj;Jopzc!M1Y)%jQzP z#jb?p%4*w7Qp|n9KXUkkCrZMLN=W(lQQ_^s2tcs? zx7}G>vJMU28|a-%74s%Wed=WvvaosS%~qdlDOf&fN_Up)yHvw#$60m`_5V(04wuOv zios2B*6!4vYjn;^$#-3JXB#Kns7L7F^Cd7U2?(g0)faCpry%m$>f>yP6=hwZh(*b^A;8^$(FU8IhrxKapz1Q8|i@fhQ`u&kQYr39E zXuLYtJ)Kk0h)aQhT8~020=XcQDI1c}dt@2EMRQS`GXhFJ;oXIXuF*9v3_8Zh2<4BB zoe4fgiEY6=RHI(!oo%m!+=$$Hc6)Y+4qjV*wK^O9O6S3^-~gR5?Ji%jajKB*(@ulG-yE=r)cL=uo4v|4cf` zFs_WC^emaqf9IsCCwYszR{Q9|{qx6Wr;ij3aF$t7TZ3ZOi-cRZq@Bs;YB`PcRMHgs zEOgznLcI2iB*q0oFKf=m!C}ZJloIc)9-WkpSlow<7sFnC$n~GAoNT}*bTsnT`*+pJ zWf0HD3Z~g68e*D$+Wvg{n=oa0rKAKdZIooahCaGf^MT4(3Raa*t4VK?_M^TG1lb~I zjhb@h-EeeEEXQKw3>y5xu$ms#> zfnP$#>8MqrcFHaFn|zhIJ}z`F5sCd&hs7V=Sc>md*-ZhjP(f%Qa?$}_&YV1f1q#i` zIr+#XaN`Z~4&kW7f>wSEE82b%_K`e-Gf@D(F&XSU)ixlgOp4fpfag3?3c5W~+fG~5 z75Sb#fte=mR1kDGB&oe$D_tMrCCO?`da^m}wwITfv#Z}Xpy`AiuNW+PIvOy*K8&wy zLzFO>lHd9e9u;~2PttejKZpgR0$xL{!&PJvAm2yGU zl=?GA_>=qoDoaCY2_iI0MsbQhshSe^XC?8>SMGu*pJnKc=c?d^$_^ZG$k=|HX71NA zP&UhUEnydxy~!SX%22WHo|8?2`@=#Lu?u@8PCpEPa8is1I}su>O0xmWDaaYxyH~Oj zGmT_zds_)>VfHYXC7n+EBe!Rq;}$IH!K>y{iYbScYdMXy9m)nJi7Pc`$yppL471Wk zJh_9`PW?(sHTvggoi!IPtDprZBN66qY4fQX!$yq(yHLWRPaR!`pQfH1(NOI+F#0t1 zMnx3Cm?aKN??~2PFGs!j`W@NmfBW+DgMV2CugbKE5|>&fe^NDO(i&F7kCVUp(q&n|Jtpt)GyS$t)D{GJ)L*h3&p5djGL0 z^Dw_^?{N`Cg2{ZdNSxVTTlt`gc&63nMH3aZ_VDPyWTlZ)Zmg$J(!r8pW1JP*IinOv zJpc4e7>~l@k>|RQjcq!&jMa8~Sr`mYStck>o_zQF{mPmuZ-_c%JWroZPcY7$N?{2}G?V(S3Fn+H^E>XqhnGrWw|&yO7OWTaiR5mm=Ez zO_BlTKSaV-(Qw>UVPjImiQBtvU;S~uv{q)j`&4JaJ+$VPqESAt3qRm(f6l;#>8E>j zgrjk}`12Ln>;`|E1uJ_7WRFsBI*+^5J{Rs>C=ap7O!HF_o$8ZtSx;h|&-e=AlAAt@ zpIBg0_tX(Bw*Iy&9=pFjJWD}TY!|7%L!iI^yqUiczlXP7_Lq5rPekWwj-D1n7kK*R z7>B%l^corUv26@_AZM8-4w=co$}Gxn_6sb;oeVP*{t5R#P&WeV;@;;7@jc&=V^v$| z8&lcb!3wv)3AnHAqO9n>WTszoUbMSI`{u)U^_zCfS;e7Qi0aqcdBg}-F{n!qWWS<_ zVa+LYSh0>p&hVRogY0`lv+bk8S1SkBlM#<|JMVbsL4QD_{;zi-o_z6LhLF;F$2GaQ+mQDo^&8Rt!q1h{?HAtikW#Q#=C=|Zp>P$`fl%o8-7WVol!5IRl9U<6Y6r-C4!qHVd(5J#{-;K|@ z(Rn_In7pHkEm@b-$r^3{g0H5$vI4;{D6ldzyY&aYy zEO_Wd@6l^b|m#Yc=T&Svnz#Vj83TAQ)rI-zTln%$@k~@{2~_L6c-r^d$B|;sEy(c zVTAnLn{mv01Mk22x2C;r4|V;jLKyme3Z`KIDGgDWqrV&6?Ug9-A6Ng%A?n34JC;K8O3k7NJjWKeg?K_y790bez0Sk3dnp| zB3WI1I>hOrK^L#Lb}A2ZZUoB*`S>5F?*c!T{ktxGy-MXJq9wm{ z{1z@M6EL+xT=Uy=$Yb+9)1WDQfGFN=_>;b0nO*(r#%7i$v7z~RUCrsj-QeNR_(o*{ zsBfw0h<=*EIts~Q1gYA7so)F~<+zi&^SXKuo`U94fKiuL<7o~mZh>m-E0TK_-`_X* zACy{t^rn2R)UInKD6&QP2vs^axZp-lL9d-uB+fSQFRM^PYRMMXByF^Jd>Lmu$*Icm z%_c=}=E~O4KpOH{ylq>;Y+)ZgcOSknkaVqz)$mzhY9wlzVljebW{`QuqX(^ms@vZm z4p&X&k0is#3#h45ma<=ECVtqo{fWT|Udv&w5R-IxG&oZe!%HZ)5yB zjg8Zf(DC6ZFBJ=HvzE@7-<;C@!8rZxnBR3xYH%b-QI- z8x}KDkWM!F#xb<_9P-H~S}~*KZSNn*Jo~ew|Fgg$l6mx=ioIX$XnbDP)}U8vciWzdpRpn7i_LQ=Tk2a z&y5ZI=qx%EcS&(^KDkpa6Ma@2$wdK~wXMC7SQ?_`B z3zK>W9DV?0bpEGDLn z7j@xVELuyb(?cJ^?>=a)lS5d!Iue=o)S$j`nn{X8Waz@|L2rtf>D--bfs<^=@5rTvTDuiU#Yy5ZT3#u z4?g(8@EGbYS6R)aMkt2qstwRaNxR*1o~xE6DXA#KZ<{p&Wyg&gjq_(`D<}Hon~lxK z2dYxUzvE|o8r6r~9~w8q>R;~TJCDpFB9m!py-`gUr1h81-e0o#JU5dQXz5g&^9!is z#*&JrU!DZ@C2ZOuif>Awkb}b;#jwWqU+D~tpf#$gS)Y{+dxmM>!)3ga`uh@HsD8X@ zbU5t^@crw+{vPRO@z0ILL1}r-ewxt^SNmo!Elq0Gygbc|6o#^!JC9C0mk)EHfddqDB-#8T*!uh{;lhkfm&uWGPd^SYimFEMtpg8KTg_ zSjv)Jwo1tUl_k69enx$l-}8E&*Yn5od40^B&pr2^d(Zov`@ZKh4DIcWAx7x;C7)D! z3Krkof_NB&8LnS-w{i4<8yEsq;4g5kY<*=j7{Vj}zTlAz;AvCCIf?7!=D)8m+qUc+ z0%|kIYYx(e?)FG$M;lYRb)*hb<|szysLau0NJ&E%SI2Aab{=-Fo@m%K!}hJAVpug( z2Iw>dA+X#Bx91VU73=n(Nc^c!u?^^2A#4jOO-${9B2BIqLC`9*4Z+smX^Av}-~s7^ z=g+$9NR1|Xq#@3Kbo+q>(lFBgqZ_L@kir#4quj$WTUt(cG~DkFuU3d#CIU$5fAn-TLWAv1z?MmJrR`h&=$+{wB-lJ%a>b1Dc|3=$ zfJectHl(cW9w-umo~B5Ke3Rtl7ARzc>L7^wv>UJ1LtecC!4IFrI9NE$w*xRB7SK=o ze!6xD#Vq`?WAcGpsgen)2cH!>HWVKIP&iR8O&_ieL1&G4Yp(G!)5qCD5X(i$kTm)5 z?`BaH^JHe?{ai*U0D_o6KX$DGUV~Y&=ATNnU_lA+t0m!h zEZF$4g1gj&uuW>&l)x|Od*Xyv<6Ko1)a@DNUjsffTOr_vpi8{+5AM{8v7m@Hyb|he z8Au=viwK@FUkmb;i%lvsV#VV!8D4@K~8(5`N<$E3r=rEAf}deLRD z!)|cVuD8R^GS;r2>SEbxY1!rV-<$4w0234t{vDtWZtd`NZ8myvA{iiVC?HKfWJnvt zBV61(>VRXcy?1P;cS2UMFue;13_uNB0jcz`iJ8Y{I3{Pp!OqDE$;i&hxss6K_t&7BjTtQQeS1Z!b}+!!Gi zY!ixxR41vjh^O;nwb3^0*pWo?M+G#6+=)eJYmijy5~HIOB*e+BI5dTNR@aHH0Eu1R<3A=wx2kGU z0wZxc;w4cyl6XwFt}Z5)Y~w6N`)rdd2S#iuq)G%XkmR&Ts=ySOoC430O$Dn0rWBdQ zW5_m##i?;X0ZcKYC{Z|PaY{E9owudvw)se!X{ zY4?>p>Y|m$Bo(AK12dqY-3_NxP=^=-Bq8V>f`SNaL8g-l+yX?h4JVeuOK8%`CffiQ z;t3#m#0gCbEaLaLb#=rkAiCm|bAaFPY=ce*a3t`AZsQdw7<(3DP@k*zjLJDwR( zkS>_06F&hn5}K0Uj(4KBQPRoyP|Q|a6wV0~i-X6KaZXY+oGu!c^zv%rfC$EdHiR*d zz}SKs1OY(M3C7Y8P6mG@LLMM@L3u#P2vrGw6TVAntb2t6=qYOikitB`R3qHORqLLr zH(^-giFFEEl;|iO)|8k|8FWfa6#hHNGoywqaxB&{KD`z1kODz3wlRahohdDmG0YP|f z7)X*3k^YD{2|5GhFxTOjP&#c6(_zMR;^EGnV4DEsK+fWwbhD`lLN2wF5qh&*5DH*( zHWT`q?r``8-Nzl!8*T_J3);pB4$1@MJ9PHRhT(%9C*lMs^tfir>MDS^;&2U#(Pu%Z zKE}S+*T9aZ<8VL%90jK%2|!LyQ5zB!c3!H+jF3n7Hjj#M;(){8jOk1OdmCgish~4K z4~zv}%0?`J%Oz$`d2)azKxh)2nkq6oZQw2*(-jaD&CCpCG3~QA(@{Jm$RSO~VILHL ztOowUr!RcC(>EzyBI9(^!B+pHAObL+f?df1qN0oD|EZtcj=N#&EAtX4sPBSTP!L-n*a61hs;VPXOtSu1hZh z^%n<$8_S$TIA=M@l+iNGY0GV52ffHNd2xv%=mX!0MECn`hKf zLDDpxvf+H2u5f740ec_sF|EvOtpw-+q@Oag2A)WKen5{{`9^v+k+|pAY%sUoQ68Px%_@uD}cC04Cn#cf^?c#(U?9}AJ z+4GwUxDBx)_D6!f#{gjfXM{hfnCi5M4@Ur{<9}fkt6!1*8u(|%lNT)K0AA-K8lgW4~0c?Y+quhf!UH`T&$4iE$}oIk*pEF3Y3rXQOM9xa>`D(shB8YhzMQS* zknnT}q?dmIPyiPcKxBOI;PU0?@4hjLsu2Ry-VbdPQ@0hPcQ_4w0p%o$VSj3T{IeG? z-_*5qfBEs(62!WTACT4Wi;)p7uHhFLP>eBdy*CCZ(VHOny%E;(iREoW`@1-~dJ+8` z4bScL@$`1~_j!RgHQ~PP>FIL)#@$#Q!?VWvriNJKGdK&?1BWv1n)ojC?&v1{xv_a5S7~K-?%DRRHj#COQ=_||EG-o{DY@a} zGVE6yOPh3|M71Nwk4kfMGsRo{s^~lZT0d6r14%f~jcX%>%mUN5mrMROibIfk23|IODnCx;Jn zRo!}BqU$KZ%2l<}k-!L9P=)89V_xyetUMz(7`0{efkeEB;r#Sj2jW6&mT)4q={mO)Z|x1sW{*Plv?$$+1$B?Hlg1A(FE% zYDG}KX*{iH-aBz}w5Vx_c;u06xS(R-4&|%3fBbN@Ygw_Q&Ug z8Y`ya7Y1tA`HRX;g`P;4+IOzJYCLw`MXUcdA;aobWw(EYZRhpj^+w7tM;G4-&PTD$ z6R{E}ls=4leK>L4WIEsa&@QNaWtpe&g&W(Q#z5;?brTcTFKj3hcR!QRpv4Cv%)6!f zos9P7^-X0{B%5>vk2^w(p-RtfI@&gq@X60@WW%~^toKTEy-~}_!A)bzYj5sf^;&8F zdXrQeyxZ67d&%l&FT_<77NnW?_GFp_d-_o-b^A8#15Y# zE&|ei%Py}>8yoxEax8A_JNe9_B*}T&H)}0PeAxR(n$7j-$Dikq&Frq9#K^aQZo#;Y z+qUWT9WRZ*2W8=BWK=x#;EM64*T`dw8|`<`Y-WPT+ivr8Z%f0c?W`txLHJr zN7=9Spu)`FKj=s-zamdt5P$k{<7=CwO4Gmv`=*jitNo>J5~e}Im(+5?e(B4&`R$L7 z{h;$I>_YG!9)|tuAI*x+Z(~@zvAlB^BzPK0;zu$4!pghuWo^^V^CIwzuKHe}xu)Yx z3cU@IR~Hr!Xa?FU&-Vr$j(S*t;YsAY{4-fBJVUMO$s)h+U&UJwXCeJ+}Y~DO@DqRZC^iyXU#+0({q4N0Ii^BHVdu2 z=VuAewqG9p6yd)%I~QSjEOtCep~r8%ngL4Gaz@4jI?>FSsf_a!usZNT6wrxsJY$=o z)5^wR+s-?5I?0>q`&lNTbgAN0k5abC#$em82!XMEb!>SL}M z!voRvV){cL!!lJC{LTOHN`y#$EV)zca6}hwJ9MtpuEBdfaldiAV;d?k{T%Vrx8nHm zrB9uu8^PusMTJSy=l2v6Krw^CPiedLExA+n&z;Y4zDH4Xq30XJfW!@al?3 zjFeSZt=!E7C-KF1&N|#H&n%81_022=D&D+>Ilj{HKKt9E$eh*m%AvgjS9OQLuKy3m zoc`&~8z3wFAoQN6<>nOi&_?ly&l;YI^Ny!xif7d4!7$D{VumloMKL7W7pA%M(Y`600_P(rj1P79JH1^r?LPBrn|eTH(+{OwmM6&3s2= zm(0-AZofrI1(nTHl8`NREHv8J`_)0!^9nnOEvr{9NPXL9EgL$XJ2n2@&3{h6JGty) z-YLJeU-Q`5S|^N3oXDoH7wUtd;d9Eu?EF~p@a#}6dbMp+I^mR(-|N=(p=X)qvDU)* z#mc@bvX6$p&f@Wz5&35dmDa{*@q?SIXP;kIG4rg?6%$SH z<6Mo(ysykYhndpTt6_hCJIGLujTV~xW#|CM&&sq?yM5j}UR70<3tlX9?`bnGA531I z_x6~1``FNM%x`niyV7OuOJi@e(DT*N$$bu?$FCY1E^kbg+4qhN1eW%q`)5XzpCmK} zp17(74cu5(@P}SNs0>mxm4z6d?Do?AV+L3?QFJzkd2?+2E+%DxgK zrz*{@N7i2F?Rn&CIPIxFTwoiqd#5{kz{_bW9FcmIt<8pb#MG)nC>XN3;hjWhp-X`8 z7}77y7+54FjN5$gnqPYqku;_@^-D0sxVQP3^4x)zq>P0n;))NYZ+GVQidL7Qs~fwL z%k$#GL~!(6#OY+Dz6;C3&3{O7--ztQ(oxT%g)Xb4Cg1YK z+tIGi&!e@9cP#uu^~UF-rl$P6DleCuSP`~&Y)lF*($ro(WZ&v`uNP<6$wNDnHNtS3 zVLo;I)r)bi;MIZ+G&GfhW$%?GbDLJ16l`shYjNPKXRV(LWHa6GyA3 za}wVcClxAzw8m444_xwo9eWTuSN3+~wcnYhbI0zmDyU;j>M!SE-sXMETpbY^EbRNs zg7sz0z?CP+(3{%!Mj{CxPL^LCE5=k;2e*fYXC9h4B(zA5ealA*d+MI)xn{4c-~4TC z`tl0O@ba2N7b(BZwk@~*^G|Ef&C%L{p!cEA-}HS}_@8w2yX3k1uaZak|4{Of{w{eo z`?vV0`Iw#BcJ}6T$I$P0e@!@`{Aj=3gtwKeKfk87`ua_2Jx0j0ApQ~a(d%y8Uc4m^ ziJWf8Uz-`s;g1+EbGQ4lgpNyfkSMI_e0XeqcrXMLbpNcST9$_(M=i2Kua$FwxFDw! zUKBC>LLo?TLawK>x6^JYJ}9aP}Pjo^+Rv>y2_pul^~Ul8GQUYazqOk3}k`@F#jsnV)=J z`J>JySe)Qx-4;42m)2-(k$1A`K}Tw_H|2e0#zIgpfAF#!>Y3llRe|yMW{he&mU*(Q z`<#Zelz4u8d=P%BDQ*=H$hZ8$3bQtJ_DjYQLFi4`ep;6MWB(h@r#ME0!VBtXyb+0+ z16(v`N~%L|8NhxR|=F3V(MLTTExPga&bE#6_$ zu5(q_QCsR)$0Li$norV=mBi~|=I>TAX?FHpUv^Oobgl(lfCAp33wOPVlVlWr*|Uug zdebR#@HLS&AjgKt{m!!>XIeyzmiMdauOWx1_*MThzO z=@W$!dxhTh%a;jPXne6m$ob^5zS|Cga_xX0cy$E7rD(9QD4ymq%_6whS6UQ}&Uz~y zADTZApAdl_xrM%7n{b_AaQ+9eF!trf%7sN?h5cs(&%|)Nx}AICkq^3*$AByJ&Pl{S z#EZeQFx-Jm*{d{nX<6EjA)2GCR)_2r_qiJFRauTMm6F?M7t??5=>6_%E0Y73=7*dE zqk?CT|8ksfuIljZ+gzVz&wko^>17DZPQc}1zbi^^8x2+?e3(xrDJELCElIvSi)~m$ zIh4b3vZubHc~tR+9CmtfWTkqePBb?x1$Dwn+*ofYcHi<|6IQcXo43H-Kj(rdk()8R z!{e`T)T3iNOd@@+pWtM(A@4XMCA#qIm9_tF-CLwp${eO4W?rzZ%H)`FTZlmz;(*gf zmeNHWbJgWn@);VP1{klt8!3y8|FRmw&B(_8O~CTfx3k~R{fhmvmeW6lY+P!o&wm{K zIVg6Jq32w7$V(1Qmu&Ir$DbXZUs5S^Z|gKSHfz6F;6MrCB9t*Q{3ln`bI#s3sx#Wn zW)bTUlaSmS$BL=nU5YVbmt#A{7A&{Fe^*}2;S(_luG86_hI`U(=eAGoGyP{lM#Nn_ z3^`(PvJG~;VUPNqNi&Y_9ygb;{_VVcxRKU&mxE(xvyb?5wS7pZ5vb++j)@%Ru~*_? zP5!`e*A9I`-ZIlHqxOqFhcs*wjsk?zWJw7TU7GKDIw(1KV}`uA9?$OIYCDUPl4OL6 zuZmCBrWy~3*i@w1-8f}_WX9|>k!Dd`qE-6QnahN)FzsUXe(@gQ;*H;FR}D^nb)rqH zLG^4FKPf?=>rlbSCSRq)Sxb(lcG{GVSai+T7>7NY;a!1VR^Kdj*&7`B1cYObPBFdD!wB^e;gA^6ubhJujn@SI-% zgZbFL*tSBKp&uE65!rq$Nx|YLPx-dpPVXMen#y;sdmen}eey)8dsX$1_yp~- #include #include #include @@ -33,6 +34,8 @@ #include "project_guideline/audio/assets/legacy_v4_2_warning_embed.h" #include "project_guideline/audio/assets/legacy_warning_embed.h" #include "project_guideline/audio/assets/obstacle_embed.h" +#include "project_guideline/audio/assets/turn_embed.h" +#include "project_guideline/audio/assets/turn_fast_embed.h" #include "project_guideline/audio/assets/warning_pitch_shift_2x_embed.h" #include "project_guideline/audio/sound_player.h" #include "project_guideline/logging/guideline_logger.h" @@ -63,6 +66,15 @@ static PanningStrategy LinearThresholdPan(float threshold) { }; } +static PanningStrategy LegacyThresholdPan(float threshold, bool reversed) { + return [threshold, reversed](float input, float& left, float& right) { + float l = util::ClampedLerp(input, threshold, 1, 0, 1); + float r = util::ClampedLerp(input, -1, -threshold, 1, 0); + left = reversed ? r : l; + right = reversed ? l : r; + }; +} + float ApplyShaping(float x, float sensitivity_curve) { if (sensitivity_curve == 0) { return x; @@ -151,12 +163,26 @@ absl::Status LegacySoundPack::Initialize() { CHECK(steering_sound_toc); CHECK(warning_sound_toc); + const util::EmbeddedFileToc* curve_sound_toc = nullptr; + if (options_.use_fast_curve_sound()) { + curve_sound_toc = turn_fast_embed_create(); + } else { + curve_sound_toc = turn_embed_create(); + } + curve_panner_ = LegacyThresholdPan(options_.turn_sensitivity(), false); + curve_rate_strategy_ = [](float input) { return 1; }; + + CHECK(curve_sound_toc); + GL_ASSIGN_OR_RETURN(steering_sound_, sound_player_->LoadStereoSound(steering_sound_toc)); sound_player_->SetLoop(steering_sound_, true); GL_ASSIGN_OR_RETURN(warning_sound_, sound_player_->LoadStereoSound(warning_sound_toc)); sound_player_->SetLoop(warning_sound_, true); + GL_ASSIGN_OR_RETURN(curve_sound_, + sound_player_->LoadStereoSound(curve_sound_toc)); + sound_player_->SetLoop(curve_sound_, true); GL_ASSIGN_OR_RETURN(obstacle_sound_, sound_player_->LoadStereoSound(obstacle_embed_create())); sound_player_->SetLoop(obstacle_sound_, true); @@ -209,6 +235,21 @@ void LegacySoundPack::OnControlSignal( sound_player_->SetPlaybackRate(warning_sound_, warning_rate_strategy_(warning_position)); sound_player_->Play(warning_sound_); + + float curve_panning_position = 0; + curve_panning_position = util::ClampedLerp( + signal.rotation_movement_ahead_degrees, -options_.max_rotation_degrees(), + options_.max_rotation_degrees(), -1, 1); + float curve_left_volume = 0; + float curve_right_volume = 0; + curve_panning_position = + ApplyShaping(curve_panning_position, options_.sensitivity_curvature()); + curve_panner_(curve_panning_position, curve_left_volume, curve_right_volume); + sound_player_->SetStereoVolume(curve_sound_, curve_left_volume, + curve_right_volume); + sound_player_->SetPlaybackRate(curve_sound_, + curve_rate_strategy_(curve_panning_position)); + sound_player_->Play(curve_sound_); } std::optional diff --git a/project_guideline/audio/legacy_sound_pack.h b/project_guideline/audio/legacy_sound_pack.h index f09557d..baef15f 100644 --- a/project_guideline/audio/legacy_sound_pack.h +++ b/project_guideline/audio/legacy_sound_pack.h @@ -63,11 +63,14 @@ class LegacySoundPack : public SoundPack { PanningStrategy steering_panner_ = nullptr; PanningStrategy warning_panner_ = nullptr; + PanningStrategy curve_panner_ = nullptr; RateStrategy steering_rate_strategy_ = nullptr; RateStrategy warning_rate_strategy_ = nullptr; + RateStrategy curve_rate_strategy_ = nullptr; SoundId steering_sound_; SoundId warning_sound_; + SoundId curve_sound_; SoundId obstacle_sound_; }; diff --git a/project_guideline/environment/BUILD b/project_guideline/environment/BUILD index edf0dab..208e406 100644 --- a/project_guideline/environment/BUILD +++ b/project_guideline/environment/BUILD @@ -56,15 +56,14 @@ cc_library( "//visibility:public", ], deps = [ + ":control_signal", + ":environment", + ":obstacle", + ":path_planning", "//project_guideline/camera:camera_model", "//project_guideline/depth:depth_align_ransac", "//project_guideline/depth:point_cloud_util", - "//project_guideline/environment", - "//project_guideline/environment:control_signal", - "//project_guideline/environment:obstacle", - "//project_guideline/environment:path_planning", "//project_guideline/logging:guideline_logger", - "//project_guideline/motion:motion_tracker", "//project_guideline/motion:tracking_feature", "//project_guideline/util:geometry", "//project_guideline/util:hit_test_util", @@ -73,8 +72,6 @@ cc_library( "//third_party/protobuf:time_util", "@com_google_absl//absl/log", "@com_google_absl//absl/log:check", - "@com_google_absl//absl/memory", - "@com_google_absl//absl/status", "@com_google_absl//absl/status:statusor", "@com_google_absl//absl/time", "@eigen_archive//:eigen3", @@ -277,8 +274,8 @@ cc_library( "//visibility:public", ], deps = [ + ":control_signal", ":obstacle", - "//project_guideline/environment:control_signal", "//project_guideline/proto:guideline_engine_config_cc_proto", "//project_guideline/util:geometry", "//project_guideline/util:windowed_value_latch", diff --git a/project_guideline/environment/control_signal.h b/project_guideline/environment/control_signal.h index 60a85e1..45338a2 100644 --- a/project_guideline/environment/control_signal.h +++ b/project_guideline/environment/control_signal.h @@ -30,6 +30,7 @@ namespace guideline::environment { // movement negative is clockwise and positive is counterclockwise. struct ControlSignal { bool stop = false; + int stop_reason = 0; // Lateral distance tangential from the line. float lateral_movement_meters = 0.; @@ -38,6 +39,10 @@ struct ControlSignal { // upcoming line. float rotation_movement_degrees = 0.; + // Desired user rotational movement based on user position, direction of + // upcoming line and ahead distance omitted. + float rotation_movement_ahead_degrees = 0.; + // World coordinates of an upcoming turn point, if any. std::optional turn_point = std::nullopt; diff --git a/project_guideline/environment/guidance_system.cc b/project_guideline/environment/guidance_system.cc index ca589fe..09fc83b 100644 --- a/project_guideline/environment/guidance_system.cc +++ b/project_guideline/environment/guidance_system.cc @@ -139,7 +139,7 @@ GuidanceSystem::GuidanceSystem(const GuidanceSystemOptions& options, control_system_(std::move(control_system)) {} absl::Status GuidanceSystem::Stop() { - ResetAndSendStopSignal(); + ResetAndSendStopSignal(1); return absl::OkStatus(); } @@ -260,7 +260,7 @@ void GuidanceSystem::OnTrackingStateChanged(const bool is_tracking) { // OnCameraPose will start being invoked again with from a new reference // point. if (!is_tracking) { - ResetAndSendStopSignal(); + ResetAndSendStopSignal(2); } } @@ -372,7 +372,7 @@ void GuidanceSystem::OnDetection( first_empty_keypoints_timestamp_us_) >= eager_stop_threshold) { first_empty_keypoints_timestamp_us_ = 0; - ResetAndSendStopSignal(); + ResetAndSendStopSignal(3); } } } @@ -445,7 +445,7 @@ void GuidanceSystem::ProcessDepthMap(int64_t timestamp_us, LogPointCloud(timestamp_us, *environment_, *logger_); } -void GuidanceSystem::ResetAndSendStopSignal() { +void GuidanceSystem::ResetAndSendStopSignal(int reason) { { absl::MutexLock lock(&pending_camera_poses_mutex_); pending_detection_poses_.clear(); @@ -457,6 +457,7 @@ void GuidanceSystem::ResetAndSendStopSignal() { // Generate a stop signal and notify callbacks. ControlSignal stop_signal; stop_signal.stop = true; + stop_signal.stop_reason = reason; std::vector callbacks; { diff --git a/project_guideline/environment/guidance_system.h b/project_guideline/environment/guidance_system.h index 32c10a7..857d0df 100644 --- a/project_guideline/environment/guidance_system.h +++ b/project_guideline/environment/guidance_system.h @@ -103,7 +103,7 @@ class GuidanceSystem { const util::DepthImage& depth_map, const PendingFeatures& features); - void ResetAndSendStopSignal(); + void ResetAndSendStopSignal(int reason); std::shared_ptr GetCameraModel() ABSL_LOCKS_EXCLUDED(camera_model_mutex_); diff --git a/project_guideline/environment/path_planning.cc b/project_guideline/environment/path_planning.cc index 35bbf6f..101bd58 100644 --- a/project_guideline/environment/path_planning.cc +++ b/project_guideline/environment/path_planning.cc @@ -280,6 +280,21 @@ RotationMovementOutput SimpleControlSystem::ComputeRotationalMovement( return RotationMovementOutput(rotation_movement_degrees, all_rotations, num_guideline_points_per_segment); } +RotationMovementOutput +SimpleControlSystem::ComputeRotationalMovementWithLookAhead( + const Vector3d& human_direction, + absl::Span guideline_points, + int closest_guideline_point_indx, float rotational_movement_ahead_meter, + const Axis3 vertical_axis) { + auto indices_skip = + static_cast(rotational_movement_ahead_meter * + options_.num_guideline_points_per_meter()); + auto closest_guideline_point_indx_offset = + closest_guideline_point_indx + indices_skip; + return ComputeRotationalMovement(human_direction, guideline_points, + closest_guideline_point_indx_offset, + vertical_axis); +} TurnPointOutput SimpleControlSystem::FindTurnPoint( const Vector3d& human_position, absl::Span guideline_points, @@ -314,14 +329,14 @@ TurnPointOutput SimpleControlSystem::FindTurnPoint( return TurnPointOutput(turn_point, turn_angle, turn_point_distance_meters); } -bool SimpleControlSystem::IsStopCondition() { +int SimpleControlSystem::IsStopCondition() { { absl::MutexLock lock(&control_signal_history_lock_); auto current_control_signal = control_signal_history_.back(); // Out of guideline scenario. if (std::isnan(current_control_signal.lateral_movement_meters)) { - return true; + return 4; } // Deviation from the guideline scenario. @@ -351,19 +366,21 @@ bool SimpleControlSystem::IsStopCondition() { } } if (!has_in_range_movement) { - return true; + return 5; } } - return false; + return 0; } } // Post processes and returns final control signal to be communicated to the // human. const ControlSignal SimpleControlSystem::PostProcessControlSignals() { - if (IsStopCondition()) { + int stop_condition = IsStopCondition(); + if (stop_condition != 0) { ControlSignal stop_signal; stop_signal.stop = true; + stop_signal.stop_reason = stop_condition; obstacle_latch_.Reset(); return stop_signal; } @@ -447,6 +464,10 @@ ControlSignal SimpleControlSystem::GenerateControlSignal( auto rotational_movement = ComputeRotationalMovement( human_direction, guideline_points, lateral_movement.closest_guideline_point_indx, vertical_axis); + auto rotational_movement_ahead = ComputeRotationalMovementWithLookAhead( + human_direction, guideline_points, + lateral_movement.closest_guideline_point_indx, + options_.rotational_movement_ahead_meters(), vertical_axis); auto turn_point = FindTurnPoint( human_position, guideline_points, lateral_movement.closest_guideline_point_indx, @@ -458,6 +479,8 @@ ControlSignal SimpleControlSystem::GenerateControlSignal( lateral_movement.lateral_movement_meters; current_control_signal.rotation_movement_degrees = rotational_movement.rotation_movement_degrees; + current_control_signal.rotation_movement_ahead_degrees = + rotational_movement_ahead.rotation_movement_degrees; current_control_signal.turn_point = turn_point.turn_point; current_control_signal.turn_angle_degrees = turn_point.turn_angle_degrees; current_control_signal.turn_point_distance_meters = diff --git a/project_guideline/environment/path_planning.h b/project_guideline/environment/path_planning.h index 59bca30..134e340 100644 --- a/project_guideline/environment/path_planning.h +++ b/project_guideline/environment/path_planning.h @@ -137,6 +137,11 @@ class SimpleControlSystem : public ControlSystem { const ::Eigen::Vector3d& human_direction, absl::Span guideline_points, int closest_guideline_point_indx, util::Axis3 vertical_axis); + RotationMovementOutput ComputeRotationalMovementWithLookAhead( + const ::Eigen::Vector3d& human_direction, + absl::Span guideline_points, + int closest_guideline_point_indx, float rotational_movement_ahead_meter, + util::Axis3 vertical_axis); TurnPointOutput FindTurnPoint( const ::Eigen::Vector3d& human_position, absl::Span guideline_points, @@ -150,7 +155,7 @@ class SimpleControlSystem : public ControlSystem { obstacle_latch_(/*value_threshold=*/0.5f, options.obstacle_smoothing_window_frames(), options.obstacle_smoothing_min_interval_frames()) {} - bool IsStopCondition(); + int IsStopCondition(); const ControlSignal PostProcessControlSignals(); SimpleControlSystemOptions options_; absl::Mutex control_signal_history_lock_; diff --git a/project_guideline/logging/BUILD b/project_guideline/logging/BUILD index 2020df0..ac298c7 100644 --- a/project_guideline/logging/BUILD +++ b/project_guideline/logging/BUILD @@ -80,3 +80,36 @@ cc_library( "@com_google_absl//absl/status", ], ) + +cc_library( + name = "debug_signal", + hdrs = ["debug_signal.h"], + visibility = ["//visibility:public"], +) + +cc_library( + name = "debug_signal_provider", + srcs = ["debug_signal_provider.cc"], + hdrs = ["debug_signal_provider.h"], + visibility = [ + "//visibility:public", + ], + deps = [ + ":debug_signal", + "//project_guideline/camera:camera_model", + "//project_guideline/environment:control_signal", + "//project_guideline/motion:tracking_feature", + "//project_guideline/proto:guideline_engine_config_cc_proto", + "//project_guideline/util:image", + "//project_guideline/util:transformation", + "@com_google_absl//absl/functional:bind_front", + "@com_google_absl//absl/log", + "@com_google_absl//absl/log:check", + "@com_google_absl//absl/memory", + "@com_google_absl//absl/status", + "@com_google_absl//absl/status:statusor", + "@com_google_absl//absl/synchronization", + "@com_google_absl//absl/time", + "@eigen_archive//:eigen3", + ], +) diff --git a/project_guideline/logging/debug_signal.h b/project_guideline/logging/debug_signal.h new file mode 100644 index 0000000..3bcdef7 --- /dev/null +++ b/project_guideline/logging/debug_signal.h @@ -0,0 +1,31 @@ +// Copyright 2023 Google LLC +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// https://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef PROJECT_GUIDELINE_LOGGING_DEBUG_SIGNAL_H_ +#define PROJECT_GUIDELINE_LOGGING_DEBUG_SIGNAL_H_ + +#include +namespace guideline::logging { +struct DebugSignal { + bool tracking; + int64_t camera_pose_fps; + int64_t camera_pose_lagging_frame; + int64_t detection_fps; + int64_t detection_lagging_frame; + int64_t tracking_features_fps; + int64_t tracking_features_lagging_frame; +}; +} // namespace guideline::logging + +#endif // PROJECT_GUIDELINE_LOGGING_DEBUG_SIGNAL_H_ diff --git a/project_guideline/logging/debug_signal_provider.cc b/project_guideline/logging/debug_signal_provider.cc new file mode 100644 index 0000000..2e8ae03 --- /dev/null +++ b/project_guideline/logging/debug_signal_provider.cc @@ -0,0 +1,198 @@ +// Copyright 2023 Google LLC +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// https://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "project_guideline/logging/debug_signal_provider.h" + + +#include +#include +#include +#include + +#include "absl/log/check.h" +#include "absl/log/log.h" +#include "absl/memory/memory.h" +#include "absl/status/status.h" +#include "absl/synchronization/mutex.h" +#include "absl/time/clock.h" +#include "absl/time/time.h" +#include "Eigen/Core" +#include "project_guideline/camera/camera_model.h" +#include "project_guideline/environment/control_signal.h" +#include "project_guideline/logging/debug_signal.h" +#include "project_guideline/motion/tracking_feature.h" +#include "project_guideline/proto/guideline_engine_config.pb.h" +#include "project_guideline/util/image.h" +#include "project_guideline/util/transformation.h" + +namespace guideline::logging { + +absl::StatusOr> +DebugSignalProvider::Create(const DebugSignalProviderOptions& options) { + return absl::WrapUnique(new DebugSignalProvider(options)); +} + +DebugSignalProvider::DebugSignalProvider( + const DebugSignalProviderOptions& options) + : options_(options) {} + +DebugSignalProvider::~DebugSignalProvider() { + // if (shouter_thread_) { + // auto status = Stop(); + // if (!status.ok()) { + // LOG(ERROR) << "Shouter thread failed to stop: " << status; + // } + // } +} + +absl::Status DebugSignalProvider::Start() { + // CHECK(shouter_thread_ == nullptr) << "Shouter thread is already started."; + // if (options_.enable_data_shouting()) { + // running_ = true; + // shouter_thread_ = std::make_unique( + // absl::bind_front(&DataGuidelineShouter::RunShouter, this)); + // } + return absl::OkStatus(); +} + +absl::Status DebugSignalProvider::Stop() { + // CHECK(shouter_thread_) << "Shouter thread is already stopped."; + // running_ = false; + // shouter_thread_->join(); + // shouter_thread_ = nullptr; + return absl::OkStatus(); +} + +void DebugSignalProvider::RunDebugSignalProvider() { + while (running_) { + auto timestamp = absl::Now(); + if ((timestamp - last_debug_timestamp) >= + absl::Milliseconds(DEBUG_PERIOD_MS)) { + NotifyDebugSignalCallbacks(); + last_debug_timestamp = timestamp; + } + } +} + +void DebugSignalProvider::OnCameraPose( + int64_t timestamp_us, const util::Transformation& world_t_camera, + std::shared_ptr camera_model) { + { + absl::MutexLock lock(&camera_pose_mutex_); + if (timestamp_us - last_camera_pose_timestamp_us_ < pow(10, 6)) { + camera_pose_update_count_++; + } else { + current_camera_pose_fps_ = camera_pose_update_count_; + last_camera_pose_timestamp_us_ = timestamp_us; + camera_pose_update_count_ = 0; + } + camera_pose_lag_count_ = 0; + } +} + +void DebugSignalProvider::OnDetection( + int64_t timestamp_us, const std::vector& keypoints, + std::shared_ptr guideline_mask, + std::shared_ptr depth_map) { + { + absl::MutexLock lock(&detection_mutex_); + if (timestamp_us - last_detection_timestamp_us_ < pow(10, 6)) { + detection_update_count_++; + } else { + current_detection_fps_ = detection_update_count_; + last_detection_timestamp_us_ = timestamp_us; + detection_update_count_ = 0; + } + detection_lag_count_ = 0; + } + NotifyDebugSignalCallbacks(); +} + +void DebugSignalProvider::OnControlSignal( + const environment::ControlSignal& control_signal) {} + +void DebugSignalProvider::OnGuideline( + const std::vector& guideline) { + { + absl::MutexLock lock(&guideline_mutex_); + guideline_count_ = guideline.size(); + } +} + +void DebugSignalProvider::OnTrackingStateChanged(bool is_tracking) { + { + absl::MutexLock lock(&tracking_state_changed_mutex_); + tracking_ = is_tracking; + } +} + +void DebugSignalProvider::OnTrackingFeatures( + int64_t timestamp_us, + const std::vector& features) { + { + absl::MutexLock lock(&tracking_features_mutex_); + if (timestamp_us - last_tracking_features_timestamp_us_ < 1000000) { + tracking_features_update_count_++; + } else { + current_tracking_features_fps_ = tracking_features_update_count_; + last_tracking_features_timestamp_us_ = timestamp_us; + tracking_features_update_count_ = 0; + } + tracking_features_lag_count_ = 0; + } +} + +void DebugSignalProvider::AddDebugSignalCallback( + const DebugSignalCallback& debug_signal_callback) { + { + absl::MutexLock lock(&debug_signal_callbacks_mutex_); + debug_signal_callbacks_.push_back(debug_signal_callback); + } +} + +void DebugSignalProvider::NotifyDebugSignalCallbacks() { + if (!options_.enable_debug_signal_provider()) { + return; + } + DebugSignal debug_signal; + { + absl::MutexLock lock(&camera_pose_mutex_); + debug_signal.camera_pose_fps = current_camera_pose_fps_; + debug_signal.camera_pose_lagging_frame = camera_pose_lag_count_; + camera_pose_lag_count_++; + } + { + absl::MutexLock lock(&detection_mutex_); + debug_signal.detection_fps = current_detection_fps_; + debug_signal.detection_lagging_frame = detection_lag_count_; + detection_lag_count_++; + } + { + absl::MutexLock lock(&tracking_features_mutex_); + debug_signal.tracking_features_fps = current_tracking_features_fps_; + debug_signal.tracking_features_lagging_frame = tracking_features_lag_count_; + tracking_features_lag_count_++; + } + { + absl::MutexLock lock(&tracking_state_changed_mutex_); + debug_signal.tracking = tracking_; + } + { + absl::MutexLock lock(&debug_signal_callbacks_mutex_); + for (const auto& callback : debug_signal_callbacks_) { + callback(debug_signal); + } + } +} +} // namespace guideline::logging diff --git a/project_guideline/logging/debug_signal_provider.h b/project_guideline/logging/debug_signal_provider.h new file mode 100644 index 0000000..4d38d97 --- /dev/null +++ b/project_guideline/logging/debug_signal_provider.h @@ -0,0 +1,119 @@ +// Copyright 2023 Google LLC +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// https://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef PROJECT_GUIDELINE_LOGGING_DEBUG_SIGNAL_PROVIDER_H_ +#define PROJECT_GUIDELINE_LOGGING_DEBUG_SIGNAL_PROVIDER_H_ + +#include +#include +#include +#include +#include + +#include "absl/status/status.h" +#include "absl/status/statusor.h" +#include "absl/synchronization/mutex.h" +#include "absl/time/clock.h" +#include "absl/time/time.h" +#include "Eigen/Core" +#include "project_guideline/camera/camera_model.h" +#include "project_guideline/environment/control_signal.h" +#include "project_guideline/logging/debug_signal.h" +#include "project_guideline/motion/tracking_feature.h" +#include "project_guideline/proto/guideline_engine_config.pb.h" +#include "project_guideline/util/image.h" +#include "project_guideline/util/transformation.h" + +namespace guideline::logging { + +class DebugSignalProvider { + public: + using DebugSignalCallback = + std::function; + static absl::StatusOr> Create( + const DebugSignalProviderOptions& options); + + DebugSignalProvider& operator=(const DebugSignalProvider&) = delete; + ~DebugSignalProvider(); + + void OnCameraPose(int64_t timestamp_us, + const util::Transformation& world_t_camera, + std::shared_ptr camera_model); + void OnDetection(int64_t timestamp_us, + const std::vector& keypoints, + std::shared_ptr guideline_mask, + std::shared_ptr depth_map); + + void OnControlSignal(const environment::ControlSignal& control_signal); + void OnGuideline(const std::vector& guideline); + void OnTrackingStateChanged(bool is_tracking); + void OnTrackingFeatures(int64_t timestamp_us, + const std::vector& features); + + void AddDebugSignalCallback(const DebugSignalCallback& debug_signal_callback) + ABSL_LOCKS_EXCLUDED(debug_signal_callbacks_mutex_); + + absl::Status Start(); + absl::Status Stop(); + + private: + explicit DebugSignalProvider(const DebugSignalProviderOptions& options); + void NotifyDebugSignalCallbacks() + ABSL_LOCKS_EXCLUDED(debug_signal_callbacks_mutex_); + void RunDebugSignalProvider(); + + private: + const DebugSignalProviderOptions options_; + absl::Mutex camera_pose_mutex_; + int64_t last_camera_pose_timestamp_us_ ABSL_GUARDED_BY(camera_pose_mutex_) = + 0; + int64_t camera_pose_update_count_ ABSL_GUARDED_BY(camera_pose_mutex_) = 0; + int64_t camera_pose_lag_count_ ABSL_GUARDED_BY(camera_pose_mutex_) = 0; + int64_t current_camera_pose_fps_ ABSL_GUARDED_BY(camera_pose_mutex_) = 0; + + absl::Mutex detection_mutex_; + int64_t last_detection_timestamp_us_ ABSL_GUARDED_BY(detection_mutex_) = 0; + int64_t detection_update_count_ ABSL_GUARDED_BY(detection_mutex_) = 0; + int64_t detection_lag_count_ ABSL_GUARDED_BY(detection_mutex_) = 0; + int64_t current_detection_fps_ ABSL_GUARDED_BY(detection_mutex_) = 0; + absl::Mutex tracking_features_mutex_; + int64_t last_tracking_features_timestamp_us_ + ABSL_GUARDED_BY(tracking_features_mutex_) = 0; + int64_t tracking_features_update_count_ + ABSL_GUARDED_BY(tracking_features_mutex_) = 0; + int64_t tracking_features_lag_count_ + ABSL_GUARDED_BY(tracking_features_mutex_) = 0; + int64_t current_tracking_features_fps_ + ABSL_GUARDED_BY(tracking_features_mutex_) = 0; + + absl::Mutex tracking_state_changed_mutex_; + bool tracking_ ABSL_GUARDED_BY(tracking_state_changed_mutex_) = false; + + absl::Mutex guideline_mutex_; + int64_t guideline_count_ ABSL_GUARDED_BY(guideline_mutex_) = 0; + + absl::Mutex debug_signal_callbacks_mutex_; + std::vector debug_signal_callbacks_ + ABSL_GUARDED_BY(debug_signal_callbacks_mutex_); + + std::atomic running_ = false; + std::unique_ptr debug_thread_; + + absl::Time last_debug_timestamp = absl::Now(); + + const int64_t DEBUG_PERIOD_MS = 500; +}; +} // namespace guideline::logging + +#endif // PROJECT_GUIDELINE_LOGGING_DEBUG_SIGNAL_PROVIDER_H_ diff --git a/project_guideline/proto/guideline_engine_config.proto b/project_guideline/proto/guideline_engine_config.proto index bd796cb..03f784e 100644 --- a/project_guideline/proto/guideline_engine_config.proto +++ b/project_guideline/proto/guideline_engine_config.proto @@ -25,7 +25,7 @@ option java_package = "com.google.research.guideline.proto"; option optimize_for = LITE_RUNTIME; // Configuration for the Guideline Engine. -// Next available index: 9 +// Next available index: 11 message GuidelineEngineConfig { message FileDescriptor { optional int32 fd = 1; @@ -59,6 +59,7 @@ message GuidelineEngineConfig { optional PointCloudOptions point_cloud_options = 7; optional AudioSystemOptions audio_system_options = 8; optional VisualizationOptions visualization_options = 9; + optional DebugSignalProviderOptions debug_signal_provider_options = 10; } message KeypointExtractorOptions { @@ -98,6 +99,9 @@ message LegacySoundPackOptions { optional float max_rotation_degrees = 2 [default = 45]; optional float sensitivity_curvature = 3 [default = 0.4]; optional float warning_threshold_meters = 4 [default = 1.5]; + optional bool enable_curve_sound = 5 [default = false]; + optional bool use_fast_curve_sound = 6 [default = false]; + optional float turn_sensitivity = 7 [default = 0.4]; } message PointCloudOptions { @@ -205,6 +209,7 @@ message SimpleControlSystemOptions { [default = 0]; optional float rotational_movement_abs_max_threshold_degrees = 10 [default = 90]; + optional float rotational_movement_ahead_meters = 16 [default = 3]; optional int32 max_history_to_keep = 11 [default = 300]; optional float min_turn_point_distance_meters = 12 [default = 2]; optional float track_width_meters = 13 [default = 3]; @@ -240,3 +245,7 @@ message VisualizationOptions { // Enable ML debug overlay showing segmentation mask and depth image results. optional bool enable_ml_overlay = 1; } + +message DebugSignalProviderOptions { + optional bool enable_debug_signal_provider = 1; +} diff --git a/project_guideline/visualization/ml_output_renderer.cc b/project_guideline/visualization/ml_output_renderer.cc index 1f52535..b44a4c5 100644 --- a/project_guideline/visualization/ml_output_renderer.cc +++ b/project_guideline/visualization/ml_output_renderer.cc @@ -14,8 +14,10 @@ #include "project_guideline/visualization/ml_output_renderer.h" +#include #include #include +#include #include #include @@ -43,9 +45,15 @@ const GLchar* const kFragmentShader = SHADER( uniform sampler2D maskTexture; uniform sampler2D depthTexture; uniform sampler2D depthColormap; + uniform vec2 uKeypoints[100]; + uniform int uKeypointCount; in vec2 textureCoords; out vec4 diffuseColor; + const float KEYPOINT_RADIUS = 0.005; + const vec4 KEYPOINT_PRIMARY_COLOR = vec4(1, 0, 0, 1.0); + const vec4 KEYPOINT_SECONDARY_COLOR = vec4(0, 0, 1, 1.0); + const float MIN_DEPTH = 0.01; const float MAX_DEPTH = 5.0; @@ -55,6 +63,15 @@ const GLchar* const kFragmentShader = SHADER( return result; } + vec4 keypoints(vec2 position) { + for (int i = 0; i < uKeypointCount; i++) { + if (distance(position, uKeypoints[i]) < KEYPOINT_RADIUS) { + return mix(KEYPOINT_PRIMARY_COLOR, KEYPOINT_SECONDARY_COLOR, float(i) / float(uKeypointCount)); + } + } + return vec4(0, 0, 0, 0); + } + void main() { // Render the line segmentation mask as green, with alpha based on // the confidence value. @@ -70,6 +87,7 @@ const GLchar* const kFragmentShader = SHADER( depthColor.w = depthTexel > MIN_DEPTH ? 0.6 : 0.0; diffuseColor = blend(depthColor, maskColor); + diffuseColor = blend(diffuseColor, keypoints(textureCoords)); }); const GLchar* const kVertexShader = SHADER( @@ -245,6 +263,9 @@ absl::Status MlOutputRenderer::OnGlInit() { uniform_depth_texture_ = glGetUniformLocation(program_, "depthTexture"); uniform_depth_colormap_ = glGetUniformLocation(program_, "depthColormap"); + uniform_keypoint_count_ = glGetUniformLocation(program_, "uKeypointCount"); + uniform_keypoints_ = glGetUniformLocation(program_, "uKeypoints"); + CheckGlError("init shaders"); return absl::OkStatus(); @@ -253,8 +274,15 @@ absl::Status MlOutputRenderer::OnGlInit() { void MlOutputRenderer::SetViewport(int width, int height) { // The camera preview uses 16:9 aspect ratio, while the CPU image used for // processing in the pipeline is the full 4:3 sensor image. - constexpr float kPreviewAspectRatio = 16.0f / 9.0f; - constexpr float kCpuImageAspectRatio = 4.0f / 3.0f; + float kPreviewAspectRatio; + float kCpuImageAspectRatio; + if (width < height) { + kPreviewAspectRatio = 9.0f / 16.0f; + kCpuImageAspectRatio = 3.0f / 4.0f; + } else { + kPreviewAspectRatio = 16.0f / 9.0f; + kCpuImageAspectRatio = 4.0f / 3.0f; + } // The actual width of the preview image. float preview_width = height * kPreviewAspectRatio; @@ -282,9 +310,11 @@ void MlOutputRenderer::SetViewport(int width, int height) { } void MlOutputRenderer::OnSegmentationMask( - std::shared_ptr segmentation_mask) { + std::shared_ptr segmentation_mask, + const std::vector& keypoints) { absl::MutexLock lock(&mutex_); segmentation_mask_ = segmentation_mask; + keypoints_ = keypoints; } void MlOutputRenderer::OnDepthMap( @@ -321,6 +351,29 @@ void MlOutputRenderer::Render() { } glUseProgram(program_); + + { + absl::MutexLock lock(&mutex_); + if (!keypoints_.empty()) { + const size_t kMaxKeypointCount = 100; + if (keypoints_.size() > kMaxKeypointCount) { + keypoints_.resize(kMaxKeypointCount); + } + glUniform1i(uniform_keypoint_count_, keypoints_.size()); + CheckGlError("update keypoint count"); + std::vector gl_points; + for (const auto& point : keypoints_) { + gl_points.push_back(point.x()); + gl_points.push_back(point.y()); + } + glUniform2fv(uniform_keypoints_, gl_points.size(), gl_points.data()); + CheckGlError("update keypoints data"); + } else { + glUniform1i(uniform_keypoint_count_, 0); + CheckGlError("no keypoints"); + } + CheckGlError("update keypoints"); + } CheckGlError("use program"); glUniform1i(uniform_mask_texture_, kMaskTextureUnit); diff --git a/project_guideline/visualization/ml_output_renderer.h b/project_guideline/visualization/ml_output_renderer.h index 74a395f..29c44a4 100644 --- a/project_guideline/visualization/ml_output_renderer.h +++ b/project_guideline/visualization/ml_output_renderer.h @@ -17,6 +17,7 @@ #include #include +#include // clang-format off // gl3 must be before gl2ext @@ -37,7 +38,8 @@ class MlOutputRenderer { MlOutputRenderer(int mask_rotation_degrees); absl::Status OnGlInit(); - void OnSegmentationMask(std::shared_ptr mask); + void OnSegmentationMask(std::shared_ptr mask, + const std::vector& keypoints); void OnDepthMap(std::shared_ptr depth); void SetViewport(int width, int height); @@ -50,6 +52,8 @@ class MlOutputRenderer { Eigen::Matrix4f transform_matrix_ ABSL_GUARDED_BY(mutex_); std::shared_ptr segmentation_mask_ ABSL_GUARDED_BY(mutex_) = nullptr; + std::vector keypoints_ ABSL_GUARDED_BY(mutex_); + std::shared_ptr depth_map_ ABSL_GUARDED_BY(mutex_) = nullptr; @@ -67,6 +71,9 @@ class MlOutputRenderer { GLint uniform_mask_texture_; GLint uniform_depth_texture_; GLint uniform_depth_colormap_; + + GLuint uniform_keypoint_count_; + GLuint uniform_keypoints_; }; } // namespace guideline::visualization