From fb8103b646012fe75bf46f12a2cd6ab99316234a Mon Sep 17 00:00:00 2001 From: FrogAi <91348155+FrogAi@users.noreply.github.com> Date: Fri, 12 Jan 2024 22:39:30 -0700 Subject: [PATCH] Custom driving personality profiles Added toggles to customize the driving personality profiles. --- common/params.cc | 7 ++ .../lib/longitudinal_mpc_lib/long_mpc.py | 60 ++++++++++++------ .../controls/lib/longitudinal_planner.py | 5 +- .../assets/toggle_icons/icon_custom.png | Bin 0 -> 13559 bytes .../frogpilot/functions/frogpilot_planner.py | 8 +++ selfdrive/frogpilot/ui/control_settings.cc | 48 ++++++++++++++ selfdrive/frogpilot/ui/control_settings.h | 3 + 7 files changed, 109 insertions(+), 22 deletions(-) create mode 100644 selfdrive/frogpilot/assets/toggle_icons/icon_custom.png diff --git a/common/params.cc b/common/params.cc index c98a24c..1fb1df7 100644 --- a/common/params.cc +++ b/common/params.cc @@ -214,6 +214,8 @@ std::unordered_map keys = { // FrogPilot parameters {"AccelerationProfile", PERSISTENT}, {"AggressiveAcceleration", PERSISTENT}, + {"AggressiveFollow", PERSISTENT}, + {"AggressiveJerk", PERSISTENT}, {"AlwaysOnLateral", PERSISTENT}, {"AlwaysOnLateralMain", PERSISTENT}, {"ApiCache_DriveStats", PERSISTENT}, @@ -230,11 +232,16 @@ std::unordered_map keys = { {"CEStopLights", PERSISTENT}, {"CEStopLightsLead", PERSISTENT}, {"ConditionalExperimental", PERSISTENT}, + {"CustomPersonalities", PERSISTENT}, {"FrogPilotTogglesUpdated", PERSISTENT}, {"GasRegenCmd", PERSISTENT}, {"LateralTune", PERSISTENT}, {"LongitudinalTune", PERSISTENT}, {"OfflineMode", PERSISTENT}, + {"RelaxedFollow", PERSISTENT}, + {"RelaxedJerk", PERSISTENT}, + {"StandardFollow", PERSISTENT}, + {"StandardJerk", PERSISTENT}, {"Updated", PERSISTENT}, {"UpdateSchedule", PERSISTENT}, {"UpdateTime", PERSISTENT}, diff --git a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py index 25e15dd..4a3fc29 100644 --- 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, aggressive_jerk, standard_jerk, relaxed_jerk, 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) @@ -272,8 +292,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, custom_personalities, aggressive_jerk, standard_jerk, relaxed_jerk, prev_accel_constraint=True, personality=log.LongitudinalPersonality.standard): + jerk_factor = get_jerk_factor(custom_personalities, aggressive_jerk, standard_jerk, relaxed_jerk, personality) if self.mode == 'acc': a_change_cost = A_CHANGE_COST if prev_accel_constraint else 0 cost_weights = [X_EGO_OBSTACLE_COST, X_EGO_COST, V_EGO_COST, A_EGO_COST, jerk_factor * a_change_cost, jerk_factor * J_EGO_COST] @@ -331,8 +351,8 @@ class LongitudinalMpc: self.cruise_min_a = min_a self.max_a = max_a - def update(self, radarstate, v_cruise, x, v, a, j, aggressive_acceleration, personality=log.LongitudinalPersonality.standard): - t_follow = get_T_FOLLOW(personality) + def update(self, radarstate, v_cruise, x, v, a, j, aggressive_acceleration, custom_personalities, aggressive_follow, standard_follow, relaxed_follow, personality=log.LongitudinalPersonality.standard): + t_follow = get_T_FOLLOW(custom_personalities, aggressive_follow, standard_follow, 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 6d2e594..cda535d 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -135,11 +135,12 @@ 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(frogpilot_planner.custom_personalities, frogpilot_planner.aggressive_jerk, frogpilot_planner.standard_jerk, frogpilot_planner.relaxed_jerk, prev_accel_constraint, 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) - self.mpc.update(sm['radarState'], v_cruise, x, v, a, j, frogpilot_planner.aggressive_acceleration, personality=self.personality) + self.mpc.update(sm['radarState'], frogpilot_planner.v_cruise, x, v, a, j, frogpilot_planner.aggressive_acceleration, + frogpilot_planner.custom_personalities, frogpilot_planner.aggressive_follow, frogpilot_planner.standard_follow, frogpilot_planner.relaxed_follow, personality=self.personality) self.v_desired_trajectory_full = np.interp(ModelConstants.T_IDXS, T_IDXS_MPC, self.mpc.v_solution) self.a_desired_trajectory_full = np.interp(ModelConstants.T_IDXS, T_IDXS_MPC, self.mpc.a_solution) 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 d7f77eb..7c7b14d 100644 --- a/selfdrive/frogpilot/functions/frogpilot_planner.py +++ b/selfdrive/frogpilot/functions/frogpilot_planner.py @@ -101,6 +101,14 @@ class FrogPilotPlanner: if not params.get_bool("ExperimentalMode"): params.put_bool("ExperimentalMode", True) + self.custom_personalities = params.get_bool("CustomPersonalities") + self.aggressive_follow = params.get_int("AggressiveFollow") / 10 + self.standard_follow = params.get_int("StandardFollow") / 10 + self.relaxed_follow = params.get_int("RelaxedFollow") / 10 + self.aggressive_jerk = params.get_int("AggressiveJerk") / 10 + self.standard_jerk = params.get_int("StandardJerk") / 10 + self.relaxed_jerk = params.get_int("RelaxedJerk") / 10 + lateral_tune = params.get_bool("LateralTune") self.average_desired_curvature = params.get_bool("AverageCurvature") and lateral_tune diff --git a/selfdrive/frogpilot/ui/control_settings.cc b/selfdrive/frogpilot/ui/control_settings.cc index 740e319..3594d77 100644 --- a/selfdrive/frogpilot/ui/control_settings.cc +++ b/selfdrive/frogpilot/ui/control_settings.cc @@ -12,6 +12,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"}, {"AverageCurvature", "Average Desired Curvature", "Use Pfeiferj's distance-based curvature adjustment for improved curve handling.", ""}, @@ -72,6 +74,46 @@ 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", + 10, 50, std::map(), this, false, " sec", 10); + FrogPilotParamValueControl *aggressiveJerk = new FrogPilotParamValueControl("AggressiveJerk", " Jerk", + "Configure brake/gas pedal responsiveness for the 'Aggressive' personality. Higher values yield a more 'relaxed' response.\n\nStock: 0.5.", "", 1, 50, + std::map(), this, false, "", 10); + 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", + 10, 50, std::map(), this, false, " sec", 10); + FrogPilotParamValueControl *standardJerk = new FrogPilotParamValueControl("StandardJerk", " Jerk", + "Adjust brake/gas pedal responsiveness for the 'Standard' personality. Higher values yield a more 'relaxed' response.\n\nStock: 1.0.", "", 1, 50, + std::map(), this, false, "", 10); + 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", + 10, 50, std::map(), this, false, " sec", 10); + FrogPilotParamValueControl *relaxedJerk = new FrogPilotParamValueControl("RelaxedJerk", " Jerk", + "Set brake/gas pedal responsiveness for the 'Relaxed' personality. Higher values yield a more 'relaxed' response.\n\nStock: 1.0.", "", 1, 50, + std::map(), this, false, "", 10); + 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]() { @@ -179,15 +221,21 @@ void FrogPilotControlsPanel::updateMetric() { } void FrogPilotControlsPanel::parentToggleClicked() { + aggressiveProfile->setVisible(false); conditionalSpeedsImperial->setVisible(false); conditionalSpeedsMetric->setVisible(false); + standardProfile->setVisible(false); + relaxedProfile->setVisible(false); this->openParentToggle(); } 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 629772a..473c94b 100644 --- a/selfdrive/frogpilot/ui/control_settings.h +++ b/selfdrive/frogpilot/ui/control_settings.h @@ -22,8 +22,11 @@ private: void updateMetric(); void updateToggles(); + FrogPilotDualParamControl *aggressiveProfile; FrogPilotDualParamControl *conditionalSpeedsImperial; FrogPilotDualParamControl *conditionalSpeedsMetric; + FrogPilotDualParamControl *standardProfile; + FrogPilotDualParamControl *relaxedProfile; std::set conditionalExperimentalKeys; std::set fireTheBabysitterKeys;