From c821b2858338a77e953c5185fbf723c7085a8493 Mon Sep 17 00:00:00 2001 From: FrogAi <91348155+FrogAi@users.noreply.github.com> Date: Tue, 27 Feb 2024 16:34:47 -0700 Subject: [PATCH] Customizable personality profiles Added toggles to customize the personality profiles. --- common/params.cc | 7 ++ .../lib/longitudinal_mpc_lib/long_mpc.py | 58 +++++++++++------ .../controls/lib/longitudinal_planner.py | 4 +- .../assets/toggle_icons/icon_custom.png | Bin 0 -> 13559 bytes .../frogpilot/functions/frogpilot_planner.py | 8 +++ selfdrive/frogpilot/ui/control_settings.cc | 60 ++++++++++++++++++ selfdrive/frogpilot/ui/control_settings.h | 3 + 7 files changed, 120 insertions(+), 20 deletions(-) create mode 100644 selfdrive/frogpilot/assets/toggle_icons/icon_custom.png diff --git a/common/params.cc b/common/params.cc index 08a4d73..b97a0de 100644 --- a/common/params.cc +++ b/common/params.cc @@ -212,6 +212,8 @@ std::unordered_map keys = { {"AccelerationPath", PERSISTENT}, {"AccelerationProfile", PERSISTENT}, {"AggressiveAcceleration", PERSISTENT}, + {"AggressiveFollow", PERSISTENT}, + {"AggressiveJerk", PERSISTENT}, {"AlertVolumeControl", PERSISTENT}, {"AlwaysOnLateral", PERSISTENT}, {"AlwaysOnLateralMain", PERSISTENT}, @@ -235,6 +237,7 @@ std::unordered_map keys = { {"CustomAlerts", PERSISTENT}, {"CustomColors", PERSISTENT}, {"CustomIcons", PERSISTENT}, + {"CustomPersonalities", PERSISTENT}, {"CustomUI", PERSISTENT}, {"CustomSignals", PERSISTENT}, {"CustomSounds", PERSISTENT}, @@ -259,8 +262,12 @@ std::unordered_map keys = { {"QOLControls", PERSISTENT}, {"QOLVisuals", PERSISTENT}, {"RefuseVolume", PERSISTENT}, + {"RelaxedFollow", PERSISTENT}, + {"RelaxedJerk", PERSISTENT}, {"RoadEdgesWidth", PERSISTENT}, {"SilentMode", PERSISTENT}, + {"StandardFollow", PERSISTENT}, + {"StandardJerk", PERSISTENT}, {"StockTune", PERSISTENT}, {"UnlimitedLength", PERSISTENT}, {"UpdateSchedule", PERSISTENT}, diff --git a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py index 46cfa12..425849f 100755 --- a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py +++ b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py @@ -56,26 +56,46 @@ T_DIFFS = np.diff(T_IDXS, prepend=[0.]) COMFORT_BRAKE = 2.5 STOP_DISTANCE = 6.0 -def get_jerk_factor(personality=log.LongitudinalPersonality.standard): - if personality==log.LongitudinalPersonality.relaxed: - return 1.0 - elif personality==log.LongitudinalPersonality.standard: - return 1.0 - elif personality==log.LongitudinalPersonality.aggressive: - return 0.5 +def get_jerk_factor(custom_personalities=False, aggressive_jerk=0.5, standard_jerk=1.0, relaxed_jerk=1.0, personality=log.LongitudinalPersonality.standard): + if custom_personalities: + if personality==log.LongitudinalPersonality.relaxed: + return relaxed_jerk + elif personality==log.LongitudinalPersonality.standard: + return standard_jerk + elif personality==log.LongitudinalPersonality.aggressive: + return aggressive_jerk + else: + raise NotImplementedError("Longitudinal personality not supported") else: - raise NotImplementedError("Longitudinal personality not supported") + if personality==log.LongitudinalPersonality.relaxed: + return 1.0 + elif personality==log.LongitudinalPersonality.standard: + return 1.0 + elif personality==log.LongitudinalPersonality.aggressive: + return 0.5 + else: + raise NotImplementedError("Longitudinal personality not supported") -def get_T_FOLLOW(personality=log.LongitudinalPersonality.standard): - if personality==log.LongitudinalPersonality.relaxed: - return 1.75 - elif personality==log.LongitudinalPersonality.standard: - return 1.45 - elif personality==log.LongitudinalPersonality.aggressive: - return 1.25 +def get_T_FOLLOW(custom_personalities=False, aggressive_follow=1.25, standard_follow=1.45, relaxed_follow=1.75, personality=log.LongitudinalPersonality.standard): + if custom_personalities: + if personality==log.LongitudinalPersonality.relaxed: + return relaxed_follow + elif personality==log.LongitudinalPersonality.standard: + return standard_follow + elif personality==log.LongitudinalPersonality.aggressive: + return aggressive_follow + else: + raise NotImplementedError("Longitudinal personality not supported") else: - raise NotImplementedError("Longitudinal personality not supported") + if personality==log.LongitudinalPersonality.relaxed: + return 1.75 + elif personality==log.LongitudinalPersonality.standard: + return 1.45 + elif personality==log.LongitudinalPersonality.aggressive: + return 1.25 + else: + raise NotImplementedError("Longitudinal personality not supported") def get_stopped_equivalence_factor(v_lead): return (v_lead**2) / (2 * COMFORT_BRAKE) @@ -274,8 +294,8 @@ class LongitudinalMpc: for i in range(N): self.solver.cost_set(i, 'Zl', Zl) - def set_weights(self, prev_accel_constraint=True, personality=log.LongitudinalPersonality.standard): - jerk_factor = get_jerk_factor(personality) + def set_weights(self, prev_accel_constraint=True, custom_personalities=False, aggressive_jerk=0.5, standard_jerk=1.0, relaxed_jerk=1.0, personality=log.LongitudinalPersonality.standard): + jerk_factor = get_jerk_factor(custom_personalities, aggressive_jerk, standard_jerk, relaxed_jerk, personality) jerk_factor /= np.mean(self.acceleration_offset) if self.mode == 'acc': @@ -336,7 +356,7 @@ class LongitudinalMpc: self.max_a = max_a def update(self, radarstate, v_cruise, x, v, a, j, frogpilot_planner, personality=log.LongitudinalPersonality.standard): - t_follow = get_T_FOLLOW(personality) + t_follow = get_T_FOLLOW(frogpilot_planner.custom_personalities, frogpilot_planner.aggressive_follow, frogpilot_planner.standard_follow, frogpilot_planner.relaxed_follow, personality) self.t_follow = t_follow v_ego = self.x0[1] self.status = radarstate.leadOne.status or radarstate.leadTwo.status diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index 7132081..a67324d 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -136,7 +136,9 @@ class LongitudinalPlanner: accel_limits_turns[0] = min(accel_limits_turns[0], self.a_desired + 0.05) accel_limits_turns[1] = max(accel_limits_turns[1], self.a_desired - 0.05) - self.mpc.set_weights(prev_accel_constraint, personality=self.personality) + self.mpc.set_weights(prev_accel_constraint, + frogpilot_planner.custom_personalities, frogpilot_planner.aggressive_jerk, frogpilot_planner.standard_jerk, frogpilot_planner.relaxed_jerk, + personality=self.personality) self.mpc.set_accel_limits(accel_limits_turns[0], accel_limits_turns[1]) self.mpc.set_cur_state(self.v_desired_filter.x, self.a_desired) x, v, a, j = self.parse_model(sm['modelV2'], self.v_model_error) diff --git a/selfdrive/frogpilot/assets/toggle_icons/icon_custom.png b/selfdrive/frogpilot/assets/toggle_icons/icon_custom.png new file mode 100644 index 0000000000000000000000000000000000000000..f1c7f4d918862ab61d47d9f38a0bad4677400e46 GIT binary patch literal 13559 zcma)jby$>NwDr(KI&?@4N=Ubq!~hb~IdnHDUD6EFAt2o)$N&P;-JJpkAs~%_gg-!0 zzQgz2`~T&6=y%S0=A5(FUVH8J#%gIO5fji6Kp+re6=ek-@O|{|6N(GI)*x#Oz&8vp z9VG;$cA9=40%3-zD9Gvgn;!)S1SO7K-l`2!J@>z%difQ*hop}J3h z55>K9CIfbY3$ukB`jp+?;p!3ZEkX=@N}XjF!$OEFA=-w~?=Z0Fv@l9|@9AT4$}Wf$ zIm_nZF7U;Obyan(k%CJCl*~U!sC$5GSTtTZ*ZC=c%Mvb2p21Pl_07%CZ?AU?A3Lvg z1~k3Xug-X6fX>nnAuV|F^1rjSu9sgtJUsj~3#0-l;TK$py|~jsJ*DwF3`*wly69mB zN#DGNmzU35oEP7{p%hxF>-m>+v;Vdb7mf;%g+3u0kUx5$0d6+9pIC0VU2E1+83?c{*7%QUME9*mdGm{J-CPI`l`)SspEXX| zEi;UPv$M12iKg;t|M&HavW*Svb7>`7=dkGHd?F zEq0{8`AzH#~bBDl;vky67#LR9oWc(- zMeq)h9~6q~>+4TLe_!kq(IR85PY)KWrCeMNcMgJMIeyXpD8p1}*Wg;|36*)Xn}kL3 zIi8v?sVC&`@5s(hsZWN6RhuQluH)$lq=A>u$zw zRutOSikzI>tK;S6F*yvO1Vg;QC{6ii2Sa?53cMJdnVu$jzCkRCfYvuOyi0_smd?GK z`65z;i83>c(s{ntDc(0SG7>dx|Lb7Uq5JQD%LOdY`%TKr%Nc@Cx-MOvY%CMrvY;~N z_7--3fWy*XTiwsWwY9zdq%7N~&b;e)OJk##E@~F*^*|db;=lRd8o8oJd#S9?)}Jf5 z2|CH*#0V?;vZ4lOk(LQPa?qO6IoiN(a56rov8hK$-s@6#SA4{0N8fa^e0LJv6cZkx z(2oZZV)s8kKsA&RM-9UeqO<;%30x0Qf==ECUZUV_TYXHE=9EE~Uf5EB~@t89RRpx|^Dnc-A9XV`h{v zSw*?8J_gGS9lxL~9Sl)fdqiVH$$Zwi_Q@2)7q;Cy$m=30!hL5I$Xwg&9%1TLI$Gu= zR7&oLwrUGl&a10}TrOEo7Szh`%ad={)pgy@|Hjx07n+-zc<^;6rl&(ZHV0!nmYdx= zC>R+T<6&RfH(upvn&4FGhutUz|AHa@N{~?pzHDFM9{~15aOb9PbY-o7+=_qBQ z-`Un!M`6&JiVSMjuDr?3HY`XM+9sEqMmod1W@Ka}qgSa_EUeC!vFo(LfIp9ai%&MB6b-Otf zpRKx2!NSvX`|r;T459gkE;72V3R1!$=1rhQ_t4kZH=glHpe?rr_YzNrbvIHqQvPyn z68~WZBu2Zpm|uBaqv9y<4wDO>EHBQ=q$W(qsX-o@cV2a_tgP@ri}eaZRq1N#d##=` z3y8|*takXFC?wK}jL(_J_R}X2!}8bq2L|e1`un3? zn!cJr;RhRQDcy<0B>CKp_ET}cE-z24#v?lfFs$q59BYMNtaVDvY|40UjWSsT|61(3 zxbXHL8(drK-pJrF`TYyP2K*iqKC{zYgWZI-m6g>oz}cOoB_T@LvqSsc>3qXz5{7nS z_=m{HS*k>0%*3zj>RwFdh`6lZTv8)On*p}ev^-z^+7tNez%(?e#<+<9!0e`DfgcZh z%^U~1P~Fv{=5%uGWe zgMU$T*;umPp4(enjsRZCeD`LH7u|8>at&k2Sa-VoPca+DgdW4iqcRKf`h1y!FXkE= z9cGej?V%)*Gz0CiTl){FYw98Mkuc0-9JQmR8k2hr3=EQRR8d$$@sd{Q9R2rePHkDt zUy|Kzp4;P()pJE_#vgJ=IS#xcrXEgWST@ErCqco!OjGtF%)?-DoNA&1>pxFV#arzr zsQg~>yyk=3idSoB6iVkW)tX7hqzeZI>X$;Wsm8JdY>XQ?6d-u?LF7hzq=;W4bW`}G z%;6MLBT)jq3sT~&tac4qZs~AwlMjydDy4G?R(bJq4P|9zleolmda03_NqAQu0$#q1 z{PG{M76oqf^Sv=t3MJaBk0zO#09x<+I%=8*;-n50A7QD1y6=6C!@)OI6=!2*oqGLp zBivH6Pwog7*%R(_hoWo^Xn1Jwm)xI<*|u(OtB2bYnITeG?m^-N4R*LH3JQZ$ zAgKiRKFFm15QjB5H|KFQmHQZ;Th!+Zc0Lp&rAz#g%u;5V=P9$csb@FW=evAvuCAS= zh+>^9ianRbmKWb< z;414Ppuf^$1pd*(KNK4JV_n2WU`xHiBcgf#XJa7xC5nc^n8lg)t$hmR%FoaP@WDt= zeBeui6Jw$WiQ8=&wzQxvE3hT z#YgN(Bx-DVX)}|W53`_V?R8DHV)M9aOn$Mc=S+%u?IdV|crhQjY7~v?3#X^2H>jwn zKnPc^-34fwbntKYHk$frO{V;LNREhg$}4ObW< zg(R;1ftB4T;;oo03{eV&=Vqb$XiCmA-p{HQ)q?}(>+ZgHb+E{lT2MfO;~r8EsZK{d z67q@KB76>V*c6yvFAav#(=E@}R%E^Ou?ZNkj)a7Sm+1Q4&tl91BYwr>wy)^;ab>kY zvNibD<`vyG+eJVoWcui&Qu!$c^asO>)pnn15Yq3XZJjxlvlhz89-%R8Ft95&d3zhZFlH1QP>+F#$Vj|d?1|gX zm~iX{w5BqkNHD|FNQfF|hU@z_3WwA2g7u`f_;10Zt<+Qe;ia&u<~ zm%7$6#<#M;t8N=E1MI1J>HB`{_n|?qj^>oO*{Psf=7}V zGU39a?CiTzh&^n?9up4L`&4eO-`CesQ~8qAXGHFg;ok)PO-A=mryfQ>f}GJkEPa|t z@a=<>E)&}$92PVCTF`Nx`_T0eL*cwl4z5tZY&s)S~YYBjWIQeCQ*LNf&c)UQUMyF6<8~ z1@e)^`|L`_Oe!b$JD+-3K-0JbvU^t3j zNJN&K^r8Fb8x~GZXJ5ed;D%{mnydz19o@sn57(1cje@Z*$Er^fNck1vgT^X1PD3FKjpfEKpHbzs+8fk z>A75c$@TH$NBD5haiw;t1Rx^^Lig|AKjuqz_mB2TV|(j7Ma*0$Cm3>b6%6X9$c3VX zR|3b(54DTtJ;{)U)dp_bfr0IH^A?E?GX-7TX6+xE=D$NNOt0Q9mYZ(t#mad;kEa~= z$3-|=Vbm^uZF$kNzBQHxv9I+7HBwK0e*VVKkc{e53~MiJ=sTVA@p-@-?>FP%-oL>D zL{?uOYB%dBUYMCHXs3DwmafC|KdyB81Jr=xx8cP5d~2;9@{usrr_gff?aq2+*|Lnu z15}X|WaWc=Mdt9|KbJ2~!J4%ExGVfTM=|XhpQUpM;KLstV&r_X{`JM75J!HrE?k^= za&mG-yGn-@nr{8vlWA>=lv#x7N#I3vem+C1C5@{VQ!pqZekQ{ms$Z`6h7WLmVJ0RH zygu9RVOH0&wq_|iiVk(H${y}u4aQ?81UrO;wOmE+bKbS?N0mGS*% z*sh-J3^Gvjn{XUEhxOq@g|^mKcL%{L)3&pDFo%Iy{1uzGQg)4Ws6 zw4c_M#^!T`z^AODQ+cR}ike!2+k6?mU#*F<-fH=?Oa_PPyzt3GKbqmxw6YTbMZ0l4 z7Y_s;VPy`fUg`cpDny*6f<^z<9eVq(uQQIE$Ai5V=%5p=+#EFd%=b?Ih&doQOiEGSf*8CT=NGh6H`y6S%>dRHeM7T zKDTrM#XDb8Qqo<2ZUgY~o&ooAKwd-Isy6dyC~3k0v`cMYuk^~k`j&6 zwYUM=Qlm!u6#Vj80(EP{u)z>f^&DX%f@6lx_)5U0U37GG)_+dV4IrT}2^PdR6nAcRS@sKtbZ~=H#6)Q(z}F)}ueYh?skoK`_V*N+){c&j zGNpp9V4j5ntDU)ovT^LJtg8-HXD9*O!)R{bASI;{pT{rOu%D6WM#{G~HHox}aN8VM z8kda!`}fZZXc=K83Lqd~sWz;Ws=25dp&h?d1)1~k^Z%S3ef_}Xr!U2{nPpy4`xF>k z3UH~nLfJixk@#?!X5Rv%bkNo0+8X2hxss3%|Gkn;uyyw;7@i+~s_XghzoTVtBZt-? zv;q?C-()a8HRUz}2t}LAD_PjrpBn=kKcD~HS>=W_fFhAxizl{%A;Ci?Hd<;Gwg^~h zUHxyu)vlM9+5p>`B_Hb6D1MbnxS_wR39E0fz9+vRxc}+Xr^kWY<8RgNJ_Z7rLJx z2pusH3t;`Z0&tLh(}n%fRtcqrss7X}!ZNXx4s_19QcRzYV9P3I2USQ&1_JAV6c1kt55>17P>ziQ-A({l|>L z&sS)8S#HV89YMv0hi6eS#6|c|WWDfVQ<(Y~IlIQnkJ)0ZD{<}nBYkhiC)A*-*toa_ zxWPa!lcnJ<`y}u=FE0u4|KV| zshOG7wr!!+=)5Wnop=}`k%wyX9aaDvIyz(D1t(b2@3=F6Xj>6CJ7EfwS6=b~%2$wR z(ykse1R?&y-*936CDHeI)<@eL8>D1(bmL9-u5NDBPXf-c)p}p#RAC5)!!QGqi9{0w zR(cuB=Cliz0gtVa5EGL%=ZsWut%?q1c8)Tq<0A}N+Xf|%5;7QlHly3Bi&0Ip;3aIt zrCl=ENzG?IX-x^~CmBzFg!M1@MRUFm2By&&^Ch|1%)axt`gX1q)@K=$g%Uo_lJev* zlChLv&3BJ~2qU0+NrInI9VD${9~9y#-2%}wZ9(V@0k^LooMZv|LWa^GDYlWE+{q~g z-&SL(nPqRCK&r##L%uKax|0+eoK3kI((%^Xyx!)1{)gz%F|0LJaRM*br(@bEPb!_G z+}Lf_*4C!>H+ylhFVB&Sdr6V3R@s=LqN1XN{^v4eGeM}3Y zQjr9KRt_yb(npxY4^Tgf@Of7JWNW0Vh9O_Ic7At$zS#d*8o+J9O&*(&(D|k1g+;4N zvC$;*b8PuFzmU+m@H2=($af+_S!pN{5fQ0ZIiD}v^&hK*4Xx`I3Uhku*Ta?WtahAn z|M~d(uDOHyuXbw_jhQ{R5iAV1eW1Q`(?LZis}S{M9@{cF1e}a-Ab9RYvpuR|grk1X zum(Np)xU{N@8fReYZ;33tKD(;mvLY3B{u%BS8p?N?0vj){YzsECfn)h<<&yLWk5t$ zo?X`DyvX?zNH{N6|HZ*xCm^d=-~=aBx&eEkG?kTvZ-ZR@{3fa z++gtDvs0%nO*gmQnE*NQU{qd@nc|9D#%4HyKQuJ-c%=gf&VTK`G}yTpJN_8>Lybx? z6K1eZz+kQ;z*5c!qK`S?PgK9U!$f1!cl$Hml0+7!6kBobZg2lpCuBUkyp$1eS>paV z67b3RdLa5<;mKSn-c_r1?#~&5=%X9Psio=ZX%{|=?#ZC#+WX3%5s{koK7?wNNw0fX zSA$P9W&YaEaytJc6to*pr)isC3D4bkgxlQJUP@Zk8V zJaHlWe>d0e)zb_7!&;>_O^loM#x=e?C$;ioo$_+!Vr=j-Qh@-S!NI|m#u!^1V4WMsIRu;|ZpBqtG zDjs8RPjK9ij8G_B$F9I%1c}MX(V^F6nBqx9s}x z{4u9gv0hJ~4)2e@;fmbFYufg68}C6L!Vpu94gt*@Sn}ryu7?J6h!mZRxIB)x(2~vO47^`#*gUFq8p~!3=LT#?ZFtmI{jYi zh+m51#eqF`J1m^z6q_}zOY?mgT718Pa*?Anz8)!;b*ddZpZLkSsoJOq*y)^Qk zw#v+A=_aS35yiOi<;*f+gIktZ(NfPiGd(?>e~B+M>FwZvWZGr(b2KuZhpu)iR}k+M`rO?^iRn(d+tg)gK^E>>2t4@-6@l)j9;xfMvE|~(SnbcV4gw1ha^o(8*hmD<`I%iD&aac;r;*W_c6}^~L z@cKE|zc)7|F;$t<9_P~_Y^~2t4koJ5STQ1SlyiTYz!6EEb=y9uciZJ1CYC-K?MRud z2+|eHQ-fC0RQoM8?VEvPpKW1aPrrp%IMeGuqbN@ z+!U#p!2u>oOh`aEs|GHkjT)_ou1IOEtR@5$BEfQ;qyL0}z1%Ui$v!TpUnA@Ypth-x zxTu$*(`hDQ9>Ryeu=Cek zf$O~#5)vXH^(QEs$Dotb5yrsqw=rwrC6Gfuv9hxEu@oYpR@3x+>FBUQ--Y`x8D!B= zxVLpcL;O!pEs7`4a6K#UD|;!DinQWOo*JWVz*&T2MMX){KRGLFp!u>e8{U{se>6 zpYP5@*HA9XmgbG#zO(~!uXgX3Mu!d4B4u*t;mD!5oC5>)-uq#)l#GBbNBUu6;Syc# z_l9FxaIkdUs8z)){v_gXVk1;|huiMl^tDi&$6)3bl&X!PojTPnR(K^OB*K5CxFF&w>jI_40xulX>76PLmTEdASXsAun79nA z&R(83KSY^Y?(gmFG_y-dtt?OKdQp9JZsKz+JoumLI}F0}%+;FPVBM^jp3vI5TrUf- z?U1k2!zl@%srEZyDo4b^ko#9Jke_qs26I*9zJI{J`Qg ziIkBsWOZta*mSiq;LpJx;nT}RW|aGg{oArKsfcs@I@tB!9?)eS&6o6J44;_-vOr%& zaABr}+;32S$!Bp4<{rBBXOyN6ZozaEDL;m?z!fZlu0pS-3ZDG=+!=6=EX5oeEp-H} z>HyvDwM+8LT9X|`12&GF0}7+3qRJ2hic8FEej{@F(O#vB*>^JZ5yb2$*U~u?%UM0` zdgPpW;&H$mkdDnU3RU3}npr420G! z6|nth^mvAs6&%ICGFoYDVmOxh>`Ly?(kCQw0Sgt3@8yA&_nfJ)SiPcU37F>F$iOL@}}v z-xoD4Oi#z&w^V2W8J>G`Y^=C~Vu)r&0m=h=l5)mSvpbo6J^|z!Go$E!8k5wacki79 z$`coTwodDa>nVlx zQO4*qO)FV*b91rkMWiS4!t5`)<0fgCqJKNie zQ^8RJxYP%bwp}<@j|vL5qvarUUQZ_U*FHNt`z_Kr0OBRD?>t0Q&~uMaHmtidE$w3j z*q>lKAX0Tdd+H(>buAc`Phdehs+()`0e>Fvpmx!@6q(2-b9-}M1GL)y@c)=3TpG(N zeL=7BJddhc16ur1b@CM@b9;SX_MFT4&QxSqS6A*_s?;d_{hfr>+A7xClsP|M4p@IZ zGK8Soo72uRHdsbkUH!=f%j1LxNHjC2mWA>=1rLf7-eGGTmq_PvYA$g;*|TJqk zuvJ31m{I;c+*_cH@WAo$C8z~o+5>U1sF}HpjdJzJT_NoMGweVoH7%`_$FP=OueFOe zkUv18+l#6ih-$|Y{y&);Tl|4SWl&LGem!1#+~1~*5kv1e^$<1rR-m6Ia{HN_+QfT-4HWO|q&fc&%camRjmCVMGDbX8^pI55fxut+LJ!0; z@9gbflJCyKITrxx!ir_9V2cF6uz`Pvlet08!}Byeqs`}5}yQ@()BFgy1-8JT(* z$X{&*QH8ONt=tAcS9Cg{ok2qRz(6XWCU7R89D`B&yy@5ORhP_<*H)_{r8 zF_MvCG7hL66^8x`Ft4{(tb7a~8xXs-1d#g86Li!`)i9Y+abZIT=YT}>8!%M^$Tg>x z@aA}Usk>rrHw}?P6WV=#Z}lgDO2lsCO=4V}>fV>ew+{|6aU|ewtfmsGvHBPWY4~t1 zg4u3ubyXWF8? z#bW`1Q&*66S`ScAAG+uVs7hSiC*S`Yotk=5W{f8Ilw?^S;q}cZd##Z}bO#$I49pFM zfMpVk21mMqcFyUy(Ay_j7TeBKLV|)C4F+PW*=_mUGFe!uB{Yl$Hx(R6GOUCZW$^mz~P^DcvXVMh4J@ zw>MHzI7A|Ymx2lU0>G@Z*4+td=j?%b7BV2>h)QMoe0VoT=cpjMdUg z$&3uNG&Il*RmEU5a$7($EUcJ~k56;JdK>2asSh)?u}KXmlh#ktq<|{@?QDA@GBzW} zJ9!aSoI4rK6wl+($(|Q1by={^pa>0N8TMhWR zF6Ug-?Vb`j(6l#b8Azp8bC+f51Z|;N>BKJQ&o+t~2A8>4$=JrdYb+NAxwy#6?y ztaTlu&-80e1Dx($JLrqy3ZeU9>0E}-wS9g66K2GL6nT1jRKl zzIe54b{IuSJ$$zE2_Vm+D`J%1AUK&$gwq=25v}@+|0wP49!cgKK@^C@f`WorKr0M$ z7pqZf5cELdd!T7FImQOU&RNUcd`%N9Q%Xw81mMwFOk7_Lk^d>q49sppUPF@P1g{XM zdvVcAnXrburhx=np%AlH_hMEhoOW9~yP-y7bkRy-V`C#Bf+JsFsiH*ak!&1?7*KVY zlNTT8@?ge(Us6_9ZtZodKZt=@pJk_)HfOrnEA2v2NGvFQtGNJ$Ghlg=_F?qgNH#vYMd%tMsoRa$k^t?@9 z(Xb5@kkS4n?9Xv~a+?>0)h1pjj;7)@_0RigZC4u*mYtFN=FMUrh}tQ8mZ#9`t1J4I z20OxuE>f;%rL(m6nN6)~i!kKsEe$j2^2*8(Z99vAKZOkrDnMe&MZwl5F-YG>IPnkr0nZpWY`&VSHd^h<+HdF#LUA&CZ?$(+0y< z0*-16OvA|&Ukfyof`lg_ZlEj7xx#trUQ=(r0($uflqsz}MWS#{SP&X1m0xwErf5kN zmfjxnH_#cRcg{vN(4xig8Xq4I91c{e_!z$4M<6mm`D}HjBk#NfeE`xb*S>tTACOb<}z__v#(u}jQicID+y>=XI;#590}u>{mSJTm7< zZu>9jnwni;$C^*p7vtlr`@7uo!cWM7iEF8ig9Ns;qT9dG^tONLT?2r@HEexdA8Gj% z|6*X{TK@k1dta$=n!d;Nerjr}pb@z>^>8Ri@k*K8MwkN2J2bZfS+Is&eLf&d1fdHW z{u6MJ7{cgp@JE&5X_1?a2~OG>))-Tk^0A))>c$8v5(ND?)mo^uvif+S`(g_XN?}2( z`fmj-PZkwWv)PSWlKa09a2wd4*bKe10sf3cp!>5Wke=1jVJn4PqmcJTMMKlsT8;yN zM6e1(gGKA|PfG0R0b#B@GS9YIm1aX}rKtnN z4md(x>{E<~)6!qR1WB_nOT>lhM@QA0nk3)?=>Xm8Ut1{_l|Kjtc;mO%*;AFOfQI}L zXvkV|Nnf$!DF=5>%E(M;!sj9TPoc3D``9+7_u=WPrR1lk6k=z|jerVxffh{|QCwh) zv{vaKU;>M#P-qaT{6tMldrgVhYjahjr40Z!E84grzR`+GB$_}s8H}b1jV>D)9eN)%!P_{=-A zhB&KJxM(pyL0y=hmFUtj^Wei5g0yLD#PS$ZwLKcbv26Qkk`bE&qaO%ATwGkR z1D^ul1f9p*`_Gh9@TzXB0@LL}BhSyyeCx+s0ro!wj9hWp+AfYySrIWR|26&)G&ht% z+jymYe9jgp8Ul+fXsI@=lNR#WRA@DvTShhDgl_HZ@Y<>0mfEn|bo#%DB0#%tRjTDKfj?&_gV&61GSV=Ml`l`r*BuJErh)0rt1yPKDzB6--@c6=|7SK)7_zX=;ZnQ808Sfb82!XBn^4(I;JPQ7rHB@-IrAnzH;7;?Rx z$%m8k3MjR&L5b?xD`{w9@h$5Omtn-CE2g#Xz`bX9I5_#%fCPlnXem^V8llxN0R_-R zC{s*Ktbp8>7>XMJdV;ouc%VsFTDkh;Z=e^B|xHfeQs`Euok45@(q}L)GZxn zmw|`klG0kD$yX-D84n0CzbCPv zT&>M8-0RkXe}cIM6jD!jx&P6@-rv9VFDIj*em zkGWE%aJ0Uk4<=z|9PE3I-ya9rU3n;ca=GlTY!kH}gtk87Ba=2jyUfv8q7)Zj{fsTS1NW_e#iN`f8!hqK9RQ@>RKP0kw-cXM0+u(RG>$Sj#PkCNVA|V)2 z!T@1aErTD;V*G=5b#{`xLZMKg7ajsPeY$u8qTv?of9stMdv{KFI17jV+9*Q-+Q*8- zm-o2O{R&!~Cr2IEsgH$;$r3;&b9nXl=fJ()kz}TOQ4Wsr$R?x*H=PY8_8?tWZf-~P zC1aS4bfAi2SY!Y)qhE~Phw>RHfh0Ku1?MA;AmLiAdZ_>W1r~+6A zp`@hb=jVUl=0Ru(fLjK>kXhB`LdE5U0UZ^P0G;1R0ojtRO!>E3|I;ZIMiOX!3(KZG(DJ2W;sr;*P}YxpcT2T( zZ>cv~`gbK;HNKJ9h-~*d%aP5{Z$?mmrZk2=efm@x^hGD$6S&13gB#C)(zx%`qmcdv z>Hoep=FVZH0j!4Q*!q+JpweQkRi(`u3SccguIvG*hjP&_gf@T;GEfk^;;~ zvTo9IJj~4N{h*HPJ>gOjCih^dYU%~1mp=|5n&}7YL#sdi1fC&va9=F)4`^Z=qKrnf z2EitG`yEVJKHZah^kV#xeq1cB7QKc9+>a}uE7wUdYkmb$0Re$h5RCeE#(Xur7Txsw zKYnmi%ji|8W#fS1u?mHnwm*ioVPGp@Wq%bH7bnh^^n3mm0}D4uYxXOSc`ImH-39|s z;RqQyIh>U^JzNxr-}_yj8q~1F|<3}$@^4QXBYeXT65rDaD0f z7Fze&H4#lf&TRmzeCnlXWoNXtrx!MQuG)nUmwSc8ukt`+=Ul=NJitErhmHs^A7>1W z*Jm;P!-`GF`hAr<|GcAK0V>6kZVrV{P6AYl_VTneT}rx|`5A#>_wcK<`R_=H&^!0L zT_}$5evPr*TYZ+K^`Wf_@_P5LU^()Q1IJ&}^>yO0 z6(Az|NsEiqrYaXILI3vU)|fkq^ThTLzyj6`*b&NTDfH)7*%dt7l5fD%7X^qY15Jn_ zvjbJGtYW$^GbJ4RUk#;G3i95{_MP^*ac3-zxS&Vuf#51Eh&&5oFG{N9ovFVtc#=ZN1lRGti1UYkKqg3`h-eGf ze>qQH9SdW=2Ntx%vNaE@3nzmxT(+}9yt7$GV1 zd$B<*m3doB;Q5j^k+|6u5J?>XZ1iOOj#mmnHyq`bN&@?GcJHmHo(_1vBz>WFe0dW* z$}zZUQ7nPM3Vu>hIeOR0V-Ehw{g3h;xz_*rvfF!SU#qa^5y3X#NhXMjqJ}~(!ZPCj E0c7*ixBvhE literal 0 HcmV?d00001 diff --git a/selfdrive/frogpilot/functions/frogpilot_planner.py b/selfdrive/frogpilot/functions/frogpilot_planner.py index 56df8f1..a7d7d5a 100644 --- a/selfdrive/frogpilot/functions/frogpilot_planner.py +++ b/selfdrive/frogpilot/functions/frogpilot_planner.py @@ -107,6 +107,14 @@ class FrogPilotPlanner: self.cem.update_frogpilot_params(self.is_metric, params) params.put_bool("ExperimentalMode", True) + self.custom_personalities = params.get_bool("CustomPersonalities") + self.aggressive_follow = params.get_float("AggressiveFollow") + self.standard_follow = params.get_float("StandardFollow") + self.relaxed_follow = params.get_float("RelaxedFollow") + self.aggressive_jerk = params.get_float("AggressiveJerk") + self.standard_jerk = params.get_float("StandardJerk") + self.relaxed_jerk = params.get_float("RelaxedJerk") + custom_ui = params.get_bool("CustomUI") self.blind_spot_path = custom_ui and params.get_bool("BlindSpotPath") diff --git a/selfdrive/frogpilot/ui/control_settings.cc b/selfdrive/frogpilot/ui/control_settings.cc index 1261704..b0629da 100644 --- a/selfdrive/frogpilot/ui/control_settings.cc +++ b/selfdrive/frogpilot/ui/control_settings.cc @@ -11,6 +11,8 @@ FrogPilotControlsPanel::FrogPilotControlsPanel(SettingsWindow *parent) : FrogPil {"CEStopLights", "Stop Lights and Stop Signs", "Switch to 'Experimental Mode' when a stop light or stop sign is detected.", ""}, {"CESignal", "Turn Signal When Below Highway Speeds", "Switch to 'Experimental Mode' when using turn signals below highway speeds to help assit with turns.", ""}, + {"CustomPersonalities", "Custom Driving Personalities", "Customize the driving personality profiles to your driving style.", "../frogpilot/assets/toggle_icons/icon_custom.png"}, + {"LateralTune", "Lateral Tuning", "Modify openpilot's steering behavior.", "../frogpilot/assets/toggle_icons/icon_lateral_tune.png"}, {"LongitudinalTune", "Longitudinal Tuning", "Modify openpilot's acceleration and braking behavior.", "../frogpilot/assets/toggle_icons/icon_longitudinal_tune.png"}, @@ -79,6 +81,58 @@ FrogPilotControlsPanel::FrogPilotControlsPanel(SettingsWindow *parent) : FrogPil std::vector stopLightToggleNames{tr("With Lead")}; toggle = new FrogPilotParamToggleControl(param, title, desc, icon, stopLightToggles, stopLightToggleNames); + } else if (param == "CustomPersonalities") { + FrogPilotParamManageControl *customPersonalitiesToggle = new FrogPilotParamManageControl(param, title, desc, icon, this); + QObject::connect(customPersonalitiesToggle, &FrogPilotParamManageControl::manageButtonClicked, this, [this]() { + parentToggleClicked(); + for (auto &[key, toggle] : toggles) { + toggle->setVisible(false); + } + aggressiveProfile->setVisible(true); + standardProfile->setVisible(true); + relaxedProfile->setVisible(true); + }); + toggle = customPersonalitiesToggle; + + FrogPilotParamValueControl *aggressiveFollow = new FrogPilotParamValueControl("AggressiveFollow", "Follow", + "Set the 'Aggressive' personality' following distance. " + "Represents seconds to follow behind the lead vehicle.\n\nStock: 1.25 seconds.", + "../frogpilot/assets/other_images/aggressive.png", + 1, 5, std::map(), this, false, " sec", 1, 0.01); + FrogPilotParamValueControl *aggressiveJerk = new FrogPilotParamValueControl("AggressiveJerk", " Jerk", + "Configure brake/gas pedal responsiveness for the 'Aggressive' personality. " + "Higher jerk value = smoother rides.\nLower jerk value = faster response.\n\nStock: 0.5.", + "", + 0.01, 5, std::map(), this, false, "", 1, 0.01); + aggressiveProfile = new FrogPilotDualParamControl(aggressiveFollow, aggressiveJerk, this, true); + addItem(aggressiveProfile); + + FrogPilotParamValueControl *standardFollow = new FrogPilotParamValueControl("StandardFollow", "Follow", + "Set the 'Standard' personality following distance. " + "Represents seconds to follow behind the lead vehicle.\n\nStock: 1.45 seconds.", + "../frogpilot/assets/other_images/standard.png", + 1, 5, std::map(), this, false, " sec", 1, 0.01); + FrogPilotParamValueControl *standardJerk = new FrogPilotParamValueControl("StandardJerk", " Jerk", + "Adjust brake/gas pedal responsiveness for the 'Standard' personality. " + "Higher jerk value = smoother rides.\nLower jerk value = faster response.\n\nStock: 1.0.", + "", + 0.01, 5, std::map(), this, false, "", 1, 0.01); + standardProfile = new FrogPilotDualParamControl(standardFollow, standardJerk, this, true); + addItem(standardProfile); + + FrogPilotParamValueControl *relaxedFollow = new FrogPilotParamValueControl("RelaxedFollow", "Follow", + "Set the 'Relaxed' personality following distance. " + "Represents seconds to follow behind the lead vehicle.\n\nStock: 1.75 seconds.", + "../frogpilot/assets/other_images/relaxed.png", + 1, 5, std::map(), this, false, " sec", 1, 0.01); + FrogPilotParamValueControl *relaxedJerk = new FrogPilotParamValueControl("RelaxedJerk", " Jerk", + "Set brake/gas pedal responsiveness for the 'Relaxed' personality. " + "Higher jerk value = smoother rides.\nLower jerk value = faster response.\n\nStock: 1.0.", + "", + 0.01, 5, std::map(), this, false, "", 1, 0.01); + relaxedProfile = new FrogPilotDualParamControl(relaxedFollow, relaxedJerk, this, true); + addItem(relaxedProfile); + } else if (param == "LateralTune") { FrogPilotParamManageControl *lateralTuneToggle = new FrogPilotParamManageControl(param, title, desc, icon, this); QObject::connect(lateralTuneToggle, &FrogPilotParamManageControl::manageButtonClicked, this, [this]() { @@ -208,8 +262,11 @@ void FrogPilotControlsPanel::updateMetric() { } void FrogPilotControlsPanel::parentToggleClicked() { + aggressiveProfile->setVisible(false); conditionalSpeedsImperial->setVisible(false); conditionalSpeedsMetric->setVisible(false); + standardProfile->setVisible(false); + relaxedProfile->setVisible(false); openParentToggle(); } @@ -219,8 +276,11 @@ void FrogPilotControlsPanel::subParentToggleClicked() { } void FrogPilotControlsPanel::hideSubToggles() { + aggressiveProfile->setVisible(false); conditionalSpeedsImperial->setVisible(false); conditionalSpeedsMetric->setVisible(false); + standardProfile->setVisible(false); + relaxedProfile->setVisible(false); for (auto &[key, toggle] : toggles) { bool subToggles = conditionalExperimentalKeys.find(key.c_str()) != conditionalExperimentalKeys.end() || diff --git a/selfdrive/frogpilot/ui/control_settings.h b/selfdrive/frogpilot/ui/control_settings.h index 176ff7d..f4ab018 100644 --- a/selfdrive/frogpilot/ui/control_settings.h +++ b/selfdrive/frogpilot/ui/control_settings.h @@ -29,8 +29,11 @@ private: void updateState(const UIState &s); void updateToggles(); + FrogPilotDualParamControl *aggressiveProfile; FrogPilotDualParamControl *conditionalSpeedsImperial; FrogPilotDualParamControl *conditionalSpeedsMetric; + FrogPilotDualParamControl *standardProfile; + FrogPilotDualParamControl *relaxedProfile; std::set conditionalExperimentalKeys = {"CECurves", "CECurvesLead", "CESlowerLead", "CENavigation", "CEStopLights", "CESignal"}; std::set fireTheBabysitterKeys = {};