From 79aea2add61d66df0694d51e6d86b3e8c9b66b8b Mon Sep 17 00:00:00 2001 From: Kenji Brameld Date: Sun, 18 Sep 2022 09:57:45 +0900 Subject: [PATCH 01/25] add rqt_controller_manager Signed-off-by: Kenji Brameld --- rqt_controller_manager/package.xml | 22 + rqt_controller_manager/plugin.xml | 19 + rqt_controller_manager/resource/cm_icon.png | Bin 0 -> 4260 bytes .../resource/controller_info.ui | 87 ++++ .../resource/controller_manager.ui | 74 +++ rqt_controller_manager/resource/led_green.png | Bin 0 -> 4265 bytes rqt_controller_manager/resource/led_off.png | Bin 0 -> 3840 bytes rqt_controller_manager/resource/led_red.png | Bin 0 -> 4237 bytes .../resource/rqt_controller_manager | 0 .../rqt_controller_manager/__init__.py | 0 .../controller_manager.py | 475 ++++++++++++++++++ .../rqt_controller_manager/main.py | 27 + .../rqt_controller_manager/update_combo.py | 66 +++ rqt_controller_manager/setup.cfg | 4 + rqt_controller_manager/setup.py | 29 ++ rqt_controller_manager/test/test_copyright.py | 25 + rqt_controller_manager/test/test_flake8.py | 25 + rqt_controller_manager/test/test_pep257.py | 23 + 18 files changed, 876 insertions(+) create mode 100644 rqt_controller_manager/package.xml create mode 100644 rqt_controller_manager/plugin.xml create mode 100644 rqt_controller_manager/resource/cm_icon.png create mode 100644 rqt_controller_manager/resource/controller_info.ui create mode 100644 rqt_controller_manager/resource/controller_manager.ui create mode 100644 rqt_controller_manager/resource/led_green.png create mode 100644 rqt_controller_manager/resource/led_off.png create mode 100644 rqt_controller_manager/resource/led_red.png create mode 100644 rqt_controller_manager/resource/rqt_controller_manager create mode 100644 rqt_controller_manager/rqt_controller_manager/__init__.py create mode 100644 rqt_controller_manager/rqt_controller_manager/controller_manager.py create mode 100644 rqt_controller_manager/rqt_controller_manager/main.py create mode 100644 rqt_controller_manager/rqt_controller_manager/update_combo.py create mode 100644 rqt_controller_manager/setup.cfg create mode 100644 rqt_controller_manager/setup.py create mode 100644 rqt_controller_manager/test/test_copyright.py create mode 100644 rqt_controller_manager/test/test_flake8.py create mode 100644 rqt_controller_manager/test/test_pep257.py diff --git a/rqt_controller_manager/package.xml b/rqt_controller_manager/package.xml new file mode 100644 index 0000000000..786b49831c --- /dev/null +++ b/rqt_controller_manager/package.xml @@ -0,0 +1,22 @@ + + + + rqt_controller_manager + 0.0.0 + TODO: Package description + ijnek + BSD + + Kelsey Hawkins + Adolfo Rodríguez Tsouroukdissian + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + + ament_python + + + diff --git a/rqt_controller_manager/plugin.xml b/rqt_controller_manager/plugin.xml new file mode 100644 index 0000000000..993d7fd56d --- /dev/null +++ b/rqt_controller_manager/plugin.xml @@ -0,0 +1,19 @@ + + + + Visual interface for managing ros_control controllers. + + + + + folder + Plugins related to robot tools. + + + resource/cm_icon.png + Manage ros_control controllers + + + diff --git a/rqt_controller_manager/resource/cm_icon.png b/rqt_controller_manager/resource/cm_icon.png new file mode 100644 index 0000000000000000000000000000000000000000..50972fe7cbb38191d87d7d00b009ea43a39c3f4a GIT binary patch literal 4260 zcmWky2|SbkAAcq>Oh*1Ji77E!ny9uCnQM*}eu#1}g&JaVBqSY}IU`q&ej{XzDEH)y zq9}fn&}cMgxkCAWdu`92?Ro9lzWcn-XEBy%O~ggzL?H+gHzgTbgT2n*4K4&8Pi_tS zgB>iu+Jp$PdgW)p3n6!N6GLeG?=7#fC=7}~jC0m}qy-OB?B(_Zr^=&-A zjAo7aIy%^OG&lAU0>9gQEcPEQoy9Y_=3kXr{!(}6XZ9NIdvFvz9S)086(n)sORuJ2 zEqaiYL?p@>e$1P^6_==_2hr0#@y5AN&>nASBiUT=iV9=c_%+s zD6Ai#p28M8cb}~0^Qu=r@h@JLt=Kl!%0LX4ws(amUp|`QW;S!~CF`mrQ30B^PqTeH z%d*zS%^du|SiiwGRD+;+6pxZ@AXmi&LZglqR)rZ4)@@XD0XnztsOjl9bC_f+| z07upmJ0s*8{$;@|S7!B1hSpJ~N#6$Ng^N+>t+OgAAroyeyYIu$a`EBr*4f$F%P>5D zZPOGlFT~q=f-r-lD5qVCx}lCpL3k8LjX9XG@#gfB-|;NlYa@To8(D>KBwca-pAp+g ztx=aiAoLI-c#9LQKdY*$$~to?^w$uhrxL4@VhWFfh5}nq235m?OzG5q)jX%6gV2qh zDgP@jkyXRbas&R18t%UyPNPY)`0lrEIepY7KUAJMW;SybYU(Cz54N?njg8g^l@DID zk?ylJOIZq*S4iNANStEq=p_j1Pu~q*J540rbV;@YOC#{c;ybx|AwMQfv-Khr=$cp! z4fpVHh0@Z}XzOm6js$vQCG?!f(G0zvtRig z)2SohAUyNKXu!DEmENaxc25#%a>_#Gj;u?VWF__vGV1kf)8(}_N@%Fu+QOJ>nVlHK zkRpDKLT6V|M}BPn?gSvay1NUswzeiJ&xp8opx{bK!F=I21Xa4`D-?fL%?~fHLTASn zU@im9Du=N)wsQyfIHV<6s9c}vKL~)G^d~$kE4z&>P>(%Iz~Rmq8yn+vbs3)8qPDgz zg!`CSKY3y3ozDv;Q>njW*f`(`G_auNSAAis*yj65%B+8}&Bl~x4A98Pl9FlKNk1pq>I>owu4_<`EMVn-!^8TG z&iDIxcer?TG;MdfSmZ(osgrxX?7e4LySQB+|dU_|C?oV1cdoCGKbn z8CjuY_shi#{rIJ$pc@8v8nP<0v%<+*d*yvzX1c80q=>+Fh4fjiuk_CqXtu)f#&(Oe>_jO)}EB(ts z4d8i3&^8%fYmJdUzBVQAb3E%}Y{8bPZtgbUQA1V{g*t6E3MEQ^y^xx;tZS(UQ-BXn!c{BK4;6W#OI-P{E{n0ikiV=|{e1;Mp9vFG9VWqPem*kyslVSk;E2tqe@YK_ zJX*KZQ5}9^tKz$z(WqB5W5iK;w}6YcPdrw_7To z`OXHNTmR?oa#z;_3;69mUYry$iosoeF)~+1Q~UMuQh|eB>#3Z5=&=W^cES%; zQUVmUUm@WD5{}O8dtz6rpRD|}{^-K9j{FIec(zWg*Uy(#p2}t{(eJ zNLcuFZ2<297Z1**vmcYDMb|X)JRB00i`Pz!)WGuoOb0GTUb%cZ)C3Tb#jpFoUE&0e z`W%?stvAVjyc^HUOI^+MrE4Z3tqB;;_7HnHLyE3!I?8I>J!%YnuAz?I7G4e3k%>H8 zH9Q;A%UxPto#Q%73&JX~t^AcJO`!qpo43^Tk%kGKDLRQtzuD^DN69JW&z^x{9Y7&g zKM`UJIQ$hNQgHouIG(Y?)^=iT(+wqr?TIQW8I2)kBc*f~Idsh&r1hIA|N9C$!RWxf z)O<^LNeS@4rzNp5F(MWg7HFBc7T!;Q0SAW=6o7#+4c1W@u9@Q&oy-06Wp?&i)fh*w zk}DZ+d>+Fzr?U@3Nhaiz&0d*L?Mk28r~_xY?S{hBrjZTKM-Z(X;C(muiFcrG+_<4r zW>@8m6ApZo zuBj8e2r;;M<>eAUi*>9gAH4&=F0E%5dSwaAyu4b+%DPL;&punki)tAU%6j|lSnn^7 zj{iK&iUIKf<)WTEd15KO6L@$}PtP=;f2X9Rg1UFgzcx1@++g7O@D7R3)$Z(RR(bhI z9c#fevxe#Q>4N&hOb>daI{cNN`a`0&++FGj=AjuTy@$lK1~hqS)?Q7`Qb~Eu&n*0y zD>8oWsI6_SvMH<;=S=V9{#abZ6}-pLoJ{g9BRw3zDV{Gzo;zxkMknsEwM{?tP&7j2 z_MwNdSs*+ijg8-b-i?o=!LYtA;em22u`>E_eW3iJ0g)Rgb0Yb5zIxxS6<(V0|JlRa zI%s6opF|QYz(gUVTyYP6^7;-)iz?;zwc-$%Sb>DjfWd{e?N3^J`!g)Q@sv$B;tls} zE_NR({|jp1sPlEHxb}+L+L)Z2eGvEpD*{hDiS+DhX3ZZ9SguZpKg22Ts@xJQZ`x2d zHZi&Rz4+q2jo;xQ)t}g?14hv9Qw~HY-cWy7cOENqwGRP?v*F;7wvEqv+nr${q?^0I z&EI93JifMheRJ`0_&pQEuGT{{&GMl`ANYZ)D1>Ndg5o*Vl-Ctp zd`Hp&ldj(0UfHxs-JwZ0fBzq;m{^yDc6XOrzlkUuf$+Tw9W+(f>hNaq%~GvN$&&%{o#t?d9g9?ASJ}u5U)i;t!QS5LMEEZ` zrO63cvn5;&m^+wxdqP6Hq4NytVix-kHhKXj(P_0Q2d((9ckRsZ0;a*YyIKzTTSrHS-}<5(LPBDVrDdPCbEF0g=H>~gF5LlyqJ~Qo-Ha*Ff#POg%>EhS zPH!aW>21$Ho|^#DLq|6#!(ZVkh#8t3;6GjSLBRvCKv3hpjs}exRSidDdq792b==kN zE_w0dMOV6h+SC^;9tmGIv$D0l`SYuHwW!lj3+g~LGRZnrATpDrd_$Xj^Q9{1{;qxA z)@HJqZ@(qFp(yv<&>0p)E9uY85sD{#K67Sgv4E8fh10Pqs^?x+VxNytKX@OjsjYQ5 zA}#9NihBp*+aaxm5aDkAk`*VRw}p(0i}Tc$OX$l1R}vVWBj}wworX>{(Z-4S9LBy= zc5d`ro~RDX(vK%Eln)iN_`%!1*Yf!%lV4x*{jZVDhJi{{`tbsteF7F%G<>a_!9~O~ z82gd%t#@*u317Y}boJ`h#n8qIYJEB+q9njvR*t-^E%&>^M!mXenKCocdLNr%F^FNF z7@1p5x=?n!vY`QMpLRD`hmn*d332-SrH&sz{>J!8(iQq|N!}K@T*UBo6@h`g3F`Fru&WSwwRH@2u%ipeiT>1YR>LuDvZkKfiNt zcwTCOjk+7C%D{dp6bea2&pYOrocN={kQB|;tmwpTdq>BuH{JZ=4(555qZ>4s@_Mpd zMJnbzbP3Vgr7ag90ChuOzGXL8S5$yEH($%rtg*++^Z|_eS6okIi`^A~&`9{1WMw3= z&LKyVsL}WKt<&~V6^YyndZ1^%*PauNJanb+F&TniOq)`Qes)AdYoW)tnG_pnJx6JGw=!3jI?DUJKiUvxX(fHoH2I9$oH)1Sqj!*~ zmtXJpEa+k9R`_YRe&-J!aJrRh9cn1uE(SU0r5Y=YB1(2V_3zzBB<<#qdn%hkc)dA4 zw8Xa_iTkXbdCBy)H@V} literal 0 HcmV?d00001 diff --git a/rqt_controller_manager/resource/controller_info.ui b/rqt_controller_manager/resource/controller_info.ui new file mode 100644 index 0000000000..b8c03799a9 --- /dev/null +++ b/rqt_controller_manager/resource/controller_info.ui @@ -0,0 +1,87 @@ + + + controller_info + + + + 0 + 0 + 164 + 238 + + + + + 0 + 0 + + + + Controller Information + + + + + + QFormLayout::ExpandingFieldsGrow + + + + + <html><head/><body><p><span style=" font-weight:600;">Name:</span></p></body></html> + + + Qt::RichText + + + + + + + Controller name + + + Qt::PlainText + + + + + + + <html><head/><body><p><span style=" font-weight:600;">Type:</span></p></body></html> + + + Qt::RichText + + + + + + + Controller type + + + Qt::PlainText + + + + + + + + + QAbstractItemView::NoSelection + + + false + + + false + + + + + + + + diff --git a/rqt_controller_manager/resource/controller_manager.ui b/rqt_controller_manager/resource/controller_manager.ui new file mode 100644 index 0000000000..612a4388a0 --- /dev/null +++ b/rqt_controller_manager/resource/controller_manager.ui @@ -0,0 +1,74 @@ + + + controller_manager + + + + 0 + 0 + 339 + 409 + + + + + 0 + 0 + + + + Controller manager + + + + + + <html><head/><body><p><span style=" font-weight:600;">controller manager</span> namespace. It is assumed that the <span style=" font-weight:600;">robot_description</span> parameter also lives in the same namesapce.</p></body></html> + + + namespace + + + cm_combo + + + + + + + + + + + 0 + 0 + + + + true + + + QAbstractItemView::NoSelection + + + QAbstractItemView::SelectRows + + + false + + + false + + + false + + + false + + + + + + + + diff --git a/rqt_controller_manager/resource/led_green.png b/rqt_controller_manager/resource/led_green.png new file mode 100644 index 0000000000000000000000000000000000000000..055538f481c90cabbd83a17532a70dd7fc090c59 GIT binary patch literal 4265 zcmV;a5LWMrP)J$CxX9gn=b@4GuvRGI-k;BI$k zc4l^Wc6N4_aD(B+7hgb`X(oEwq@q0N^|ky(WZs-Zag34<9~kuP49j zrhu1TdMRXC)=?qE5di(nEY^!Q01%M|Frc-5Rw;Gtfd?K)uM6GlC6G)eV@jzf0Q@$`^!AD+D)T5IMgnM_Kr*ZT-F{}X^sj>OkVU^igqt3>ow!!Uld z)}^r~4qtxxDWp zE4jh6bq!<2NBG#dj9F_M3w8zD-0=xwX?;V{~*BGcz;gQP1c!PG^3F zOXhj(+xdCixBE-jz2$cJJ^n__RzS|k;hoXb`1zTi;*G)AvEAE`J42sEYe^AAg!cA! zba!{xGnUNEPv3j*y?^D_z*PaSzWVARX8th%N9v|&;=+Xs$Ye6bp1^oN_rG{M|7+ZL z`vZ9N(~o0Acw>{|)`#)AEBL{GAH#|F{sVUgKZ`vrcS4lxa4;A|Pfrg#9*-NkCZdP# zx#u3o-N8`-Cr+H`C8E;+9Jjhcp@5;GA>{LU5D_vujbEgHj>yI^{^kqc#7*%Y&^jM8 z7>uE7=kX86zmEKzfdi2*qD6#2MDY21*tTsOyk4&x`W!Rgap1s#8b6_i%VaVsqtWPV z06ynL$uv!zKYt#DLeYp#+t={Q;!kkkw)^mBzxyOS(sd-(1qxOk&z$^6oErEg4o3a} z9coiq34MKi^^D`|sZ?rzwLPe5-PYFj2#OwF4QQ=#@!~}oh5;hNRqF~~$^HaizUzMUG*`;n0mOixb(0A}i zv$L}xBIG!OUl>2bEnPeC&?k<-u*F7#M(QnqV+anm@%PKE^k` z@HDz2n;QzW7I7VM914f0FW{RezK$-r754@of*=8<6t-{Q?r0B~dD`ps_S|*XU1?y& z3@{AiC=ofH=f}s#VcRx@5IAR_!BvdmGd=sz5!wht7o1Ssfr}P>^*9cN0J__@;Px$d z;LMfNIA@(fzxNZcZ5!9FT|;+wcMTR1#VyM^3g8)_w8tltNhyRlB7{KoXPV~n)Tb=o z$NxYniBI?JgJ~CF+D64^H2i9D8q&}Om^GhK{ywv9AEcBxW50Xj`N9P6% zpgXz+;a~(AV-X+d)7UHb11P%aot>RdMEVnn#2x@|t0Lq4u+tIOwr!+Rsp9A(gAq9h z35m{VH#FPOD^7c>p4K&DInMLOH~>Jvrbs77XD{IrpTi#7iy&GtKR=I6n>INer3EuT zj3osKA@)0UFh4&J+qOYO7@NP$`T4R_F(xlUqNqtF6->R-;c0H~c9*0vhEt1` zvrb7>9_bMp#BT8^5C{tk3yxbNfE}k!ok}Pn#9pVkz1AApY_@1nXa-reP*y-e=V6tC z8O>~0YZI@!nk)ZSfvNLQvbd>buz&@cMHF%5aybBw@kyT$Vy{w4?Q-ffpU*>U4I;vf zoTykw^Tr%ZZ8c`R38*!Ijb@Wm31h*Shg8cvPGKA|YydMOpU)#04A$T&rFJPI+U~?y zKA$g7|2fZ8g#s3g6mr%gJksaLkUOh&ph?fi$=K%195Qygg2x$LgDBn27>0qCmKLWD zwks6x>S}714m<#mQ)x)COq`+d@#1B4hi|GcQ5_9jhcjGLawR>~e5 z{(uAAqJ$8R-ro|ECXIq82SqEk5(b9nhp;KsRX+Ex-VCngpi~Q*HAdzy)@*eJWWicP zK)Nb`h~i2J5n=}`%*+r%fJuXuE*rol$XIDyNsk~As&zLj39bDwVgB-YfCQ9F2Y|+4Y5+T8{ZPU=64bOpYMh|@)C}9eP-*}WsF|Hh zAcTP6<#SC9HD)LwM4Fjv`~WG7mlPiILP_Q7>KdHKh52*1InoD@SQ(RRF!5`uo1Z&| zUBJlv1z0fM2ZxJy4T3~jmTDlPv_dgEqs9;_rAk_p@CrXHau0eKoX7COAi7$&AfOz# z8`KC$O$UW4Dl+CG#xrBEVAj1ZQ|y5x2?9YWgv;^!#!$OmFaVovWXv?u z<~%g(6{B8Xm0=w#^Y5krW*$;RH0V?r2n5PVF?$nc)p-3?T~(R3=dq~M@Jl~@G61je zHC6@zV1~^$3|&Cp&Y@rzz+jN1Wn<^yGFL-fC&~d3zu)hu03pPnBBHZy3h;P5ux%Ud z<|cgPxZWDuYXb!sb^)*fNfMG&X>E#<6UAUKG;3(qu-Ssm%W($hEx5khMNH_>osd#O z2;r&#WDJE7LiB<*=!a7kjUSRQUBT1J37~K zU~H^s=N8?$<;#&!DCFh@o!_~0=fqM}_%$M`Q9w8x#?;glwAM&udoU5Z2%D_xW=#sv z%$a}fu!JsIq1)&!J3lQgEpF5QHJ}s}C8Fnv=r2%p6D*}fG#W)Jm4csJib{xFTB8u_ zad*%dp)1o1zit7jIQlNWg7$bk&jTd|eDJ{s@9y5cdjP<#)h%PO7}Du9m>HYXJ1`p^ zhb0Vcr_TgO(rbl~mbEdyM`q60AZT}y5=bd-n1y;BmgM))l64BF6gr}ya%F*Gg z(P50n-(M#Inh8#>c{0E(mUmO(;9PxbZnJp-(`8PZxGQ%e0c9InRV@i?;C zENt6GVqqK7p()I_Og7IM&~(J7ag8%Xa~si>xdkK$QcAS7wYhqKrfD84|6f^o@#4k5 z1@H~04i*;|%a!oFOYnES zy}h-UqW~~4Fc1p_0⪻ENo&cA@6Ex(2(&Bh!yuCCv@4zko<>>B?av>3(M z|59$D+x2)Zm)q6f-(S8SuW22XhxC^eL$qoc1e^XHs8UAlQS&7#n`zygLh zzXj7%yBw}RXkJBa1wpSBK<~$&LQ5e6pm==m=;)|-M1Fni)~)-As3Jwi(M7s&;X<$1 z>pjiPj#ny{WnpS+s+<>Ssyr@ty^nP3R3j0t1D3JFiyhcHep`9XE2R*R$Lr+>%^8Mq zM_*rG<0Ppi7#SHkL_|M!%L}$`8&gwL<>deZX4*$_b>jutt6dTec8H~4%JyR8wLZjW zx0a*PUauGNc-(d5nOQUQLpR-YQ}bzZOE5Mz_HAZ<$}K;bnaj!0OJI8PnA$LmRBRkt zSnl$z0)#EmHrs`csUCRD<%G+62Cu{)6VcO~H*a2jqGXAH92*<^HzCBM_1a}J8KhFF za{4<^R75H^j`{Wp7`}{SPxTx`0^rRD5t&UOI@?ttxDWzuZEf|ox+VBwS6A0ps@HvW zKXrkbz2oEKF94{UEUmRhDwRSelR@=8Lm{w;Yk1(?a@Ju3+_*%VbzYbz1%d%iu7PQtCPiBNDrnf1j;Prap^?FzP=4ei9eYCT)^Ts@xSqay!UF!wV z@X^$72pFYDQyVvKyz!5w%DpnnnVFeKwAQZA=6(ZkRS5Cb_V)H4H9O_2;krlf^5x4h zzu*4^5jA``xsEW+%-_l9^WWXFWy}B1!^vuxnwkm;A&xTh5di&gd1!GRpaBdJ(X(3X zV;vnGza39!t09?8%DeBrdyi#V4-?UTBHH0}`Pvk7LOK5TSS`W6e&M zu~cKLDNEL}n}j@Mf4BQP=icA_{m#Aj-0ytf|NBn7wbd0N$X^f;2qa``g1HV{_5XA* zA8@97-=6?3y#5BJwqW201G~oo_X77!?EQi9LqLz`BEUHLAdqmrDdw{6gX~@h2d|T! z=?8r-U(nSz8E;K*+XvmT|AoEtYwG!!MHkl@HLhvciK7VZK4FW!X97@RZEe-YA8(=B zaz_#CGfIu-I)+p$ZUIteVjj$3GgeP{aB@9oDQ zJNsYnOfVb1x>%IUr)jBuhO8yv8b|9SeB=Yks)LdwW7bGYEKldq#aBNH2gZ>kjI2ny z$F>ltg$u^emb{z%_D0c%hna=@iyiUD5>Mn+`zDJ5pABks@t8a09&j@cd&(QctP%C1aDD7uP4r(~msDAKy2MPK@q{eN%I=bl1Cr=cRz znaggHe2{2vpqafA1zLbpEM5`>Lh#C&KI)z(0j+=jG0~&)TUOH=AvY%MUrYq_uo~H#clM9 z8C{q#5eO7}Vj}l)b93{M>t3aEV|4Tp3rkC^02T>Hoj)HFA1`2%O3uqe{PZ$@pHT+l}c@GZ-2D$C9zZ1cdU-&t{#6j=K(j9 z$sBGBCTbKqMBl22OGy#Q){B(BQRZyq;&N(pd6Yl5zqVaVL^cJyv$M0LoZizzZ2A2? z@$mcS)@xiY;TvD>6AlzX4okCs$7_j`G5N?j6Gy+02oKrCw6 z?uJ6zJeLuxjU=Nd!;XcAhi66bxe}88`RChs@MLs8^1Yt;XUBYNZMM!34o1E)V}1Gr&N<493KUmWt?NV?20i;QtNsUs>&3jFckb$S zezTCA@J;)s&96SaN>(Qfif!9>S6V2n4Ap5sUNz!olTQ5iKga5Mu-MBOoTFn$n&OSsUo6$>g^vXwS7maV_GLw{&y={;9ZuP47)zyVUp{Cx*?}cG&j?x#f2U@{D9dYTioO@cM6bj{Zni&@;JtJeS z7d<&uKcB-~o9_0ph-il35<>&}a&!~TGgQ9OJaaC4dY%QzoD_EB3^HnLp3posEk`4N zui{xGdL^iKbe~US6w*>x7uVSdzzAsZ4~z!~5hpMB@y_~%u%mtWx|5}t!^4uLj`fcW zV};u|8-EBVB_#!pzMY{eyL7*CoCx}Tl@mF4DnrEy@-O`&yZ_*hzpc1A$Ui zRaFZd&TH?hm}{17E3OCpxbbPAWCBIRJEvPQ*RTE#?Jb=S@Gx4=RO{Xp=)-K~bOD{d zY*R*I7kJBuBn><>*H zG@2hYIE04s}9c7B|=|`FdikfZ}l0PS!6CZB<#1EA^0gMG$xUjfb z;s5H0MhmwMmq8p}JD+C3*HHR64yB>NBGgVz$`o-->XEHT(!r-=Yn; zqV9%$I!%6XJ>4UuMXK#q1(=D_#{g5ut|PO02dkt0fmG#e);pFabO&7g#}x%87+2s* z63$Ye0OKt%jmu!Wh_&jDsu3kfw1KWffMNR^vjSTML~j3$wYsJzM~{k#mdWHX-{Xq) z4Vmadrkk*}X*NwwL&K)J^Ufw$k=S1VRY3G?88(*R^*R5Ya&vKm;hcjWtDYpkddI3X z3o9#G@x*h>TSQx2(LqmAadGiq2e#NF8V<1+v@gXY6$BC7K^?t)4W$l|Hw>=9Ev{bG zL%FoZ8>`bwDy*w?MeM`0kuowoV6D1jn1Mc9vzxj%Gb6`M(jR}|Qq^8|#T39Gcx57R zrvF)+ZHy3i78GvK7Y>8K1WJB>IG3@Roak|ipQ=c#08av zFK&e9At7X#myeI(rQ9;bD1e6YPZt&z^gTU2&EqYf8tYP%?ayab^Oo?pOcL#1c2D20 zjCy8)sf}AWHT3qs(B^a)un-B{=tucTBpl*JBwlfFaOmC6v!~t_Vn*d70Yb#!HPhsd z*kzx78hQ2zYF(KY=liw!zwI!)qU2=Zm+Zwt8o!+t1#rl}i5aXfZ?Fpu!dNVzwik58 z;^q&XrN@nef`V>x!89Vv#SP-J)J2?vuSZ?j%CfvV+LrXSMA(a=Ebm{aPYAm_v_an} z{8H>L3x`}zl>;MAinaC(d6=fEl-apqmw5>hmoivDCSa~?72uRg6KQ58Vgte)Y>hx} ze=MLCkYce0|Cw|$EMqo&chmCl_cR3!(rKtMLP@?6Z}HMhSX^A>uN=z4t>6IrP9GrY zw=Tj0+$P_Kv*)_PbK-8p2R?ozZEuH2#*lDIF}4a=F}_iy7<9si`)5oIu3n`C{$AVN zx0;KT#t9{eevHNZ=TP#V*F2Tn*Jt&T?fpPU8pr~Q8gJ~O#PdK2U=)PN!0u&Q#js{~ zU^r~+QN6v@eZU$oa!;KatbxWd-w~=@2|#vGQdLD~Gmo-F+^D_8jcOmE!)3HQ_4Ph-t$46c00 z2>v;?jRqa!(fTSGRjG1-&leOF3?e+L%AI`xTNtkLq=@AwVq|O7TCsb(t*!cZ{oo*D zW8?Nqx&3OozIGv8iio0$ii&j3;g9kZZ&_1OU_Q#E-meBl|2@dF9@f*;bbr0#TXuRCp^E#wSSv--NnOPa3_ped^tREh*wDetNminyiHkNnxlGB z>2$plCr{pSl=HY17PhA)vj3%61veDf)8BtXnQU%uPR-3F-gd%K4CG9Ki5hC1teLAD zT{hPIiyv9m2v(Y}k{)Idm4~}SQ8F?}~-`w2X9q+Z@`AOa`ZFLfKP<*Yv zsnFZbsE!SHC2+W`v2SnjvtJAHa`$3#Xtcwz(a~LwaXXK)Bh{7Q9U8iG6qMP3!cT|- zfgYJ=rd-Bv00so{rn>rE_(L*H2(;2My*4?dv1|6maUjb=o+60oYt2g@CaJwP`Jfa) zSE^8JM%$+|yr+U19T;e5b?CG$k+`y7uWl1RGBUyi5>mC)mBD2c0?b`GeD$}FEx{X) z|Au9vPL}oRf_lJ+2#I*i!Kaez`moY-BR=%>=d-j|waC_{UyQ9*5 zo$srGB|Hj7SgEa>f(-A~c~x*pf{f!9c$lcZj$0Kil-E^0DL{aq&y74#vt&CX^6|1? z-7>=NCS1rD)o{otYB|{9v`n(tR=}1cQ4o;>H~4Wjf4rwjP|@bq($3z}Q`vlno-j5l miQ`?`G_Z`sumvNAUxHl0u@ED%`p5mz+GrXF{nJ;F)J6+9f2tI1VaG7M zwGBT4Lw1ceLfsgC$#zWHB=w?1iKMtoa>;$)z4uIi>_f}ldv`A%N;AL(cJG}tXU?2C zbLPy%AAIn^ z+3U$4Z8pJZG^)5F}y3Lw(3 zGuj+^mzK)2hGG2T@ZrOY8=)TR zMx%-{M+KS4xIIWtWY%gf7HT3SLnorYbV)c8Kr|ZFd_LbVnfVcFqSyI+9`p0_SX^AR9yBwAX@X1>s;&b73{3+o3Z$X{ z7UUI0K{y;nXJ;qe?xs5`GxM*q+3XJwA3m%%LvAX7(%@evqI(+Q8HRzGnHj|6aWHc= zI|y98ipYfv2w%E{mUtZAL;`9)U+J6y7;ZPR!5~tR2o?tiv9N6$l3iV^1VTiJL?Y4B=M%~*dkolYNWCWM9ph(@DIOH0eY0yx?TZ((5p)6>&145RYSv$BGLPd`Q9=bt0U z4Bx(e@ZNYM+@qs#_4Pq*Z-?;t0A|Qc2FAhy3RkWmH$INs=bt0}@yA$E6kOiB7ng6o z8Tr=M3T>Cmg}%N%gu~%xLU=xvNz-s1 z!~gRh2FJfb=-b~$@PP;5z4>MckEa>4t7&Jm$b9%A60g3BrIRNyy?Zy#A2@(qpy)jl z5!&0^(cjIk3Poe zZ~qsa_ur59AN&BWuCjZ;RwNvQe&q@l{`oj!zy3AO9Jmb^_wNTvgZBA+7#bRa%jL3b zE{W)&yY9Nna(A#4z=;zlhKcAy0G3-_KA*?rPjjS49XTBO{HB<8Km)#2vNvpbk3}3O$UXhgSzuN}N4=7P_v3h!DDb1^Zuq z1>KK6iqQ9u0$^~VfzhoSoy~B$0R|B|pLz;PCClZ#N%-QK-+u_x4ifgIv%(m;Xiq76(QSMwEVyh zaIBg!;{W(hSa|0@@zEm>BiY>r00@V}7#J9^T!PFzXBfurg9i^*4p6H!K-09xnb~si zi;Ig`TwDYZ!IMs7-)nE6b;k~b9{C-oed}A_ zN&;0HpzHcEBCz5Q{I}l*wW|}*o2_}Se`Sg1=w%(a2DiYs zZy)HRk1_b!?{MksUxR6yxO(*}`uqFq@QEm57{)OG&j6)8J{pZGLWrY62-H4>LZPDi z-b@CgAASs>s_-AU4Xo=$^|yFrUWe3mNWHM?;krk+d79u<_2oZs8%R+w{M!%UO=lp4 zKq8SSYAlh}b2J)_DnLmCdU|^HGV>lQ3D3{Z11KI@M^1eXw_!p=I^gQ+0_%mEN!ysi zFL=E;1jjjFCVcw;+m$3L?Tf# zd>IT(oCPTgw1IxGDFIX3u+)TOr-*A<8m``6=;tn=@2g2%yk$RfEq*L7FQdD=+v+GS znE43G0uVynVMTO#c^Rf@f`~9MaTYGgfTBWaZ37C0Ys{>3C$npBk8Y2sw6%d$1*(+j z`)U#wzWH@XDUnL05DW(E@XKRXPo6y4Lqt2Qh%PTLLkIx^(0}d%NGaM(Qk5F@cWUL= z^|1lUwU?x-75;iB&*9=N-vEfPva({iB?8!S^5n@LRS0pbRoq@miA<(wPy!1}2qe=$ zsRPdGB@J*kh&7uAM-6Irm%I+4s9*s8l_Ua-38W%nWV2ZSmhnlq5aL!LQoRReb7jY_XzVK(RAUXF#h?Uk(M)(EWE}2?d>VX>^9)Z*@6?{Fu+p6s4~!H z@MdyQBtt5~Rscj4QH2lzw&;wRi}89X;bN?kE&#}63dYnl)PWjD!|D3n$bqQpuQ7EA zoXvw&Wz{#AWGFx-a%3ZbfC?aBb996d0;(w?stjN?D}^)XAcAcW-l}+?(=&gQPuEZy z>@KM^jPp}<+F1sdX@cDvnAvU#1XR1wRrPtGTD?|bOGe>~ub}PO=^zpW*bc{$Hq>u> z`soP(RHy!Bs6E%zQDlZHgh(=Togbhm3Wx}-X(a<_qlS4Mh4C|R4G)2|CPH9#7C5nP zd~NmQbrjB?2MUJm-~oW$S|#mfvjmbViq#o)hEP@2qSO??^thoGuEqT8IM3-tqqwCX zqvhRfD%~W4c^`qJoH^s;XUtD5(-roQSs7e)M=e#gMlU z$hQXIN;IfPUC5N)lXKuZHbI6Og_Cge&Ays`pF z3E?0~3IMrqTZMl+0WkBVN<1mH`QXawY4FWDGqvmTOYQ4d49tl?rPYqXwqtkZ{JcGbRZD0bArZq?b`K`SeJbML7qe_pB20hjbRj$0Ls)!G7sW?A=zu!*%-vvrR zQ6hSgi2e*UnP5dx5DW&35mvt!^V|B+cVVW%aMqjra01R;>iF;g3O)}A#o4zht&uL5 z>qVd>fG@uI;x@io+jkTEJa2UyC63mRtI|k7;y8u_UQ?y22ox%DD7%mmFyN4>X z9}EWV_W!X{r%rtaRJryO6BADm(bHChW3gBzI^26<7Td=zIA*Hxs@PmLxp>ns=7x*O zkU$_{TR}oZPmYX?JOfl^hNP6oRaN~hLxq zkLAt~Vj}|(L{Jn3p-{-y`zsU*$IJgKYkud>ok=2k)+&>tC<;0{I!Y%)1?Rpt0=>lt zQj{i-x`C$|?Q2gV;KS7ZouJab9}b5r2cX&qz_UAd?yUIrt3)(5HWv1Jy|(G`cs!0| zve?$YlELl|CRQJk)(vZz`kI)jsi~Owqdmy96=Qzo+CsbZcs85ey=Tv!N_xBw>-_oi zM*;l1)i-9w?CdP`QbIJal){cvXQAqADT3U*b#J&-oc-n~QsH6+naky>Xlh-r#jw}+ zhlYmi3nWU2%EgNp-(}|8tvW4dUJIqXfIpeV=&5tia#fikJJ>{c*`oc#2_pRaFs*L>koxE$O2TNidCf{%=xU#()^IIa73Q|qe5Q#)=XP%iQGe5L->sIGwa%GsF zp8h#AKWSGV%*>Ty=rXwS1@unE5Sd9pR8Dmp(0v9>RUqCI!tAyV6x`RsZjB6Hi9aTy zrw0ZG)?O%CBA`r9Pyd?`;*myp>2w;2M50pu4gjuP0g<@`B6G{|WSZ72TSCsKA>P%7 zcux=o&$WRIArK0MEYn>z@O)oi-=nqbz8aePDa@?R%*?zDpkcAJloE+V0_k)bwRr|# zDvv-SgTP7_-i!_{Z$L4swS!<)~6xa>P0f>L#AbQtd@x2_xlkH2A$j`c(b>+ z_Ye`SzgW5qB_aF^fbTUcmp2Rp$z&4gbb6IWRD+NVVumn_f|3e>i6Di2h~Dq_BM=BU zsS;;q{#9>p?+=>|9$3SL31(JiXJ>y3z|n+}By83{y8-+Ve;00000NkvXXu0mjfzc1Da literal 0 HcmV?d00001 diff --git a/rqt_controller_manager/resource/rqt_controller_manager b/rqt_controller_manager/resource/rqt_controller_manager new file mode 100644 index 0000000000..e69de29bb2 diff --git a/rqt_controller_manager/rqt_controller_manager/__init__.py b/rqt_controller_manager/rqt_controller_manager/__init__.py new file mode 100644 index 0000000000..e69de29bb2 diff --git a/rqt_controller_manager/rqt_controller_manager/controller_manager.py b/rqt_controller_manager/rqt_controller_manager/controller_manager.py new file mode 100644 index 0000000000..f913d731cd --- /dev/null +++ b/rqt_controller_manager/rqt_controller_manager/controller_manager.py @@ -0,0 +1,475 @@ +#!/usr/bin/env python +# Copyright (C) 2013, Georgia Tech +# Copyright (C) 2015, PAL Robotics S.L. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions are met: +# * Redistributions of source code must retain the above copyright notice, +# this list of conditions and the following disclaimer. +# * Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in the +# documentation and/or other materials provided with the distribution. +# * Neither the name of PAL Robotics S.L. nor the names of its +# contributors may be used to endorse or promote products derived from +# this software without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. + +import os +import rospkg +import rospy + +from python_qt_binding import loadUi +from python_qt_binding.QtCore import QAbstractTableModel, QModelIndex, Qt,\ + QTimer, QVariant, Signal +from python_qt_binding.QtWidgets import QWidget, QFormLayout, QHeaderView,\ + QMenu, QStyledItemDelegate +from python_qt_binding.QtGui import QCursor, QFont, QIcon, QStandardItem,\ + QStandardItemModel +from qt_gui.plugin import Plugin + +from controller_manager_msgs.msg import ControllerState +from controller_manager_msgs.srv import * +from controller_manager_msgs.utils\ + import ControllerLister, ControllerManagerLister,\ + get_rosparam_controller_names + +from .update_combo import update_combo + + +class ControllerManager(Plugin): + """ + Graphical frontend for managing ros_control controllers. + """ + _cm_update_freq = 1 # Hz + + def __init__(self, context): + super(ControllerManager, self).__init__(context) + self.setObjectName('ControllerManager') + + # Create QWidget and extend it with all the attributes and children + # from the UI file + self._widget = QWidget() + rp = rospkg.RosPack() + ui_file = os.path.join(rp.get_path('rqt_controller_manager'), + 'resource', + 'controller_manager.ui') + loadUi(ui_file, self._widget) + self._widget.setObjectName('ControllerManagerUi') + + # Pop-up that displays controller information + self._popup_widget = QWidget() + ui_file = os.path.join(rp.get_path('rqt_controller_manager'), + 'resource', + 'controller_info.ui') + loadUi(ui_file, self._popup_widget) + self._popup_widget.setObjectName('ControllerInfoUi') + + # Show _widget.windowTitle on left-top of each plugin (when + # it's set in _widget). This is useful when you open multiple + # plugins at once. Also if you open multiple instances of your + # plugin at once, these lines add number to make it easy to + # tell from pane to pane. + if context.serial_number() > 1: + self._widget.setWindowTitle(self._widget.windowTitle() + + (' (%d)' % context.serial_number())) + # Add widget to the user interface + context.add_widget(self._widget) + + # Initialize members + self._cm_ns = [] # Namespace of the selected controller manager + self._controllers = [] # State of each controller + self._table_model = None + self._controller_lister = None + + # Controller manager service proxies + self._load_srv = None + self._unload_srv = None + self._switch_srv = None + + # Controller state icons + rospack = rospkg.RosPack() + path = rospack.get_path('rqt_controller_manager') + self._icons = {'running': QIcon(path + '/resource/led_green.png'), + 'stopped': QIcon(path + '/resource/led_red.png'), + 'uninitialized': QIcon(path + '/resource/led_off.png'), + 'initialized': QIcon(path + '/resource/led_red.png')} + + # Controllers display + table_view = self._widget.table_view + table_view.setContextMenuPolicy(Qt.CustomContextMenu) + table_view.customContextMenuRequested.connect(self._on_ctrl_menu) + + table_view.doubleClicked.connect(self._on_ctrl_info) + + header = table_view.horizontalHeader() + header.setSectionResizeMode(QHeaderView.ResizeToContents) + header.setContextMenuPolicy(Qt.CustomContextMenu) + header.customContextMenuRequested.connect(self._on_header_menu) + + # Timer for controller manager updates + self._list_cm = ControllerManagerLister() + self._update_cm_list_timer = QTimer(self) + self._update_cm_list_timer.setInterval(1000.0 / + self._cm_update_freq) + self._update_cm_list_timer.timeout.connect(self._update_cm_list) + self._update_cm_list_timer.start() + + # Timer for running controller updates + self._update_ctrl_list_timer = QTimer(self) + self._update_ctrl_list_timer.setInterval(1000.0 / + self._cm_update_freq) + self._update_ctrl_list_timer.timeout.connect(self._update_controllers) + self._update_ctrl_list_timer.start() + + # Signal connections + w = self._widget + w.cm_combo.currentIndexChanged[str].connect(self._on_cm_change) + + def shutdown_plugin(self): + self._update_cm_list_timer.stop() + self._update_ctrl_list_timer.stop() + self._popup_widget.hide() + + def save_settings(self, plugin_settings, instance_settings): + instance_settings.set_value('cm_ns', self._cm_ns) + + def restore_settings(self, plugin_settings, instance_settings): + # Restore last session's controller_manager, if present + self._update_cm_list() + cm_ns = instance_settings.value('cm_ns') + cm_combo = self._widget.cm_combo + cm_list = [cm_combo.itemText(i) for i in range(cm_combo.count())] + try: + idx = cm_list.index(cm_ns) + cm_combo.setCurrentIndex(idx) + except (ValueError): + pass + + # def trigger_configuration(self): + # Comment in to signal that the plugin has a way to configure + # This will enable a setting button (gear icon) in each dock widget + # title bar + # Usually used to open a modal configuration dialog + + def _update_cm_list(self): + update_combo(self._widget.cm_combo, self._list_cm()) + + def _on_cm_change(self, cm_ns): + self._cm_ns = cm_ns + + # Setup services for communicating with the selected controller manager + self._set_cm_services(cm_ns) + + # Controller lister for the selected controller manager + if cm_ns: + self._controller_lister = ControllerLister(cm_ns) + self._update_controllers() + else: + self._controller_lister = None + + def _set_cm_services(self, cm_ns): + if cm_ns: + # NOTE: Persistent services are used for performance reasons. + # If the controller manager dies, we detect it and disconnect from + # it anyway + load_srv_name = _append_ns(cm_ns, 'load_controller') + self._load_srv = rospy.ServiceProxy(load_srv_name, + LoadController, + persistent=True) + unload_srv_name = _append_ns(cm_ns, 'unload_controller') + self._unload_srv = rospy.ServiceProxy(unload_srv_name, + UnloadController, + persistent=True) + switch_srv_name = _append_ns(cm_ns, 'switch_controller') + self._switch_srv = rospy.ServiceProxy(switch_srv_name, + SwitchController, + persistent=True) + else: + self._load_srv = None + self._unload_srv = None + self._switch_srv = None + + def _update_controllers(self): + # Find controllers associated to the selected controller manager + controllers = self._list_controllers() + + # Update controller display, if necessary + if self._controllers != controllers: + self._controllers = controllers + self._show_controllers() # NOTE: Model is recomputed from scratch + + def _list_controllers(self): + """ + @return List of controllers associated to a controller manager + namespace. Contains both stopped/running controllers, as returned by + the C{list_controllers} service, plus uninitialized controllers with + configurations loaded in the parameter server. + @rtype [str] + """ + if not self._cm_ns: + return [] + + # Add loaded controllers first + controllers = self._controller_lister() + + # Append potential controller configs found in the parameter server + all_ctrls_ns = _resolve_controllers_ns(self._cm_ns) + for name in get_rosparam_controller_names(all_ctrls_ns): + add_ctrl = not any(name == ctrl.name for ctrl in controllers) + if add_ctrl: + type_str = _rosparam_controller_type(all_ctrls_ns, name) + uninit_ctrl = ControllerState(name=name, + type=type_str, + state='uninitialized') + controllers.append(uninit_ctrl) + return controllers + + def _show_controllers(self): + table_view = self._widget.table_view + self._table_model = ControllerTable(self._controllers, self._icons) + table_view.setModel(self._table_model) + + def _on_ctrl_menu(self, pos): + # Get data of selected controller + row = self._widget.table_view.rowAt(pos.y()) + if row < 0: + return # Cursor is not under a valid item + + ctrl = self._controllers[row] + + # Show context menu + menu = QMenu(self._widget.table_view) + if ctrl.state == 'running': + action_stop = menu.addAction(self._icons['stopped'], 'Stop') + action_kill = menu.addAction(self._icons['uninitialized'], + 'Stop and Unload') + elif ctrl.state == 'stopped': + action_start = menu.addAction(self._icons['running'], + 'Start again') + action_unload = menu.addAction(self._icons['uninitialized'], + 'Unload') + elif ctrl.state == 'initialized': + action_start = menu.addAction(self._icons['running'], 'Start') + action_unload = menu.addAction(self._icons['uninitialized'], + 'Unload') + elif ctrl.state == 'uninitialized': + action_load = menu.addAction(self._icons['stopped'], 'Load') + action_spawn = menu.addAction(self._icons['running'], + 'Load and Start') + + action = menu.exec_(self._widget.table_view.mapToGlobal(pos)) + + # Evaluate user action + if ctrl.state == 'running': + if action is action_stop: + self._stop_controller(ctrl.name) + elif action is action_kill: + self._stop_controller(ctrl.name) + self._unload_controller(ctrl.name) + elif ctrl.state == 'stopped' or ctrl.state == 'initialized': + if action is action_start: + self._start_controller(ctrl.name) + elif action is action_unload: + self._unload_controller(ctrl.name) + elif ctrl.state == 'uninitialized': + if action is action_load: + self._load_controller(ctrl.name) + if action is action_spawn: + self._load_controller(ctrl.name) + self._start_controller(ctrl.name) + + def _on_ctrl_info(self, index): + popup = self._popup_widget + + ctrl = self._controllers[index.row()] + popup.ctrl_name.setText(ctrl.name) + popup.ctrl_type.setText(ctrl.type) + + res_model = QStandardItemModel() + model_root = QStandardItem('Claimed Resources') + res_model.appendRow(model_root) + for hw_res in ctrl.claimed_resources: + hw_iface_item = QStandardItem(hw_res.hardware_interface) + model_root.appendRow(hw_iface_item) + for res in hw_res.resources: + res_item = QStandardItem(res) + hw_iface_item.appendRow(res_item) + + popup.resource_tree.setModel(res_model) + popup.resource_tree.setItemDelegate(FontDelegate(popup.resource_tree)) + popup.resource_tree.expandAll() + popup.move(QCursor.pos()) + popup.show() + + def _on_header_menu(self, pos): + header = self._widget.table_view.horizontalHeader() + + # Show context menu + menu = QMenu(self._widget.table_view) + action_toggle_auto_resize = menu.addAction('Toggle Auto-Resize') + action = menu.exec_(header.mapToGlobal(pos)) + + # Evaluate user action + if action is action_toggle_auto_resize: + if header.resizeMode(0) == QHeaderView.ResizeToContents: + header.setSectionResizeMode(QHeaderView.Interactive) + else: + header.setSectionResizeMode(QHeaderView.ResizeToContents) + + def _load_controller(self, name): + self._load_srv.call(LoadControllerRequest(name=name)) + + def _unload_controller(self, name): + self._unload_srv.call(UnloadControllerRequest(name=name)) + + def _start_controller(self, name): + strict = SwitchControllerRequest.STRICT + req = SwitchControllerRequest(start_controllers=[name], + stop_controllers=[], + strictness=strict) + self._switch_srv.call(req) + + def _stop_controller(self, name): + strict = SwitchControllerRequest.STRICT + req = SwitchControllerRequest(start_controllers=[], + stop_controllers=[name], + strictness=strict) + self._switch_srv.call(req) + + +class ControllerTable(QAbstractTableModel): + """ + Model containing controller information for tabular display. + + The model allows display of basic read-only information like controller + name and state. + """ + def __init__(self, controller_info, icons, parent=None): + QAbstractTableModel.__init__(self, parent) + self._data = controller_info + self._icons = icons + + def rowCount(self, parent): + return len(self._data) + + def columnCount(self, parent): + return 2 + + def headerData(self, col, orientation, role): + if orientation == Qt.Horizontal and role == Qt.DisplayRole: + if col == 0: + return 'controller' + elif col == 1: + return 'state' + else: + return None + + def data(self, index, role): + if not index.isValid(): + return None + + ctrl = self._data[index.row()] + + if role == Qt.DisplayRole: + if index.column() == 0: + return ctrl.name + elif index.column() == 1: + return ctrl.state + + if role == Qt.DecorationRole: + if index.column() == 0: + return self._icons[ctrl.state] + + if role == Qt.FontRole: + if index.column() == 0: + bf = QFont() + bf.setBold(True) + return bf + + if role == Qt.TextAlignmentRole: + if index.column() == 1: + return Qt.AlignCenter + + +class FontDelegate(QStyledItemDelegate): + """ + Simple delegate for customizing font weight and italization when + displaying resources claimed by a controller + """ + def paint(self, painter, option, index): + if not index.parent().isValid(): + # Root level + option.font.setWeight(QFont.Bold) + if index.parent().isValid() and not index.parent().parent().isValid(): + # Hardware interface level + option.font.setItalic(True) + option.font.setWeight(QFont.Bold) + QStyledItemDelegate.paint(self, painter, option, index) + + +def _resolve_controllers_ns(cm_ns): + """ + Resolve the namespace containing controller configurations from that of + the controller manager. + Controllers are assumed to live one level above the controller + manager, e.g. + + >>> _resolve_controller_ns('/path/to/controller_manager') + '/path/to' + + In the particular case in which the controller manager is not + namespaced, the controller is assumed to live in the root namespace + + >>> _resolve_controller_ns('/') + '/' + >>> _resolve_controller_ns('') + '/' + @param cm_ns Controller manager namespace (can be an empty string) + @type cm_ns str + @return Controllers namespace + @rtype str + """ + ns = cm_ns.rsplit('/', 1)[0] + if not ns: + ns += '/' + return ns + + +def _append_ns(in_ns, suffix): + """ + Append a sub-namespace (suffix) to the input namespace + @param in_ns Input namespace + @type in_ns str + @return Suffix namespace + @rtype str + """ + ns = in_ns + if ns[-1] != '/': + ns += '/' + ns += suffix + return ns + + +def _rosparam_controller_type(ctrls_ns, ctrl_name): + """ + Get a controller's type from its ROS parameter server configuration + @param ctrls_ns Namespace where controllers should be located + @type ctrls_ns str + @param ctrl_name Controller name + @type ctrl_name str + @return Controller type + @rtype str + """ + type_param = _append_ns(ctrls_ns, ctrl_name) + '/type' + return rospy.get_param(type_param) diff --git a/rqt_controller_manager/rqt_controller_manager/main.py b/rqt_controller_manager/rqt_controller_manager/main.py new file mode 100644 index 0000000000..4dd77d350b --- /dev/null +++ b/rqt_controller_manager/rqt_controller_manager/main.py @@ -0,0 +1,27 @@ +# Copyright (c) 2022 Kenji Brameld +# +# 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. + + +import sys + +from rqt_gui.main import Main + + +def main(): + main = Main() + sys.exit(main.main(sys.argv, standalone='rqt_controller_manager')) + + +if __name__ == '__main__': + main() diff --git a/rqt_controller_manager/rqt_controller_manager/update_combo.py b/rqt_controller_manager/rqt_controller_manager/update_combo.py new file mode 100644 index 0000000000..b06e38e453 --- /dev/null +++ b/rqt_controller_manager/rqt_controller_manager/update_combo.py @@ -0,0 +1,66 @@ +#!/usr/bin/env python + +# Copyright (C) 2014, PAL Robotics S.L. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions are met: +# * Redistributions of source code must retain the above copyright notice, +# this list of conditions and the following disclaimer. +# * Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in the +# documentation and/or other materials provided with the distribution. +# * Neither the name of PAL Robotics S.L. nor the names of its +# contributors may be used to endorse or promote products derived from +# this software without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. + +def update_combo(combo, new_vals): + """ + Update the contents of a combo box with a set of new values. + + If the previously selected element is still present in the new values, it + will remain as active selection, even if its index has changed. This will + not trigger any signal. + + If the previously selected element is no longer present in the new values, + the combo will unset its selection. This will trigger signals for changed + element. + """ + selected_val = combo.currentText() + old_vals = [combo.itemText(i) for i in range(combo.count())] + + # Check if combo items changed + if not _is_permutation(old_vals, new_vals): + # Determine if selected value is in the new list + selected_id = -1 + try: + selected_id = new_vals.index(selected_val) + except (ValueError): + combo.setCurrentIndex(-1) + + # Re-populate items + combo.blockSignals(True) # No need to notify these changes + combo.clear() + combo.insertItems(0, new_vals) + combo.setCurrentIndex(selected_id) # Restore selection + combo.blockSignals(False) + +def _is_permutation(a, b): + """ + @type a [] + @type b [] + @return True if C{a} is a permutation of C{b}, false otherwise + @rtype bool + """ + return len(a) == len(b) and sorted(a) == sorted(b) diff --git a/rqt_controller_manager/setup.cfg b/rqt_controller_manager/setup.cfg new file mode 100644 index 0000000000..df165e1674 --- /dev/null +++ b/rqt_controller_manager/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/rqt_controller_manager +[install] +install_scripts=$base/lib/rqt_controller_manager diff --git a/rqt_controller_manager/setup.py b/rqt_controller_manager/setup.py new file mode 100644 index 0000000000..3e350bd412 --- /dev/null +++ b/rqt_controller_manager/setup.py @@ -0,0 +1,29 @@ +from setuptools import setup +from glob import glob + +package_name = 'rqt_controller_manager' + +setup( + name=package_name, + version='0.0.0', + packages=[package_name], + data_files=[ + ('share/ament_index/resource_index/packages', ['resource/' + package_name]), + ('share/' + package_name, ['package.xml']), + ("share/" + package_name + "/resource", glob("resource/*.*")), + ("share/" + package_name, ["plugin.xml"]), + ], + install_requires=['setuptools'], + zip_safe=True, + maintainer='ijnek', + maintainer_email='kenjibrameld@gmail.com', + description='TODO: Package description', + license='TODO: License declaration', + tests_require=['pytest'], + entry_points={ + 'console_scripts': [ + "rqt_controller_manager = \ + rqt_controller_manager.main:main", + ], + }, +) diff --git a/rqt_controller_manager/test/test_copyright.py b/rqt_controller_manager/test/test_copyright.py new file mode 100644 index 0000000000..97a39196e8 --- /dev/null +++ b/rqt_controller_manager/test/test_copyright.py @@ -0,0 +1,25 @@ +# Copyright 2015 Open Source Robotics Foundation, Inc. +# +# 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. + +from ament_copyright.main import main +import pytest + + +# Remove the `skip` decorator once the source file(s) have a copyright header +@pytest.mark.skip(reason='No copyright header has been placed in the generated source file.') +@pytest.mark.copyright +@pytest.mark.linter +def test_copyright(): + rc = main(argv=['.', 'test']) + assert rc == 0, 'Found errors' diff --git a/rqt_controller_manager/test/test_flake8.py b/rqt_controller_manager/test/test_flake8.py new file mode 100644 index 0000000000..27ee1078ff --- /dev/null +++ b/rqt_controller_manager/test/test_flake8.py @@ -0,0 +1,25 @@ +# Copyright 2017 Open Source Robotics Foundation, Inc. +# +# 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. + +from ament_flake8.main import main_with_errors +import pytest + + +@pytest.mark.flake8 +@pytest.mark.linter +def test_flake8(): + rc, errors = main_with_errors(argv=[]) + assert rc == 0, \ + 'Found %d code style errors / warnings:\n' % len(errors) + \ + '\n'.join(errors) diff --git a/rqt_controller_manager/test/test_pep257.py b/rqt_controller_manager/test/test_pep257.py new file mode 100644 index 0000000000..b234a3840f --- /dev/null +++ b/rqt_controller_manager/test/test_pep257.py @@ -0,0 +1,23 @@ +# Copyright 2015 Open Source Robotics Foundation, Inc. +# +# 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. + +from ament_pep257.main import main +import pytest + + +@pytest.mark.linter +@pytest.mark.pep257 +def test_pep257(): + rc = main(argv=['.', 'test']) + assert rc == 0, 'Found code style errors / warnings' From dfef7d501192c8b5c3407a15b06e1ec23eba7c1d Mon Sep 17 00:00:00 2001 From: Kenji Brameld Date: Sun, 18 Sep 2022 10:01:26 +0900 Subject: [PATCH 02/25] port rqt_controller_manager package.xml from ros1 Signed-off-by: Kenji Brameld --- rqt_controller_manager/package.xml | 16 ++++++++++++++-- 1 file changed, 14 insertions(+), 2 deletions(-) diff --git a/rqt_controller_manager/package.xml b/rqt_controller_manager/package.xml index 786b49831c..b4bb4ee3ba 100644 --- a/rqt_controller_manager/package.xml +++ b/rqt_controller_manager/package.xml @@ -3,13 +3,25 @@ rqt_controller_manager 0.0.0 - TODO: Package description - ijnek + Graphical frontend for interacting with the controller manager. + TODO BSD + http://ros.org/wiki/rqt_controller_manager + https://github.com/ros-controls/ros2_control/issues + https://github.com/ros-controls/ros2_control + + Bence Magyar + Enrique Fernandez + Mathias Lüdtke Kelsey Hawkins Adolfo Rodríguez Tsouroukdissian + controller_manager_msgs + rclpy + rqt_gui + rqt_gui_py + ament_copyright ament_flake8 ament_pep257 From a7e72f598e2a964c2888a25d0009f82cea861576 Mon Sep 17 00:00:00 2001 From: Kenji Brameld Date: Sun, 18 Sep 2022 11:45:10 +0900 Subject: [PATCH 03/25] add controller_manager_utils package Signed-off-by: Kenji Brameld --- .../controller_manager_utils/__init__.py | 0 controller_manager_utils/package.xml | 18 +++++++++++++ .../resource/controller_manager_utils | 0 controller_manager_utils/setup.cfg | 4 +++ controller_manager_utils/setup.py | 25 +++++++++++++++++++ .../test/test_copyright.py | 25 +++++++++++++++++++ controller_manager_utils/test/test_flake8.py | 25 +++++++++++++++++++ controller_manager_utils/test/test_pep257.py | 23 +++++++++++++++++ 8 files changed, 120 insertions(+) create mode 100644 controller_manager_utils/controller_manager_utils/__init__.py create mode 100644 controller_manager_utils/package.xml create mode 100644 controller_manager_utils/resource/controller_manager_utils create mode 100644 controller_manager_utils/setup.cfg create mode 100644 controller_manager_utils/setup.py create mode 100644 controller_manager_utils/test/test_copyright.py create mode 100644 controller_manager_utils/test/test_flake8.py create mode 100644 controller_manager_utils/test/test_pep257.py diff --git a/controller_manager_utils/controller_manager_utils/__init__.py b/controller_manager_utils/controller_manager_utils/__init__.py new file mode 100644 index 0000000000..e69de29bb2 diff --git a/controller_manager_utils/package.xml b/controller_manager_utils/package.xml new file mode 100644 index 0000000000..7eccaef177 --- /dev/null +++ b/controller_manager_utils/package.xml @@ -0,0 +1,18 @@ + + + + controller_manager_utils + 0.0.0 + TODO: Package description + ijnek + TODO: License declaration + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + + ament_python + + diff --git a/controller_manager_utils/resource/controller_manager_utils b/controller_manager_utils/resource/controller_manager_utils new file mode 100644 index 0000000000..e69de29bb2 diff --git a/controller_manager_utils/setup.cfg b/controller_manager_utils/setup.cfg new file mode 100644 index 0000000000..ef29745e4d --- /dev/null +++ b/controller_manager_utils/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/controller_manager_utils +[install] +install_scripts=$base/lib/controller_manager_utils diff --git a/controller_manager_utils/setup.py b/controller_manager_utils/setup.py new file mode 100644 index 0000000000..886af656d7 --- /dev/null +++ b/controller_manager_utils/setup.py @@ -0,0 +1,25 @@ +from setuptools import setup + +package_name = 'controller_manager_utils' + +setup( + name=package_name, + version='0.0.0', + packages=[package_name], + data_files=[ + ('share/ament_index/resource_index/packages', + ['resource/' + package_name]), + ('share/' + package_name, ['package.xml']), + ], + install_requires=['setuptools'], + zip_safe=True, + maintainer='ijnek', + maintainer_email='kenjibrameld@gmail.com', + description='TODO: Package description', + license='TODO: License declaration', + tests_require=['pytest'], + entry_points={ + 'console_scripts': [ + ], + }, +) diff --git a/controller_manager_utils/test/test_copyright.py b/controller_manager_utils/test/test_copyright.py new file mode 100644 index 0000000000..97a39196e8 --- /dev/null +++ b/controller_manager_utils/test/test_copyright.py @@ -0,0 +1,25 @@ +# Copyright 2015 Open Source Robotics Foundation, Inc. +# +# 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. + +from ament_copyright.main import main +import pytest + + +# Remove the `skip` decorator once the source file(s) have a copyright header +@pytest.mark.skip(reason='No copyright header has been placed in the generated source file.') +@pytest.mark.copyright +@pytest.mark.linter +def test_copyright(): + rc = main(argv=['.', 'test']) + assert rc == 0, 'Found errors' diff --git a/controller_manager_utils/test/test_flake8.py b/controller_manager_utils/test/test_flake8.py new file mode 100644 index 0000000000..27ee1078ff --- /dev/null +++ b/controller_manager_utils/test/test_flake8.py @@ -0,0 +1,25 @@ +# Copyright 2017 Open Source Robotics Foundation, Inc. +# +# 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. + +from ament_flake8.main import main_with_errors +import pytest + + +@pytest.mark.flake8 +@pytest.mark.linter +def test_flake8(): + rc, errors = main_with_errors(argv=[]) + assert rc == 0, \ + 'Found %d code style errors / warnings:\n' % len(errors) + \ + '\n'.join(errors) diff --git a/controller_manager_utils/test/test_pep257.py b/controller_manager_utils/test/test_pep257.py new file mode 100644 index 0000000000..b234a3840f --- /dev/null +++ b/controller_manager_utils/test/test_pep257.py @@ -0,0 +1,23 @@ +# Copyright 2015 Open Source Robotics Foundation, Inc. +# +# 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. + +from ament_pep257.main import main +import pytest + + +@pytest.mark.linter +@pytest.mark.pep257 +def test_pep257(): + rc = main(argv=['.', 'test']) + assert rc == 0, 'Found code style errors / warnings' From b4d8440f66d01f4cf69357f50a955f3f92e9b61b Mon Sep 17 00:00:00 2001 From: Kenji Brameld Date: Sun, 18 Sep 2022 11:50:04 +0900 Subject: [PATCH 04/25] copy utils.py from ros1 Signed-off-by: Kenji Brameld --- .../controller_manager_utils/utils.py | 440 ++++++++++++++++++ 1 file changed, 440 insertions(+) create mode 100644 controller_manager_utils/controller_manager_utils/utils.py diff --git a/controller_manager_utils/controller_manager_utils/utils.py b/controller_manager_utils/controller_manager_utils/utils.py new file mode 100644 index 0000000000..c41a5ff3da --- /dev/null +++ b/controller_manager_utils/controller_manager_utils/utils.py @@ -0,0 +1,440 @@ +#!/usr/bin/env python + +# Copyright (C) 2014, PAL Robotics S.L. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions are met: +# * Redistributions of source code must retain the above copyright notice, +# this list of conditions and the following disclaimer. +# * Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in the +# documentation and/or other materials provided with the distribution. +# * Neither the name of PAL Robotics S.L. nor the names of its +# contributors may be used to endorse or promote products derived from +# this software without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. + +# NOTE: The Python API contained in this file is considered UNSTABLE and +# subject to change. +# No backwards compatibility guarrantees are provided at this moment. + + +############################################################################### +# +# Convenience methods for finding controller managers +# +############################################################################### + +import rosservice +from rospy import ServiceProxy +from controller_manager_msgs.srv import * + +# Names of controller manager services, and their respective types +_LIST_CONTROLLERS_STR = 'list_controllers' +_LIST_CONTROLLERS_TYPE = 'controller_manager_msgs/ListControllers' +_LIST_CONTROLLER_TYPES_STR = 'list_controller_types' +_LIST_CONTROLLER_TYPES_TYPE = 'controller_manager_msgs/ListControllerTypes' +_LOAD_CONTROLLER_STR = 'load_controller' +_LOAD_CONTROLLER_TYPE = 'controller_manager_msgs/LoadController' +_UNLOAD_CONTROLLER_STR = 'unload_controller' +_UNLOAD_CONTROLLER_TYPE = 'controller_manager_msgs/UnloadController' +_SWITCH_CONTROLLER_STR = 'switch_controller' +_SWITCH_CONTROLLER_TYPE = 'controller_manager_msgs/SwitchController' +_RELOAD_CONTROLLER_LIBS_STR = 'reload_controller_libraries' +_RELOAD_CONTROLLER_LIBS_TYPE = ('controller_manager_msgs/' + 'ReloadControllerLibraries') + +# Map from service names to their respective type +cm_services = { + _LIST_CONTROLLERS_STR: _LIST_CONTROLLERS_TYPE, + _LIST_CONTROLLER_TYPES_STR: _LIST_CONTROLLER_TYPES_TYPE, + _LOAD_CONTROLLER_STR: _LOAD_CONTROLLER_TYPE, + _UNLOAD_CONTROLLER_STR: _UNLOAD_CONTROLLER_TYPE, + _SWITCH_CONTROLLER_STR: _SWITCH_CONTROLLER_TYPE, + _RELOAD_CONTROLLER_LIBS_STR: _RELOAD_CONTROLLER_LIBS_TYPE +} + + +def get_controller_managers(namespace='/', initial_guess=None): + """ + Get list of active controller manager namespaces. + + @param namespace: Namespace where to look for controller managers. + @type namespace: str + @param initial_guess: Initial guess of the active controller managers. + Typically c{initial_guess} is the output of a previous call to this method, + and is useful when periodically checking for changes in the list of + active controller managers. + Elements in this list will go through a lazy validity check (as opposed to + a full name+type API verification), so providing a good estimate can + significantly reduce the number of ROS master queries incurred by this + method. + @type initial_guess: [str] + @return: Sorted list of active controller manager namespaces. + @rtype: [str] + """ + ns_list = [] + if initial_guess is not None: + ns_list = initial_guess[:] # force copy + + # Get list of (potential) currently running controller managers + ns_list_curr = _sloppy_get_controller_managers(namespace) + + # Update initial guess: + # 1. Remove entries not found in current list + # 2. Add new untracked controller managers + ns_list[:] = [ns for ns in ns_list if ns in ns_list_curr] + ns_list += [ns for ns in ns_list_curr + if ns not in ns_list and is_controller_manager(ns)] + + return sorted(ns_list) + + +def is_controller_manager(namespace): + """ + Check if the input namespace exposes the controller_manager ROS interface. + + This method has the overhead of several ROS master queries + (one per ROS API member). + + @param namespace: Namespace to check + @type namespace: str + @return: True if namespace exposes the controller_manager ROS interface + @rtype: bool + """ + cm_ns = namespace + if not cm_ns or cm_ns[-1] != '/': + cm_ns += '/' + for srv_name in cm_services.keys(): + if not _srv_exists(cm_ns + srv_name, cm_services[srv_name]): + return False + return True + + +def _sloppy_get_controller_managers(namespace): + """ + Get list of I{potential} active controller manager namespaces. + + The method name is prepended with I{sloppy}, and the returned list contains + I{potential} active controller managers because it does not perform a + thorough check of the expected ROS API. + It rather tries to minimize the number of ROS master queries. + + This method has the overhead of one ROS master query. + + @param namespace: Namespace where to look for controller managers. + @type namespace: str + @return: List of I{potential} active controller manager namespaces. + @rtype: [str] + """ + try: + # refresh the list of controller managers we can find + srv_list = rosservice.get_service_list(namespace=namespace) + except rosservice.ROSServiceIOException: + return [] + + ns_list = [] + for srv_name in srv_list: + match_str = '/' + _LIST_CONTROLLERS_STR + if match_str in srv_name: + ns = srv_name.split(match_str)[0] + if ns == '': + # controller manager services live in root namespace + # unlikely, but still possible + ns = '/' + ns_list.append(ns) + return ns_list + + +def _srv_exists(srv_name, srv_type): + """ + Check if a ROS service of specific name and type exists. + + This method has the overhead of one ROS master query. + + @param srv_name: Fully qualified service name + @type srv_name: str + @param srv_type: Service type + @type srv_type: str + """ + if not srv_name or not srv_type: + return False + return srv_type == rosservice.get_service_type(srv_name) + + +############################################################################### +# +# Convenience classes for querying controller managers and controllers +# +############################################################################### + +class ControllerManagerLister: + """ + Convenience functor for querying the list of active controller managers. + + Useful when frequently updating the list, as it internally performs + some optimizations that reduce the number of interactions with the + ROS master. + + Example usage: + >>> list_cm = ControllerManagerLister() + >>> print(list_cm()) + """ + def __init__(self, namespace='/'): + self._ns = namespace + self._cm_list = [] + + def __call__(self): + """ + Get list of running controller managers. + """ + self._cm_list = get_controller_managers(self._ns, self._cm_list) + return self._cm_list + + +class ControllerLister: + """ + Convenience functor for querying loaded controller data. + + The output of calling this functor can be used as input to the different + controller filtering functions available in this module. + + Example usage. Get I{running} controllers of type C{bar_base/bar}: + >>> list_controllers = ControllerLister('foo_robot/controller_manager') + >>> all_ctrl = list_controllers() + >>> running_ctrl = filter_by_state(all_ctrl, 'running') + >>> running_bar_ctrl = filter_by_type(running_ctrl, 'bar_base/bar') + """ + def __init__(self, namespace='/controller_manager'): + """ + @param namespace Namespace of controller manager to monitor. + @type namespace str + """ + self._srv_name = namespace + '/' + _LIST_CONTROLLERS_STR + self._srv = self._make_srv() + + """ + @return: Controller list. + @rtype: [controller_manager_msgs/ControllerState] + """ + def __call__(self): + try: + controller_list = self._srv.call().controller + except: + # Attempt service proxy reconnection + self._srv = self._make_srv() + try: + controller_list = self._srv.call().controller + except: + controller_list = [] + return controller_list + + def _make_srv(self): + return ServiceProxy(self._srv_name, + ListControllers, + persistent=True) # NOTE: For performance + + +############################################################################### +# +# Convenience methods for filtering controller state information +# +############################################################################### + + +def filter_by_name(ctrl_list, ctrl_name, match_substring=False): + """ + Filter controller state list by controller name. + + @param ctrl_list: Controller state list + @type ctrl_list: [controller_manager_msgs/ControllerState] + @param ctrl_name: Controller name + @type ctrl_name: str + @param match_substring: Set to True to allow substring matching + @type match_substring: bool + @return: Controllers matching the specified name + @rtype: [controller_manager_msgs/ControllerState] + """ + return _filter_by_attr(ctrl_list, 'name', ctrl_name, match_substring) + + +def filter_by_type(ctrl_list, ctrl_type, match_substring=False): + """ + Filter controller state list by controller type. + + @param ctrl_list: Controller state list + @type ctrl_list: [controller_manager_msgs/ControllerState] + @param ctrl_type: Controller type + @type ctrl_type: str + @param match_substring: Set to True to allow substring matching + @type match_substring: bool + @return: Controllers matching the specified type + @rtype: [controller_manager_msgs/ControllerState] + """ + return _filter_by_attr(ctrl_list, 'type', ctrl_type, match_substring) + + +def filter_by_state(ctrl_list, ctrl_state, match_substring=False): + """ + Filter controller state list by controller state. + + @param ctrl_list: Controller state list + @type ctrl_list: [controller_manager_msgs/ControllerState] + @param ctrl_state: Controller state + @type ctrl_state: str + @param match_substring: Set to True to allow substring matching + @type match_substring: bool + @return: Controllers matching the specified state + @rtype: [controller_manager_msgs/ControllerState] + """ + return _filter_by_attr(ctrl_list, 'state', ctrl_state, match_substring) + + +def filter_by_hardware_interface(ctrl_list, + hardware_interface, + match_substring=False): + """ + Filter controller state list by controller hardware interface. + + @param ctrl_list: Controller state list + @type ctrl_list: [controller_manager_msgs/ControllerState] + @param hardware_interface: Controller hardware interface + @type hardware_interface: str + @param match_substring: Set to True to allow substring matching + @type match_substring: bool + @return: Controllers matching the specified hardware interface + @rtype: [controller_manager_msgs/ControllerState] + """ + list_out = [] + for ctrl in ctrl_list: + for resource_set in ctrl.claimed_resources: + if match_substring: + if hardware_interface in resource_set.hardware_interface: + list_out.append(ctrl) + break + else: + if resource_set.hardware_interface == hardware_interface: + list_out.append(ctrl) + break + return list_out + + +def filter_by_resources(ctrl_list, + resources, + hardware_interface=None, + match_any=False): + """ + Filter controller state list by claimed resources. + + @param ctrl_list: Controller state list + @type ctrl_list: [controller_manager_msgs/ControllerState] + @param resources: Controller resources + @type resources: [str] + @param hardware_interface Controller hardware interface where to look for + resources. If specified, the requested resources will only be searched for + in this interface. If unspecified, all controller hardware interfaces will + be searched for; i.e., if a controller claims resources from multiple + interfaces, the method will succeed if _any_ interface contains the + requested resources (any or all, depending on the value of C{match_any}). + Specifying this parameter allows finer control over determining which + interfaces claim specific resources. + @param match_any: If set to False, all elements in C{resources} must + be claimed by the interface specified in C{hardware_interface} (or _any_ + interface, if C{hardware_interface} is unspecified) for a positive match. + Note that a controller's resources can contain additional entries than + those in C{resources}). + If set to True, at least one element in C{resources} must be claimed by + the interface specified in C{hardware_interface} (or _any_ interface, if + C{hardware_interface} is unspecified) for a positive match. + @type match_any: bool + @return: Controllers matching the specified hardware interface + @rtype: [controller_manager_msgs/ControllerState] + """ + list_out = [] + for ctrl in ctrl_list: + for resource_set in ctrl.claimed_resources: + if (hardware_interface is None or + hardware_interface == resource_set.hardware_interface): + for res in resources: + add_ctrl = not match_any # Initial flag value + if res in resource_set.resources: + if match_any: # One hit: enough to accept controller + add_ctrl = True + break + else: + if not match_any: # One miss: enough to discard controller + add_ctrl = False + break + if add_ctrl: + list_out.append(ctrl) + break + return list_out + + +def _filter_by_attr(list_in, attr_name, attr_val, match_substring=False): + """ + Filter input list by the value of its elements' attributes. + """ + list_out = [] + for val in list_in: + if match_substring: + if attr_val in getattr(val, attr_name): + list_out.append(val) + else: + if getattr(val, attr_name) == attr_val: + list_out.append(val) + return list_out + +############################################################################### +# +# Convenience methods for finding potential controller configurations +# +############################################################################### + +def get_rosparam_controller_names(namespace='/'): + """ + Get list of ROS parameter names that potentially represent a controller + configuration. + + Example usage: + - Assume the following parameters exist in the ROS parameter: + server: + - C{/foo/type} + - C{/xxx/type/xxx} + - C{/ns/bar/type} + - C{/ns/yyy/type/yyy} + - The potential controllers found by this method are: + + >>> names = get_rosparam_controller_names() # returns ['foo'] + >>> names_ns = get_rosparam_controller_names('/ns') # returns ['bar'] + + @param namespace: Namespace where to look for controllers. + @type namespace: str + @return: Sorted list of ROS parameter names. + @rtype: [str] + """ + import rosparam + list_out = [] + all_params = rosparam.list_params(namespace) + for param in all_params: + # Remove namespace from parameter string + if not namespace or namespace[-1] != '/': + namespace += '/' + param_no_ns = param.split(namespace, 1)[1] + + # Check if parameter corresponds to a controller configuration + param_split = param_no_ns.split('/') + if (len(param_split) == 2 and param_split[1] == 'type'): + list_out.append(param_split[0]) # It does! + return sorted(list_out) From 8888a48e98edf3605bf92949a9149f7f17e1b14d Mon Sep 17 00:00:00 2001 From: Kenji Brameld Date: Sun, 18 Sep 2022 11:51:36 +0900 Subject: [PATCH 05/25] override utils.py with one from rqt_joint_trajectory_controlle Signed-off-by: Kenji Brameld --- .../controller_manager_utils/utils.py | 263 ++++++++---------- 1 file changed, 117 insertions(+), 146 deletions(-) diff --git a/controller_manager_utils/controller_manager_utils/utils.py b/controller_manager_utils/controller_manager_utils/utils.py index c41a5ff3da..577cc1ab07 100644 --- a/controller_manager_utils/controller_manager_utils/utils.py +++ b/controller_manager_utils/controller_manager_utils/utils.py @@ -1,72 +1,53 @@ #!/usr/bin/env python -# Copyright (C) 2014, PAL Robotics S.L. +# Copyright 2022 PAL Robotics S.L. # -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions are met: -# * Redistributions of source code must retain the above copyright notice, -# this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above copyright -# notice, this list of conditions and the following disclaimer in the -# documentation and/or other materials provided with the distribution. -# * Neither the name of PAL Robotics S.L. nor the names of its -# contributors may be used to endorse or promote products derived from -# this software without specific prior written permission. +# 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 # -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE -# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE -# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR -# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF -# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS -# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN -# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) -# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. +# 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. # NOTE: The Python API contained in this file is considered UNSTABLE and # subject to change. -# No backwards compatibility guarrantees are provided at this moment. +# No backwards compatibility guarantees are provided at this moment. -############################################################################### -# -# Convenience methods for finding controller managers -# -############################################################################### - -import rosservice -from rospy import ServiceProxy -from controller_manager_msgs.srv import * +import rclpy +from controller_manager_msgs.srv import ListControllers # Names of controller manager services, and their respective types -_LIST_CONTROLLERS_STR = 'list_controllers' -_LIST_CONTROLLERS_TYPE = 'controller_manager_msgs/ListControllers' -_LIST_CONTROLLER_TYPES_STR = 'list_controller_types' -_LIST_CONTROLLER_TYPES_TYPE = 'controller_manager_msgs/ListControllerTypes' -_LOAD_CONTROLLER_STR = 'load_controller' -_LOAD_CONTROLLER_TYPE = 'controller_manager_msgs/LoadController' -_UNLOAD_CONTROLLER_STR = 'unload_controller' -_UNLOAD_CONTROLLER_TYPE = 'controller_manager_msgs/UnloadController' -_SWITCH_CONTROLLER_STR = 'switch_controller' -_SWITCH_CONTROLLER_TYPE = 'controller_manager_msgs/SwitchController' -_RELOAD_CONTROLLER_LIBS_STR = 'reload_controller_libraries' -_RELOAD_CONTROLLER_LIBS_TYPE = ('controller_manager_msgs/' - 'ReloadControllerLibraries') +_LIST_CONTROLLERS_STR = "list_controllers" +_LIST_CONTROLLERS_TYPE = "controller_manager_msgs/srv/ListControllers" +_LIST_CONTROLLER_TYPES_STR = "list_controller_types" +_LIST_CONTROLLER_TYPES_TYPE = "controller_manager_msgs/srv/ListControllerTypes" +_LOAD_CONTROLLER_STR = "load_controller" +_LOAD_CONTROLLER_TYPE = "controller_manager_msgs/srv/LoadController" +_UNLOAD_CONTROLLER_STR = "unload_controller" +_UNLOAD_CONTROLLER_TYPE = "controller_manager_msgs/srv/UnloadController" +_SWITCH_CONTROLLER_STR = "switch_controller" +_SWITCH_CONTROLLER_TYPE = "controller_manager_msgs/srv/SwitchController" +_RELOAD_CONTROLLER_LIBS_STR = "reload_controller_libraries" +_RELOAD_CONTROLLER_LIBS_TYPE = "controller_manager_msgs/srv/" "ReloadControllerLibraries" # Map from service names to their respective type cm_services = { - _LIST_CONTROLLERS_STR: _LIST_CONTROLLERS_TYPE, - _LIST_CONTROLLER_TYPES_STR: _LIST_CONTROLLER_TYPES_TYPE, - _LOAD_CONTROLLER_STR: _LOAD_CONTROLLER_TYPE, - _UNLOAD_CONTROLLER_STR: _UNLOAD_CONTROLLER_TYPE, - _SWITCH_CONTROLLER_STR: _SWITCH_CONTROLLER_TYPE, - _RELOAD_CONTROLLER_LIBS_STR: _RELOAD_CONTROLLER_LIBS_TYPE + _LIST_CONTROLLERS_STR: _LIST_CONTROLLERS_TYPE, + _LIST_CONTROLLER_TYPES_STR: _LIST_CONTROLLER_TYPES_TYPE, + _LOAD_CONTROLLER_STR: _LOAD_CONTROLLER_TYPE, + _UNLOAD_CONTROLLER_STR: _UNLOAD_CONTROLLER_TYPE, + _SWITCH_CONTROLLER_STR: _SWITCH_CONTROLLER_TYPE, + _RELOAD_CONTROLLER_LIBS_STR: _RELOAD_CONTROLLER_LIBS_TYPE, } -def get_controller_managers(namespace='/', initial_guess=None): +def get_controller_managers(namespace="/", initial_guess=None): """ Get list of active controller manager namespaces. @@ -89,19 +70,19 @@ def get_controller_managers(namespace='/', initial_guess=None): ns_list = initial_guess[:] # force copy # Get list of (potential) currently running controller managers - ns_list_curr = _sloppy_get_controller_managers(namespace) + node = rclpy.node.Node("get_controller_managers_node") + ns_list_curr = _sloppy_get_controller_managers(node, namespace) # Update initial guess: # 1. Remove entries not found in current list # 2. Add new untracked controller managers ns_list[:] = [ns for ns in ns_list if ns in ns_list_curr] - ns_list += [ns for ns in ns_list_curr - if ns not in ns_list and is_controller_manager(ns)] + ns_list += [ns for ns in ns_list_curr if ns not in ns_list and is_controller_manager(node, ns)] return sorted(ns_list) -def is_controller_manager(namespace): +def is_controller_manager(node, namespace): """ Check if the input namespace exposes the controller_manager ROS interface. @@ -114,15 +95,15 @@ def is_controller_manager(namespace): @rtype: bool """ cm_ns = namespace - if not cm_ns or cm_ns[-1] != '/': - cm_ns += '/' + if not cm_ns or cm_ns[-1] != "/": + cm_ns += "/" for srv_name in cm_services.keys(): - if not _srv_exists(cm_ns + srv_name, cm_services[srv_name]): + if not _srv_exists(node, cm_ns + srv_name, cm_services[srv_name]): return False return True -def _sloppy_get_controller_managers(namespace): +def _sloppy_get_controller_managers(node, namespace): """ Get list of I{potential} active controller manager namespaces. @@ -138,26 +119,24 @@ def _sloppy_get_controller_managers(namespace): @return: List of I{potential} active controller manager namespaces. @rtype: [str] """ - try: - # refresh the list of controller managers we can find - srv_list = rosservice.get_service_list(namespace=namespace) - except rosservice.ROSServiceIOException: - return [] + # refresh the list of controller managers we can find + srv_list = node.get_service_names_and_types() ns_list = [] - for srv_name in srv_list: - match_str = '/' + _LIST_CONTROLLERS_STR - if match_str in srv_name: - ns = srv_name.split(match_str)[0] - if ns == '': + for srv_info in srv_list: + match_str = "/" + _LIST_CONTROLLERS_STR + # First element of srv_name is the service name + if match_str in srv_info[0]: + ns = srv_info[0].split(match_str)[0] + if ns == "": # controller manager services live in root namespace # unlikely, but still possible - ns = '/' + ns = "/" ns_list.append(ns) return ns_list -def _srv_exists(srv_name, srv_type): +def _srv_exists(node, srv_name, srv_type): """ Check if a ROS service of specific name and type exists. @@ -170,7 +149,11 @@ def _srv_exists(srv_name, srv_type): """ if not srv_name or not srv_type: return False - return srv_type == rosservice.get_service_type(srv_name) + + srv_list = node.get_service_names_and_types() + srv_info = [item for item in srv_list if item[0] == srv_name] + srv_obtained_type = srv_info[0][1][0] + return srv_type == srv_obtained_type ############################################################################### @@ -179,6 +162,7 @@ def _srv_exists(srv_name, srv_type): # ############################################################################### + class ControllerManagerLister: """ Convenience functor for querying the list of active controller managers. @@ -191,14 +175,13 @@ class ControllerManagerLister: >>> list_cm = ControllerManagerLister() >>> print(list_cm()) """ - def __init__(self, namespace='/'): + + def __init__(self, namespace="/"): self._ns = namespace self._cm_list = [] def __call__(self): - """ - Get list of running controller managers. - """ + """Get list of running controller managers.""" self._cm_list = get_controller_managers(self._ns, self._cm_list) return self._cm_list @@ -216,34 +199,29 @@ class ControllerLister: >>> running_ctrl = filter_by_state(all_ctrl, 'running') >>> running_bar_ctrl = filter_by_type(running_ctrl, 'bar_base/bar') """ - def __init__(self, namespace='/controller_manager'): + + def __init__(self, namespace="/controller_manager"): """ @param namespace Namespace of controller manager to monitor. + @type namespace str """ - self._srv_name = namespace + '/' + _LIST_CONTROLLERS_STR - self._srv = self._make_srv() + self._node = rclpy.node.Node("controller_lister") + self._srv_name = namespace + "/" + _LIST_CONTROLLERS_STR + self._srv_client = self._create_client() """ @return: Controller list. @rtype: [controller_manager_msgs/ControllerState] """ + def __call__(self): - try: - controller_list = self._srv.call().controller - except: - # Attempt service proxy reconnection - self._srv = self._make_srv() - try: - controller_list = self._srv.call().controller - except: - controller_list = [] - return controller_list - - def _make_srv(self): - return ServiceProxy(self._srv_name, - ListControllers, - persistent=True) # NOTE: For performance + controller_list = self._srv_client.call_async(ListControllers.Request()) + rclpy.spin_until_future_complete(self._node, controller_list) + return controller_list.result().controller + + def _create_client(self): + return self._node.create_client(ListControllers, self._srv_name) ############################################################################### @@ -266,7 +244,7 @@ def filter_by_name(ctrl_list, ctrl_name, match_substring=False): @return: Controllers matching the specified name @rtype: [controller_manager_msgs/ControllerState] """ - return _filter_by_attr(ctrl_list, 'name', ctrl_name, match_substring) + return _filter_by_attr(ctrl_list, "name", ctrl_name, match_substring) def filter_by_type(ctrl_list, ctrl_type, match_substring=False): @@ -282,7 +260,7 @@ def filter_by_type(ctrl_list, ctrl_type, match_substring=False): @return: Controllers matching the specified type @rtype: [controller_manager_msgs/ControllerState] """ - return _filter_by_attr(ctrl_list, 'type', ctrl_type, match_substring) + return _filter_by_attr(ctrl_list, "type", ctrl_type, match_substring) def filter_by_state(ctrl_list, ctrl_state, match_substring=False): @@ -298,12 +276,10 @@ def filter_by_state(ctrl_list, ctrl_state, match_substring=False): @return: Controllers matching the specified state @rtype: [controller_manager_msgs/ControllerState] """ - return _filter_by_attr(ctrl_list, 'state', ctrl_state, match_substring) + return _filter_by_attr(ctrl_list, "state", ctrl_state, match_substring) -def filter_by_hardware_interface(ctrl_list, - hardware_interface, - match_substring=False): +def filter_by_hardware_interface(ctrl_list, hardware_interface, match_substring=False): """ Filter controller state list by controller hardware interface. @@ -330,10 +306,7 @@ def filter_by_hardware_interface(ctrl_list, return list_out -def filter_by_resources(ctrl_list, - resources, - hardware_interface=None, - match_any=False): +def filter_by_resources(ctrl_list, resources, hardware_interface=None, match_any=False): """ Filter controller state list by claimed resources. @@ -364,8 +337,7 @@ def filter_by_resources(ctrl_list, list_out = [] for ctrl in ctrl_list: for resource_set in ctrl.claimed_resources: - if (hardware_interface is None or - hardware_interface == resource_set.hardware_interface): + if hardware_interface is None or hardware_interface == resource_set.hardware_interface: for res in resources: add_ctrl = not match_any # Initial flag value if res in resource_set.resources: @@ -383,9 +355,7 @@ def filter_by_resources(ctrl_list, def _filter_by_attr(list_in, attr_name, attr_val, match_substring=False): - """ - Filter input list by the value of its elements' attributes. - """ + """Filter input list by the value of its elements' attributes.""" list_out = [] for val in list_in: if match_substring: @@ -396,45 +366,46 @@ def _filter_by_attr(list_in, attr_name, attr_val, match_substring=False): list_out.append(val) return list_out + ############################################################################### # # Convenience methods for finding potential controller configurations # ############################################################################### -def get_rosparam_controller_names(namespace='/'): - """ - Get list of ROS parameter names that potentially represent a controller - configuration. - - Example usage: - - Assume the following parameters exist in the ROS parameter: - server: - - C{/foo/type} - - C{/xxx/type/xxx} - - C{/ns/bar/type} - - C{/ns/yyy/type/yyy} - - The potential controllers found by this method are: - - >>> names = get_rosparam_controller_names() # returns ['foo'] - >>> names_ns = get_rosparam_controller_names('/ns') # returns ['bar'] - - @param namespace: Namespace where to look for controllers. - @type namespace: str - @return: Sorted list of ROS parameter names. - @rtype: [str] - """ - import rosparam - list_out = [] - all_params = rosparam.list_params(namespace) - for param in all_params: - # Remove namespace from parameter string - if not namespace or namespace[-1] != '/': - namespace += '/' - param_no_ns = param.split(namespace, 1)[1] - - # Check if parameter corresponds to a controller configuration - param_split = param_no_ns.split('/') - if (len(param_split) == 2 and param_split[1] == 'type'): - list_out.append(param_split[0]) # It does! - return sorted(list_out) +# def get_rosparam_controller_names(namespace='/'): +# """ +# Get list of ROS parameter names that potentially represent a controller +# configuration. + +# Example usage: +# - Assume the following parameters exist in the ROS parameter: +# server: +# - C{/foo/type} +# - C{/xxx/type/xxx} +# - C{/ns/bar/type} +# - C{/ns/yyy/type/yyy} +# - The potential controllers found by this method are: + +# >>> names = get_rosparam_controller_names() # returns ['foo'] +# >>> names_ns = get_rosparam_controller_names('/ns') # returns ['bar'] + +# @param namespace: Namespace where to look for controllers. +# @type namespace: str +# @return: Sorted list of ROS parameter names. +# @rtype: [str] +# """ +# import rosparam +# list_out = [] +# all_params = rosparam.list_params(namespace) +# for param in all_params: +# # Remove namespace from parameter string +# if not namespace or namespace[-1] != '/': +# namespace += '/' +# param_no_ns = param.split(namespace, 1)[1] + +# # Check if parameter corresponds to a controller configuration +# param_split = param_no_ns.split('/') +# if (len(param_split) == 2 and param_split[1] == 'type'): +# list_out.append(param_split[0]) # It does! +# return sorted(list_out) From f870e6983c5c260187960f8c20d4317179fae8b1 Mon Sep 17 00:00:00 2001 From: Kenji Brameld Date: Sun, 18 Sep 2022 12:06:24 +0900 Subject: [PATCH 06/25] updates for ros2 Signed-off-by: Kenji Brameld --- .../controller_manager_utils/utils.py | 3 +- rqt_controller_manager/package.xml | 1 + .../controller_manager.py | 52 ++++++++++--------- 3 files changed, 30 insertions(+), 26 deletions(-) diff --git a/controller_manager_utils/controller_manager_utils/utils.py b/controller_manager_utils/controller_manager_utils/utils.py index 577cc1ab07..60219bb0ec 100644 --- a/controller_manager_utils/controller_manager_utils/utils.py +++ b/controller_manager_utils/controller_manager_utils/utils.py @@ -373,7 +373,8 @@ def _filter_by_attr(list_in, attr_name, attr_val, match_substring=False): # ############################################################################### -# def get_rosparam_controller_names(namespace='/'): +def get_rosparam_controller_names(namespace='/'): + pass # """ # Get list of ROS parameter names that potentially represent a controller # configuration. diff --git a/rqt_controller_manager/package.xml b/rqt_controller_manager/package.xml index b4bb4ee3ba..4fe1dc3c0f 100644 --- a/rqt_controller_manager/package.xml +++ b/rqt_controller_manager/package.xml @@ -18,6 +18,7 @@ Adolfo Rodríguez Tsouroukdissian controller_manager_msgs + controller_manager_utils rclpy rqt_gui rqt_gui_py diff --git a/rqt_controller_manager/rqt_controller_manager/controller_manager.py b/rqt_controller_manager/rqt_controller_manager/controller_manager.py index f913d731cd..59cd9afdab 100644 --- a/rqt_controller_manager/rqt_controller_manager/controller_manager.py +++ b/rqt_controller_manager/rqt_controller_manager/controller_manager.py @@ -26,9 +26,8 @@ # POSSIBILITY OF SUCH DAMAGE. import os -import rospkg -import rospy +from ament_index_python.packages import get_package_share_directory from python_qt_binding import loadUi from python_qt_binding.QtCore import QAbstractTableModel, QModelIndex, Qt,\ QTimer, QVariant, Signal @@ -40,7 +39,7 @@ from controller_manager_msgs.msg import ControllerState from controller_manager_msgs.srv import * -from controller_manager_msgs.utils\ +from controller_manager_utils.utils\ import ControllerLister, ControllerManagerLister,\ get_rosparam_controller_names @@ -60,18 +59,19 @@ def __init__(self, context): # Create QWidget and extend it with all the attributes and children # from the UI file self._widget = QWidget() - rp = rospkg.RosPack() - ui_file = os.path.join(rp.get_path('rqt_controller_manager'), - 'resource', - 'controller_manager.ui') + ui_file = os.path.join( + get_package_share_directory("rqt_controller_manager"), + "resource", + "controller_manager.ui") loadUi(ui_file, self._widget) self._widget.setObjectName('ControllerManagerUi') # Pop-up that displays controller information self._popup_widget = QWidget() - ui_file = os.path.join(rp.get_path('rqt_controller_manager'), - 'resource', - 'controller_info.ui') + ui_file = os.path.join( + get_package_share_directory("rqt_controller_manager"), + 'resource', + 'controller_info.ui') loadUi(ui_file, self._popup_widget) self._popup_widget.setObjectName('ControllerInfoUi') @@ -97,9 +97,11 @@ def __init__(self, context): self._unload_srv = None self._switch_srv = None + # Store reference to node + self._node = context.node + # Controller state icons - rospack = rospkg.RosPack() - path = rospack.get_path('rqt_controller_manager') + path = get_package_share_directory("rqt_controller_manager") self._icons = {'running': QIcon(path + '/resource/led_green.png'), 'stopped': QIcon(path + '/resource/led_red.png'), 'uninitialized': QIcon(path + '/resource/led_off.png'), @@ -119,16 +121,16 @@ def __init__(self, context): # Timer for controller manager updates self._list_cm = ControllerManagerLister() + # self._list_cm = list_controllers(context.node, parsed_args.controller_manager).controller + # self._list_cm = list_controllers(context.node, '/controller_manager').controller self._update_cm_list_timer = QTimer(self) - self._update_cm_list_timer.setInterval(1000.0 / - self._cm_update_freq) + self._update_cm_list_timer.setInterval(int(1000.0 / self._cm_update_freq)) self._update_cm_list_timer.timeout.connect(self._update_cm_list) self._update_cm_list_timer.start() # Timer for running controller updates self._update_ctrl_list_timer = QTimer(self) - self._update_ctrl_list_timer.setInterval(1000.0 / - self._cm_update_freq) + self._update_ctrl_list_timer.setInterval(int(1000.0 / self._cm_update_freq)) self._update_ctrl_list_timer.timeout.connect(self._update_controllers) self._update_ctrl_list_timer.start() @@ -184,17 +186,17 @@ def _set_cm_services(self, cm_ns): # If the controller manager dies, we detect it and disconnect from # it anyway load_srv_name = _append_ns(cm_ns, 'load_controller') - self._load_srv = rospy.ServiceProxy(load_srv_name, - LoadController, - persistent=True) + self._load_srv = self._node.create_client(LoadController, + load_srv_name) + # persistent=True) unload_srv_name = _append_ns(cm_ns, 'unload_controller') - self._unload_srv = rospy.ServiceProxy(unload_srv_name, - UnloadController, - persistent=True) + self._unload_srv = self._node.create_client(UnloadController, + unload_srv_name) + # persistent=True) switch_srv_name = _append_ns(cm_ns, 'switch_controller') - self._switch_srv = rospy.ServiceProxy(switch_srv_name, - SwitchController, - persistent=True) + self._switch_srv = self._node.create_client(SwitchController, + switch_srv_name) + # persistent=True) else: self._load_srv = None self._unload_srv = None From d1e207f6805e691d1a5fb4f71273ebe9c4e5232d Mon Sep 17 00:00:00 2001 From: Kenji Brameld Date: Sun, 18 Sep 2022 14:38:34 +0900 Subject: [PATCH 07/25] got controllers loading and unloading Signed-off-by: Kenji Brameld --- .../controller_manager.py | 133 +++++++++--------- 1 file changed, 67 insertions(+), 66 deletions(-) diff --git a/rqt_controller_manager/rqt_controller_manager/controller_manager.py b/rqt_controller_manager/rqt_controller_manager/controller_manager.py index 59cd9afdab..caa6940c2d 100644 --- a/rqt_controller_manager/rqt_controller_manager/controller_manager.py +++ b/rqt_controller_manager/rqt_controller_manager/controller_manager.py @@ -38,10 +38,11 @@ from qt_gui.plugin import Plugin from controller_manager_msgs.msg import ControllerState -from controller_manager_msgs.srv import * +from controller_manager_msgs.srv import LoadController, SwitchController, UnloadController from controller_manager_utils.utils\ import ControllerLister, ControllerManagerLister,\ get_rosparam_controller_names +from controller_manager.controller_manager_services import list_controllers from .update_combo import update_combo @@ -102,10 +103,10 @@ def __init__(self, context): # Controller state icons path = get_package_share_directory("rqt_controller_manager") - self._icons = {'running': QIcon(path + '/resource/led_green.png'), - 'stopped': QIcon(path + '/resource/led_red.png'), - 'uninitialized': QIcon(path + '/resource/led_off.png'), - 'initialized': QIcon(path + '/resource/led_red.png')} + self._icons = {'active': QIcon(path + '/resource/led_green.png'), + 'finalized': QIcon(path + '/resource/led_red.png'), + 'unconfigured': QIcon(path + '/resource/led_off.png'), + 'inactive': QIcon(path + '/resource/led_red.png')} # Controllers display table_view = self._widget.table_view @@ -188,15 +189,12 @@ def _set_cm_services(self, cm_ns): load_srv_name = _append_ns(cm_ns, 'load_controller') self._load_srv = self._node.create_client(LoadController, load_srv_name) - # persistent=True) unload_srv_name = _append_ns(cm_ns, 'unload_controller') self._unload_srv = self._node.create_client(UnloadController, unload_srv_name) - # persistent=True) switch_srv_name = _append_ns(cm_ns, 'switch_controller') self._switch_srv = self._node.create_client(SwitchController, switch_srv_name) - # persistent=True) else: self._load_srv = None self._unload_srv = None @@ -204,38 +202,41 @@ def _set_cm_services(self, cm_ns): def _update_controllers(self): # Find controllers associated to the selected controller manager - controllers = self._list_controllers() + controllers = list_controllers(self._node, self._cm_ns).controller + + if not controllers: + return # Update controller display, if necessary if self._controllers != controllers: self._controllers = controllers self._show_controllers() # NOTE: Model is recomputed from scratch - def _list_controllers(self): - """ - @return List of controllers associated to a controller manager - namespace. Contains both stopped/running controllers, as returned by - the C{list_controllers} service, plus uninitialized controllers with - configurations loaded in the parameter server. - @rtype [str] - """ - if not self._cm_ns: - return [] - - # Add loaded controllers first - controllers = self._controller_lister() - - # Append potential controller configs found in the parameter server - all_ctrls_ns = _resolve_controllers_ns(self._cm_ns) - for name in get_rosparam_controller_names(all_ctrls_ns): - add_ctrl = not any(name == ctrl.name for ctrl in controllers) - if add_ctrl: - type_str = _rosparam_controller_type(all_ctrls_ns, name) - uninit_ctrl = ControllerState(name=name, - type=type_str, - state='uninitialized') - controllers.append(uninit_ctrl) - return controllers + # def _list_controllers(self): + # """ + # @return List of controllers associated to a controller manager + # namespace. Contains both stopped/running controllers, as returned by + # the C{list_controllers} service, plus uninitialized controllers with + # configurations loaded in the parameter server. + # @rtype [str] + # """ + # if not self._cm_ns: + # return [] + + # # Add loaded controllers first + # controllers = self._controller_lister() + + # # Append potential controller configs found in the parameter server + # all_ctrls_ns = _resolve_controllers_ns(self._cm_ns) + # for name in get_rosparam_controller_names(all_ctrls_ns): + # add_ctrl = not any(name == ctrl.name for ctrl in controllers) + # if add_ctrl: + # type_str = _rosparam_controller_type(all_ctrls_ns, name) + # uninit_ctrl = ControllerState(name=name, + # type=type_str, + # state='uninitialized') + # controllers.append(uninit_ctrl) + # return controllers def _show_controllers(self): table_view = self._widget.table_view @@ -252,39 +253,39 @@ def _on_ctrl_menu(self, pos): # Show context menu menu = QMenu(self._widget.table_view) - if ctrl.state == 'running': - action_stop = menu.addAction(self._icons['stopped'], 'Stop') - action_kill = menu.addAction(self._icons['uninitialized'], + if ctrl.state == 'active': + action_stop = menu.addAction(self._icons['finalized'], 'Stop') + action_kill = menu.addAction(self._icons['unconfigured'], 'Stop and Unload') - elif ctrl.state == 'stopped': - action_start = menu.addAction(self._icons['running'], + elif ctrl.state == 'finalized': + action_start = menu.addAction(self._icons['active'], 'Start again') - action_unload = menu.addAction(self._icons['uninitialized'], + action_unload = menu.addAction(self._icons['unconfigured'], 'Unload') - elif ctrl.state == 'initialized': - action_start = menu.addAction(self._icons['running'], 'Start') - action_unload = menu.addAction(self._icons['uninitialized'], + elif ctrl.state == 'inactive': + action_start = menu.addAction(self._icons['active'], 'Start') + action_unload = menu.addAction(self._icons['unconfigured'], 'Unload') - elif ctrl.state == 'uninitialized': - action_load = menu.addAction(self._icons['stopped'], 'Load') - action_spawn = menu.addAction(self._icons['running'], + elif ctrl.state == 'unconfigured': + action_load = menu.addAction(self._icons['finalized'], 'Load') + action_spawn = menu.addAction(self._icons['active'], 'Load and Start') action = menu.exec_(self._widget.table_view.mapToGlobal(pos)) # Evaluate user action - if ctrl.state == 'running': + if ctrl.state == 'active': if action is action_stop: self._stop_controller(ctrl.name) elif action is action_kill: self._stop_controller(ctrl.name) self._unload_controller(ctrl.name) - elif ctrl.state == 'stopped' or ctrl.state == 'initialized': + elif ctrl.state == 'finalized' or ctrl.state == 'inactive': if action is action_start: self._start_controller(ctrl.name) elif action is action_unload: self._unload_controller(ctrl.name) - elif ctrl.state == 'uninitialized': + elif ctrl.state == 'unconfigured': if action is action_load: self._load_controller(ctrl.name) if action is action_spawn: @@ -301,12 +302,12 @@ def _on_ctrl_info(self, index): res_model = QStandardItemModel() model_root = QStandardItem('Claimed Resources') res_model.appendRow(model_root) - for hw_res in ctrl.claimed_resources: - hw_iface_item = QStandardItem(hw_res.hardware_interface) + for claimed_interface in ctrl.claimed_interfaces: + hw_iface_item = QStandardItem(claimed_interface) model_root.appendRow(hw_iface_item) - for res in hw_res.resources: - res_item = QStandardItem(res) - hw_iface_item.appendRow(res_item) + # for res in claimed_interface.resources: + # res_item = QStandardItem(res) + # hw_iface_item.appendRow(res_item) popup.resource_tree.setModel(res_model) popup.resource_tree.setItemDelegate(FontDelegate(popup.resource_tree)) @@ -330,24 +331,24 @@ def _on_header_menu(self, pos): header.setSectionResizeMode(QHeaderView.ResizeToContents) def _load_controller(self, name): - self._load_srv.call(LoadControllerRequest(name=name)) + self._load_srv.call_async(LoadController.Request(name=name)) def _unload_controller(self, name): - self._unload_srv.call(UnloadControllerRequest(name=name)) + self._unload_srv.call_async(UnloadController.Request(name=name)) def _start_controller(self, name): - strict = SwitchControllerRequest.STRICT - req = SwitchControllerRequest(start_controllers=[name], - stop_controllers=[], - strictness=strict) - self._switch_srv.call(req) + strict = SwitchController.Request.STRICT + req = SwitchController.Request(activate_controllers=[name], + deactivate_controllers=[], + strictness=strict) + self._switch_srv.call_async(req) def _stop_controller(self, name): - strict = SwitchControllerRequest.STRICT - req = SwitchControllerRequest(start_controllers=[], - stop_controllers=[name], - strictness=strict) - self._switch_srv.call(req) + strict = SwitchController.Request.STRICT + req = SwitchController.Request(activate_controllers=[], + deactivate_controllers=[name], + strictness=strict) + self._switch_srv.call_async(req) class ControllerTable(QAbstractTableModel): From 7212ef991d3aa827a8949d8956358d415a49e8ea Mon Sep 17 00:00:00 2001 From: Kenji Brameld Date: Sun, 18 Sep 2022 16:02:12 +0900 Subject: [PATCH 08/25] fix up state transition Signed-off-by: Kenji Brameld --- .../controller_manager.py | 30 ++++++++----------- 1 file changed, 13 insertions(+), 17 deletions(-) diff --git a/rqt_controller_manager/rqt_controller_manager/controller_manager.py b/rqt_controller_manager/rqt_controller_manager/controller_manager.py index caa6940c2d..e7e0af5148 100644 --- a/rqt_controller_manager/rqt_controller_manager/controller_manager.py +++ b/rqt_controller_manager/rqt_controller_manager/controller_manager.py @@ -104,9 +104,9 @@ def __init__(self, context): # Controller state icons path = get_package_share_directory("rqt_controller_manager") self._icons = {'active': QIcon(path + '/resource/led_green.png'), - 'finalized': QIcon(path + '/resource/led_red.png'), - 'unconfigured': QIcon(path + '/resource/led_off.png'), - 'inactive': QIcon(path + '/resource/led_red.png')} + 'finalized': QIcon(path + '/resource/led_off.png'), + 'inactive': QIcon(path + '/resource/led_red.png'), + 'unconfigured': QIcon(path + '/resource/led_off.png')} # Controllers display table_view = self._widget.table_view @@ -201,12 +201,13 @@ def _set_cm_services(self, cm_ns): self._switch_srv = None def _update_controllers(self): - # Find controllers associated to the selected controller manager - controllers = list_controllers(self._node, self._cm_ns).controller - if not controllers: + if not self._cm_ns: return + # Find controllers associated to the selected controller manager + controllers = list_controllers(self._node, self._cm_ns).controller + # Update controller display, if necessary if self._controllers != controllers: self._controllers = controllers @@ -254,22 +255,17 @@ def _on_ctrl_menu(self, pos): # Show context menu menu = QMenu(self._widget.table_view) if ctrl.state == 'active': - action_stop = menu.addAction(self._icons['finalized'], 'Stop') - action_kill = menu.addAction(self._icons['unconfigured'], - 'Stop and Unload') - elif ctrl.state == 'finalized': - action_start = menu.addAction(self._icons['active'], - 'Start again') - action_unload = menu.addAction(self._icons['unconfigured'], - 'Unload') + action_stop = menu.addAction(self._icons['inactive'], 'Deactivate') + action_kill = menu.addAction(self._icons['finalized'], + 'Deactivate and Unload') elif ctrl.state == 'inactive': - action_start = menu.addAction(self._icons['active'], 'Start') + action_start = menu.addAction(self._icons['active'], 'Activate') action_unload = menu.addAction(self._icons['unconfigured'], 'Unload') elif ctrl.state == 'unconfigured': - action_load = menu.addAction(self._icons['finalized'], 'Load') + action_load = menu.addAction(self._icons['inactive'], 'Load') action_spawn = menu.addAction(self._icons['active'], - 'Load and Start') + 'Load and Activate') action = menu.exec_(self._widget.table_view.mapToGlobal(pos)) From 7a84a4f86236655dfb17a39c30fd7ca3cfbe87a4 Mon Sep 17 00:00:00 2001 From: Kenji Brameld Date: Sun, 18 Sep 2022 16:04:49 +0900 Subject: [PATCH 09/25] don't skip copyright check Signed-off-by: Kenji Brameld --- controller_manager_utils/test/test_copyright.py | 2 -- rqt_controller_manager/test/test_copyright.py | 2 -- 2 files changed, 4 deletions(-) diff --git a/controller_manager_utils/test/test_copyright.py b/controller_manager_utils/test/test_copyright.py index 97a39196e8..cc8ff03f79 100644 --- a/controller_manager_utils/test/test_copyright.py +++ b/controller_manager_utils/test/test_copyright.py @@ -16,8 +16,6 @@ import pytest -# Remove the `skip` decorator once the source file(s) have a copyright header -@pytest.mark.skip(reason='No copyright header has been placed in the generated source file.') @pytest.mark.copyright @pytest.mark.linter def test_copyright(): diff --git a/rqt_controller_manager/test/test_copyright.py b/rqt_controller_manager/test/test_copyright.py index 97a39196e8..cc8ff03f79 100644 --- a/rqt_controller_manager/test/test_copyright.py +++ b/rqt_controller_manager/test/test_copyright.py @@ -16,8 +16,6 @@ import pytest -# Remove the `skip` decorator once the source file(s) have a copyright header -@pytest.mark.skip(reason='No copyright header has been placed in the generated source file.') @pytest.mark.copyright @pytest.mark.linter def test_copyright(): From e77518ced654ddf2a17b6933dff6f31128961d57 Mon Sep 17 00:00:00 2001 From: Kenji Brameld Date: Sun, 18 Sep 2022 16:05:40 +0900 Subject: [PATCH 10/25] fill maintainer tag Signed-off-by: Kenji Brameld --- rqt_controller_manager/package.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rqt_controller_manager/package.xml b/rqt_controller_manager/package.xml index 4fe1dc3c0f..a1c1f4ad6b 100644 --- a/rqt_controller_manager/package.xml +++ b/rqt_controller_manager/package.xml @@ -4,7 +4,7 @@ rqt_controller_manager 0.0.0 Graphical frontend for interacting with the controller manager. - TODO + ijnek BSD http://ros.org/wiki/rqt_controller_manager From 5f779fb41f7818221bfb7f5af207c58b07ece0ac Mon Sep 17 00:00:00 2001 From: Kenji Brameld Date: Sun, 18 Sep 2022 16:36:57 +0900 Subject: [PATCH 11/25] got loading controller names from parameters somewhat working Signed-off-by: Kenji Brameld --- .../controller_manager_utils/utils.py | 78 ++++++++++--------- .../controller_manager.py | 63 ++++++++------- 2 files changed, 75 insertions(+), 66 deletions(-) diff --git a/controller_manager_utils/controller_manager_utils/utils.py b/controller_manager_utils/controller_manager_utils/utils.py index 60219bb0ec..e9d8d123b3 100644 --- a/controller_manager_utils/controller_manager_utils/utils.py +++ b/controller_manager_utils/controller_manager_utils/utils.py @@ -21,6 +21,7 @@ import rclpy from controller_manager_msgs.srv import ListControllers +from ros2param.api import call_list_parameters # Names of controller manager services, and their respective types _LIST_CONTROLLERS_STR = "list_controllers" @@ -373,40 +374,43 @@ def _filter_by_attr(list_in, attr_name, attr_val, match_substring=False): # ############################################################################### -def get_rosparam_controller_names(namespace='/'): - pass -# """ -# Get list of ROS parameter names that potentially represent a controller -# configuration. - -# Example usage: -# - Assume the following parameters exist in the ROS parameter: -# server: -# - C{/foo/type} -# - C{/xxx/type/xxx} -# - C{/ns/bar/type} -# - C{/ns/yyy/type/yyy} -# - The potential controllers found by this method are: - -# >>> names = get_rosparam_controller_names() # returns ['foo'] -# >>> names_ns = get_rosparam_controller_names('/ns') # returns ['bar'] - -# @param namespace: Namespace where to look for controllers. -# @type namespace: str -# @return: Sorted list of ROS parameter names. -# @rtype: [str] -# """ -# import rosparam -# list_out = [] -# all_params = rosparam.list_params(namespace) -# for param in all_params: -# # Remove namespace from parameter string -# if not namespace or namespace[-1] != '/': -# namespace += '/' -# param_no_ns = param.split(namespace, 1)[1] - -# # Check if parameter corresponds to a controller configuration -# param_split = param_no_ns.split('/') -# if (len(param_split) == 2 and param_split[1] == 'type'): -# list_out.append(param_split[0]) # It does! -# return sorted(list_out) +def get_rosparam_controller_names(node, namespace='/'): + # """ + # Get list of ROS parameter names that potentially represent a controller + # configuration. + + # Example usage: + # - Assume the following parameters exist in the ROS parameter: + # server: + # - C{/foo/type} + # - C{/xxx/type/xxx} + # - C{/ns/bar/type} + # - C{/ns/yyy/type/yyy} + # - The potential controllers found by this method are: + + # >>> names = get_rosparam_controller_names() # returns ['foo'] + # >>> names_ns = get_rosparam_controller_names('/ns') # returns ['bar'] + + # @param namespace: Namespace where to look for controllers. + # @type namespace: str + # @return: Sorted list of ROS parameter names. + # @rtype: [str] + # """ + # import rosparam + # list_out = [] + # all_params = rosparam.list_params(namespace) + # for param in all_params: + # # Remove namespace from parameter string + # if not namespace or namespace[-1] != '/': + # namespace += '/' + # param_no_ns = param.split(namespace, 1)[1] + + # # Check if parameter corresponds to a controller configuration + # param_split = param_no_ns.split('/') + # if (len(param_split) == 2 and param_split[1] == 'type'): + # list_out.append(param_split[0]) # It does! + # return sorted(list_out) + + parameter_names = call_list_parameters(node=node, node_name=namespace) + suffix = '.type' + return [n[: -len(suffix)] for n in parameter_names if n.endswith(suffix)] diff --git a/rqt_controller_manager/rqt_controller_manager/controller_manager.py b/rqt_controller_manager/rqt_controller_manager/controller_manager.py index e7e0af5148..db98bfba31 100644 --- a/rqt_controller_manager/rqt_controller_manager/controller_manager.py +++ b/rqt_controller_manager/rqt_controller_manager/controller_manager.py @@ -45,6 +45,7 @@ from controller_manager.controller_manager_services import list_controllers from .update_combo import update_combo +from ros2param.api import call_get_parameters class ControllerManager(Plugin): @@ -206,38 +207,39 @@ def _update_controllers(self): return # Find controllers associated to the selected controller manager - controllers = list_controllers(self._node, self._cm_ns).controller + controllers = self._list_controllers() # Update controller display, if necessary if self._controllers != controllers: self._controllers = controllers self._show_controllers() # NOTE: Model is recomputed from scratch - # def _list_controllers(self): - # """ - # @return List of controllers associated to a controller manager - # namespace. Contains both stopped/running controllers, as returned by - # the C{list_controllers} service, plus uninitialized controllers with - # configurations loaded in the parameter server. - # @rtype [str] - # """ - # if not self._cm_ns: - # return [] - - # # Add loaded controllers first - # controllers = self._controller_lister() - - # # Append potential controller configs found in the parameter server - # all_ctrls_ns = _resolve_controllers_ns(self._cm_ns) - # for name in get_rosparam_controller_names(all_ctrls_ns): - # add_ctrl = not any(name == ctrl.name for ctrl in controllers) - # if add_ctrl: - # type_str = _rosparam_controller_type(all_ctrls_ns, name) - # uninit_ctrl = ControllerState(name=name, - # type=type_str, - # state='uninitialized') - # controllers.append(uninit_ctrl) - # return controllers + def _list_controllers(self): + """ + @return List of controllers associated to a controller manager + namespace. Contains both stopped/running controllers, as returned by + the C{list_controllers} service, plus uninitialized controllers with + configurations loaded in the parameter server. + @rtype [str] + """ + if not self._cm_ns: + return [] + + # Add loaded controllers first + controllers = self._controller_lister() + controllers = list_controllers(self._node, self._cm_ns).controller + + # Append potential controller configs found in the parameter server + # all_ctrls_ns = _resolve_controllers_ns(self._cm_ns) + for name in get_rosparam_controller_names(self._node, self._cm_ns): + add_ctrl = not any(name == ctrl.name for ctrl in controllers) + if add_ctrl: + type_str = _rosparam_controller_type(self._node, self._cm_ns, name) + uninit_ctrl = ControllerState(name=name, + type=type_str, + state='unconfigured') + controllers.append(uninit_ctrl) + return controllers def _show_controllers(self): table_view = self._widget.table_view @@ -460,7 +462,7 @@ def _append_ns(in_ns, suffix): return ns -def _rosparam_controller_type(ctrls_ns, ctrl_name): +def _rosparam_controller_type(node, ctrls_ns, ctrl_name): """ Get a controller's type from its ROS parameter server configuration @param ctrls_ns Namespace where controllers should be located @@ -470,5 +472,8 @@ def _rosparam_controller_type(ctrls_ns, ctrl_name): @return Controller type @rtype str """ - type_param = _append_ns(ctrls_ns, ctrl_name) + '/type' - return rospy.get_param(type_param) + response = call_get_parameters(node=node, node_name=ctrls_ns, parameter_names=[ctrl_name]) + if not response.values: + return '' + else: + return response.values[0].string_value From f7ff0471cdd954534f26393968bc770150802021 Mon Sep 17 00:00:00 2001 From: Kenji Brameld Date: Mon, 19 Sep 2022 09:56:07 +0900 Subject: [PATCH 12/25] rename get_rosparam_controller_names to get_parameter_controller_names Signed-off-by: Kenji Brameld --- .../controller_manager_utils/utils.py | 44 +++---------------- .../controller_manager.py | 16 +++---- 2 files changed, 12 insertions(+), 48 deletions(-) diff --git a/controller_manager_utils/controller_manager_utils/utils.py b/controller_manager_utils/controller_manager_utils/utils.py index e9d8d123b3..dba62da6cd 100644 --- a/controller_manager_utils/controller_manager_utils/utils.py +++ b/controller_manager_utils/controller_manager_utils/utils.py @@ -374,43 +374,11 @@ def _filter_by_attr(list_in, attr_name, attr_val, match_substring=False): # ############################################################################### -def get_rosparam_controller_names(node, namespace='/'): - # """ - # Get list of ROS parameter names that potentially represent a controller - # configuration. - - # Example usage: - # - Assume the following parameters exist in the ROS parameter: - # server: - # - C{/foo/type} - # - C{/xxx/type/xxx} - # - C{/ns/bar/type} - # - C{/ns/yyy/type/yyy} - # - The potential controllers found by this method are: - - # >>> names = get_rosparam_controller_names() # returns ['foo'] - # >>> names_ns = get_rosparam_controller_names('/ns') # returns ['bar'] - - # @param namespace: Namespace where to look for controllers. - # @type namespace: str - # @return: Sorted list of ROS parameter names. - # @rtype: [str] - # """ - # import rosparam - # list_out = [] - # all_params = rosparam.list_params(namespace) - # for param in all_params: - # # Remove namespace from parameter string - # if not namespace or namespace[-1] != '/': - # namespace += '/' - # param_no_ns = param.split(namespace, 1)[1] - - # # Check if parameter corresponds to a controller configuration - # param_split = param_no_ns.split('/') - # if (len(param_split) == 2 and param_split[1] == 'type'): - # list_out.append(param_split[0]) # It does! - # return sorted(list_out) - - parameter_names = call_list_parameters(node=node, node_name=namespace) +def get_parameter_controller_names(node, node_name): + """ + Get list of ROS parameter names that potentially represent a controller + configuration. + """ + parameter_names = call_list_parameters(node=node, node_name=node_name) suffix = '.type' return [n[: -len(suffix)] for n in parameter_names if n.endswith(suffix)] diff --git a/rqt_controller_manager/rqt_controller_manager/controller_manager.py b/rqt_controller_manager/rqt_controller_manager/controller_manager.py index db98bfba31..76b36db3a0 100644 --- a/rqt_controller_manager/rqt_controller_manager/controller_manager.py +++ b/rqt_controller_manager/rqt_controller_manager/controller_manager.py @@ -39,9 +39,8 @@ from controller_manager_msgs.msg import ControllerState from controller_manager_msgs.srv import LoadController, SwitchController, UnloadController -from controller_manager_utils.utils\ - import ControllerLister, ControllerManagerLister,\ - get_rosparam_controller_names +from controller_manager_utils.utils import ControllerLister, ControllerManagerLister, \ + get_parameter_controller_names from controller_manager.controller_manager_services import list_controllers from .update_combo import update_combo @@ -123,8 +122,6 @@ def __init__(self, context): # Timer for controller manager updates self._list_cm = ControllerManagerLister() - # self._list_cm = list_controllers(context.node, parsed_args.controller_manager).controller - # self._list_cm = list_controllers(context.node, '/controller_manager').controller self._update_cm_list_timer = QTimer(self) self._update_cm_list_timer.setInterval(int(1000.0 / self._cm_update_freq)) self._update_cm_list_timer.timeout.connect(self._update_cm_list) @@ -217,9 +214,9 @@ def _update_controllers(self): def _list_controllers(self): """ @return List of controllers associated to a controller manager - namespace. Contains both stopped/running controllers, as returned by + node. Contains both stopped/running controllers, as returned by the C{list_controllers} service, plus uninitialized controllers with - configurations loaded in the parameter server. + returned by the C{list_parameters} service.. @rtype [str] """ if not self._cm_ns: @@ -229,9 +226,8 @@ def _list_controllers(self): controllers = self._controller_lister() controllers = list_controllers(self._node, self._cm_ns).controller - # Append potential controller configs found in the parameter server - # all_ctrls_ns = _resolve_controllers_ns(self._cm_ns) - for name in get_rosparam_controller_names(self._node, self._cm_ns): + # Append potential controller configs found in the node's parameters + for name in get_parameter_controller_names(self._node, self._cm_ns): add_ctrl = not any(name == ctrl.name for ctrl in controllers) if add_ctrl: type_str = _rosparam_controller_type(self._node, self._cm_ns, name) From 79e3dbeecf970062bb94d3d77e8596f2ea57eed8 Mon Sep 17 00:00:00 2001 From: Kenji Brameld Date: Mon, 19 Sep 2022 10:12:15 +0900 Subject: [PATCH 13/25] change logic to use node names rather than namespaces Signed-off-by: Kenji Brameld --- .../controller_manager.py | 103 +++++------------- 1 file changed, 27 insertions(+), 76 deletions(-) diff --git a/rqt_controller_manager/rqt_controller_manager/controller_manager.py b/rqt_controller_manager/rqt_controller_manager/controller_manager.py index 76b36db3a0..f2a0dd82aa 100644 --- a/rqt_controller_manager/rqt_controller_manager/controller_manager.py +++ b/rqt_controller_manager/rqt_controller_manager/controller_manager.py @@ -88,7 +88,7 @@ def __init__(self, context): context.add_widget(self._widget) # Initialize members - self._cm_ns = [] # Namespace of the selected controller manager + self._cm_name = [] # Name of the selected controller manager's node self._controllers = [] # State of each controller self._table_model = None self._controller_lister = None @@ -143,16 +143,16 @@ def shutdown_plugin(self): self._popup_widget.hide() def save_settings(self, plugin_settings, instance_settings): - instance_settings.set_value('cm_ns', self._cm_ns) + instance_settings.set_value('cm_name', self._cm_name) def restore_settings(self, plugin_settings, instance_settings): # Restore last session's controller_manager, if present self._update_cm_list() - cm_ns = instance_settings.value('cm_ns') + cm_name = instance_settings.value('cm_name') cm_combo = self._widget.cm_combo cm_list = [cm_combo.itemText(i) for i in range(cm_combo.count())] try: - idx = cm_list.index(cm_ns) + idx = cm_list.index(cm_name) cm_combo.setCurrentIndex(idx) except (ValueError): pass @@ -166,33 +166,27 @@ def restore_settings(self, plugin_settings, instance_settings): def _update_cm_list(self): update_combo(self._widget.cm_combo, self._list_cm()) - def _on_cm_change(self, cm_ns): - self._cm_ns = cm_ns + def _on_cm_change(self, cm_name): + self._cm_name = cm_name # Setup services for communicating with the selected controller manager - self._set_cm_services(cm_ns) + self._set_cm_services(cm_name) # Controller lister for the selected controller manager - if cm_ns: - self._controller_lister = ControllerLister(cm_ns) + if cm_name: + self._controller_lister = ControllerLister(cm_name) self._update_controllers() else: self._controller_lister = None - def _set_cm_services(self, cm_ns): - if cm_ns: - # NOTE: Persistent services are used for performance reasons. - # If the controller manager dies, we detect it and disconnect from - # it anyway - load_srv_name = _append_ns(cm_ns, 'load_controller') - self._load_srv = self._node.create_client(LoadController, - load_srv_name) - unload_srv_name = _append_ns(cm_ns, 'unload_controller') - self._unload_srv = self._node.create_client(UnloadController, - unload_srv_name) - switch_srv_name = _append_ns(cm_ns, 'switch_controller') - self._switch_srv = self._node.create_client(SwitchController, - switch_srv_name) + def _set_cm_services(self, cm_name): + if cm_name: + self._load_srv = self._node.create_client( + LoadController, cm_name + '/load_controller') + self._unload_srv = self._node.create_client( + UnloadController, cm_name + '/unload_controller') + self._switch_srv = self._node.create_client( + SwitchController, cm_name + '/switch_controller') else: self._load_srv = None self._unload_srv = None @@ -200,7 +194,7 @@ def _set_cm_services(self, cm_ns): def _update_controllers(self): - if not self._cm_ns: + if not self._cm_name: return # Find controllers associated to the selected controller manager @@ -219,18 +213,18 @@ def _list_controllers(self): returned by the C{list_parameters} service.. @rtype [str] """ - if not self._cm_ns: + if not self._cm_name: return [] # Add loaded controllers first controllers = self._controller_lister() - controllers = list_controllers(self._node, self._cm_ns).controller + controllers = list_controllers(self._node, self._cm_name).controller # Append potential controller configs found in the node's parameters - for name in get_parameter_controller_names(self._node, self._cm_ns): + for name in get_parameter_controller_names(self._node, self._cm_name): add_ctrl = not any(name == ctrl.name for ctrl in controllers) if add_ctrl: - type_str = _rosparam_controller_type(self._node, self._cm_ns, name) + type_str = _get_controller_type(self._node, self._cm_name, name) uninit_ctrl = ControllerState(name=name, type=type_str, state='unconfigured') @@ -415,60 +409,17 @@ def paint(self, painter, option, index): QStyledItemDelegate.paint(self, painter, option, index) -def _resolve_controllers_ns(cm_ns): +def _get_controller_type(node, node_name, ctrl_name): """ - Resolve the namespace containing controller configurations from that of - the controller manager. - Controllers are assumed to live one level above the controller - manager, e.g. - - >>> _resolve_controller_ns('/path/to/controller_manager') - '/path/to' - - In the particular case in which the controller manager is not - namespaced, the controller is assumed to live in the root namespace - - >>> _resolve_controller_ns('/') - '/' - >>> _resolve_controller_ns('') - '/' - @param cm_ns Controller manager namespace (can be an empty string) - @type cm_ns str - @return Controllers namespace - @rtype str - """ - ns = cm_ns.rsplit('/', 1)[0] - if not ns: - ns += '/' - return ns - - -def _append_ns(in_ns, suffix): - """ - Append a sub-namespace (suffix) to the input namespace - @param in_ns Input namespace - @type in_ns str - @return Suffix namespace - @rtype str - """ - ns = in_ns - if ns[-1] != '/': - ns += '/' - ns += suffix - return ns - - -def _rosparam_controller_type(node, ctrls_ns, ctrl_name): - """ - Get a controller's type from its ROS parameter server configuration - @param ctrls_ns Namespace where controllers should be located - @type ctrls_ns str + Get the controller's type from the controller manager node with the call_get_parameter service. + @param node_name Controller manager node's name + @type node_name str @param ctrl_name Controller name @type ctrl_name str @return Controller type @rtype str """ - response = call_get_parameters(node=node, node_name=ctrls_ns, parameter_names=[ctrl_name]) + response = call_get_parameters(node=node, node_name=node_name, parameter_names=[ctrl_name]) if not response.values: return '' else: From b5a59a6a79aa99de7fba6858d11d23d8d8caadd2 Mon Sep 17 00:00:00 2001 From: Kenji Brameld Date: Mon, 19 Sep 2022 11:40:29 +0900 Subject: [PATCH 14/25] allow activation of controller from rqt_controller_manager Signed-off-by: Kenji Brameld --- .../controller_manager.py | 53 ++++++++++++++----- 1 file changed, 40 insertions(+), 13 deletions(-) diff --git a/rqt_controller_manager/rqt_controller_manager/controller_manager.py b/rqt_controller_manager/rqt_controller_manager/controller_manager.py index f2a0dd82aa..80d76e3910 100644 --- a/rqt_controller_manager/rqt_controller_manager/controller_manager.py +++ b/rqt_controller_manager/rqt_controller_manager/controller_manager.py @@ -38,7 +38,7 @@ from qt_gui.plugin import Plugin from controller_manager_msgs.msg import ControllerState -from controller_manager_msgs.srv import LoadController, SwitchController, UnloadController +from controller_manager_msgs.srv import ConfigureController, LoadController, SwitchController, UnloadController from controller_manager_utils.utils import ControllerLister, ControllerManagerLister, \ get_parameter_controller_names from controller_manager.controller_manager_services import list_controllers @@ -94,6 +94,7 @@ def __init__(self, context): self._controller_lister = None # Controller manager service proxies + self._configure_srv = None self._load_srv = None self._unload_srv = None self._switch_srv = None @@ -181,6 +182,8 @@ def _on_cm_change(self, cm_name): def _set_cm_services(self, cm_name): if cm_name: + self._configure_srv = self._node.create_client( + ConfigureController, cm_name + '/configure_controller') self._load_srv = self._node.create_client( LoadController, cm_name + '/load_controller') self._unload_srv = self._node.create_client( @@ -226,8 +229,7 @@ def _list_controllers(self): if add_ctrl: type_str = _get_controller_type(self._node, self._cm_name, name) uninit_ctrl = ControllerState(name=name, - type=type_str, - state='unconfigured') + type=type_str) controllers.append(uninit_ctrl) return controllers @@ -251,13 +253,18 @@ def _on_ctrl_menu(self, pos): action_kill = menu.addAction(self._icons['finalized'], 'Deactivate and Unload') elif ctrl.state == 'inactive': - action_start = menu.addAction(self._icons['active'], 'Activate') + action_activate = menu.addAction(self._icons['active'], 'Activate') action_unload = menu.addAction(self._icons['unconfigured'], 'Unload') elif ctrl.state == 'unconfigured': - action_load = menu.addAction(self._icons['inactive'], 'Load') + action_configure = menu.addAction(self._icons['inactive'], 'Configure') action_spawn = menu.addAction(self._icons['active'], - 'Load and Activate') + 'Configure and Activate') + else: + # Controller isn't loaded + action_load = menu.addAction(self._icons['unconfigured'], 'Load') + action_configure = menu.addAction(self._icons['inactive'], 'Load and Configure') + action_activate = menu.addAction(self._icons['active'], 'Load, Configure and Activate') action = menu.exec_(self._widget.table_view.mapToGlobal(pos)) @@ -269,16 +276,27 @@ def _on_ctrl_menu(self, pos): self._stop_controller(ctrl.name) self._unload_controller(ctrl.name) elif ctrl.state == 'finalized' or ctrl.state == 'inactive': - if action is action_start: - self._start_controller(ctrl.name) + if action is action_activate: + self._activate_controller(ctrl.name) elif action is action_unload: self._unload_controller(ctrl.name) elif ctrl.state == 'unconfigured': + if action is action_configure: + self._configure_controller(ctrl.name) + elif action is action_spawn: + self._load_controller(ctrl.name) + self._activate_controller(ctrl.name) + else: + # Assume controller isn't loaded if action is action_load: self._load_controller(ctrl.name) - if action is action_spawn: + elif action is action_configure: self._load_controller(ctrl.name) - self._start_controller(ctrl.name) + self._configure_controller(ctrl.name) + elif action is action_activate: + self._load_controller(ctrl.name) + self._configure_controller(ctrl.name) + self._activate_controller(ctrl.name) def _on_ctrl_info(self, index): popup = self._popup_widget @@ -318,13 +336,16 @@ def _on_header_menu(self, pos): else: header.setSectionResizeMode(QHeaderView.ResizeToContents) + def _configure_controller(self, name): + self._configure_srv.call_async(ConfigureController.Request(name=name)) + def _load_controller(self, name): self._load_srv.call_async(LoadController.Request(name=name)) def _unload_controller(self, name): self._unload_srv.call_async(UnloadController.Request(name=name)) - def _start_controller(self, name): + def _activate_controller(self, name): strict = SwitchController.Request.STRICT req = SwitchController.Request(activate_controllers=[name], deactivate_controllers=[], @@ -376,11 +397,17 @@ def data(self, index, role): if index.column() == 0: return ctrl.name elif index.column() == 1: - return ctrl.state + if ctrl.state: + return ctrl.state + else: + return 'not loaded' if role == Qt.DecorationRole: if index.column() == 0: - return self._icons[ctrl.state] + if ctrl.state in self._icons: + return self._icons[ctrl.state] + else: + return None if role == Qt.FontRole: if index.column() == 0: From 06ff836624aee89d40fc66f180e3ff188df4931f Mon Sep 17 00:00:00 2001 From: Kenji Brameld Date: Mon, 19 Sep 2022 11:52:34 +0900 Subject: [PATCH 15/25] fix up imports Signed-off-by: Kenji Brameld --- .../rqt_controller_manager/controller_manager.py | 13 ++++++------- 1 file changed, 6 insertions(+), 7 deletions(-) diff --git a/rqt_controller_manager/rqt_controller_manager/controller_manager.py b/rqt_controller_manager/rqt_controller_manager/controller_manager.py index 80d76e3910..f113c5929b 100644 --- a/rqt_controller_manager/rqt_controller_manager/controller_manager.py +++ b/rqt_controller_manager/rqt_controller_manager/controller_manager.py @@ -1,6 +1,7 @@ #!/usr/bin/env python # Copyright (C) 2013, Georgia Tech # Copyright (C) 2015, PAL Robotics S.L. +# Copyright (C) 2022, Kenji Brameld # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions are met: @@ -29,16 +30,14 @@ from ament_index_python.packages import get_package_share_directory from python_qt_binding import loadUi -from python_qt_binding.QtCore import QAbstractTableModel, QModelIndex, Qt,\ - QTimer, QVariant, Signal -from python_qt_binding.QtWidgets import QWidget, QFormLayout, QHeaderView,\ - QMenu, QStyledItemDelegate -from python_qt_binding.QtGui import QCursor, QFont, QIcon, QStandardItem,\ - QStandardItemModel +from python_qt_binding.QtCore import QAbstractTableModel, Qt, QTimer +from python_qt_binding.QtWidgets import QWidget, QHeaderView, QMenu, QStyledItemDelegate +from python_qt_binding.QtGui import QCursor, QFont, QIcon, QStandardItem, QStandardItemModel from qt_gui.plugin import Plugin from controller_manager_msgs.msg import ControllerState -from controller_manager_msgs.srv import ConfigureController, LoadController, SwitchController, UnloadController +from controller_manager_msgs.srv import ConfigureController, LoadController, SwitchController, \ + UnloadController from controller_manager_utils.utils import ControllerLister, ControllerManagerLister, \ get_parameter_controller_names from controller_manager.controller_manager_services import list_controllers From 54d5f32a40735beb539555217584348a5af9e685 Mon Sep 17 00:00:00 2001 From: Kenji Brameld Date: Mon, 19 Sep 2022 12:06:55 +0900 Subject: [PATCH 16/25] use services from controller_manager_services instead of setting up service clients Signed-off-by: Kenji Brameld --- .../controller_manager.py | 85 +++++++------------ 1 file changed, 30 insertions(+), 55 deletions(-) diff --git a/rqt_controller_manager/rqt_controller_manager/controller_manager.py b/rqt_controller_manager/rqt_controller_manager/controller_manager.py index f113c5929b..9a458a127d 100644 --- a/rqt_controller_manager/rqt_controller_manager/controller_manager.py +++ b/rqt_controller_manager/rqt_controller_manager/controller_manager.py @@ -36,11 +36,11 @@ from qt_gui.plugin import Plugin from controller_manager_msgs.msg import ControllerState -from controller_manager_msgs.srv import ConfigureController, LoadController, SwitchController, \ - UnloadController +from controller_manager_msgs.srv import SwitchController from controller_manager_utils.utils import ControllerLister, ControllerManagerLister, \ get_parameter_controller_names -from controller_manager.controller_manager_services import list_controllers +from controller_manager.controller_manager_services import configure_controller, \ + list_controllers, load_controller, switch_controllers, unload_controller from .update_combo import update_combo from ros2param.api import call_get_parameters @@ -92,12 +92,6 @@ def __init__(self, context): self._table_model = None self._controller_lister = None - # Controller manager service proxies - self._configure_srv = None - self._load_srv = None - self._unload_srv = None - self._switch_srv = None - # Store reference to node self._node = context.node @@ -169,9 +163,6 @@ def _update_cm_list(self): def _on_cm_change(self, cm_name): self._cm_name = cm_name - # Setup services for communicating with the selected controller manager - self._set_cm_services(cm_name) - # Controller lister for the selected controller manager if cm_name: self._controller_lister = ControllerLister(cm_name) @@ -179,21 +170,6 @@ def _on_cm_change(self, cm_name): else: self._controller_lister = None - def _set_cm_services(self, cm_name): - if cm_name: - self._configure_srv = self._node.create_client( - ConfigureController, cm_name + '/configure_controller') - self._load_srv = self._node.create_client( - LoadController, cm_name + '/load_controller') - self._unload_srv = self._node.create_client( - UnloadController, cm_name + '/unload_controller') - self._switch_srv = self._node.create_client( - SwitchController, cm_name + '/switch_controller') - else: - self._load_srv = None - self._unload_srv = None - self._switch_srv = None - def _update_controllers(self): if not self._cm_name: @@ -273,28 +249,28 @@ def _on_ctrl_menu(self, pos): self._stop_controller(ctrl.name) elif action is action_kill: self._stop_controller(ctrl.name) - self._unload_controller(ctrl.name) + unload_controller(self._node, self._cm_name, ctrl.name) elif ctrl.state == 'finalized' or ctrl.state == 'inactive': if action is action_activate: self._activate_controller(ctrl.name) elif action is action_unload: - self._unload_controller(ctrl.name) + unload_controller(self._node, self._cm_name, ctrl.name) elif ctrl.state == 'unconfigured': if action is action_configure: - self._configure_controller(ctrl.name) + configure_controller(self._node, self._cm_name, ctrl.name) elif action is action_spawn: - self._load_controller(ctrl.name) + load_controller(self._node, self._cm_name, ctrl.name) self._activate_controller(ctrl.name) else: # Assume controller isn't loaded if action is action_load: - self._load_controller(ctrl.name) + load_controller(self._node, self._cm_name, ctrl.name) elif action is action_configure: - self._load_controller(ctrl.name) - self._configure_controller(ctrl.name) + load_controller(self._node, self._cm_name, ctrl.name) + configure_controller(self._node, self._cm_name, ctrl.name) elif action is action_activate: - self._load_controller(ctrl.name) - self._configure_controller(ctrl.name) + load_controller(self._node, self._cm_name, ctrl.name) + configure_controller(self._node, self._cm_name, ctrl.name) self._activate_controller(ctrl.name) def _on_ctrl_info(self, index): @@ -335,28 +311,27 @@ def _on_header_menu(self, pos): else: header.setSectionResizeMode(QHeaderView.ResizeToContents) - def _configure_controller(self, name): - self._configure_srv.call_async(ConfigureController.Request(name=name)) - - def _load_controller(self, name): - self._load_srv.call_async(LoadController.Request(name=name)) - - def _unload_controller(self, name): - self._unload_srv.call_async(UnloadController.Request(name=name)) - def _activate_controller(self, name): - strict = SwitchController.Request.STRICT - req = SwitchController.Request(activate_controllers=[name], - deactivate_controllers=[], - strictness=strict) - self._switch_srv.call_async(req) + switch_controllers( + node=self._node, + controller_manager_name=self._cm_name, + deactivate_controllers=[], + activate_controllers=[name], + strict=SwitchController.Request.STRICT, + activate_asap=False, + timeout=0.3 + ) def _stop_controller(self, name): - strict = SwitchController.Request.STRICT - req = SwitchController.Request(activate_controllers=[], - deactivate_controllers=[name], - strictness=strict) - self._switch_srv.call_async(req) + switch_controllers( + node=self._node, + controller_manager_name=self._cm_name, + deactivate_controllers=[name], + activate_controllers=[], + strict=SwitchController.Request.STRICT, + activate_asap=False, + timeout=0.3 + ) class ControllerTable(QAbstractTableModel): From 4c918de3d0f5485797340ca5599a6f84ba043c18 Mon Sep 17 00:00:00 2001 From: Kenji Brameld Date: Mon, 19 Sep 2022 12:09:10 +0900 Subject: [PATCH 17/25] clean up cm_name Signed-off-by: Kenji Brameld --- .../rqt_controller_manager/controller_manager.py | 5 +---- 1 file changed, 1 insertion(+), 4 deletions(-) diff --git a/rqt_controller_manager/rqt_controller_manager/controller_manager.py b/rqt_controller_manager/rqt_controller_manager/controller_manager.py index 9a458a127d..05faabd23f 100644 --- a/rqt_controller_manager/rqt_controller_manager/controller_manager.py +++ b/rqt_controller_manager/rqt_controller_manager/controller_manager.py @@ -87,7 +87,7 @@ def __init__(self, context): context.add_widget(self._widget) # Initialize members - self._cm_name = [] # Name of the selected controller manager's node + self._cm_name = '' # Name of the selected controller manager's node self._controllers = [] # State of each controller self._table_model = None self._controller_lister = None @@ -191,9 +191,6 @@ def _list_controllers(self): returned by the C{list_parameters} service.. @rtype [str] """ - if not self._cm_name: - return [] - # Add loaded controllers first controllers = self._controller_lister() controllers = list_controllers(self._node, self._cm_name).controller From af206a2fc2105e5807d847d480221846c4055abd Mon Sep 17 00:00:00 2001 From: Kenji Brameld Date: Mon, 19 Sep 2022 13:21:04 +0900 Subject: [PATCH 18/25] clean up, change "stop" to "deactivate" Signed-off-by: Kenji Brameld --- .../controller_manager.py | 21 ++++++------------- 1 file changed, 6 insertions(+), 15 deletions(-) diff --git a/rqt_controller_manager/rqt_controller_manager/controller_manager.py b/rqt_controller_manager/rqt_controller_manager/controller_manager.py index 05faabd23f..35b6a0adf3 100644 --- a/rqt_controller_manager/rqt_controller_manager/controller_manager.py +++ b/rqt_controller_manager/rqt_controller_manager/controller_manager.py @@ -151,12 +151,6 @@ def restore_settings(self, plugin_settings, instance_settings): except (ValueError): pass - # def trigger_configuration(self): - # Comment in to signal that the plugin has a way to configure - # This will enable a setting button (gear icon) in each dock widget - # title bar - # Usually used to open a modal configuration dialog - def _update_cm_list(self): update_combo(self._widget.cm_combo, self._list_cm()) @@ -221,7 +215,7 @@ def _on_ctrl_menu(self, pos): # Show context menu menu = QMenu(self._widget.table_view) if ctrl.state == 'active': - action_stop = menu.addAction(self._icons['inactive'], 'Deactivate') + action_deactivate = menu.addAction(self._icons['inactive'], 'Deactivate') action_kill = menu.addAction(self._icons['finalized'], 'Deactivate and Unload') elif ctrl.state == 'inactive': @@ -242,10 +236,10 @@ def _on_ctrl_menu(self, pos): # Evaluate user action if ctrl.state == 'active': - if action is action_stop: - self._stop_controller(ctrl.name) + if action is action_deactivate: + self._deactivate_controller(ctrl.name) elif action is action_kill: - self._stop_controller(ctrl.name) + self._deactivate_controller(ctrl.name) unload_controller(self._node, self._cm_name, ctrl.name) elif ctrl.state == 'finalized' or ctrl.state == 'inactive': if action is action_activate: @@ -278,14 +272,11 @@ def _on_ctrl_info(self, index): popup.ctrl_type.setText(ctrl.type) res_model = QStandardItemModel() - model_root = QStandardItem('Claimed Resources') + model_root = QStandardItem('Claimed Interfaces') res_model.appendRow(model_root) for claimed_interface in ctrl.claimed_interfaces: hw_iface_item = QStandardItem(claimed_interface) model_root.appendRow(hw_iface_item) - # for res in claimed_interface.resources: - # res_item = QStandardItem(res) - # hw_iface_item.appendRow(res_item) popup.resource_tree.setModel(res_model) popup.resource_tree.setItemDelegate(FontDelegate(popup.resource_tree)) @@ -319,7 +310,7 @@ def _activate_controller(self, name): timeout=0.3 ) - def _stop_controller(self, name): + def _deactivate_controller(self, name): switch_controllers( node=self._node, controller_manager_name=self._cm_name, From 88c9e94898f45c4ee9d9cc3b84ec050e00fd9691 Mon Sep 17 00:00:00 2001 From: Kenji Brameld Date: Mon, 19 Sep 2022 13:56:00 +0900 Subject: [PATCH 19/25] remove controller_manager_utils, and port necessary functionality into controller_manager.py Signed-off-by: Kenji Brameld --- .../controller_manager_utils/__init__.py | 0 .../controller_manager_utils/utils.py | 384 ------------------ controller_manager_utils/package.xml | 18 - .../resource/controller_manager_utils | 0 controller_manager_utils/setup.cfg | 4 - controller_manager_utils/setup.py | 25 -- .../test/test_copyright.py | 23 -- controller_manager_utils/test/test_flake8.py | 25 -- controller_manager_utils/test/test_pep257.py | 23 -- .../controller_manager.py | 43 +- 10 files changed, 31 insertions(+), 514 deletions(-) delete mode 100644 controller_manager_utils/controller_manager_utils/__init__.py delete mode 100644 controller_manager_utils/controller_manager_utils/utils.py delete mode 100644 controller_manager_utils/package.xml delete mode 100644 controller_manager_utils/resource/controller_manager_utils delete mode 100644 controller_manager_utils/setup.cfg delete mode 100644 controller_manager_utils/setup.py delete mode 100644 controller_manager_utils/test/test_copyright.py delete mode 100644 controller_manager_utils/test/test_flake8.py delete mode 100644 controller_manager_utils/test/test_pep257.py diff --git a/controller_manager_utils/controller_manager_utils/__init__.py b/controller_manager_utils/controller_manager_utils/__init__.py deleted file mode 100644 index e69de29bb2..0000000000 diff --git a/controller_manager_utils/controller_manager_utils/utils.py b/controller_manager_utils/controller_manager_utils/utils.py deleted file mode 100644 index dba62da6cd..0000000000 --- a/controller_manager_utils/controller_manager_utils/utils.py +++ /dev/null @@ -1,384 +0,0 @@ -#!/usr/bin/env python - -# Copyright 2022 PAL Robotics S.L. -# -# 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. - -# NOTE: The Python API contained in this file is considered UNSTABLE and -# subject to change. -# No backwards compatibility guarantees are provided at this moment. - - -import rclpy -from controller_manager_msgs.srv import ListControllers -from ros2param.api import call_list_parameters - -# Names of controller manager services, and their respective types -_LIST_CONTROLLERS_STR = "list_controllers" -_LIST_CONTROLLERS_TYPE = "controller_manager_msgs/srv/ListControllers" -_LIST_CONTROLLER_TYPES_STR = "list_controller_types" -_LIST_CONTROLLER_TYPES_TYPE = "controller_manager_msgs/srv/ListControllerTypes" -_LOAD_CONTROLLER_STR = "load_controller" -_LOAD_CONTROLLER_TYPE = "controller_manager_msgs/srv/LoadController" -_UNLOAD_CONTROLLER_STR = "unload_controller" -_UNLOAD_CONTROLLER_TYPE = "controller_manager_msgs/srv/UnloadController" -_SWITCH_CONTROLLER_STR = "switch_controller" -_SWITCH_CONTROLLER_TYPE = "controller_manager_msgs/srv/SwitchController" -_RELOAD_CONTROLLER_LIBS_STR = "reload_controller_libraries" -_RELOAD_CONTROLLER_LIBS_TYPE = "controller_manager_msgs/srv/" "ReloadControllerLibraries" - -# Map from service names to their respective type -cm_services = { - _LIST_CONTROLLERS_STR: _LIST_CONTROLLERS_TYPE, - _LIST_CONTROLLER_TYPES_STR: _LIST_CONTROLLER_TYPES_TYPE, - _LOAD_CONTROLLER_STR: _LOAD_CONTROLLER_TYPE, - _UNLOAD_CONTROLLER_STR: _UNLOAD_CONTROLLER_TYPE, - _SWITCH_CONTROLLER_STR: _SWITCH_CONTROLLER_TYPE, - _RELOAD_CONTROLLER_LIBS_STR: _RELOAD_CONTROLLER_LIBS_TYPE, -} - - -def get_controller_managers(namespace="/", initial_guess=None): - """ - Get list of active controller manager namespaces. - - @param namespace: Namespace where to look for controller managers. - @type namespace: str - @param initial_guess: Initial guess of the active controller managers. - Typically c{initial_guess} is the output of a previous call to this method, - and is useful when periodically checking for changes in the list of - active controller managers. - Elements in this list will go through a lazy validity check (as opposed to - a full name+type API verification), so providing a good estimate can - significantly reduce the number of ROS master queries incurred by this - method. - @type initial_guess: [str] - @return: Sorted list of active controller manager namespaces. - @rtype: [str] - """ - ns_list = [] - if initial_guess is not None: - ns_list = initial_guess[:] # force copy - - # Get list of (potential) currently running controller managers - node = rclpy.node.Node("get_controller_managers_node") - ns_list_curr = _sloppy_get_controller_managers(node, namespace) - - # Update initial guess: - # 1. Remove entries not found in current list - # 2. Add new untracked controller managers - ns_list[:] = [ns for ns in ns_list if ns in ns_list_curr] - ns_list += [ns for ns in ns_list_curr if ns not in ns_list and is_controller_manager(node, ns)] - - return sorted(ns_list) - - -def is_controller_manager(node, namespace): - """ - Check if the input namespace exposes the controller_manager ROS interface. - - This method has the overhead of several ROS master queries - (one per ROS API member). - - @param namespace: Namespace to check - @type namespace: str - @return: True if namespace exposes the controller_manager ROS interface - @rtype: bool - """ - cm_ns = namespace - if not cm_ns or cm_ns[-1] != "/": - cm_ns += "/" - for srv_name in cm_services.keys(): - if not _srv_exists(node, cm_ns + srv_name, cm_services[srv_name]): - return False - return True - - -def _sloppy_get_controller_managers(node, namespace): - """ - Get list of I{potential} active controller manager namespaces. - - The method name is prepended with I{sloppy}, and the returned list contains - I{potential} active controller managers because it does not perform a - thorough check of the expected ROS API. - It rather tries to minimize the number of ROS master queries. - - This method has the overhead of one ROS master query. - - @param namespace: Namespace where to look for controller managers. - @type namespace: str - @return: List of I{potential} active controller manager namespaces. - @rtype: [str] - """ - # refresh the list of controller managers we can find - srv_list = node.get_service_names_and_types() - - ns_list = [] - for srv_info in srv_list: - match_str = "/" + _LIST_CONTROLLERS_STR - # First element of srv_name is the service name - if match_str in srv_info[0]: - ns = srv_info[0].split(match_str)[0] - if ns == "": - # controller manager services live in root namespace - # unlikely, but still possible - ns = "/" - ns_list.append(ns) - return ns_list - - -def _srv_exists(node, srv_name, srv_type): - """ - Check if a ROS service of specific name and type exists. - - This method has the overhead of one ROS master query. - - @param srv_name: Fully qualified service name - @type srv_name: str - @param srv_type: Service type - @type srv_type: str - """ - if not srv_name or not srv_type: - return False - - srv_list = node.get_service_names_and_types() - srv_info = [item for item in srv_list if item[0] == srv_name] - srv_obtained_type = srv_info[0][1][0] - return srv_type == srv_obtained_type - - -############################################################################### -# -# Convenience classes for querying controller managers and controllers -# -############################################################################### - - -class ControllerManagerLister: - """ - Convenience functor for querying the list of active controller managers. - - Useful when frequently updating the list, as it internally performs - some optimizations that reduce the number of interactions with the - ROS master. - - Example usage: - >>> list_cm = ControllerManagerLister() - >>> print(list_cm()) - """ - - def __init__(self, namespace="/"): - self._ns = namespace - self._cm_list = [] - - def __call__(self): - """Get list of running controller managers.""" - self._cm_list = get_controller_managers(self._ns, self._cm_list) - return self._cm_list - - -class ControllerLister: - """ - Convenience functor for querying loaded controller data. - - The output of calling this functor can be used as input to the different - controller filtering functions available in this module. - - Example usage. Get I{running} controllers of type C{bar_base/bar}: - >>> list_controllers = ControllerLister('foo_robot/controller_manager') - >>> all_ctrl = list_controllers() - >>> running_ctrl = filter_by_state(all_ctrl, 'running') - >>> running_bar_ctrl = filter_by_type(running_ctrl, 'bar_base/bar') - """ - - def __init__(self, namespace="/controller_manager"): - """ - @param namespace Namespace of controller manager to monitor. - - @type namespace str - """ - self._node = rclpy.node.Node("controller_lister") - self._srv_name = namespace + "/" + _LIST_CONTROLLERS_STR - self._srv_client = self._create_client() - - """ - @return: Controller list. - @rtype: [controller_manager_msgs/ControllerState] - """ - - def __call__(self): - controller_list = self._srv_client.call_async(ListControllers.Request()) - rclpy.spin_until_future_complete(self._node, controller_list) - return controller_list.result().controller - - def _create_client(self): - return self._node.create_client(ListControllers, self._srv_name) - - -############################################################################### -# -# Convenience methods for filtering controller state information -# -############################################################################### - - -def filter_by_name(ctrl_list, ctrl_name, match_substring=False): - """ - Filter controller state list by controller name. - - @param ctrl_list: Controller state list - @type ctrl_list: [controller_manager_msgs/ControllerState] - @param ctrl_name: Controller name - @type ctrl_name: str - @param match_substring: Set to True to allow substring matching - @type match_substring: bool - @return: Controllers matching the specified name - @rtype: [controller_manager_msgs/ControllerState] - """ - return _filter_by_attr(ctrl_list, "name", ctrl_name, match_substring) - - -def filter_by_type(ctrl_list, ctrl_type, match_substring=False): - """ - Filter controller state list by controller type. - - @param ctrl_list: Controller state list - @type ctrl_list: [controller_manager_msgs/ControllerState] - @param ctrl_type: Controller type - @type ctrl_type: str - @param match_substring: Set to True to allow substring matching - @type match_substring: bool - @return: Controllers matching the specified type - @rtype: [controller_manager_msgs/ControllerState] - """ - return _filter_by_attr(ctrl_list, "type", ctrl_type, match_substring) - - -def filter_by_state(ctrl_list, ctrl_state, match_substring=False): - """ - Filter controller state list by controller state. - - @param ctrl_list: Controller state list - @type ctrl_list: [controller_manager_msgs/ControllerState] - @param ctrl_state: Controller state - @type ctrl_state: str - @param match_substring: Set to True to allow substring matching - @type match_substring: bool - @return: Controllers matching the specified state - @rtype: [controller_manager_msgs/ControllerState] - """ - return _filter_by_attr(ctrl_list, "state", ctrl_state, match_substring) - - -def filter_by_hardware_interface(ctrl_list, hardware_interface, match_substring=False): - """ - Filter controller state list by controller hardware interface. - - @param ctrl_list: Controller state list - @type ctrl_list: [controller_manager_msgs/ControllerState] - @param hardware_interface: Controller hardware interface - @type hardware_interface: str - @param match_substring: Set to True to allow substring matching - @type match_substring: bool - @return: Controllers matching the specified hardware interface - @rtype: [controller_manager_msgs/ControllerState] - """ - list_out = [] - for ctrl in ctrl_list: - for resource_set in ctrl.claimed_resources: - if match_substring: - if hardware_interface in resource_set.hardware_interface: - list_out.append(ctrl) - break - else: - if resource_set.hardware_interface == hardware_interface: - list_out.append(ctrl) - break - return list_out - - -def filter_by_resources(ctrl_list, resources, hardware_interface=None, match_any=False): - """ - Filter controller state list by claimed resources. - - @param ctrl_list: Controller state list - @type ctrl_list: [controller_manager_msgs/ControllerState] - @param resources: Controller resources - @type resources: [str] - @param hardware_interface Controller hardware interface where to look for - resources. If specified, the requested resources will only be searched for - in this interface. If unspecified, all controller hardware interfaces will - be searched for; i.e., if a controller claims resources from multiple - interfaces, the method will succeed if _any_ interface contains the - requested resources (any or all, depending on the value of C{match_any}). - Specifying this parameter allows finer control over determining which - interfaces claim specific resources. - @param match_any: If set to False, all elements in C{resources} must - be claimed by the interface specified in C{hardware_interface} (or _any_ - interface, if C{hardware_interface} is unspecified) for a positive match. - Note that a controller's resources can contain additional entries than - those in C{resources}). - If set to True, at least one element in C{resources} must be claimed by - the interface specified in C{hardware_interface} (or _any_ interface, if - C{hardware_interface} is unspecified) for a positive match. - @type match_any: bool - @return: Controllers matching the specified hardware interface - @rtype: [controller_manager_msgs/ControllerState] - """ - list_out = [] - for ctrl in ctrl_list: - for resource_set in ctrl.claimed_resources: - if hardware_interface is None or hardware_interface == resource_set.hardware_interface: - for res in resources: - add_ctrl = not match_any # Initial flag value - if res in resource_set.resources: - if match_any: # One hit: enough to accept controller - add_ctrl = True - break - else: - if not match_any: # One miss: enough to discard controller - add_ctrl = False - break - if add_ctrl: - list_out.append(ctrl) - break - return list_out - - -def _filter_by_attr(list_in, attr_name, attr_val, match_substring=False): - """Filter input list by the value of its elements' attributes.""" - list_out = [] - for val in list_in: - if match_substring: - if attr_val in getattr(val, attr_name): - list_out.append(val) - else: - if getattr(val, attr_name) == attr_val: - list_out.append(val) - return list_out - - -############################################################################### -# -# Convenience methods for finding potential controller configurations -# -############################################################################### - -def get_parameter_controller_names(node, node_name): - """ - Get list of ROS parameter names that potentially represent a controller - configuration. - """ - parameter_names = call_list_parameters(node=node, node_name=node_name) - suffix = '.type' - return [n[: -len(suffix)] for n in parameter_names if n.endswith(suffix)] diff --git a/controller_manager_utils/package.xml b/controller_manager_utils/package.xml deleted file mode 100644 index 7eccaef177..0000000000 --- a/controller_manager_utils/package.xml +++ /dev/null @@ -1,18 +0,0 @@ - - - - controller_manager_utils - 0.0.0 - TODO: Package description - ijnek - TODO: License declaration - - ament_copyright - ament_flake8 - ament_pep257 - python3-pytest - - - ament_python - - diff --git a/controller_manager_utils/resource/controller_manager_utils b/controller_manager_utils/resource/controller_manager_utils deleted file mode 100644 index e69de29bb2..0000000000 diff --git a/controller_manager_utils/setup.cfg b/controller_manager_utils/setup.cfg deleted file mode 100644 index ef29745e4d..0000000000 --- a/controller_manager_utils/setup.cfg +++ /dev/null @@ -1,4 +0,0 @@ -[develop] -script_dir=$base/lib/controller_manager_utils -[install] -install_scripts=$base/lib/controller_manager_utils diff --git a/controller_manager_utils/setup.py b/controller_manager_utils/setup.py deleted file mode 100644 index 886af656d7..0000000000 --- a/controller_manager_utils/setup.py +++ /dev/null @@ -1,25 +0,0 @@ -from setuptools import setup - -package_name = 'controller_manager_utils' - -setup( - name=package_name, - version='0.0.0', - packages=[package_name], - data_files=[ - ('share/ament_index/resource_index/packages', - ['resource/' + package_name]), - ('share/' + package_name, ['package.xml']), - ], - install_requires=['setuptools'], - zip_safe=True, - maintainer='ijnek', - maintainer_email='kenjibrameld@gmail.com', - description='TODO: Package description', - license='TODO: License declaration', - tests_require=['pytest'], - entry_points={ - 'console_scripts': [ - ], - }, -) diff --git a/controller_manager_utils/test/test_copyright.py b/controller_manager_utils/test/test_copyright.py deleted file mode 100644 index cc8ff03f79..0000000000 --- a/controller_manager_utils/test/test_copyright.py +++ /dev/null @@ -1,23 +0,0 @@ -# Copyright 2015 Open Source Robotics Foundation, Inc. -# -# 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. - -from ament_copyright.main import main -import pytest - - -@pytest.mark.copyright -@pytest.mark.linter -def test_copyright(): - rc = main(argv=['.', 'test']) - assert rc == 0, 'Found errors' diff --git a/controller_manager_utils/test/test_flake8.py b/controller_manager_utils/test/test_flake8.py deleted file mode 100644 index 27ee1078ff..0000000000 --- a/controller_manager_utils/test/test_flake8.py +++ /dev/null @@ -1,25 +0,0 @@ -# Copyright 2017 Open Source Robotics Foundation, Inc. -# -# 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. - -from ament_flake8.main import main_with_errors -import pytest - - -@pytest.mark.flake8 -@pytest.mark.linter -def test_flake8(): - rc, errors = main_with_errors(argv=[]) - assert rc == 0, \ - 'Found %d code style errors / warnings:\n' % len(errors) + \ - '\n'.join(errors) diff --git a/controller_manager_utils/test/test_pep257.py b/controller_manager_utils/test/test_pep257.py deleted file mode 100644 index b234a3840f..0000000000 --- a/controller_manager_utils/test/test_pep257.py +++ /dev/null @@ -1,23 +0,0 @@ -# Copyright 2015 Open Source Robotics Foundation, Inc. -# -# 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. - -from ament_pep257.main import main -import pytest - - -@pytest.mark.linter -@pytest.mark.pep257 -def test_pep257(): - rc = main(argv=['.', 'test']) - assert rc == 0, 'Found code style errors / warnings' diff --git a/rqt_controller_manager/rqt_controller_manager/controller_manager.py b/rqt_controller_manager/rqt_controller_manager/controller_manager.py index 35b6a0adf3..f675249239 100644 --- a/rqt_controller_manager/rqt_controller_manager/controller_manager.py +++ b/rqt_controller_manager/rqt_controller_manager/controller_manager.py @@ -37,13 +37,12 @@ from controller_manager_msgs.msg import ControllerState from controller_manager_msgs.srv import SwitchController -from controller_manager_utils.utils import ControllerLister, ControllerManagerLister, \ - get_parameter_controller_names from controller_manager.controller_manager_services import configure_controller, \ list_controllers, load_controller, switch_controllers, unload_controller from .update_combo import update_combo -from ros2param.api import call_get_parameters +from ros2param.api import call_get_parameters, call_list_parameters +from ros2service.api import get_service_names_and_types class ControllerManager(Plugin): @@ -90,7 +89,6 @@ def __init__(self, context): self._cm_name = '' # Name of the selected controller manager's node self._controllers = [] # State of each controller self._table_model = None - self._controller_lister = None # Store reference to node self._node = context.node @@ -115,7 +113,6 @@ def __init__(self, context): header.customContextMenuRequested.connect(self._on_header_menu) # Timer for controller manager updates - self._list_cm = ControllerManagerLister() self._update_cm_list_timer = QTimer(self) self._update_cm_list_timer.setInterval(int(1000.0 / self._cm_update_freq)) self._update_cm_list_timer.timeout.connect(self._update_cm_list) @@ -152,17 +149,13 @@ def restore_settings(self, plugin_settings, instance_settings): pass def _update_cm_list(self): - update_combo(self._widget.cm_combo, self._list_cm()) + update_combo(self._widget.cm_combo, _list_controller_managers(self._node)) def _on_cm_change(self, cm_name): self._cm_name = cm_name - # Controller lister for the selected controller manager if cm_name: - self._controller_lister = ControllerLister(cm_name) self._update_controllers() - else: - self._controller_lister = None def _update_controllers(self): @@ -186,11 +179,10 @@ def _list_controllers(self): @rtype [str] """ # Add loaded controllers first - controllers = self._controller_lister() controllers = list_controllers(self._node, self._cm_name).controller # Append potential controller configs found in the node's parameters - for name in get_parameter_controller_names(self._node, self._cm_name): + for name in _get_parameter_controller_names(self._node, self._cm_name): add_ctrl = not any(name == ctrl.name for ctrl in controllers) if add_ctrl: type_str = _get_controller_type(self._node, self._cm_name, name) @@ -413,3 +405,30 @@ def _get_controller_type(node, node_name, ctrl_name): return '' else: return response.values[0].string_value + + +def _list_controller_managers(node): + """ + List controller manager nodes that are active. Does this by looking for a service that should + be exclusive to a controller manager node. The "list_controllers" service is used to determine + if a node is a controller manager. + @return List of controller manager node names + @rtype list of str + """ + controller_manager_names = [] + for (name, _) in get_service_names_and_types(node=node): + if name.endswith('list_controllers'): + name = name.rstrip('list_controllers') + name = name.rstrip('/') + controller_manager_names.append(name) + return controller_manager_names + + +def _get_parameter_controller_names(node, node_name): + """ + Get list of ROS parameter names that potentially represent a controller + configuration. + """ + parameter_names = call_list_parameters(node=node, node_name=node_name) + suffix = '.type' + return [n[: -len(suffix)] for n in parameter_names if n.endswith(suffix)] From 7de548b8e30045b8e24a5976ad7b175da4215517 Mon Sep 17 00:00:00 2001 From: Kenji Brameld Date: Mon, 19 Sep 2022 14:11:30 +0900 Subject: [PATCH 20/25] fix format and copyright to pass tests Signed-off-by: Kenji Brameld --- .../controller_manager.py | 81 +++++++++---------- .../rqt_controller_manager/update_combo.py | 38 ++++----- rqt_controller_manager/setup.py | 3 +- 3 files changed, 52 insertions(+), 70 deletions(-) diff --git a/rqt_controller_manager/rqt_controller_manager/controller_manager.py b/rqt_controller_manager/rqt_controller_manager/controller_manager.py index f675249239..639d1a7582 100644 --- a/rqt_controller_manager/rqt_controller_manager/controller_manager.py +++ b/rqt_controller_manager/rqt_controller_manager/controller_manager.py @@ -1,54 +1,40 @@ -#!/usr/bin/env python -# Copyright (C) 2013, Georgia Tech -# Copyright (C) 2015, PAL Robotics S.L. -# Copyright (C) 2022, Kenji Brameld +# Copyright 2013 Georgia Tech +# Copyright 2015 PAL Robotics S.L. +# Copyright 2022 Kenji Brameld # -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions are met: -# * Redistributions of source code must retain the above copyright notice, -# this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above copyright -# notice, this list of conditions and the following disclaimer in the -# documentation and/or other materials provided with the distribution. -# * Neither the name of PAL Robotics S.L. nor the names of its -# contributors may be used to endorse or promote products derived from -# this software without specific prior written permission. +# 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 # -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE -# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE -# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR -# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF -# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS -# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN -# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) -# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. +# 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. import os from ament_index_python.packages import get_package_share_directory +from controller_manager.controller_manager_services import configure_controller, \ + list_controllers, load_controller, switch_controllers, unload_controller +from controller_manager_msgs.msg import ControllerState +from controller_manager_msgs.srv import SwitchController from python_qt_binding import loadUi from python_qt_binding.QtCore import QAbstractTableModel, Qt, QTimer -from python_qt_binding.QtWidgets import QWidget, QHeaderView, QMenu, QStyledItemDelegate from python_qt_binding.QtGui import QCursor, QFont, QIcon, QStandardItem, QStandardItemModel +from python_qt_binding.QtWidgets import QHeaderView, QMenu, QStyledItemDelegate, QWidget from qt_gui.plugin import Plugin - -from controller_manager_msgs.msg import ControllerState -from controller_manager_msgs.srv import SwitchController -from controller_manager.controller_manager_services import configure_controller, \ - list_controllers, load_controller, switch_controllers, unload_controller - -from .update_combo import update_combo from ros2param.api import call_get_parameters, call_list_parameters from ros2service.api import get_service_names_and_types +from .update_combo import update_combo + class ControllerManager(Plugin): - """ - Graphical frontend for managing ros_control controllers. - """ + """Graphical frontend for managing ros_control controllers.""" + _cm_update_freq = 1 # Hz def __init__(self, context): @@ -172,6 +158,8 @@ def _update_controllers(self): def _list_controllers(self): """ + List the controllers associated to a controller manager node. + @return List of controllers associated to a controller manager node. Contains both stopped/running controllers, as returned by the C{list_controllers} service, plus uninitialized controllers with @@ -321,6 +309,7 @@ class ControllerTable(QAbstractTableModel): The model allows display of basic read-only information like controller name and state. """ + def __init__(self, controller_info, icons, parent=None): QAbstractTableModel.__init__(self, parent) self._data = controller_info @@ -376,9 +365,12 @@ def data(self, index, role): class FontDelegate(QStyledItemDelegate): """ - Simple delegate for customizing font weight and italization when - displaying resources claimed by a controller + Simple delegate for customizing font weight and italization. + + Simple delegate for customizing font weight and italization when displaying resources claimed + by a controller. """ + def paint(self, painter, option, index): if not index.parent().isValid(): # Root level @@ -393,6 +385,7 @@ def paint(self, painter, option, index): def _get_controller_type(node, node_name, ctrl_name): """ Get the controller's type from the controller manager node with the call_get_parameter service. + @param node_name Controller manager node's name @type node_name str @param ctrl_name Controller name @@ -409,9 +402,10 @@ def _get_controller_type(node, node_name, ctrl_name): def _list_controller_managers(node): """ - List controller manager nodes that are active. Does this by looking for a service that should - be exclusive to a controller manager node. The "list_controllers" service is used to determine - if a node is a controller manager. + List controller manager nodes that are active. + + Does this by looking for a service that should be exclusive to a controller manager node. + The "list_controllers" service is used to determine if a node is a controller manager. @return List of controller manager node names @rtype list of str """ @@ -425,10 +419,7 @@ def _list_controller_managers(node): def _get_parameter_controller_names(node, node_name): - """ - Get list of ROS parameter names that potentially represent a controller - configuration. - """ + """Get list of ROS parameter names that potentially represent a controller configuration.""" parameter_names = call_list_parameters(node=node, node_name=node_name) suffix = '.type' return [n[: -len(suffix)] for n in parameter_names if n.endswith(suffix)] diff --git a/rqt_controller_manager/rqt_controller_manager/update_combo.py b/rqt_controller_manager/rqt_controller_manager/update_combo.py index b06e38e453..f1ebe8ed74 100644 --- a/rqt_controller_manager/rqt_controller_manager/update_combo.py +++ b/rqt_controller_manager/rqt_controller_manager/update_combo.py @@ -1,29 +1,16 @@ -#!/usr/bin/env python - -# Copyright (C) 2014, PAL Robotics S.L. +# Copyright 2021 PAL Robotics S.L. +# +# 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 # -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions are met: -# * Redistributions of source code must retain the above copyright notice, -# this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above copyright -# notice, this list of conditions and the following disclaimer in the -# documentation and/or other materials provided with the distribution. -# * Neither the name of PAL Robotics S.L. nor the names of its -# contributors may be used to endorse or promote products derived from -# this software without specific prior written permission. +# http://www.apache.org/licenses/LICENSE-2.0 # -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE -# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE -# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR -# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF -# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS -# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN -# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) -# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. +# 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. def update_combo(combo, new_vals): """ @@ -56,8 +43,11 @@ def update_combo(combo, new_vals): combo.setCurrentIndex(selected_id) # Restore selection combo.blockSignals(False) + def _is_permutation(a, b): """ + Return true if a is a permutation of b, false otherwise. + @type a [] @type b [] @return True if C{a} is a permutation of C{b}, false otherwise diff --git a/rqt_controller_manager/setup.py b/rqt_controller_manager/setup.py index 3e350bd412..cec8e2a18f 100644 --- a/rqt_controller_manager/setup.py +++ b/rqt_controller_manager/setup.py @@ -1,6 +1,7 @@ -from setuptools import setup from glob import glob +from setuptools import setup + package_name = 'rqt_controller_manager' setup( From f58698fa7a20134d8dc58eee7d049c6d95c80fce Mon Sep 17 00:00:00 2001 From: Kenji Brameld Date: Mon, 19 Sep 2022 14:12:00 +0900 Subject: [PATCH 21/25] change license of rqt_controller_manager Signed-off-by: Kenji Brameld --- rqt_controller_manager/package.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rqt_controller_manager/package.xml b/rqt_controller_manager/package.xml index a1c1f4ad6b..04f0783f0c 100644 --- a/rqt_controller_manager/package.xml +++ b/rqt_controller_manager/package.xml @@ -5,7 +5,7 @@ 0.0.0 Graphical frontend for interacting with the controller manager. ijnek - BSD + Apache License 2.0 http://ros.org/wiki/rqt_controller_manager https://github.com/ros-controls/ros2_control/issues From fa97b3bab46201cf8d5a40f7f13fc0793ab37322 Mon Sep 17 00:00:00 2001 From: Kenji Brameld Date: Mon, 19 Sep 2022 14:19:15 +0900 Subject: [PATCH 22/25] update plugin.xml Signed-off-by: Kenji Brameld --- rqt_controller_manager/plugin.xml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/rqt_controller_manager/plugin.xml b/rqt_controller_manager/plugin.xml index 993d7fd56d..ec89de61d7 100644 --- a/rqt_controller_manager/plugin.xml +++ b/rqt_controller_manager/plugin.xml @@ -3,7 +3,7 @@ type="rqt_controller_manager.controller_manager.ControllerManager" base_class_type="rqt_gui_py::Plugin"> - Visual interface for managing ros_control controllers. + Graphical frontend for interacting with the controller manager. @@ -13,7 +13,7 @@ resource/cm_icon.png - Manage ros_control controllers + Interact with controller managers From eddc16d7c5bd84b0a5fc15d2c6c0a744a80635ab Mon Sep 17 00:00:00 2001 From: Kenji Brameld Date: Mon, 19 Sep 2022 14:19:29 +0900 Subject: [PATCH 23/25] remove redundant dependency Signed-off-by: Kenji Brameld --- rqt_controller_manager/package.xml | 1 - 1 file changed, 1 deletion(-) diff --git a/rqt_controller_manager/package.xml b/rqt_controller_manager/package.xml index 04f0783f0c..c8de4e30ff 100644 --- a/rqt_controller_manager/package.xml +++ b/rqt_controller_manager/package.xml @@ -18,7 +18,6 @@ Adolfo Rodríguez Tsouroukdissian controller_manager_msgs - controller_manager_utils rclpy rqt_gui rqt_gui_py From fa8af821493d5f9f8995621109c2aaae5f9cf893 Mon Sep 17 00:00:00 2001 From: Kenji Brameld Date: Mon, 19 Sep 2022 14:20:53 +0900 Subject: [PATCH 24/25] stretch last section in rqt_controller_manager table Signed-off-by: Kenji Brameld --- rqt_controller_manager/resource/controller_manager.ui | 3 +++ 1 file changed, 3 insertions(+) diff --git a/rqt_controller_manager/resource/controller_manager.ui b/rqt_controller_manager/resource/controller_manager.ui index 612a4388a0..d6394c1b0d 100644 --- a/rqt_controller_manager/resource/controller_manager.ui +++ b/rqt_controller_manager/resource/controller_manager.ui @@ -62,6 +62,9 @@ false + + true + false From ad05d42cd965dda7ad559c4f6547802de064854d Mon Sep 17 00:00:00 2001 From: Kenji Brameld Date: Mon, 19 Sep 2022 14:21:05 +0900 Subject: [PATCH 25/25] update docstring Signed-off-by: Kenji Brameld --- .../rqt_controller_manager/controller_manager.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rqt_controller_manager/rqt_controller_manager/controller_manager.py b/rqt_controller_manager/rqt_controller_manager/controller_manager.py index 639d1a7582..7fde28b802 100644 --- a/rqt_controller_manager/rqt_controller_manager/controller_manager.py +++ b/rqt_controller_manager/rqt_controller_manager/controller_manager.py @@ -33,7 +33,7 @@ class ControllerManager(Plugin): - """Graphical frontend for managing ros_control controllers.""" + """Graphical frontend for interacting with the controller manager.""" _cm_update_freq = 1 # Hz