From 0a708dab29f759334335bd365ec1ecd9ddff48cf Mon Sep 17 00:00:00 2001 From: "Takagi, Isamu" <43976882+isamu-takagi@users.noreply.github.com> Date: Mon, 27 Jan 2025 09:49:14 +0900 Subject: [PATCH] feat: operation mode debug panel (#8933) Signed-off-by: Takagi, Isamu --- .../tier4_adapi_rviz_plugin/CMakeLists.txt | 2 +- .../tier4_adapi_rviz_plugin/README.md | 2 + .../icons/classes/StatePanel.png | Bin 18815 -> 0 bytes .../plugins/plugin_description.xml | 4 +- .../src/operation_mode_debug_panel.cpp | 221 ++++++ .../src/operation_mode_debug_panel.hpp | 76 +++ .../src/state_panel.cpp | 633 ------------------ .../src/state_panel.hpp | 206 ------ 8 files changed, 302 insertions(+), 842 deletions(-) delete mode 100644 visualization/tier4_adapi_rviz_plugin/icons/classes/StatePanel.png create mode 100644 visualization/tier4_adapi_rviz_plugin/src/operation_mode_debug_panel.cpp create mode 100644 visualization/tier4_adapi_rviz_plugin/src/operation_mode_debug_panel.hpp delete mode 100644 visualization/tier4_adapi_rviz_plugin/src/state_panel.cpp delete mode 100644 visualization/tier4_adapi_rviz_plugin/src/state_panel.hpp diff --git a/visualization/tier4_adapi_rviz_plugin/CMakeLists.txt b/visualization/tier4_adapi_rviz_plugin/CMakeLists.txt index f06ab33823a3f..c65f1de69e414 100644 --- a/visualization/tier4_adapi_rviz_plugin/CMakeLists.txt +++ b/visualization/tier4_adapi_rviz_plugin/CMakeLists.txt @@ -12,7 +12,7 @@ set(CMAKE_INCLUDE_CURRENT_DIR ON) ament_auto_add_library(${PROJECT_NAME} SHARED src/route_tool.cpp src/route_panel.cpp - src/state_panel.cpp + src/operation_mode_debug_panel.cpp src/door_panel.cpp ) diff --git a/visualization/tier4_adapi_rviz_plugin/README.md b/visualization/tier4_adapi_rviz_plugin/README.md index 4056130776178..e067e6d09b11d 100644 --- a/visualization/tier4_adapi_rviz_plugin/README.md +++ b/visualization/tier4_adapi_rviz_plugin/README.md @@ -1,5 +1,7 @@ # tier4_adapi_rviz_plugin +This package contains tools for testing AD API. For general AD API usage, we recommend using [tier4_state_rviz_plugin](../tier4_state_rviz_plugin/README.md). + ## RoutePanel To use the panel, set the topic name from 2D Goal Pose Tool to `/rviz/routing/pose`. diff --git a/visualization/tier4_adapi_rviz_plugin/icons/classes/StatePanel.png b/visualization/tier4_adapi_rviz_plugin/icons/classes/StatePanel.png deleted file mode 100644 index 6a67573717ae18b6008e00077defb27256415663..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 18815 zcmc#)ig}7!VL~0BI3P>4u>jX{5WQyQGE==@gXiZcsv`yJ3)ShVHnpzyIKV zx%UIl!@PCQ*=Oy&*IwtGaAid)9L!gkAP@*gMjEOL0wKly`$K;YTnXsp8Ulf2BxRtY zY95&f%V-`irs2ZJD-E_>W|QJ#$f(lDGUzn9FhfdJ6ls+f7g13tl1h)8=nGVpD&-eU zEXJbkUMvxiN-bt%sdy?0Ws#;2U}Tww$D9k?tJrv3b90N4w6q7mY>Vu>j)cq`Gq>&g ztxVotd7x=_%0QrfCTU6#h!Tqc9R&LG1r-?ts`)>?Y`h5pRS_lWag8OXawfXJHk9@r zwSCxzbPRm-g9n1hq$!Q)8a9)#!i(M_cz&h|6vnpsyKVD7twGSL0-4KL6Q=BD1ZeP- zzfzYzWVM-q+?d)>KwJl;5;rOeaVP-p8?mb$R~c z!r4STVAB1bJ?W`Q1N^wk6a>;QVI%JxOu}Vf{^G|)Co3MGH{(9RMV<;nHU4Za)%KSC z`;I=`QN*K~)?CjlT%q3CJ0d$G%J_7^;$JI4Ms3f{6oyJDbjK_N!4ju2 z&$?3P$rm$WTureE-aq`r%H%qf&xx}1+^E65XE7*U*@ASHv#;8ex#{Xeuo?I&+x*og zCp;~E10-qE582|U!y4K~`SF#B31Gjpj3?LDSx6hf<#=NRr5x>o;P$^l9osYrEfL{WYjQydU9;g?K5#3k=W*FphTCuCHewl5&`Uu9F)(f>MMOxB>pE z88huBN|GrNq|eH~M(ELpl-tdZkMhs%;OV+^2?6{7^3=m>h4(Sn1AF4SLTeDL;O9g! zBWaM6c%X0DechlP8B_BUQ^1_B5ImG0q$&AGba@Q1!Sx#uv;b)FNlKJ%FF!gu4goAi z+jugMiX+lt3%Db%$x}1fa)yh%A&xxGW-Lle9BaRZcr(Jl8YJvmXVoM8b*d?nDn zr>xqjT|4gjOmgjyb!-I_L=1WSO(};jF5)F9(KK9k$6juv0YKyrQRHz_DK~8jI2)+w zm-Yt$QfvYLhOutzxp@d+SUG4qO=vJ%C2^D=7atD;N#3;^c;DQzmR&&!oPfkcBJT}x-4FY#kT02m@DxoqS-{xT|$ z3PAj4Y06h5S$bR{FjZhmf2ImPCOB3Cm{OLqda0jhqvfLSxmZ+?A2<^h+z64sYjMk*OaM^512bTNz1yJX#3oL;p>F~$69vB)J^K&J0kyCuHxo2qc9?N7 zpx^)Vh!`XhEMW3-8hQ#=jbwx3L_Yk3cspR5f1h~;US}c=&En=n$AFqJRa-Trw&}AF z#AgA`>wx2@JWAg4l9l5}1GY|0Z9CNsXNdgw(z**Vj@S4BOp0I}AA5Z|Zl~>Z3kVbj zqQ+>~?`j3P7S~|HAAK#cX8I@4)zn@Yx%DiDRJ@~c4bTC4ySPXJL&So)v8RA@6#{J%M_y$DCRzdd0*FPR zEH--gPG5+|{w5Yegr~;hMSkzb)vYj91mGV5xcRrf*}fIu`8(Gk(oJHQTMWSTlpe3W z7U|f@97Ozui1Pqn>xtK!34&Gp1$<#-lg>4RGs7K!6%!+v&|>f%Vhw^400u&sGY~q5 zJ^R2hK)+x%+?N_ujGMppd5=D@{V_pS_0HC2*7x*eApoKv>S`$MhIW>f zMXBEn^5P16FfqDGOxdOTX$;a&7%;G-28$P)$1&8Il!2^?K;(-9A|F432ZYVYM^|}I z3>OV1q`?mb9wc7}dJzwJ)_0FSe8o7Cm(><8hzS0{vBcWu7VZ9!xVd$!*%0qZqPEJb=%hJ$Z_L zz~Miu9bQOq9)39Df;+w>C4k8SnA^IR``!NH4|SUf61t|;@Y}Cs7L7w2kT8`ufki#> z(|*WcvwwZbRQK{xx%^U^9it@ui(i`PZ(a!{U!?$lf%er!QwQi?Jq@BTnXsQ0^KiZy z)@H)?s!l`=|Lg4jvo;>IFs7CP;9l8V4UpAo>sjIq#`%v=kuRv~Dh zsiW)pz26t!@MhZgf>?_yj35jsQu16~;-8_l{?rj9%$d(V-9L6Mv^t{|GGZ5-C`W&WqNh29e6d|hsvmvzE$*g zQmyMXe|TD_9?2}u%kdXf_x#(4xd=B4sAmzB{R*=+ZDMv*{0=ZZw`U z1zHnRLzGA%_kVvk=&UnLH`*nK6OLz)IZ+1RaP z$_a6)pzGO}t0_(S2EeN@?u0pWGf^QOT;Fy&fIf+061Vv&k6#eQ9E?CJbN7 zR~KK=eS#kf?q9C)BZqF)@T7l`iF(-E2|R4|?91b}4Ez)Kk2FyYsGq$iY-#*_u@~1} zergZr={EWSme9lc1Lvw@G~X6iq*U zJ>6TJJ1l)Wu8Wk+%I_jO@X>ttg#2Gtvr6H&5?wENhU3wfOD|1HK$EE0JWPIo?o%@w7QX>1z=lRIcDYmm^RdaH!J$U0qdKjR{v?Nj@k8=mLg z!QaffX+{G^hFLx>yzv{!{4I7+*?C}Mw;W;Akw&+^oGNB zKbo1D;RxOCe8*HY+Wq2Z{a&$bq>kL0E*5uqV%bZU97(Rgg}axR>R3#V{3&SGo$cMU zo>il+6}(CD_7>Sn+RG>LD*ZH>q}A^P_|jLN;2-ZSZ)e^-CR7g&UCtF)C)#ml2sjLp zna0G_?3W<_14PCTp7u%y`1jJLdPCb;WW==}ZA1fH$8?tCsR>bJF?vJ$I|J{? zlTYZ~d#P!IC7nc5MI zRsbBkP4l%D7PaBB{&6DKt4yR^NwBqPM*lr^I~U=AH?+LJ*&C$ZAXr^~8P$6rvc|OQ z@5*T^JLt$TP5T0t(=wiG^sXg zEd~AVuZVrio-s!T&LrqYTve>D*P81_=>7;qF3oJb%9-_$ zl7tF5{GK`$Y)Po@y_Df6U|SUSg7!xbv_*4yVe!Q#Ebc;ZcwoSK)rHUB2ho1zr!2(h z%dVk7oadz}SQ849EKz@)p|3b8py_+bZq+KNZ|#0KdDLC_ZhWXy#9XXyyOh3&D_KBd$d5r~dZ&@?gHCJhG5mniAQ@R>y{O zFvmlJ?JS~a-%K~9k;T?h@u&=-btl<6W(Mr?#p_#Nvx_0v`ilrR<$^+%&Dej}o4vNPn* z;owRb6rB0y*XFvkd6OhCQqGB6I?r)a!X@j6i&CJJ1K0a?>M~fR$7l@HLqkBtR{DNi z#>_e~=e6CHkiV3$L3^TB!NYiWD6Xr+?LjRJ218fc9?44c9VrfHEASeRf4u5=;4Pz2 zbmAarlXGC+<}+MNXuEB2%{j_j84W=YGt*J6aB$pdMP%f^yA?e@n6EWm{~fLLbbmH# zB#6dX=5Pv<(zsg%*jkAKL5r0knZM%tivqc1CatP(x82cXPdwWwcmjs-W@q|ImYY99 zYwlE~Y-hze~lk`x3mjPB!27mzk`xg^y;8^_L_ahVv6~)|t~mfqeU``w(TZ zxrTjOPssfK`oySR$6>Am1>m)tK+)!_P=0!6FfqlwTf!xubMJn*u$lh(uwJ8c<*Bgn zDIx;XIk9`S^Zs-&ZN{2BW%h21L^x#Hy{t&3#$u%2EIrNNs9uMGjS4;G4WkVIc79S?9<_)}5sGh^o2GGu01pfp6?z7wr_`sf6M}tS@hT zNCXk5MDOIEc)b#>BL5Byp4r;f?uV$qGqs%c8FpR1U>>e-v`u`iQ65*US#Q)pH|_z0GX?kiG%%(LOU!%Ckq@v`f@JK|pfX2{xiK)~<5soB5N zZ^n6hgiX)Z0q$xxBO+$zshAz%JT;G?R^ui!yaQF=CYolEefA_D418x5EBtuv6S_T# zt>8C5ZK^P2{Tk3jP{74C9i5Z*XB90Em|L^1Lp$Ty?ZVFx%zQ38_!1Hlo_wx*Jxm>5 z;cSTnM62^vnW0! zv(<%*u%dQ#ZEdDm`^UPysNb89-plAwdG@K0)y_jBD-qUQUN%)_5(TsN78%{K)f*=5FXVTI-5jC-M0r)_e2{Uu>Io;LFZ)WTSayCh zH$y%7yq?2_WZ>t-lJJ(ErM5;8)CMsVjU&indTZFQ!V%ji9)yN-+O%Xp_zMN?x2c&~ z$h26bQIZIOY$0|Hl-A{A>HW>D%Vl99Us^*}%6q+6(o_lL&Dq|Mo_&23{zG+CyVGoa zZdPuB-3?-rE;hgTQB^{1j%hnpE81vUZxe{q7+r62)9R zQjuMBfm(#Ms?@4Z$;Ik^iw9oWqRW`6isW|}$3~+7uuRp_uSRo5+uoV%o;%fYOqe&( z|2-;yrIE5RJ-g8wo6hVoIj1wlF#nl)eXp0d4XYB-b^eG|Cv?<)1s7iS_r5%WWTvIf z2b#SW2vI-|e@Dud15=%5ScGn5xI-4JpMXeprsukuG_&3pJ9vw|;{D{iSX$ZsyN-QS zIkazCCe z5tjTIqBD|W;A`f4 z(o2%2vBi}U($ZixbedSg2KMZ`WSTOQSc*AmMXpEHQN>d_Ts3UpJGs9*!n#G=T_Pyr zRXRI_^rJ$49%InZRr>IOdrQ?H&qWVYSv0t<)_MjA=@l6!>}m{uAv-nCbu4CkNL1I0 zszx2HgbeMhEPPFWT{;-iq%i3PmyXoFdjD&`g5k23{NZw;w9<>q|Iu^oZgl-bE6dSw z^mlKsUR{#$i%`XX%3&%n7Tqbvr-=iv3Vn!kYG;%8(aknQHmT914|hK(l9+ATj)ON8U+A7k}qtQoh z(o1n!2r2de-qqvX8LPa^T2)TVwotK{{5PUlaF}h}t$?CyJIDXX_j2AsLij`a@Rsi6 zT9`WmSJ@(5~J3(YDA@J|o3An#c=nA#EWWa`laHX*T$SjobapA&u?(UWP+Q{C%@7Q}QA^ zJNv{`cm#_$-9Pc`0QTcIA;7WaQ;?NPKQ)wA2wg2XAF*`a%1+DL{Ei|8qxs#0>_+qE zIygdQ6ZJGXrnY0=Ug)~OeO>jx(RvJib8E2<`5+hRj<=UQ`XSK&;bJyBjKUK-vv>=H zGCKa(@!Sj)ME<{YC-0A1b(Bqx63NF8LaQ+pwr7++?Y;83W-@6>tv_S?kk#VM|MVs2LY-DySvyPfa--K5g8|I^)y zxR)i5=c%cxpXe{y&Af@{;>*~{b02i&{aB_$54p;>&#@87qjme%iw$;g@1y3hdBV!k zZH`JGW7Tng^Ak~%}#;Q-2Kv@J$5 zPoiIQ%EmkU-JkTeJJ;0SF{mVKykranq(ebDsHb&e)<5FL`@L~x6=ryQdf$-7vCQ0< zVKw1(9v>zMv~Hr4MzS?k%k=W&z-eNFmOLHeG{ZZM>d@B7MqQoA_xwa^NGPaDE9_=G zqs6AdhWYsONzd@+OaTr-8XOBq(wvBuPJ?Raqs7jrj-Q(vB?55luOh~nXxNd`{VH~I zM;8yWZJr7r(}dDk^xNhxwsalXR!sA1N(T6xx8%3 z9i?;He4?%(U&RqXs^zm7y_}z_XZ`rKJWdqi+!lDPbqjFCc03mCyq$|H4*?12d18Xu zsc9-IL6-9k#5yU9z|37?M|bFc*A_tKQa1XE*M2$dVt-b%RtQm5BcL!+`EtBIz{=SB z>EVhR7s6-iy&6K2n(c0w;sbP}fDO(N@z^>W*1LXDq9TP$lSj^&IGFi{KpGIP-++mV zR2tI$TwO=$)6FcJ+Rs%5-rd7wzS5;BWefCwh}pkOiJM7&g&1R6o^Ft!tBsfjK$~LV zchl!;(BZ>dy(Cy;~wVJ*0yFzq6!EcXR5$rY?4gI54mUi2JbS2$QJGSQ5Xyxt;M>AF4Sv@28`bN@} zlqwK4bFtGi`_*QHwN9+3#N)ub$|i36r9T2zdP63$?>Q-i9luzG2$gB(w!3~+Q&2P3 ztg~7hwvPLAu**HR*1BKO`siW+cbQb!0_qL(aGzg2%;p*MKf-C??dsxN<>Sk(?%vrz zzc_PV+EeGDFeZ;i84*ZtW=H3=#2j|W^|ND<(N6Ys^vYL`H`p!um`KLX43QJ|#XvZ& z5VNDI>gv4*nZ&heTY8$rMpSG1;mfQ=IFry0%W*2X;r!dHgLxC(0(fUSoX7 zwNjEXD0t!s_`m5$=5BFW$;8yBAcYLBacVr_p>vrk2KwkD((ynWWJf^m6M0>Rs^j&K zcuG{Bb;k~FsXL?edF#}7S zHZDLmq@^Y_Y4fI*ecn|MwalQipJKC!`&+AEi=Oxumg^tYvS0s6v}&dED>#4Eap zAJNq9yf;-G>9>swxt+fKLmkVyKVy>_Z%8^@94F5{sE|(%yu=RF} zU|f+?>1X;0!Jp9#;p0r9dszlE)NBFz%Xb>g4B!|F;iIe%Rte_9u|W9MNnz3b-6obM zRP{c^sD1ZD1@8DThqDat@NK>7r_aG&(BpD|3!JTXho#kHn#b%IMn={-%208He(I{j z8s5K{sLB<8*d9?YNvdlAkE1Xq4M&^5V`}zED!eajyno=M;-hVhny2RT;+R}Z9r4hs zHg0|z%N0-eaMaHz%=Q`>bdt9ENtBg7zrZD9*0FRwt#|ss^{%z@XNh|0y-*Iz&V$F+ z5b1tlj^6+i1CSkW24b4a6t9GRb-61JetIWG3TakcE8lU{iJ1*WfY zhT3M?F_AEMwx8^2o>J9?B73d2M-Ao0yblIV=w|pGk7+ zChG0Dy~rHOK1Z}(>0iZDOF^?7HV2aYQZklSUv5SgTzl!7E#o4p(rOC24ScRx|^VD=~3JdRynp4w?)W(5A0 z)_C3;5w{#ut;AeK9af+D3$6?#Btje>?{BQL*L-vD2I8Ik7Vg!@QnLe zppa1pgsr!=`wq1_mTHZ}C3xl@K9Vf_gQQRAD_36GdaZ87H=x1+REu&}7i5gjPv`MP z?=NN3^VvFySA4Is?wUDmnv~`eOyF%d=CNUipe$ZOJ}FZgXz3+QW*Yt>xpMko-JL05 zks6_=q<8Zb!SRc%GSdhvd#^TbuH?h6Nef0sA3Wjvs`Q+zGw6AI#HO>wHLBBX;iyJI zFZ5A$b~Ji_KEX=v=l0q7@oKkklDfM5JEnZw<(BE1>)c*>-8TRsJgfKBDNYu#C!HGU zbLvbP6<#?=;4iU0T&Pb*LBnMo{A)%V&E)8oVNDoVfNMGIGqDC~Tscy8s*HW5h(C^+ zG%$nRO&>=ccGarn_c)}SF|TH0#$7Cx8+pN%20+veAhSU?RF1MOcSo&5t!{_L=^j!S zwpKQodunYpRvLi#(OCA4LtJoKXSCHK;j8k9qyr5?e~KQ|t>1+SXe) z>)&;E*Mlsb=gfL7YJkKP1nqI#dXEOw6i8@(OKrTvK3jR+J9~Xn-iY+EwA~bLWlsRR z23NE2O*dU)=1+e<1VW`GQj_6h3MbAnyDHL5QY*f|gl9W++|d2#xN2PS<3Ii9G|AA= zOQyQ5g5I}YLVzr($N9aHm+H&mo*)(pv+|j@cP5Nnz1nAZ%$JsKX2hLXNhUIS$$QnG z1AL<~%s^invTXLbb=gI`FD6^i`_j_7{ay3FG_nui=Feq_SET$RM11j`*GRQ*M5-@2 zd2E<{+liz^%K)8H_K-`#W19`%J018J8$M5t{XUL%67!Kv?T5Y6%HoSPfj9xo@SRNA zO&i%cnz)EbdPRxH)73v%VP9f)rk1ApgoAeZVYB+))9p8Bqd4&bBR~*`oNY>09O~N2 z!Rp>7KnHX=U8>;-697~ZJ_sN<+Irs|Hl_`;G1z694OixT`*V|Cz_YBGLc(tM@Kohm z>JD&MYd3Xf+B|B;lk$xwEPD9_~%=V(vx(C@a6Y-H$#=bfVOn57$&>rkwrVs4#bi zIXh7e(IM6w&y9tw+~I(;y6HV5=yM_(oXxC~r9=|;e>##2d}?n{dgmZtm$kdn2_X2$ zZn1&1c*eQwvMIIRLX^A-fxxJ(8WBPe?||LSuy z{t{iR`A!>-41uqe*4_8|FWHQLq|ffyeq*aJzDYNo!z||t!@KwR;k$ICGOlv)`wp!Q zFe7=&V)-C1g>f{5f5E>X@5^pc*;|x_)H3t0Wj(Hdf}t6f7dj;Lm3y|b61$Fgis3HI zZZTE=iadp@o=Y|={({-`cL3Mp!SitW{cQ0t++xA?d#mq<7jHsy-_FH#^te&BG6e9=TM4K?yE4$gLtJB{QzeetB zryUa6*x1vKi=Ur?^*FjI-@8jse%C$IlbLGM)=y!06b)*c>gqrMkCt)IoHxxwf;h>K zI$ZxUI0Qd&(@HM3+nD1SE<}@A-dyZ1FM9v|iBvCi@E-ah@k7-!4x7-5u`g+?MqO*l zm)P-Zzu$iR6iAlY+w9>D6kT>tx3it8Zf2XNw7OwQn=-dqz_;&l(OUI~)JrvPrtJfn zwVGJjkAIYJ@iHeSyf)eOw~G0ZJld2UGPIIZu3h{4455%Qbz6lZW74FexmYeYM;8aI z$K-h3Y^GFFCuw>-YX)_9?Ig^9A4wx1xQ2#eYVG!-LK}|bv$R8@eqlJgC_fD*}q9KhIE1f_i#O7_ zT)%x`)T#K2OJP6!v6Vb?t~cc+MQq6?;as!glC6mA<(e z<%lFRX`h0@1C67z0NHWh|ZjuVZ|V8t@FP@kIUYdtPp)2I9s3B_BXeBp}-ZP z5|l~AJ-|-~e7adHXKm6rZHQ~nBqb!qRwUhuE_?tsh^Kl=Hlz{jJv5 z%a`iA?S8PdtMf0~>M^6t2;7z3^z?rn(s|R}N+Jo!JO1-*rxQyqqw0F6X>RM!ACLOT zz2%2LmeJ%LT4#%}#EGCHI_{2UWy@EDyhONJs|(RvT?Z?St5 zJe~uJKn9oH8ehO#Qs>h$z{xbsOPOK-DR@vG_GmD%r#DqpIINVFeAg~=yeiV@S@ycUgHP|m(|5vgek*ZEH4RdwK{d$6jL>+4X z`4ChMuWjS9nJF{!ij95ygn<6)v21l;z0gZ@UjZab`9s9KK++(ZU}hS#jlPp@YHsdX+N+j<%}bAm_k9)T8@LAgyH4JzSyw>)>XO5eYZ-f(KZq4CZ8KAE}&p33a#nAw#ZL!zhkYZa0_fy&Jh12UfTAj4W?< z)O2(X-CC!M1+7n}8-n0j-m&cbyUhc#29lDYkyrH{#~wfcNz*@%?SyYHR3jK#qYkg` zfOxt;Xp>!NetI3?-j2$*c6E0peFJ1k0qtRyN~0J}eb0QIcI)6yPMaCaqc?e?=5%L0 z!wWTLgHwtNdLFYf->C5f&&TSGJKKO9egTz|q7H=dL_|bSuP9CLa~>76 znCtPqJkWLkR->ldp7iOcx({hE83#s3x$hnxA+ElF{8rgnRi*qnM5H$vfR4=(~l-hR-WQi)59iT6|TZNDzuRd5R$FCqNF)uguYRz#H!g{_d46@8|;6d)C31g+?wwl9UWUN!_7@iU(y8@`IP%@=plBH%=02yJ&ynGhwto!JlUOX z4ZlnR=UHTX(m73bJ`MY1G35^fepmv;GCh*P+1Gq7e{QeWW1q~jSlc~K9=z&1x!R5Sw+>Pg|4d7B*QMP1eov^xZ{boH28L>~l{Zhb#6){LZ zm@6K(L>wtG?EU!}!s|J9nZo{zoX5(&q`-6B*F~kgn1RWbz(S1+KjtWyba%eqCgn)S zFN@z*^(@_A3M$`75z6`%5E_kQVPm~8vUW(0-)Xpgi@MOw2BBVq0JDHsQp!G~mGIp` zt+g8jO^5Tn$1Dr}bH1cx#Vy}6vXegp0~5m=T_@{(WO?uY<)bNy2_cW;Z}e~9$_)RA z&R+=3N)8DzIPSQK_PD=ZR~&XP^$^GwPJke6@_WV`6v&1R5CO#>3rm*Q23Pe8MP7zS zN4FFUff1Z$-rRg+ zUOSJkl&izPHIn6>?4GZlL7(#}f01PSSIocavf%MkVJQEWW_pySvMFl!k06}^LD&vwY0rFM7Sdo{5 z!^6JOK>wyp&Wqx=>r``x(>dQU>14zTUKY_wnmZX{if`ugWtEtf5;FbfaR!RPMaJd=iiHkN)+{o zw=7_sEj@pbvWiNnS(}3oHAP5B$P(ALz1hl~T>aEY-XZhaZD8r8xs7nX)xEONDCjv; zOUTNmo&)Iqji8`liKzLOKAgKFJ{IZWuJ!WaoUryyAsQa}xqK??8(ZCEx7KBV%c+g} zKb}#TJ3DI#oj_gNy)Lv4->#&&w<8@3lV0qv{JbmRz6E=*>$kXAjlB;|IzAm_X;uCB zv75{DlZUCPX_L_PUzBOWE8nZ-zMA|iD#R`FU!}FRHDgRnO#Q#U-C^A-YHAzgg4T%^ zfT5UUrP9zgG!1;~y?UlKy%*}N2#Hw+^T|{6+|stQd7Vod9abu&XJ-I|3zTY{NPshIIhMP4(e)?%0Uv@SDj|pppr_Jb?SU%^oes09s*d=hLG% ze`5g7Pg|}#S=k6d+Fsg@%%xAxPEJlx7nk#=$7MiN?f*(I2$fs2BE8qna6zlDpw#AvWn%g%t!02^>Yj~m`Jc$>iH%XJ+e zZKMxoZxEm;V0$ox2?Us2H~)s3I>H<)HIzL9C2*B2%2% zuoz$qGG)~AC_jk0_89l)-w}Kjrep=sU*p1KA%bo<^)Jj!7x@I^8msO> z4>V)boi5cC6{?m%cYKi1zpQXO7Sj9MjcR~0ZwA;qj*h|~Zix)e+JeBwbFm_`UgUBj zgZ3CD_k(a;jH;dGQ*p3Bd%O`FzipXyFXu?vx-t zRRt(cFcEnGVV%fzDOH|TTM%e0EhS#yS{w_?&t<&+7NqhCIwh;+O2Q)sTsc~YfC3bU zq)x}416#3Ue7ioXI|D~PEaz~kUWr15fPZx;b=F9Tzwr0+<&|tGXq@ z-!p+0U!6Y(5C2L=UkFUuSvc=t6~zQS+H_VW)Q7oK9zW$`$z@cCY7+mxgHfQ2uQhZ-Bg6F)mmEz|j=0p4I}11k?as_b+mA%#j1I8iXEwQXk2s&yQGt+}5bJPGb zYs;@L`=D=*mVrBcjz(@?+%AG&0^6ZbXHPVwwQtYzgjukZEJ2bVC7?Lfli&RQ%4ql> zZ3Bjstq?yQBdHTP-^B*^9^)d5i0V_-08a5k6bqU|cZ>@XF)H;$15M;U%QGN@6}Z0a%$-dB3&%`n1N7*~lrtIJ9})K!Z&`SKbd?mA;INup)SGh_}|FuM4}K3U^9Fb8+f3I)=dOLQaPRBqE3YvwXIT1%t;9*xKkQh>Kn}+ z;Q<4<$i;f+^O`#E1n{wLZ`^)rpKs3y{DkQ8RNlH1vu<1qB`m4Ar|D>VgG6BuM+f>~Dm_|t=53>L+p^5?Ap9utF zppJ$KnZyZs@R1KmMUXW&i^`seS3FLP#h)k*_YGfPxP+Lf^-x z_sPQj+kVNDcrI(6;l2Q!R|cD$^7tp7bf7;!t@&f~h=LG73c#aHVNd&EsO|JDJ9OOx z4GDygnsW{dr3{vF5xpd#k62wqCZIlHJHXZfU5XlU_yV<>Rk0;H^GfkD(N zC8;^cd!U6lfJ^KX0@2Cm4 zd=8d)ev_63t*c7*w-JRN1{3v+Dme+gIiEr$x928QNy2pGAlduak_Nks{AB%r4d$fH zGhrW1qRbq8d%BIF&*L}E-fw%oD&cP&m%}Rqb%t5mz_v?@_2O7|JOgdKNWi}S@&06z zP{QC}7)~36DkZQSelp`cI3;!}dXmDmA1C4Sk2IJkXca2?Rq1CeU5pr=Z$Eb|MA79r zy57rE7PN*8KqGRYA39>(eT2c|(NGsfr{%mU54|>P>rf=dU$a zVl0M^67U>rcEgp*DN>+P_xfhO3&BA|ja^IR9nZ)3c|`h+cLeVW#xuq+3uv}nG%A6$ z#JM=Sf#pn?8f_ORN$Xqfs`b>*IN!zY+eSWe1i~INHf-G~8xDL(&1R?cv=sOgWv}k5 zvteF@Dw-i34yuxg_Ahk2WZ_t?;YIDiHl87$aT0m1go-04L)r0u>_1x^n=umxKkx@T zXKB)YVqzl*IYMPkmI~jo$xVn-{?=xiJ8`YE6AE&DmVLZJomVQhqu;I!nEhjZSW}A& z9aUJDnK_|~5?EL>2L>l;*|=qJIbzC{IPFmO%Mz2JRMpR3A?xzA_EP8?g@56F6FM$q zslHsKdNUki-|0=HpNsVPE(Hds%j;8TT#?q*MZ~(-<~C%b_A2|X}`fHdwvLeljj~n5@a2L|D4uGt&g#XQX&30uL+(&sRQi{`=)W|ELz*%Z{nK;cEHSxzI zsrA`<4Rz6Z8InRbWPpDMuP7)CHtM(&gT2jMVmcA6EvW4pd-}Qk^8M1f ztkSjGqJ~jOXiyi^+0T{nVA#S8`(S%RprmWLq)G}RWd2UQ3q_lH)oTEP+*uVvg!LsN zp4D^TqM1J4uq;@HeYBs!V3ZI;JIi2~d7$Bqe6W{zYP=5{*J{>)N}3r95vhNZ5{>PP zuI(%1IF+xoU4$(JU!{WLTR0_#*HMh#ml-ls=i#wjLt?A2l@z+gU?w4~g&W3HA^s`4 z4F}UB7IHlO-A+nWfeRdy@gjRdiHelQl{X=Mi%^&FKd0*<@l5nI+3Vc#h{rC$Kmoe- zvpZ3bMn*|-qZj5396y5q;CzC9kn7s`b9To2s502|i^nLOu-LIIk#*-sAS;X19xv|T z{0*ls}liCZmq|f?Z;8zxgGkM?Er0=gxe!}5B)Cfts%zx+KRG~>%xK$^Qwgd zxLTF&cv9bZ<-o^?!iTp33(@-}y0vl_)2I~G)HF}92jl3v=#<)O=E7Rmdy2oglf|Hk z9aQ(|WOUn)IJ?Ev)&Fx6bF6)kaLn)DLr)thofgM!&@~!rT^O7bt@e5Or7|3fA$%9Z zdcgKVh^j?@4Sr`l^A=M|1^Pj?Yjm{L@U2TTmvgF?=HwY>`I&p}76Dqk08?8TUT-A< znh@%m4Kr8Z2eAG|xkt@vbVWTWWo?7J*w`48u>8$6i$5I*42V{tPKX%OlI`)B z()!3ib)k>8#b55^e=w>k^6=B1ZbJ{JnSv5WmCH4hd6f6{<7;|@XRLCWiDm99yGhA{}Wd_r0D9El$(X5~ZD@6Yeq%JJ_WnN-%QoT_^& zls{`|J%9EnfAEk~Gyx};)G-ocS*iS3?;B@s#A40HNR)|D9E{SnkxmA^FH@JMsvova zX8&ayT=lAu;Am_z@Z2<%$ z6{BjEfS{w}t2VXwOJOAH%tlL96JNh1#=Wf*>*+C=wXQ*vNIs(oaU1*ElJg~AWBoz3 zYsEM6%XU&tu#eu|7g)PJi(C|&tXR}Xn&hnaxd#gx>+j>0Lv??-YpZnq{-}DCX85D* z_dAPe6Sn6vuwUIAo->ylZ}KI6nQVFL;mT@J6n=}jh-Pjuf%pEv++Pdkdj*Zwp7DBP zrhTtVw6{gZP)&6$66l-|HJ4w8%;eg~^EPs4nrqIew2C|SrN%S*vk{KJr_g;`Y{qKK za@>)WYnnH1$Y7g~H208STe0$q*DizuUr(^Zw@=B+o)Nq~aS$e4yGfT@>daH^DKIsz z)cwow6@MRvpi+T4eltCU8Nd3QQI$>-+ zFgW?Ai;?m(M6`3~3t0MUNoj|7dB1(@$eb{5jiAiUXxRcsX%N7syb8a-?5DbXQY&dt z%QtBuucYrm*1Dmz5?dn{A0>WU5x4YjETQ|M(8cAmSNvkb`2Ku7q`f}>zYyO<$8g@x zUzs*Gk#M%Gr7AVQ9ITO)uwuolOW^S4&Y0P4a>%SIVE2Gjj|O|=mQu{^v#Iu?g_u(5R=+V-wf&8^chyn4kgY80C{bYAldhLKCoo`h9f)w_=Um8!5STwbwF{_U3Tizp{AyHC84h8bs|GATle zy3F*$Rqlvr6MURu^<;ctoJ-C-Z#8oO_$tvow?$;EY1DOi-(^S-2TIzlGbRuwb&;-U z7yZw0dus6-cg1D}!{`%wA1C;jA;~pKJ=T-XVnJJKHRncn?@V?ZaEO-)De!(_{cb?PQZM`vF>be}<$+yB>x!n%iMG>Ka&r0;UkQJwE7Mk z$`Z}Z4DInTXYHvlPzHtdM7q)!O@~4(CDCwL6=&P!6H8`ZIu8IG){ebnFOOsgJi5#J zo*23$q%3OfclD3VJx10L$%S6mR#0O{Z-fm;yH{W~(%~1?#@9nDw|X-FV4WFNNE?u= zNsUpZB9>X39R6ko>B{E@palselu!fi@;2{*99+kw!vd7==a0k1U~mtD?VxZfC+5?F zS*Nm{8|Z%mvGF+lu?k#)zW_q@WO_h`!~-H-wk-C(55!&CS9;0~tCO{qo!|0Ld`A4R z!?{Ezel$sA7!Q>QFASC6a)j3Q9elimCP?JOd_Bc7+iW%D38BRl_}=XI4MruG!$~Y! z)#3G0Ing0FLqmnq)N|04xKy0c__kp$%3zYQ=jbnv1!pZF??**9r5!AA6QQz8HaBac z-)DvlmLg^;&QlPoDNzVPj2csI+Sp_GZ>Y~5tTw?OOsB`~G>Igi6#!z}vZxRn z_mZ3%^Wj5E=1-Gbo9g3YL(lz--KY}z4P9;Rev`lWf1j9yQ{VAp+Sf5=7Hy=={c`2yJadWY-BAQd=Lk{ z_R5BjK&)=KT9B)gLV{1>rb2!28ThgAIW#%qO5B*yF`N0*A1srvHd0v*yWIk#^?)Ts zM5jSs*i*xFz2D7|(i500ZVx1J5^<9kp}e$hGHAC2LutRtBH%ytYf07!h%N|p$O_p@ zM76*Ze2pGv`4>e77PC@W+j}V~VvZ$pJA7_u0J6SUt6#Bso7y)~k6x^LN|Y_7g&3t~ zc23kDyJ8C;xg3Hmb~#>n;^|83;U1180$1Uh7VPe5U0Yj3=xI`|HIDic);1 z%Zx)#tB~cm_w^{1mrymqhd`i6ceMi`P*4dL1i~7qi~&@l{y)Y0VJg~EDlzW{YoEjk QxT*u><#`b=z=h}h2b9~@i2wiq diff --git a/visualization/tier4_adapi_rviz_plugin/plugins/plugin_description.xml b/visualization/tier4_adapi_rviz_plugin/plugins/plugin_description.xml index 0b4f00bd56278..5eaa5ec658be1 100644 --- a/visualization/tier4_adapi_rviz_plugin/plugins/plugin_description.xml +++ b/visualization/tier4_adapi_rviz_plugin/plugins/plugin_description.xml @@ -8,8 +8,8 @@ RoutePanel - - StatePanel + + OperationModeDebugPanel diff --git a/visualization/tier4_adapi_rviz_plugin/src/operation_mode_debug_panel.cpp b/visualization/tier4_adapi_rviz_plugin/src/operation_mode_debug_panel.cpp new file mode 100644 index 0000000000000..8476039b3d2a4 --- /dev/null +++ b/visualization/tier4_adapi_rviz_plugin/src/operation_mode_debug_panel.cpp @@ -0,0 +1,221 @@ +// +// Copyright 2020 TIER IV, Inc. All rights reserved. +// +// 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 +// +// http://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 "operation_mode_debug_panel.hpp" + +#include +#include +#include + +#include +#include + +namespace tier4_adapi_rviz_plugins +{ + +OperationModeDebugPanel::OperationModeDebugPanel(QWidget * parent) : rviz_common::Panel(parent) +{ + auto * layout = new QGridLayout; + setLayout(layout); + + operation_mode_label_ptr_ = new QLabel("INIT"); + operation_mode_label_ptr_->setAlignment(Qt::AlignCenter); + operation_mode_label_ptr_->setStyleSheet("border:1px solid black;"); + layout->addWidget(operation_mode_label_ptr_, 0, 0, 2, 1); + + auto_button_ptr_ = new QPushButton("AUTO"); + connect(auto_button_ptr_, SIGNAL(clicked()), SLOT(onClickAutonomous())); + layout->addWidget(auto_button_ptr_, 0, 1); + + stop_button_ptr_ = new QPushButton("STOP"); + connect(stop_button_ptr_, SIGNAL(clicked()), SLOT(onClickStop())); + layout->addWidget(stop_button_ptr_, 0, 2); + + local_button_ptr_ = new QPushButton("LOCAL"); + connect(local_button_ptr_, SIGNAL(clicked()), SLOT(onClickLocal())); + layout->addWidget(local_button_ptr_, 1, 1); + + remote_button_ptr_ = new QPushButton("REMOTE"); + connect(remote_button_ptr_, SIGNAL(clicked()), SLOT(onClickRemote())); + layout->addWidget(remote_button_ptr_, 1, 2); + + control_mode_label_ptr_ = new QLabel("INIT"); + control_mode_label_ptr_->setAlignment(Qt::AlignCenter); + control_mode_label_ptr_->setStyleSheet("border:1px solid black;"); + layout->addWidget(control_mode_label_ptr_, 2, 0); + + enable_button_ptr_ = new QPushButton("Enable"); + connect(enable_button_ptr_, SIGNAL(clicked()), SLOT(onClickAutowareControl())); + layout->addWidget(enable_button_ptr_, 2, 1); + + disable_button_ptr_ = new QPushButton("Disable"); + connect(disable_button_ptr_, SIGNAL(clicked()), SLOT(onClickDirectControl())); + layout->addWidget(disable_button_ptr_, 2, 2); +} + +void OperationModeDebugPanel::onInitialize() +{ + raw_node_ = this->getDisplayContext()->getRosNodeAbstraction().lock()->get_raw_node(); + + sub_operation_mode_ = raw_node_->create_subscription( + "/api/operation_mode/state", rclcpp::QoS{1}.transient_local(), + std::bind(&OperationModeDebugPanel::onOperationMode, this, std::placeholders::_1)); + + client_change_to_autonomous_ = + raw_node_->create_client("/api/operation_mode/change_to_autonomous"); + + client_change_to_stop_ = + raw_node_->create_client("/api/operation_mode/change_to_stop"); + + client_change_to_local_ = + raw_node_->create_client("/api/operation_mode/change_to_local"); + + client_change_to_remote_ = + raw_node_->create_client("/api/operation_mode/change_to_remote"); + + client_enable_autoware_control_ = + raw_node_->create_client("/api/operation_mode/enable_autoware_control"); + + client_enable_direct_control_ = + raw_node_->create_client("/api/operation_mode/disable_autoware_control"); +} + +template +void callService(const rclcpp::Logger & logger, const typename rclcpp::Client::SharedPtr client) +{ + auto req = std::make_shared(); + + RCLCPP_DEBUG(logger, "client request"); + + if (!client->service_is_ready()) { + RCLCPP_DEBUG(logger, "client is unavailable"); + return; + } + + client->async_send_request(req, [logger](typename rclcpp::Client::SharedFuture result) { + RCLCPP_DEBUG( + logger, "Status: %d, %s", result.get()->status.code, result.get()->status.message.c_str()); + }); +} + +void OperationModeDebugPanel::onClickAutonomous() +{ + callService(raw_node_->get_logger(), client_change_to_autonomous_); +} + +void OperationModeDebugPanel::onClickStop() +{ + callService(raw_node_->get_logger(), client_change_to_stop_); +} + +void OperationModeDebugPanel::onClickLocal() +{ + callService(raw_node_->get_logger(), client_change_to_local_); +} + +void OperationModeDebugPanel::onClickRemote() +{ + callService(raw_node_->get_logger(), client_change_to_remote_); +} + +void OperationModeDebugPanel::onClickAutowareControl() +{ + callService(raw_node_->get_logger(), client_enable_autoware_control_); +} + +void OperationModeDebugPanel::onClickDirectControl() +{ + callService(raw_node_->get_logger(), client_enable_direct_control_); +} + +void OperationModeDebugPanel::onOperationMode(const OperationModeState::ConstSharedPtr msg) +{ + const auto updateLabel = [](QLabel * label, QString text, QString style) { + label->setText(text); + label->setStyleSheet(style); + }; + + const auto updateButton = [](QPushButton * button, bool available) { + if (available) { + button->setStyleSheet("color: black;"); + } else { + button->setStyleSheet("color: white;"); + } + }; + + // Update current operation mode. + + QString state = ""; + QString style = ""; + + switch (msg->mode) { + case OperationModeState::AUTONOMOUS: + state = "AUTONOMOUS"; + style = "background-color: #00FF00;"; // green + break; + + case OperationModeState::LOCAL: + state = "LOCAL"; + style = "background-color: #FFFF00;"; // yellow + break; + + case OperationModeState::REMOTE: + state = "REMOTE"; + style = "background-color: #FFFF00;"; // yellow + break; + + case OperationModeState::STOP: + state = "STOP"; + style = "background-color: #FFA500;"; // orange + break; + + default: + state = "UNKNOWN (" + QString::number(msg->mode) + ")"; + style = "background-color: #FF0000;"; // red + break; + } + + if (msg->is_in_transition) { + state += "\n(TRANSITION)"; + } + + updateLabel(operation_mode_label_ptr_, state, style); + + // Update current control mode. + + if (msg->is_autoware_control_enabled) { + updateLabel(control_mode_label_ptr_, "Enable", "background-color: #00FF00;"); // green + } else { + updateLabel(control_mode_label_ptr_, "Disable", "background-color: #FFFF00;"); // yellow + } + + // Update operation mode available. + + updateButton(auto_button_ptr_, msg->is_autonomous_mode_available); + updateButton(stop_button_ptr_, msg->is_stop_mode_available); + updateButton(local_button_ptr_, msg->is_local_mode_available); + updateButton(remote_button_ptr_, msg->is_remote_mode_available); + + // Update control mode available. + + updateButton(enable_button_ptr_, true); + updateButton(disable_button_ptr_, true); +} + +} // namespace tier4_adapi_rviz_plugins + +#include +PLUGINLIB_EXPORT_CLASS(tier4_adapi_rviz_plugins::OperationModeDebugPanel, rviz_common::Panel) diff --git a/visualization/tier4_adapi_rviz_plugin/src/operation_mode_debug_panel.hpp b/visualization/tier4_adapi_rviz_plugin/src/operation_mode_debug_panel.hpp new file mode 100644 index 0000000000000..52eeb8da74493 --- /dev/null +++ b/visualization/tier4_adapi_rviz_plugin/src/operation_mode_debug_panel.hpp @@ -0,0 +1,76 @@ +// +// Copyright 2020 TIER IV, Inc. All rights reserved. +// +// 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 +// +// http://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 OPERATION_MODE_DEBUG_PANEL_HPP_ +#define OPERATION_MODE_DEBUG_PANEL_HPP_ + +#include +#include +#include +#include + +#include +#include + +namespace tier4_adapi_rviz_plugins +{ + +class OperationModeDebugPanel : public rviz_common::Panel +{ + Q_OBJECT + +public: + explicit OperationModeDebugPanel(QWidget * parent = nullptr); + void onInitialize() override; + +public Q_SLOTS: // NOLINT for Qt + void onClickAutonomous(); + void onClickStop(); + void onClickLocal(); + void onClickRemote(); + void onClickAutowareControl(); + void onClickDirectControl(); + +protected: + using OperationModeState = autoware_adapi_v1_msgs::msg::OperationModeState; + using ChangeOperationMode = autoware_adapi_v1_msgs::srv::ChangeOperationMode; + + QLabel * operation_mode_label_ptr_{nullptr}; + QPushButton * stop_button_ptr_{nullptr}; + QPushButton * auto_button_ptr_{nullptr}; + QPushButton * local_button_ptr_{nullptr}; + QPushButton * remote_button_ptr_{nullptr}; + + QLabel * control_mode_label_ptr_{nullptr}; + QPushButton * enable_button_ptr_{nullptr}; + QPushButton * disable_button_ptr_{nullptr}; + + rclcpp::Node::SharedPtr raw_node_; + rclcpp::Subscription::SharedPtr sub_operation_mode_; + rclcpp::Client::SharedPtr client_change_to_autonomous_; + rclcpp::Client::SharedPtr client_change_to_stop_; + rclcpp::Client::SharedPtr client_change_to_local_; + rclcpp::Client::SharedPtr client_change_to_remote_; + rclcpp::Client::SharedPtr client_enable_autoware_control_; + rclcpp::Client::SharedPtr client_enable_direct_control_; + + void onOperationMode(const OperationModeState::ConstSharedPtr msg); + void changeOperationMode(const rclcpp::Client::SharedPtr client); +}; + +} // namespace tier4_adapi_rviz_plugins + +#endif // OPERATION_MODE_DEBUG_PANEL_HPP_ diff --git a/visualization/tier4_adapi_rviz_plugin/src/state_panel.cpp b/visualization/tier4_adapi_rviz_plugin/src/state_panel.cpp deleted file mode 100644 index 0525f5fdc1a09..0000000000000 --- a/visualization/tier4_adapi_rviz_plugin/src/state_panel.cpp +++ /dev/null @@ -1,633 +0,0 @@ -// -// Copyright 2020 TIER IV, Inc. All rights reserved. -// -// 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 -// -// http://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 "state_panel.hpp" - -#include -#include -#include -#include -#include -#include - -#include -#include - -namespace tier4_adapi_rviz_plugins -{ - -StatePanel::StatePanel(QWidget * parent) : rviz_common::Panel(parent) -{ - // Gear - auto * gear_prefix_label_ptr = new QLabel("GEAR: "); - gear_prefix_label_ptr->setAlignment(Qt::AlignRight | Qt::AlignVCenter); - gear_label_ptr_ = new QLabel("INIT"); - gear_label_ptr_->setAlignment(Qt::AlignCenter); - auto * gear_layout = new QHBoxLayout; - gear_layout->addWidget(gear_prefix_label_ptr); - gear_layout->addWidget(gear_label_ptr_); - - // Velocity Limit - velocity_limit_button_ptr_ = new QPushButton("Send Velocity Limit"); - pub_velocity_limit_input_ = new QSpinBox(); - pub_velocity_limit_input_->setRange(-100.0, 100.0); - pub_velocity_limit_input_->setValue(0.0); - pub_velocity_limit_input_->setSingleStep(5.0); - connect(velocity_limit_button_ptr_, SIGNAL(clicked()), this, SLOT(onClickVelocityLimit())); - - // Emergency Button - emergency_button_ptr_ = new QPushButton("Set Emergency"); - connect(emergency_button_ptr_, SIGNAL(clicked()), this, SLOT(onClickEmergencyButton())); - - // Layout - auto * v_layout = new QVBoxLayout; - auto * velocity_limit_layout = new QHBoxLayout(); - v_layout->addWidget(makeOperationModeGroup()); - v_layout->addWidget(makeControlModeGroup()); - { - auto * h_layout = new QHBoxLayout(); - h_layout->addWidget(makeRoutingGroup()); - h_layout->addWidget(makeLocalizationGroup()); - h_layout->addWidget(makeMotionGroup()); - h_layout->addWidget(makeFailSafeGroup()); - v_layout->addLayout(h_layout); - } - - v_layout->addLayout(gear_layout); - velocity_limit_layout->addWidget(velocity_limit_button_ptr_); - velocity_limit_layout->addWidget(pub_velocity_limit_input_); - velocity_limit_layout->addWidget(new QLabel(" [km/h]")); - velocity_limit_layout->addWidget(emergency_button_ptr_); - v_layout->addLayout(velocity_limit_layout); - setLayout(v_layout); -} - -QGroupBox * StatePanel::makeOperationModeGroup() -{ - auto * group = new QGroupBox("OperationMode"); - auto * grid = new QGridLayout; - - operation_mode_label_ptr_ = new QLabel("INIT"); - operation_mode_label_ptr_->setAlignment(Qt::AlignCenter); - operation_mode_label_ptr_->setStyleSheet("border:1px solid black;"); - grid->addWidget(operation_mode_label_ptr_, 0, 0, 0, 1); - - auto_button_ptr_ = new QPushButton("AUTO"); - auto_button_ptr_->setCheckable(true); - connect(auto_button_ptr_, SIGNAL(clicked()), SLOT(onClickAutonomous())); - grid->addWidget(auto_button_ptr_, 0, 1); - - stop_button_ptr_ = new QPushButton("STOP"); - stop_button_ptr_->setCheckable(true); - connect(stop_button_ptr_, SIGNAL(clicked()), SLOT(onClickStop())); - grid->addWidget(stop_button_ptr_, 0, 2); - - local_button_ptr_ = new QPushButton("LOCAL"); - local_button_ptr_->setCheckable(true); - connect(local_button_ptr_, SIGNAL(clicked()), SLOT(onClickLocal())); - grid->addWidget(local_button_ptr_, 1, 1); - - remote_button_ptr_ = new QPushButton("REMOTE"); - remote_button_ptr_->setCheckable(true); - connect(remote_button_ptr_, SIGNAL(clicked()), SLOT(onClickRemote())); - grid->addWidget(remote_button_ptr_, 1, 2); - - group->setLayout(grid); - return group; -} - -QGroupBox * StatePanel::makeControlModeGroup() -{ - auto * group = new QGroupBox("AutowareControl"); - auto * grid = new QGridLayout; - - control_mode_label_ptr_ = new QLabel("INIT"); - control_mode_label_ptr_->setAlignment(Qt::AlignCenter); - control_mode_label_ptr_->setStyleSheet("border:1px solid black;"); - grid->addWidget(control_mode_label_ptr_, 0, 0); - - enable_button_ptr_ = new QPushButton("Enable"); - enable_button_ptr_->setCheckable(true); - connect(enable_button_ptr_, SIGNAL(clicked()), SLOT(onClickAutowareControl())); - grid->addWidget(enable_button_ptr_, 0, 1); - - disable_button_ptr_ = new QPushButton("Disable"); - disable_button_ptr_->setCheckable(true); - connect(disable_button_ptr_, SIGNAL(clicked()), SLOT(onClickDirectControl())); - grid->addWidget(disable_button_ptr_, 0, 2); - - group->setLayout(grid); - return group; -} - -QGroupBox * StatePanel::makeRoutingGroup() -{ - auto * group = new QGroupBox("Routing"); - auto * grid = new QGridLayout; - - routing_label_ptr_ = new QLabel("INIT"); - routing_label_ptr_->setAlignment(Qt::AlignCenter); - routing_label_ptr_->setStyleSheet("border:1px solid black;"); - grid->addWidget(routing_label_ptr_, 0, 0); - - clear_route_button_ptr_ = new QPushButton("Clear Route"); - clear_route_button_ptr_->setCheckable(true); - connect(clear_route_button_ptr_, SIGNAL(clicked()), SLOT(onClickClearRoute())); - grid->addWidget(clear_route_button_ptr_, 1, 0); - - group->setLayout(grid); - return group; -} - -QGroupBox * StatePanel::makeLocalizationGroup() -{ - auto * group = new QGroupBox("Localization"); - auto * grid = new QGridLayout; - - localization_label_ptr_ = new QLabel("INIT"); - localization_label_ptr_->setAlignment(Qt::AlignCenter); - localization_label_ptr_->setStyleSheet("border:1px solid black;"); - grid->addWidget(localization_label_ptr_, 0, 0); - - init_by_gnss_button_ptr_ = new QPushButton("Init by GNSS"); - connect(init_by_gnss_button_ptr_, SIGNAL(clicked()), SLOT(onClickInitByGnss())); - grid->addWidget(init_by_gnss_button_ptr_, 1, 0); - - group->setLayout(grid); - return group; -} - -QGroupBox * StatePanel::makeMotionGroup() -{ - auto * group = new QGroupBox("Motion"); - auto * grid = new QGridLayout; - - motion_label_ptr_ = new QLabel("INIT"); - motion_label_ptr_->setAlignment(Qt::AlignCenter); - motion_label_ptr_->setStyleSheet("border:1px solid black;"); - grid->addWidget(motion_label_ptr_, 0, 0); - - accept_start_button_ptr_ = new QPushButton("Accept Start"); - accept_start_button_ptr_->setCheckable(true); - connect(accept_start_button_ptr_, SIGNAL(clicked()), SLOT(onClickAcceptStart())); - grid->addWidget(accept_start_button_ptr_, 1, 0); - - group->setLayout(grid); - return group; -} - -QGroupBox * StatePanel::makeFailSafeGroup() -{ - auto * group = new QGroupBox("FailSafe"); - auto * grid = new QGridLayout; - - mrm_state_label_ptr_ = new QLabel("INIT"); - mrm_state_label_ptr_->setAlignment(Qt::AlignCenter); - mrm_state_label_ptr_->setStyleSheet("border:1px solid black;"); - grid->addWidget(mrm_state_label_ptr_, 0, 0); - - mrm_behavior_label_ptr_ = new QLabel("INIT"); - mrm_behavior_label_ptr_->setAlignment(Qt::AlignCenter); - mrm_behavior_label_ptr_->setStyleSheet("border:1px solid black;"); - grid->addWidget(mrm_behavior_label_ptr_, 1, 0); - - group->setLayout(grid); - return group; -} - -void StatePanel::onInitialize() -{ - using std::placeholders::_1; - - raw_node_ = this->getDisplayContext()->getRosNodeAbstraction().lock()->get_raw_node(); - - // Operation Mode - sub_operation_mode_ = raw_node_->create_subscription( - "/api/operation_mode/state", rclcpp::QoS{1}.transient_local(), - std::bind(&StatePanel::onOperationMode, this, _1)); - - client_change_to_autonomous_ = - raw_node_->create_client("/api/operation_mode/change_to_autonomous"); - - client_change_to_stop_ = - raw_node_->create_client("/api/operation_mode/change_to_stop"); - - client_change_to_local_ = - raw_node_->create_client("/api/operation_mode/change_to_local"); - - client_change_to_remote_ = - raw_node_->create_client("/api/operation_mode/change_to_remote"); - - client_enable_autoware_control_ = - raw_node_->create_client("/api/operation_mode/enable_autoware_control"); - - client_enable_direct_control_ = - raw_node_->create_client("/api/operation_mode/disable_autoware_control"); - - // Routing - sub_route_ = raw_node_->create_subscription( - "/api/routing/state", rclcpp::QoS{1}.transient_local(), - std::bind(&StatePanel::onRoute, this, _1)); - - client_clear_route_ = raw_node_->create_client("/api/routing/clear_route"); - - // Localization - sub_localization_ = raw_node_->create_subscription( - "/api/localization/initialization_state", rclcpp::QoS{1}.transient_local(), - std::bind(&StatePanel::onLocalization, this, _1)); - - client_init_by_gnss_ = - raw_node_->create_client("/api/localization/initialize"); - - // Motion - sub_motion_ = raw_node_->create_subscription( - "/api/motion/state", rclcpp::QoS{1}.transient_local(), - std::bind(&StatePanel::onMotion, this, _1)); - - client_accept_start_ = raw_node_->create_client("/api/motion/accept_start"); - - // FailSafe - sub_mrm_ = raw_node_->create_subscription( - "/api/fail_safe/mrm_state", rclcpp::QoS{1}.transient_local(), - std::bind(&StatePanel::onMRMState, this, _1)); - - // Others - sub_gear_ = raw_node_->create_subscription( - "/vehicle/status/gear_status", 10, std::bind(&StatePanel::onShift, this, _1)); - - sub_emergency_ = raw_node_->create_subscription( - "/api/autoware/get/emergency", 10, std::bind(&StatePanel::onEmergencyStatus, this, _1)); - - client_emergency_stop_ = raw_node_->create_client( - "/api/autoware/set/emergency"); - - pub_velocity_limit_ = raw_node_->create_publisher( - "/planning/scenario_planning/max_velocity_default", rclcpp::QoS{1}.transient_local()); -} - -void StatePanel::onOperationMode(const OperationModeState::ConstSharedPtr msg) -{ - auto changeButtonState = []( - QPushButton * button, const bool is_desired_mode_available, - const uint8_t current_mode = OperationModeState::UNKNOWN, - const uint8_t desired_mode = OperationModeState::STOP) { - if (is_desired_mode_available && current_mode != desired_mode) { - activateButton(button); - } else { - deactivateButton(button); - } - }; - - QString text = ""; - QString style_sheet = ""; - // Operation Mode - switch (msg->mode) { - case OperationModeState::AUTONOMOUS: - text = "AUTONOMOUS"; - style_sheet = "background-color: #00FF00;"; // green - break; - - case OperationModeState::LOCAL: - text = "LOCAL"; - style_sheet = "background-color: #FFFF00;"; // yellow - break; - - case OperationModeState::REMOTE: - text = "REMOTE"; - style_sheet = "background-color: #FFFF00;"; // yellow - break; - - case OperationModeState::STOP: - text = "STOP"; - style_sheet = "background-color: #FFA500;"; // orange - break; - - default: - text = "UNKNOWN"; - style_sheet = "background-color: #FF0000;"; // red - break; - } - - if (msg->is_in_transition) { - text += "\n(TRANSITION)"; - } - - updateLabel(operation_mode_label_ptr_, text, style_sheet); - - // Control Mode - if (msg->is_autoware_control_enabled) { - updateLabel(control_mode_label_ptr_, "Enable", "background-color: #00FF00;"); // green - } else { - updateLabel(control_mode_label_ptr_, "Disable", "background-color: #FFFF00;"); // yellow - } - - // Button - changeButtonState( - auto_button_ptr_, msg->is_autonomous_mode_available, msg->mode, OperationModeState::AUTONOMOUS); - changeButtonState( - stop_button_ptr_, msg->is_stop_mode_available, msg->mode, OperationModeState::STOP); - changeButtonState( - local_button_ptr_, msg->is_local_mode_available, msg->mode, OperationModeState::LOCAL); - changeButtonState( - remote_button_ptr_, msg->is_remote_mode_available, msg->mode, OperationModeState::REMOTE); - - changeButtonState(enable_button_ptr_, !msg->is_autoware_control_enabled); - changeButtonState(disable_button_ptr_, msg->is_autoware_control_enabled); -} - -void StatePanel::onRoute(const RouteState::ConstSharedPtr msg) -{ - QString text = ""; - QString style_sheet = ""; - switch (msg->state) { - case RouteState::UNSET: - text = "UNSET"; - style_sheet = "background-color: #FFFF00;"; // yellow - break; - - case RouteState::SET: - text = "SET"; - style_sheet = "background-color: #00FF00;"; // green - break; - - case RouteState::ARRIVED: - text = "ARRIVED"; - style_sheet = "background-color: #FFA500;"; // orange - break; - - case RouteState::CHANGING: - text = "CHANGING"; - style_sheet = "background-color: #FFFF00;"; // yellow - break; - - default: - text = "UNKNOWN"; - style_sheet = "background-color: #FF0000;"; // red - break; - } - - updateLabel(routing_label_ptr_, text, style_sheet); - - if (msg->state == RouteState::SET) { - activateButton(clear_route_button_ptr_); - } else { - deactivateButton(clear_route_button_ptr_); - } -} - -void StatePanel::onLocalization(const LocalizationInitializationState::ConstSharedPtr msg) -{ - QString text = ""; - QString style_sheet = ""; - switch (msg->state) { - case LocalizationInitializationState::UNINITIALIZED: - text = "UNINITIALIZED"; - style_sheet = "background-color: #FFFF00;"; // yellow - break; - - case LocalizationInitializationState::INITIALIZING: - text = "INITIALIZING"; - style_sheet = "background-color: #FFA500;"; // orange - break; - - case LocalizationInitializationState::INITIALIZED: - text = "INITIALIZED"; - style_sheet = "background-color: #00FF00;"; // green - break; - - default: - text = "UNKNOWN"; - style_sheet = "background-color: #FF0000;"; // red - break; - } - - updateLabel(localization_label_ptr_, text, style_sheet); -} - -void StatePanel::onMotion(const MotionState::ConstSharedPtr msg) -{ - QString text = ""; - QString style_sheet = ""; - switch (msg->state) { - case MotionState::STARTING: - text = "STARTING"; - style_sheet = "background-color: #FFFF00;"; // yellow - break; - - case MotionState::STOPPED: - text = "STOPPED"; - style_sheet = "background-color: #FFA500;"; // orange - break; - - case MotionState::MOVING: - text = "MOVING"; - style_sheet = "background-color: #00FF00;"; // green - break; - - default: - text = "UNKNOWN"; - style_sheet = "background-color: #FF0000;"; // red - break; - } - - updateLabel(motion_label_ptr_, text, style_sheet); - - if (msg->state == MotionState::STARTING) { - activateButton(accept_start_button_ptr_); - } else { - deactivateButton(accept_start_button_ptr_); - } -} - -void StatePanel::onMRMState(const MRMState::ConstSharedPtr msg) -{ - // state - { - QString text = ""; - QString style_sheet = ""; - switch (msg->state) { - case MRMState::NONE: - text = "NONE"; - style_sheet = "background-color: #00FF00;"; // green - break; - - case MRMState::MRM_OPERATING: - text = "MRM_OPERATING"; - style_sheet = "background-color: #FFA500;"; // orange - break; - - case MRMState::MRM_SUCCEEDED: - text = "MRM_SUCCEEDED"; - style_sheet = "background-color: #FFFF00;"; // yellow - break; - - case MRMState::MRM_FAILED: - text = "MRM_FAILED"; - style_sheet = "background-color: #FF0000;"; // red - break; - - default: - text = "UNKNOWN"; - style_sheet = "background-color: #FF0000;"; // red - break; - } - - updateLabel(mrm_state_label_ptr_, text, style_sheet); - } - - // behavior - { - QString text = ""; - QString style_sheet = ""; - switch (msg->behavior) { - case MRMState::NONE: - text = "NONE"; - style_sheet = "background-color: #00FF00;"; // green - break; - - case MRMState::PULL_OVER: - text = "PULL_OVER"; - style_sheet = "background-color: #FFFF00;"; // yellow - break; - - case MRMState::COMFORTABLE_STOP: - text = "COMFORTABLE_STOP"; - style_sheet = "background-color: #FFFF00;"; // yellow - break; - - case MRMState::EMERGENCY_STOP: - text = "EMERGENCY_STOP"; - style_sheet = "background-color: #FFA500;"; // orange - break; - - default: - text = "UNKNOWN"; - style_sheet = "background-color: #FF0000;"; // red - break; - } - - updateLabel(mrm_behavior_label_ptr_, text, style_sheet); - } -} - -void StatePanel::onShift(const autoware_vehicle_msgs::msg::GearReport::ConstSharedPtr msg) -{ - switch (msg->report) { - case autoware_vehicle_msgs::msg::GearReport::PARK: - gear_label_ptr_->setText("PARKING"); - break; - case autoware_vehicle_msgs::msg::GearReport::REVERSE: - gear_label_ptr_->setText("REVERSE"); - break; - case autoware_vehicle_msgs::msg::GearReport::DRIVE: - gear_label_ptr_->setText("DRIVE"); - break; - case autoware_vehicle_msgs::msg::GearReport::NEUTRAL: - gear_label_ptr_->setText("NEUTRAL"); - break; - case autoware_vehicle_msgs::msg::GearReport::LOW: - gear_label_ptr_->setText("LOW"); - break; - } -} - -void StatePanel::onEmergencyStatus( - const tier4_external_api_msgs::msg::Emergency::ConstSharedPtr msg) -{ - current_emergency_ = msg->emergency; - if (msg->emergency) { - emergency_button_ptr_->setText(QString::fromStdString("Clear Emergency")); - emergency_button_ptr_->setStyleSheet("background-color: #FF0000;"); - } else { - emergency_button_ptr_->setText(QString::fromStdString("Set Emergency")); - emergency_button_ptr_->setStyleSheet("background-color: #00FF00;"); - } -} - -void StatePanel::onClickVelocityLimit() -{ - auto velocity_limit = std::make_shared(); - velocity_limit->stamp = raw_node_->now(); - velocity_limit->max_velocity = pub_velocity_limit_input_->value() / 3.6; - pub_velocity_limit_->publish(*velocity_limit); -} - -void StatePanel::onClickAutonomous() -{ - callServiceWithoutResponse(client_change_to_autonomous_); -} -void StatePanel::onClickStop() -{ - callServiceWithoutResponse(client_change_to_stop_); -} -void StatePanel::onClickLocal() -{ - callServiceWithoutResponse(client_change_to_local_); -} -void StatePanel::onClickRemote() -{ - callServiceWithoutResponse(client_change_to_remote_); -} -void StatePanel::onClickAutowareControl() -{ - callServiceWithoutResponse(client_enable_autoware_control_); -} -void StatePanel::onClickDirectControl() -{ - callServiceWithoutResponse(client_enable_direct_control_); -} - -void StatePanel::onClickClearRoute() -{ - callServiceWithoutResponse(client_clear_route_); -} - -void StatePanel::onClickInitByGnss() -{ - callServiceWithoutResponse(client_init_by_gnss_); -} - -void StatePanel::onClickAcceptStart() -{ - callServiceWithoutResponse(client_accept_start_); -} - -void StatePanel::onClickEmergencyButton() -{ - using tier4_external_api_msgs::msg::ResponseStatus; - using tier4_external_api_msgs::srv::SetEmergency; - - auto request = std::make_shared(); - request->emergency = !current_emergency_; - - RCLCPP_INFO(raw_node_->get_logger(), request->emergency ? "Set Emergency" : "Clear Emergency"); - - client_emergency_stop_->async_send_request( - request, [this](rclcpp::Client::SharedFuture result) { - const auto & response = result.get(); - if (response->status.code == ResponseStatus::SUCCESS) { - RCLCPP_INFO(raw_node_->get_logger(), "service succeeded"); - } else { - RCLCPP_WARN( - raw_node_->get_logger(), "service failed: %s", response->status.message.c_str()); - } - }); -} - -} // namespace tier4_adapi_rviz_plugins - -#include -PLUGINLIB_EXPORT_CLASS(tier4_adapi_rviz_plugins::StatePanel, rviz_common::Panel) diff --git a/visualization/tier4_adapi_rviz_plugin/src/state_panel.hpp b/visualization/tier4_adapi_rviz_plugin/src/state_panel.hpp deleted file mode 100644 index b30494772061e..0000000000000 --- a/visualization/tier4_adapi_rviz_plugin/src/state_panel.hpp +++ /dev/null @@ -1,206 +0,0 @@ -// -// Copyright 2020 TIER IV, Inc. All rights reserved. -// -// 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 -// -// http://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 STATE_PANEL_HPP_ -#define STATE_PANEL_HPP_ - -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include - -namespace tier4_adapi_rviz_plugins -{ - -class StatePanel : public rviz_common::Panel -{ - using OperationModeState = autoware_adapi_v1_msgs::msg::OperationModeState; - using ChangeOperationMode = autoware_adapi_v1_msgs::srv::ChangeOperationMode; - using RouteState = autoware_adapi_v1_msgs::msg::RouteState; - using ClearRoute = autoware_adapi_v1_msgs::srv::ClearRoute; - using LocalizationInitializationState = - autoware_adapi_v1_msgs::msg::LocalizationInitializationState; - using InitializeLocalization = autoware_adapi_v1_msgs::srv::InitializeLocalization; - using MotionState = autoware_adapi_v1_msgs::msg::MotionState; - using AcceptStart = autoware_adapi_v1_msgs::srv::AcceptStart; - using MRMState = autoware_adapi_v1_msgs::msg::MrmState; - - Q_OBJECT - -public: - explicit StatePanel(QWidget * parent = nullptr); - void onInitialize() override; - -public Q_SLOTS: // NOLINT for Qt - void onClickAutonomous(); - void onClickStop(); - void onClickLocal(); - void onClickRemote(); - void onClickAutowareControl(); - void onClickDirectControl(); - void onClickClearRoute(); - void onClickInitByGnss(); - void onClickAcceptStart(); - void onClickVelocityLimit(); - void onClickEmergencyButton(); - -protected: - // Layout - QGroupBox * makeOperationModeGroup(); - QGroupBox * makeControlModeGroup(); - QGroupBox * makeRoutingGroup(); - QGroupBox * makeLocalizationGroup(); - QGroupBox * makeMotionGroup(); - QGroupBox * makeFailSafeGroup(); - - void onShift(const autoware_vehicle_msgs::msg::GearReport::ConstSharedPtr msg); - void onEmergencyStatus(const tier4_external_api_msgs::msg::Emergency::ConstSharedPtr msg); - - rclcpp::Node::SharedPtr raw_node_; - rclcpp::Subscription::SharedPtr sub_gear_; - rclcpp::Client::SharedPtr client_emergency_stop_; - rclcpp::Subscription::SharedPtr sub_emergency_; - rclcpp::Publisher::SharedPtr pub_velocity_limit_; - - // Operation Mode - QLabel * operation_mode_label_ptr_{nullptr}; - QPushButton * stop_button_ptr_{nullptr}; - QPushButton * auto_button_ptr_{nullptr}; - QPushButton * local_button_ptr_{nullptr}; - QPushButton * remote_button_ptr_{nullptr}; - - rclcpp::Subscription::SharedPtr sub_operation_mode_; - rclcpp::Client::SharedPtr client_change_to_autonomous_; - rclcpp::Client::SharedPtr client_change_to_stop_; - rclcpp::Client::SharedPtr client_change_to_local_; - rclcpp::Client::SharedPtr client_change_to_remote_; - - // Control Mode - QLabel * control_mode_label_ptr_{nullptr}; - QPushButton * enable_button_ptr_{nullptr}; - QPushButton * disable_button_ptr_{nullptr}; - rclcpp::Client::SharedPtr client_enable_autoware_control_; - rclcpp::Client::SharedPtr client_enable_direct_control_; - - // Functions - void onOperationMode(const OperationModeState::ConstSharedPtr msg); - void changeOperationMode(const rclcpp::Client::SharedPtr client); - - // Routing - QLabel * routing_label_ptr_{nullptr}; - QPushButton * clear_route_button_ptr_{nullptr}; - - rclcpp::Subscription::SharedPtr sub_route_; - rclcpp::Client::SharedPtr client_clear_route_; - - void onRoute(const RouteState::ConstSharedPtr msg); - - // Localization - QLabel * localization_label_ptr_{nullptr}; - QPushButton * init_by_gnss_button_ptr_{nullptr}; - - rclcpp::Subscription::SharedPtr sub_localization_; - rclcpp::Client::SharedPtr client_init_by_gnss_; - - void onLocalization(const LocalizationInitializationState::ConstSharedPtr msg); - - // Motion - QLabel * motion_label_ptr_{nullptr}; - QPushButton * accept_start_button_ptr_{nullptr}; - - rclcpp::Subscription::SharedPtr sub_motion_; - rclcpp::Client::SharedPtr client_accept_start_; - - void onMotion(const MotionState::ConstSharedPtr msg); - - // FailSafe - QLabel * mrm_state_label_ptr_{nullptr}; - QLabel * mrm_behavior_label_ptr_{nullptr}; - - rclcpp::Subscription::SharedPtr sub_mrm_; - - void onMRMState(const MRMState::ConstSharedPtr msg); - - // Others - QPushButton * velocity_limit_button_ptr_; - QLabel * gear_label_ptr_; - - QSpinBox * pub_velocity_limit_input_; - QPushButton * emergency_button_ptr_; - - bool current_emergency_{false}; - - template - void callServiceWithoutResponse(const typename rclcpp::Client::SharedPtr client) - { - auto req = std::make_shared(); - - RCLCPP_DEBUG(raw_node_->get_logger(), "client request"); - - if (!client->service_is_ready()) { - RCLCPP_DEBUG(raw_node_->get_logger(), "client is unavailable"); - return; - } - - client->async_send_request(req, [this](typename rclcpp::Client::SharedFuture result) { - RCLCPP_DEBUG( - raw_node_->get_logger(), "Status: %d, %s", result.get()->status.code, - result.get()->status.message.c_str()); - }); - } - - static void updateLabel(QLabel * label, QString text, QString style_sheet) - { - label->setText(text); - label->setStyleSheet(style_sheet); - } - - static void activateButton(QAbstractButton * button) - { - button->setChecked(false); - button->setEnabled(true); - } - - static void deactivateButton(QAbstractButton * button) - { - button->setChecked(true); - button->setEnabled(false); - } -}; - -} // namespace tier4_adapi_rviz_plugins - -#endif // STATE_PANEL_HPP_